Point Cloud Library (PCL)  1.10.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 <algorithm>
45 #include <list>
46 #include <tuple>
47 
48 #include <pcl/common/transforms.h>
49 #include <pcl/registration/eigen.h>
50 #include <pcl/registration/boost.h>
51 #include <pcl/registration/registration.h>
52 
53 //////////////////////////////////////////////////////////////////////////////////////////////
54 template <typename PointT> void
56 {
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;
62 
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)];
67  double dist;
68  bool do_swap = false;
69  std::list<int>::iterator start_min, end_min;
70 
71  // process all junctions
72  while (!crossings.empty ())
73  {
74  dist = -1;
75  // find shortest crossing for all vertices on the loop
76  for (auto crossings_it = crossings.begin (); crossings_it != crossings.end (); )
77  {
78  dijkstra_shortest_paths (g, *crossings_it,
79  predecessor_map(boost::make_iterator_property_map(p, get(boost::vertex_index, g))).
80  distance_map(boost::make_iterator_property_map(d, get(boost::vertex_index, g))));
81 
82  auto end_it = crossings_it;
83  end_it++;
84  // find shortest crossing for one vertex
85  for (; end_it != crossings.end (); end_it++)
86  {
87  if (*end_it != p[*end_it] && (dist < 0 || d[*end_it] < dist))
88  {
89  dist = d[*end_it];
90  start_min = crossings_it;
91  end_min = end_it;
92  do_swap = true;
93  }
94  }
95  if (do_swap)
96  {
97  std::swap (p, p_min);
98  std::swap (d, d_min);
99  do_swap = false;
100  }
101  // vertex starts a branch
102  if (dist < 0)
103  {
104  branches.push_back (static_cast<int> (*crossings_it));
105  crossings_it = crossings.erase (crossings_it);
106  }
107  else
108  crossings_it++;
109  }
110 
111  if (dist > -1)
112  {
113  remove_edge (*end_min, p_min[*end_min], g);
114  for (int i = p_min[*end_min]; i != *start_min; i = p_min[i])
115  {
116  //even right with weights[*start_min] > weights[*end_min]! (math works)
117  weights[i] = weights[*start_min] + (weights[*end_min] - weights[*start_min]) * d_min[i] / d_min[*end_min];
118  remove_edge (i, p_min[i], g);
119  if (degree (i, g) > 0)
120  {
121  crossings.push_back (i);
122  }
123  }
124 
125  if (degree (*start_min, g) == 0)
126  crossings.erase (start_min);
127 
128  if (degree (*end_min, g) == 0)
129  crossings.erase (end_min);
130  }
131  }
132 
133  delete[] p;
134  delete[] p_min;
135  delete[] d;
136  delete[] d_min;
137 
138  boost::graph_traits<LOAGraph>::adjacency_iterator adjacent_it, adjacent_it_end;
139  int s;
140 
141  // error propagation
142  while (!branches.empty ())
143  {
144  s = branches.front ();
145  branches.pop_front ();
146 
147  for (std::tie (adjacent_it, adjacent_it_end) = adjacent_vertices (s, g); adjacent_it != adjacent_it_end; ++adjacent_it)
148  {
149  weights[*adjacent_it] = weights[s];
150  if (degree (*adjacent_it, g) > 1)
151  branches.push_back (static_cast<int> (*adjacent_it));
152  }
153  clear_vertex (s, g);
154  }
155 }
156 
157 //////////////////////////////////////////////////////////////////////////////////////////////
158 template <typename PointT> bool
160 {
161  /*if (!PCLBase<PointT>::initCompute ())
162  {
163  PCL_ERROR ("[pcl::registration:ELCH::initCompute] Init failed.\n");
164  return (false);
165  }*/ //TODO
166 
167  if (loop_end_ == 0)
168  {
169  PCL_ERROR ("[pcl::registration::ELCH::initCompute] no end of loop defined!\n");
170  deinitCompute ();
171  return (false);
172  }
173 
174  //compute transformation if it's not given
175  if (compute_loop_)
176  {
177  PointCloudPtr meta_start (new PointCloud);
178  PointCloudPtr meta_end (new PointCloud);
179  *meta_start = *(*loop_graph_)[loop_start_].cloud;
180  *meta_end = *(*loop_graph_)[loop_end_].cloud;
181 
182  typename boost::graph_traits<LoopGraph>::adjacency_iterator si, si_end;
183  for (std::tie (si, si_end) = adjacent_vertices (loop_start_, *loop_graph_); si != si_end; si++)
184  *meta_start += *(*loop_graph_)[*si].cloud;
185 
186  for (std::tie (si, si_end) = adjacent_vertices (loop_end_, *loop_graph_); si != si_end; si++)
187  *meta_end += *(*loop_graph_)[*si].cloud;
188 
189  //TODO use real pose instead of centroid
190  //Eigen::Vector4f pose_start;
191  //pcl::compute3DCentroid (*(*loop_graph_)[loop_start_].cloud, pose_start);
192 
193  //Eigen::Vector4f pose_end;
194  //pcl::compute3DCentroid (*(*loop_graph_)[loop_end_].cloud, pose_end);
195 
196  PointCloudPtr tmp (new PointCloud);
197  //Eigen::Vector4f diff = pose_start - pose_end;
198  //Eigen::Translation3f translation (diff.head (3));
199  //Eigen::Affine3f trans = translation * Eigen::Quaternionf::Identity ();
200  //pcl::transformPointCloud (*(*loop_graph_)[loop_end_].cloud, *tmp, trans);
201 
202  reg_->setInputTarget (meta_start);
203 
204  reg_->setInputSource (meta_end);
205 
206  reg_->align (*tmp);
207 
208  loop_transform_ = reg_->getFinalTransformation ();
209  //TODO hack
210  //loop_transform_ *= trans.matrix ();
211 
212  }
213 
214  return (true);
215 }
216 
217 //////////////////////////////////////////////////////////////////////////////////////////////
218 template <typename PointT> void
220 {
221  if (!initCompute ())
222  {
223  return;
224  }
225 
226  LOAGraph grb[4];
227 
228  typename boost::graph_traits<LoopGraph>::edge_iterator edge_it, edge_it_end;
229  for (std::tie (edge_it, edge_it_end) = edges (*loop_graph_); edge_it != edge_it_end; edge_it++)
230  {
231  for (auto &j : grb)
232  add_edge (source (*edge_it, *loop_graph_), target (*edge_it, *loop_graph_), 1, j); //TODO add variance
233  }
234 
235  double *weights[4];
236  for (int i = 0; i < 4; i++)
237  {
238  weights[i] = new double[num_vertices (*loop_graph_)];
239  loopOptimizerAlgorithm (grb[i], weights[i]);
240  }
241 
242  //TODO use pose
243  //Eigen::Vector4f cend;
244  //pcl::compute3DCentroid (*((*loop_graph_)[loop_end_].cloud), cend);
245  //Eigen::Translation3f tend (cend.head (3));
246  //Eigen::Affine3f aend (tend);
247  //Eigen::Affine3f aendI = aend.inverse ();
248 
249  //TODO iterate ovr loop_graph_
250  //typename boost::graph_traits<LoopGraph>::vertex_iterator vertex_it, vertex_it_end;
251  //for (std::tie (vertex_it, vertex_it_end) = vertices (*loop_graph_); vertex_it != vertex_it_end; vertex_it++)
252  for (std::size_t i = 0; i < num_vertices (*loop_graph_); i++)
253  {
254  Eigen::Vector3f t2;
255  t2[0] = loop_transform_ (0, 3) * static_cast<float> (weights[0][i]);
256  t2[1] = loop_transform_ (1, 3) * static_cast<float> (weights[1][i]);
257  t2[2] = loop_transform_ (2, 3) * static_cast<float> (weights[2][i]);
258 
259  Eigen::Affine3f bl (loop_transform_);
260  Eigen::Quaternionf q (bl.rotation ());
261  Eigen::Quaternionf q2;
262  q2 = Eigen::Quaternionf::Identity ().slerp (static_cast<float> (weights[3][i]), q);
263 
264  //TODO use rotation from branch start
265  Eigen::Translation3f t3 (t2);
266  Eigen::Affine3f a (t3 * q2);
267  //a = aend * a * aendI;
268 
269  pcl::transformPointCloud (*(*loop_graph_)[i].cloud, *(*loop_graph_)[i].cloud, a);
270  (*loop_graph_)[i].transform = a;
271  }
272 
273  add_edge (loop_start_, loop_end_, *loop_graph_);
274 
275  deinitCompute ();
276 }
277 
278 #endif // PCL_REGISTRATION_IMPL_ELCH_H_
pcl::PCLBase::PointCloudPtr
typename PointCloud::Ptr PointCloudPtr
Definition: pcl_base.h:73
pcl::registration::ELCH::compute
void compute()
Computes new poses for all point clouds by closing the loop between start and end point cloud.
Definition: elch.hpp:219
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: projection_matrix.h:52
pcl::transformPointCloud
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transform, bool copy_all_fields=true)
Apply an affine transform defined by an Eigen Transform.
Definition: transforms.hpp:215
pcl::registration::ELCH
ELCH (Explicit Loop Closing Heuristic) class
Definition: elch.h:62
pcl::registration::ELCH::initCompute
virtual bool initCompute()
This method should get called before starting the actual computation.
Definition: elch.hpp:159