Point Cloud Library (PCL)  1.14.1-dev
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 //////////////////////////////////////////////////////////////////////////////////////////////
54 template <typename PointT>
55 void
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 //////////////////////////////////////////////////////////////////////////////////////////////
152 template <typename PointT>
153 bool
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 //////////////////////////////////////////////////////////////////////////////////////////////
214 template <typename PointT>
215 void
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