Point Cloud Library (PCL) 1.13.0
elch.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2011, Willow Garage, Inc.
6 * Copyright (c) 2012-, Open Perception, Inc.
7 *
8 * All rights reserved.
9 *
10 * Redistribution and use in source and binary forms, with or without
11 * modification, are permitted provided that the following conditions
12 * are met:
13 *
14 * * Redistributions of source code must retain the above copyright
15 * notice, this list of conditions and the following disclaimer.
16 * * Redistributions in binary form must reproduce the above
17 * copyright notice, this list of conditions and the following
18 * disclaimer in the documentation and/or other materials provided
19 * with the distribution.
20 * * Neither the name of the copyright holder(s) nor the names of its
21 * contributors may be used to endorse or promote products derived
22 * from this software without specific prior written permission.
23 *
24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35 * POSSIBILITY OF SUCH DAMAGE.
36 *
37 * $Id$
38 *
39 */
40
41#ifndef PCL_REGISTRATION_IMPL_ELCH_H_
42#define PCL_REGISTRATION_IMPL_ELCH_H_
43
44#include <pcl/common/transforms.h>
45#include <pcl/registration/registration.h>
46
47#include <boost/graph/dijkstra_shortest_paths.hpp> // for dijkstra_shortest_paths
48
49#include <algorithm>
50#include <list>
51#include <tuple>
52
53//////////////////////////////////////////////////////////////////////////////////////////////
54template <typename PointT>
55void
57{
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;
63
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)];
68 bool do_swap = false;
69 std::list<int>::iterator start_min, end_min;
70
71 // process all junctions
72 while (!crossings.empty()) {
73 double dist = -1;
74 // find shortest crossing for all vertices on the loop
75 for (auto crossings_it = crossings.begin(); crossings_it != crossings.end();) {
76 dijkstra_shortest_paths(g,
77 *crossings_it,
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))));
82
83 auto end_it = crossings_it;
84 end_it++;
85 // find shortest crossing for one vertex
86 for (; end_it != crossings.end(); end_it++) {
87 if (*end_it != p[*end_it] && (dist < 0 || d[*end_it] < dist)) {
88 dist = d[*end_it];
89 start_min = crossings_it;
90 end_min = end_it;
91 do_swap = true;
92 }
93 }
94 if (do_swap) {
95 std::swap(p, p_min);
96 std::swap(d, d_min);
97 do_swap = false;
98 }
99 // vertex starts a branch
100 if (dist < 0) {
101 branches.push_back(static_cast<int>(*crossings_it));
102 crossings_it = crossings.erase(crossings_it);
103 }
104 else
105 crossings_it++;
106 }
107
108 if (dist > -1) {
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]) {
111 // even right with weights[*start_min] > weights[*end_min]! (math works)
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);
117 }
118 }
119
120 if (degree(*start_min, g) == 0)
121 crossings.erase(start_min);
122
123 if (degree(*end_min, g) == 0)
124 crossings.erase(end_min);
125 }
126 }
127
128 delete[] p;
129 delete[] p_min;
130 delete[] d;
131 delete[] d_min;
132
133 boost::graph_traits<LOAGraph>::adjacency_iterator adjacent_it, adjacent_it_end;
134
135 // error propagation
136 while (!branches.empty()) {
137 int s = branches.front();
138 branches.pop_front();
139
140 for (std::tie(adjacent_it, adjacent_it_end) = adjacent_vertices(s, g);
141 adjacent_it != adjacent_it_end;
142 ++adjacent_it) {
143 weights[*adjacent_it] = weights[s];
144 if (degree(*adjacent_it, g) > 1)
145 branches.push_back(static_cast<int>(*adjacent_it));
146 }
147 clear_vertex(s, g);
148 }
149}
150
151//////////////////////////////////////////////////////////////////////////////////////////////
152template <typename PointT>
153bool
155{
156 /*if (!PCLBase<PointT>::initCompute ())
157 {
158 PCL_ERROR ("[pcl::registration:ELCH::initCompute] Init failed.\n");
159 return (false);
160 }*/ //TODO
161
162 if (loop_end_ == 0) {
163 PCL_ERROR("[pcl::registration::ELCH::initCompute] no end of loop defined!\n");
164 deinitCompute();
165 return (false);
166 }
167
168 // compute transformation if it's not given
169 if (compute_loop_) {
170 PointCloudPtr meta_start(new PointCloud);
171 PointCloudPtr meta_end(new PointCloud);
172 *meta_start = *(*loop_graph_)[loop_start_].cloud;
173 *meta_end = *(*loop_graph_)[loop_end_].cloud;
174
175 typename boost::graph_traits<LoopGraph>::adjacency_iterator si, si_end;
176 for (std::tie(si, si_end) = adjacent_vertices(loop_start_, *loop_graph_);
177 si != si_end;
178 si++)
179 *meta_start += *(*loop_graph_)[*si].cloud;
180
181 for (std::tie(si, si_end) = adjacent_vertices(loop_end_, *loop_graph_);
182 si != si_end;
183 si++)
184 *meta_end += *(*loop_graph_)[*si].cloud;
185
186 // TODO use real pose instead of centroid
187 // Eigen::Vector4f pose_start;
188 // pcl::compute3DCentroid (*(*loop_graph_)[loop_start_].cloud, pose_start);
189
190 // Eigen::Vector4f pose_end;
191 // pcl::compute3DCentroid (*(*loop_graph_)[loop_end_].cloud, pose_end);
192
193 PointCloudPtr tmp(new PointCloud);
194 // Eigen::Vector4f diff = pose_start - pose_end;
195 // Eigen::Translation3f translation (diff.head (3));
196 // Eigen::Affine3f trans = translation * Eigen::Quaternionf::Identity ();
197 // pcl::transformPointCloud (*(*loop_graph_)[loop_end_].cloud, *tmp, trans);
198
199 reg_->setInputTarget(meta_start);
200
201 reg_->setInputSource(meta_end);
202
203 reg_->align(*tmp);
204
205 loop_transform_ = reg_->getFinalTransformation();
206 // TODO hack
207 // loop_transform_ *= trans.matrix ();
208 }
209
210 return (true);
211}
212
213//////////////////////////////////////////////////////////////////////////////////////////////
214template <typename PointT>
215void
217{
218 if (!initCompute()) {
219 return;
220 }
221
222 LOAGraph grb[4];
223
224 typename boost::graph_traits<LoopGraph>::edge_iterator edge_it, edge_it_end;
225 for (std::tie(edge_it, edge_it_end) = edges(*loop_graph_); edge_it != edge_it_end;
226 edge_it++) {
227 for (auto& j : grb)
228 add_edge(source(*edge_it, *loop_graph_),
229 target(*edge_it, *loop_graph_),
230 1,
231 j); // TODO add variance
232 }
233
234 double* weights[4];
235 for (int i = 0; i < 4; i++) {
236 weights[i] = new double[num_vertices(*loop_graph_)];
237 loopOptimizerAlgorithm(grb[i], weights[i]);
238 }
239
240 // TODO use pose
241 // Eigen::Vector4f cend;
242 // pcl::compute3DCentroid (*((*loop_graph_)[loop_end_].cloud), cend);
243 // Eigen::Translation3f tend (cend.head (3));
244 // Eigen::Affine3f aend (tend);
245 // Eigen::Affine3f aendI = aend.inverse ();
246
247 // TODO iterate ovr loop_graph_
248 // typename boost::graph_traits<LoopGraph>::vertex_iterator vertex_it, vertex_it_end;
249 // for (std::tie (vertex_it, vertex_it_end) = vertices (*loop_graph_); vertex_it !=
250 // vertex_it_end; vertex_it++)
251 for (std::size_t i = 0; i < num_vertices(*loop_graph_); i++) {
252 Eigen::Vector3f t2;
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]);
256
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);
261
262 // TODO use rotation from branch start
263 Eigen::Translation3f t3(t2);
264 Eigen::Affine3f a(t3 * q2);
265 // a = aend * a * aendI;
266
267 pcl::transformPointCloud(*(*loop_graph_)[i].cloud, *(*loop_graph_)[i].cloud, a);
268 (*loop_graph_)[i].transform = a;
269 }
270
271 add_edge(loop_start_, loop_end_, *loop_graph_);
272
273 deinitCompute();
274}
275
276#endif // PCL_REGISTRATION_IMPL_ELCH_H_
typename PointCloud::Ptr PointCloudPtr
Definition: pcl_base.h:73
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
ELCH (Explicit Loop Closing Heuristic) class
Definition: elch.h:59
virtual bool initCompute()
This method should get called before starting the actual computation.
Definition: elch.hpp:154
void compute()
Computes new poses for all point clouds by closing the loop between start and end point cloud.
Definition: elch.hpp:216
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.
Definition: transforms.hpp:221