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_);
typename PointCloud::Ptr PointCloudPtr
PointCloud represents the base class in PCL for storing collections of 3D points.
ELCH (Explicit Loop Closing Heuristic) class
virtual bool initCompute()
This method should get called before starting the actual computation.
void compute()
Computes new poses for all point clouds by closing the loop between start and end point cloud.
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.