41 #ifndef PCL_REGISTRATION_IMPL_ELCH_H_
42 #define PCL_REGISTRATION_IMPL_ELCH_H_
48 #include <pcl/common/transforms.h>
49 #include <pcl/registration/eigen.h>
50 #include <pcl/registration/boost.h>
51 #include <pcl/registration/registration.h>
54 template <
typename Po
intT>
void
57 std::list<int> crossings, branches;
58 crossings.push_back (
static_cast<int> (loop_start_));
59 crossings.push_back (
static_cast<int> (loop_end_));
60 weights[loop_start_] = 0;
61 weights[loop_end_] = 1;
63 int *p =
new int[num_vertices (g)];
64 int *p_min =
new int[num_vertices (g)];
65 double *d =
new double[num_vertices (g)];
66 double *d_min =
new double[num_vertices (g)];
69 std::list<int>::iterator start_min, end_min;
72 while (!crossings.empty ())
76 for (
auto crossings_it = crossings.begin (); crossings_it != crossings.end (); )
78 dijkstra_shortest_paths (g, *crossings_it,
79 predecessor_map(boost::make_iterator_property_map(p, get(boost::vertex_index, g))).
80 distance_map(boost::make_iterator_property_map(d, get(boost::vertex_index, g))));
82 auto end_it = crossings_it;
85 for (; end_it != crossings.end (); end_it++)
87 if (*end_it != p[*end_it] && (dist < 0 || d[*end_it] < dist))
90 start_min = crossings_it;
104 branches.push_back (
static_cast<int> (*crossings_it));
105 crossings_it = crossings.erase (crossings_it);
113 remove_edge (*end_min, p_min[*end_min], g);
114 for (
int i = p_min[*end_min]; i != *start_min; i = p_min[i])
117 weights[i] = weights[*start_min] + (weights[*end_min] - weights[*start_min]) * d_min[i] / d_min[*end_min];
118 remove_edge (i, p_min[i], g);
119 if (degree (i, g) > 0)
121 crossings.push_back (i);
125 if (degree (*start_min, g) == 0)
126 crossings.erase (start_min);
128 if (degree (*end_min, g) == 0)
129 crossings.erase (end_min);
138 boost::graph_traits<LOAGraph>::adjacency_iterator adjacent_it, adjacent_it_end;
142 while (!branches.empty ())
144 s = branches.front ();
145 branches.pop_front ();
147 for (std::tie (adjacent_it, adjacent_it_end) = adjacent_vertices (s, g); adjacent_it != adjacent_it_end; ++adjacent_it)
149 weights[*adjacent_it] = weights[s];
150 if (degree (*adjacent_it, g) > 1)
151 branches.push_back (
static_cast<int> (*adjacent_it));
158 template <
typename Po
intT>
bool
169 PCL_ERROR (
"[pcl::registration::ELCH::initCompute] no end of loop defined!\n");
179 *meta_start = *(*loop_graph_)[loop_start_].cloud;
180 *meta_end = *(*loop_graph_)[loop_end_].cloud;
182 typename boost::graph_traits<LoopGraph>::adjacency_iterator si, si_end;
183 for (std::tie (si, si_end) = adjacent_vertices (loop_start_, *loop_graph_); si != si_end; si++)
184 *meta_start += *(*loop_graph_)[*si].cloud;
186 for (std::tie (si, si_end) = adjacent_vertices (loop_end_, *loop_graph_); si != si_end; si++)
187 *meta_end += *(*loop_graph_)[*si].cloud;
202 reg_->setInputTarget (meta_start);
204 reg_->setInputSource (meta_end);
208 loop_transform_ = reg_->getFinalTransformation ();
218 template <
typename Po
intT>
void
228 typename boost::graph_traits<LoopGraph>::edge_iterator edge_it, edge_it_end;
229 for (std::tie (edge_it, edge_it_end) = edges (*loop_graph_); edge_it != edge_it_end; edge_it++)
232 add_edge (source (*edge_it, *loop_graph_), target (*edge_it, *loop_graph_), 1, j);
236 for (
int i = 0; i < 4; i++)
238 weights[i] =
new double[num_vertices (*loop_graph_)];
239 loopOptimizerAlgorithm (grb[i], weights[i]);
252 for (std::size_t i = 0; i < num_vertices (*loop_graph_); i++)
255 t2[0] = loop_transform_ (0, 3) *
static_cast<float> (weights[0][i]);
256 t2[1] = loop_transform_ (1, 3) *
static_cast<float> (weights[1][i]);
257 t2[2] = loop_transform_ (2, 3) *
static_cast<float> (weights[2][i]);
259 Eigen::Affine3f bl (loop_transform_);
260 Eigen::Quaternionf q (bl.rotation ());
261 Eigen::Quaternionf q2;
262 q2 = Eigen::Quaternionf::Identity ().slerp (
static_cast<float> (weights[3][i]), q);
265 Eigen::Translation3f t3 (t2);
266 Eigen::Affine3f a (t3 * q2);
270 (*loop_graph_)[i].transform = a;
273 add_edge (loop_start_, loop_end_, *loop_graph_);
278 #endif // PCL_REGISTRATION_IMPL_ELCH_H_