41#ifndef PCL_REGISTRATION_IMPL_ELCH_H_
42#define PCL_REGISTRATION_IMPL_ELCH_H_
44#include <pcl/common/transforms.h>
45#include <pcl/registration/registration.h>
47#include <boost/graph/dijkstra_shortest_paths.hpp>
54template <
typename Po
intT>
58 std::list<int> crossings, branches;
59 crossings.push_back(
static_cast<int>(loop_start_));
60 crossings.push_back(
static_cast<int>(loop_end_));
61 weights[loop_start_] = 0;
62 weights[loop_end_] = 1;
64 int* p =
new int[num_vertices(g)];
65 int* p_min =
new int[num_vertices(g)];
66 double* d =
new double[num_vertices(g)];
67 double* d_min =
new double[num_vertices(g)];
69 std::list<int>::iterator start_min, end_min;
72 while (!crossings.empty()) {
75 for (
auto crossings_it = crossings.begin(); crossings_it != crossings.end();) {
76 dijkstra_shortest_paths(g,
78 predecessor_map(boost::make_iterator_property_map(
79 p, get(boost::vertex_index, g)))
80 .distance_map(boost::make_iterator_property_map(
81 d, get(boost::vertex_index, g))));
83 auto end_it = crossings_it;
86 for (; end_it != crossings.end(); end_it++) {
87 if (*end_it != p[*end_it] && (dist < 0 || d[*end_it] < dist)) {
89 start_min = crossings_it;
101 branches.push_back(
static_cast<int>(*crossings_it));
102 crossings_it = crossings.erase(crossings_it);
109 remove_edge(*end_min, p_min[*end_min], g);
110 for (
int i = p_min[*end_min]; i != *start_min; i = p_min[i]) {
112 weights[i] = weights[*start_min] + (weights[*end_min] - weights[*start_min]) *
113 d_min[i] / d_min[*end_min];
114 remove_edge(i, p_min[i], g);
115 if (degree(i, g) > 0) {
116 crossings.push_back(i);
120 if (degree(*start_min, g) == 0)
121 crossings.erase(start_min);
123 if (degree(*end_min, g) == 0)
124 crossings.erase(end_min);
133 boost::graph_traits<LOAGraph>::adjacency_iterator adjacent_it, adjacent_it_end;
136 while (!branches.empty()) {
137 int s = branches.front();
138 branches.pop_front();
140 for (std::tie(adjacent_it, adjacent_it_end) = adjacent_vertices(s, g);
141 adjacent_it != adjacent_it_end;
143 weights[*adjacent_it] = weights[s];
144 if (degree(*adjacent_it, g) > 1)
145 branches.push_back(
static_cast<int>(*adjacent_it));
152template <
typename Po
intT>
162 if (loop_end_ == 0) {
163 PCL_ERROR(
"[pcl::registration::ELCH::initCompute] no end of loop defined!\n");
172 *
meta_start = *(*loop_graph_)[loop_start_].cloud;
173 *
meta_end = *(*loop_graph_)[loop_end_].cloud;
175 typename boost::graph_traits<LoopGraph>::adjacency_iterator
si,
si_end;
205 loop_transform_ = reg_->getFinalTransformation();
214template <
typename Po
intT>
218 if (!initCompute()) {
235 for (
int i = 0; i < 4; i++) {
237 loopOptimizerAlgorithm(
grb[i], weights[i]);
251 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_);
Iterator class for point clouds with or without given indices.
ELCH (Explicit Loop Closing Heuristic) class
typename PointCloud::Ptr PointCloudPtr
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::Matrix< Scalar, 4, 4 > &transform, bool copy_all_fields)
Apply a rigid transform defined by a 4x4 matrix.