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)];
68 std::list<int>::iterator start_min, end_min;
71 while (!crossings.empty ())
75 for (
auto 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 auto 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;
140 while (!branches.empty ())
142 int s = branches.front ();
143 branches.pop_front ();
145 for (std::tie (adjacent_it, adjacent_it_end) = adjacent_vertices (s, g); adjacent_it != adjacent_it_end; ++adjacent_it)
147 weights[*adjacent_it] = weights[s];
148 if (degree (*adjacent_it, g) > 1)
149 branches.push_back (static_cast<int> (*adjacent_it));
156 template <
typename Po
intT>
bool
167 PCL_ERROR (
"[pcl::registration::ELCH::initCompute] no end of loop defined!\n");
177 *meta_start = *(*loop_graph_)[loop_start_].cloud;
178 *meta_end = *(*loop_graph_)[loop_end_].cloud;
180 typename boost::graph_traits<LoopGraph>::adjacency_iterator si, si_end;
181 for (std::tie (si, si_end) = adjacent_vertices (loop_start_, *loop_graph_); si != si_end; si++)
182 *meta_start += *(*loop_graph_)[*si].cloud;
184 for (std::tie (si, si_end) = adjacent_vertices (loop_end_, *loop_graph_); si != si_end; si++)
185 *meta_end += *(*loop_graph_)[*si].cloud;
200 reg_->setInputTarget (meta_start);
202 reg_->setInputSource (meta_end);
206 loop_transform_ = reg_->getFinalTransformation ();
216 template <
typename Po
intT>
void
226 typename boost::graph_traits<LoopGraph>::edge_iterator edge_it, edge_it_end;
227 for (std::tie (edge_it, edge_it_end) = edges (*loop_graph_); edge_it != edge_it_end; edge_it++)
230 add_edge (source (*edge_it, *loop_graph_), target (*edge_it, *loop_graph_), 1, j);
234 for (
int i = 0; i < 4; i++)
236 weights[i] =
new double[num_vertices (*loop_graph_)];
237 loopOptimizerAlgorithm (grb[i], weights[i]);
250 for (std::size_t i = 0; i < num_vertices (*loop_graph_); i++)
253 t2[0] = loop_transform_ (0, 3) *
static_cast<float> (weights[0][i]);
254 t2[1] = loop_transform_ (1, 3) *
static_cast<float> (weights[1][i]);
255 t2[2] = loop_transform_ (2, 3) *
static_cast<float> (weights[2][i]);
257 Eigen::Affine3f bl (loop_transform_);
258 Eigen::Quaternionf q (bl.rotation ());
259 Eigen::Quaternionf q2;
260 q2 = Eigen::Quaternionf::Identity ().slerp (static_cast<float> (weights[3][i]), q);
263 Eigen::Translation3f t3 (t2);
264 Eigen::Affine3f a (t3 * q2);
268 (*loop_graph_)[i].transform = a;
271 add_edge (loop_start_, loop_end_, *loop_graph_);
276 #endif // PCL_REGISTRATION_IMPL_ELCH_H_
void compute()
Computes new poses for all point clouds by closing the loop between start and end point cloud...
ELCH (Explicit Loop Closing Heuristic) class
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transform, bool copy_all_fields)
Apply an affine transform defined by an Eigen Transform.
typename PointCloud::Ptr PointCloudPtr
virtual bool initCompute()
This method should get called before starting the actual computation.
PointCloud represents the base class in PCL for storing collections of 3D points. ...