41 #ifndef PCL_REGISTRATION_IMPL_ELCH_H_
42 #define PCL_REGISTRATION_IMPL_ELCH_H_
44 #include <pcl/common/transforms.h>
45 #include <pcl/registration/boost.h>
46 #include <pcl/registration/registration.h>
53 template <
typename Po
intT>
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()) {
74 for (
auto crossings_it = crossings.begin(); crossings_it != crossings.end();) {
75 dijkstra_shortest_paths(g,
77 predecessor_map(boost::make_iterator_property_map(
78 p, get(boost::vertex_index, g)))
79 .distance_map(boost::make_iterator_property_map(
80 d, get(boost::vertex_index, g))));
82 auto end_it = crossings_it;
85 for (; end_it != crossings.end(); end_it++) {
86 if (*end_it != p[*end_it] && (dist < 0 || d[*end_it] < dist)) {
88 start_min = crossings_it;
100 branches.push_back(
static_cast<int>(*crossings_it));
101 crossings_it = crossings.erase(crossings_it);
108 remove_edge(*end_min, p_min[*end_min], g);
109 for (
int i = p_min[*end_min]; i != *start_min; i = p_min[i]) {
111 weights[i] = weights[*start_min] + (weights[*end_min] - weights[*start_min]) *
112 d_min[i] / d_min[*end_min];
113 remove_edge(i, p_min[i], g);
114 if (degree(i, g) > 0) {
115 crossings.push_back(i);
119 if (degree(*start_min, g) == 0)
120 crossings.erase(start_min);
122 if (degree(*end_min, g) == 0)
123 crossings.erase(end_min);
132 boost::graph_traits<LOAGraph>::adjacency_iterator adjacent_it, adjacent_it_end;
135 while (!branches.empty()) {
136 int s = branches.front();
137 branches.pop_front();
139 for (std::tie(adjacent_it, adjacent_it_end) = adjacent_vertices(s, g);
140 adjacent_it != adjacent_it_end;
142 weights[*adjacent_it] = weights[s];
143 if (degree(*adjacent_it, g) > 1)
144 branches.push_back(
static_cast<int>(*adjacent_it));
151 template <
typename Po
intT>
161 if (loop_end_ == 0) {
162 PCL_ERROR(
"[pcl::registration::ELCH::initCompute] no end of loop defined!\n");
171 *meta_start = *(*loop_graph_)[loop_start_].cloud;
172 *meta_end = *(*loop_graph_)[loop_end_].cloud;
174 typename boost::graph_traits<LoopGraph>::adjacency_iterator si, si_end;
175 for (std::tie(si, si_end) = adjacent_vertices(loop_start_, *loop_graph_);
178 *meta_start += *(*loop_graph_)[*si].cloud;
180 for (std::tie(si, si_end) = adjacent_vertices(loop_end_, *loop_graph_);
183 *meta_end += *(*loop_graph_)[*si].cloud;
198 reg_->setInputTarget(meta_start);
200 reg_->setInputSource(meta_end);
204 loop_transform_ = reg_->getFinalTransformation();
213 template <
typename Po
intT>
217 if (!initCompute()) {
223 typename boost::graph_traits<LoopGraph>::edge_iterator edge_it, edge_it_end;
224 for (std::tie(edge_it, edge_it_end) = edges(*loop_graph_); edge_it != edge_it_end;
227 add_edge(source(*edge_it, *loop_graph_),
228 target(*edge_it, *loop_graph_),
234 for (
int i = 0; i < 4; i++) {
235 weights[i] =
new double[num_vertices(*loop_graph_)];
236 loopOptimizerAlgorithm(grb[i], weights[i]);
250 for (std::size_t i = 0; i < num_vertices(*loop_graph_); i++) {
252 t2[0] = loop_transform_(0, 3) *
static_cast<float>(weights[0][i]);
253 t2[1] = loop_transform_(1, 3) *
static_cast<float>(weights[1][i]);
254 t2[2] = loop_transform_(2, 3) *
static_cast<float>(weights[2][i]);
256 Eigen::Affine3f bl(loop_transform_);
257 Eigen::Quaternionf q(bl.rotation());
258 Eigen::Quaternionf q2;
259 q2 = Eigen::Quaternionf::Identity().slerp(
static_cast<float>(weights[3][i]), q);
262 Eigen::Translation3f t3(t2);
263 Eigen::Affine3f a(t3 * q2);
267 (*loop_graph_)[i].transform = a;
270 add_edge(loop_start_, loop_end_, *loop_graph_);
275 #endif // PCL_REGISTRATION_IMPL_ELCH_H_