41 #ifndef PCL_REGISTRATION_IMPL_ELCH_H_
42 #define PCL_REGISTRATION_IMPL_ELCH_H_
47 #include <pcl/common/transforms.h>
48 #include <pcl/registration/eigen.h>
49 #include <pcl/registration/boost.h>
50 #include <pcl/registration/registration.h>
53 template <
typename Po
intT>
void
56 std::list<int> crossings, branches;
57 crossings.push_back (
static_cast<int> (loop_start_));
58 crossings.push_back (
static_cast<int> (loop_end_));
59 weights[loop_start_] = 0;
60 weights[loop_end_] = 1;
62 int *p =
new int[num_vertices (g)];
63 int *p_min =
new int[num_vertices (g)];
64 double *d =
new double[num_vertices (g)];
65 double *d_min =
new double[num_vertices (g)];
68 std::list<int>::iterator crossings_it, end_it, start_min, end_min;
71 while (!crossings.empty ())
75 for (crossings_it = crossings.begin (); crossings_it != crossings.end (); )
77 dijkstra_shortest_paths (g, *crossings_it,
78 predecessor_map(boost::make_iterator_property_map(p, get(boost::vertex_index, g))).
79 distance_map(boost::make_iterator_property_map(d, get(boost::vertex_index, g))));
81 end_it = crossings_it;
84 for (; end_it != crossings.end (); end_it++)
86 if (*end_it != p[*end_it] && (dist < 0 || d[*end_it] < dist))
89 start_min = crossings_it;
103 branches.push_back (
static_cast<int> (*crossings_it));
104 crossings_it = crossings.erase (crossings_it);
112 remove_edge (*end_min, p_min[*end_min], g);
113 for (
int i = p_min[*end_min]; i != *start_min; i = p_min[i])
116 weights[i] = weights[*start_min] + (weights[*end_min] - weights[*start_min]) * d_min[i] / d_min[*end_min];
117 remove_edge (i, p_min[i], g);
118 if (degree (i, g) > 0)
120 crossings.push_back (i);
124 if (degree (*start_min, g) == 0)
125 crossings.erase (start_min);
127 if (degree (*end_min, g) == 0)
128 crossings.erase (end_min);
137 boost::graph_traits<LOAGraph>::adjacency_iterator adjacent_it, adjacent_it_end;
141 while (!branches.empty ())
143 s = branches.front ();
144 branches.pop_front ();
146 for (boost::tuples::tie (adjacent_it, adjacent_it_end) = adjacent_vertices (s, g); adjacent_it != adjacent_it_end; ++adjacent_it)
148 weights[*adjacent_it] = weights[s];
149 if (degree (*adjacent_it, g) > 1)
150 branches.push_back (
static_cast<int> (*adjacent_it));
157 template <
typename Po
intT>
bool
168 PCL_ERROR (
"[pcl::registration::ELCH::initCompute] no end of loop defined!\n");
178 *meta_start = *(*loop_graph_)[loop_start_].cloud;
179 *meta_end = *(*loop_graph_)[loop_end_].cloud;
181 typename boost::graph_traits<LoopGraph>::adjacency_iterator si, si_end;
182 for (boost::tuples::tie (si, si_end) = adjacent_vertices (loop_start_, *loop_graph_); si != si_end; si++)
183 *meta_start += *(*loop_graph_)[*si].cloud;
185 for (boost::tuples::tie (si, si_end) = adjacent_vertices (loop_end_, *loop_graph_); si != si_end; si++)
186 *meta_end += *(*loop_graph_)[*si].cloud;
201 reg_->setInputTarget (meta_start);
203 reg_->setInputSource (meta_end);
207 loop_transform_ = reg_->getFinalTransformation ();
217 template <
typename Po
intT>
void
227 typename boost::graph_traits<LoopGraph>::edge_iterator edge_it, edge_it_end;
228 for (boost::tuples::tie (edge_it, edge_it_end) = edges (*loop_graph_); edge_it != edge_it_end; edge_it++)
230 for (
int j = 0; j < 4; j++)
231 add_edge (source (*edge_it, *loop_graph_), target (*edge_it, *loop_graph_), 1, grb[j]);
235 for (
int i = 0; i < 4; i++)
237 weights[i] =
new double[num_vertices (*loop_graph_)];
238 loopOptimizerAlgorithm (grb[i], weights[i]);
251 for (
size_t i = 0; i < num_vertices (*loop_graph_); i++)
254 t2[0] = loop_transform_ (0, 3) *
static_cast<float> (weights[0][i]);
255 t2[1] = loop_transform_ (1, 3) *
static_cast<float> (weights[1][i]);
256 t2[2] = loop_transform_ (2, 3) *
static_cast<float> (weights[2][i]);
258 Eigen::Affine3f bl (loop_transform_);
259 Eigen::Quaternionf q (bl.rotation ());
260 Eigen::Quaternionf q2;
261 q2 = Eigen::Quaternionf::Identity ().slerp (
static_cast<float> (weights[3][i]), q);
264 Eigen::Translation3f t3 (t2);
265 Eigen::Affine3f a (t3 * q2);
269 (*loop_graph_)[i].transform = a;
272 add_edge (loop_start_, loop_end_, *loop_graph_);
277 #endif // PCL_REGISTRATION_IMPL_ELCH_H_