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);
271 add_edge (loop_start_, loop_end_, *loop_graph_);
276 #endif // PCL_REGISTRATION_IMPL_ELCH_H_
void compute()
Computes now poses for all point clouds by closing the loop between start and end point cloud...
PointCloud::Ptr PointCloudPtr
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transform)
Apply an affine transform defined by an Eigen Transform.
ELCH (Explicit Loop Closing Heuristic) class
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. ...