Point Cloud Library (PCL)  1.11.1
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  bool do_swap = false;
68  std::list<int>::iterator start_min, end_min;
69 
70  // process all junctions
71  while (!crossings.empty ())
72  {
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  {
77  dijkstra_shortest_paths (g, *crossings_it,
78  predecessor_map(boost::make_iterator_property_map(p, get(boost::vertex_index, g))).
79  distance_map(boost::make_iterator_property_map(d, get(boost::vertex_index, g))));
80 
81  auto end_it = crossings_it;
82  end_it++;
83  // find shortest crossing for one vertex
84  for (; end_it != crossings.end (); end_it++)
85  {
86  if (*end_it != p[*end_it] && (dist < 0 || d[*end_it] < dist))
87  {
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  {
96  std::swap (p, p_min);
97  std::swap (d, d_min);
98  do_swap = false;
99  }
100  // vertex starts a branch
101  if (dist < 0)
102  {
103  branches.push_back (static_cast<int> (*crossings_it));
104  crossings_it = crossings.erase (crossings_it);
105  }
106  else
107  crossings_it++;
108  }
109 
110  if (dist > -1)
111  {
112  remove_edge (*end_min, p_min[*end_min], g);
113  for (int i = p_min[*end_min]; i != *start_min; i = p_min[i])
114  {
115  //even right with weights[*start_min] > weights[*end_min]! (math works)
116  weights[i] = weights[*start_min] + (weights[*end_min] - weights[*start_min]) * d_min[i] / d_min[*end_min];
117  remove_edge (i, p_min[i], g);
118  if (degree (i, g) > 0)
119  {
120  crossings.push_back (i);
121  }
122  }
123 
124  if (degree (*start_min, g) == 0)
125  crossings.erase (start_min);
126 
127  if (degree (*end_min, g) == 0)
128  crossings.erase (end_min);
129  }
130  }
131 
132  delete[] p;
133  delete[] p_min;
134  delete[] d;
135  delete[] d_min;
136 
137  boost::graph_traits<LOAGraph>::adjacency_iterator adjacent_it, adjacent_it_end;
138 
139  // error propagation
140  while (!branches.empty ())
141  {
142  int s = branches.front ();
143  branches.pop_front ();
144 
145  for (std::tie (adjacent_it, adjacent_it_end) = adjacent_vertices (s, g); adjacent_it != adjacent_it_end; ++adjacent_it)
146  {
147  weights[*adjacent_it] = weights[s];
148  if (degree (*adjacent_it, g) > 1)
149  branches.push_back (static_cast<int> (*adjacent_it));
150  }
151  clear_vertex (s, g);
152  }
153 }
154 
155 //////////////////////////////////////////////////////////////////////////////////////////////
156 template <typename PointT> bool
158 {
159  /*if (!PCLBase<PointT>::initCompute ())
160  {
161  PCL_ERROR ("[pcl::registration:ELCH::initCompute] Init failed.\n");
162  return (false);
163  }*/ //TODO
164 
165  if (loop_end_ == 0)
166  {
167  PCL_ERROR ("[pcl::registration::ELCH::initCompute] no end of loop defined!\n");
168  deinitCompute ();
169  return (false);
170  }
171 
172  //compute transformation if it's not given
173  if (compute_loop_)
174  {
175  PointCloudPtr meta_start (new PointCloud);
176  PointCloudPtr meta_end (new PointCloud);
177  *meta_start = *(*loop_graph_)[loop_start_].cloud;
178  *meta_end = *(*loop_graph_)[loop_end_].cloud;
179 
180  typename boost::graph_traits<LoopGraph>::adjacency_iterator si, si_end;
181  for (std::tie (si, si_end) = adjacent_vertices (loop_start_, *loop_graph_); si != si_end; si++)
182  *meta_start += *(*loop_graph_)[*si].cloud;
183 
184  for (std::tie (si, si_end) = adjacent_vertices (loop_end_, *loop_graph_); si != si_end; si++)
185  *meta_end += *(*loop_graph_)[*si].cloud;
186 
187  //TODO use real pose instead of centroid
188  //Eigen::Vector4f pose_start;
189  //pcl::compute3DCentroid (*(*loop_graph_)[loop_start_].cloud, pose_start);
190 
191  //Eigen::Vector4f pose_end;
192  //pcl::compute3DCentroid (*(*loop_graph_)[loop_end_].cloud, pose_end);
193 
194  PointCloudPtr tmp (new PointCloud);
195  //Eigen::Vector4f diff = pose_start - pose_end;
196  //Eigen::Translation3f translation (diff.head (3));
197  //Eigen::Affine3f trans = translation * Eigen::Quaternionf::Identity ();
198  //pcl::transformPointCloud (*(*loop_graph_)[loop_end_].cloud, *tmp, trans);
199 
200  reg_->setInputTarget (meta_start);
201 
202  reg_->setInputSource (meta_end);
203 
204  reg_->align (*tmp);
205 
206  loop_transform_ = reg_->getFinalTransformation ();
207  //TODO hack
208  //loop_transform_ *= trans.matrix ();
209 
210  }
211 
212  return (true);
213 }
214 
215 //////////////////////////////////////////////////////////////////////////////////////////////
216 template <typename PointT> void
218 {
219  if (!initCompute ())
220  {
221  return;
222  }
223 
224  LOAGraph grb[4];
225 
226  typename boost::graph_traits<LoopGraph>::edge_iterator edge_it, edge_it_end;
227  for (std::tie (edge_it, edge_it_end) = edges (*loop_graph_); edge_it != edge_it_end; edge_it++)
228  {
229  for (auto &j : grb)
230  add_edge (source (*edge_it, *loop_graph_), target (*edge_it, *loop_graph_), 1, j); //TODO add variance
231  }
232 
233  double *weights[4];
234  for (int i = 0; i < 4; i++)
235  {
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 != vertex_it_end; vertex_it++)
250  for (std::size_t i = 0; i < num_vertices (*loop_graph_); i++)
251  {
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_
pcl::PCLBase::PointCloudPtr
typename PointCloud::Ptr PointCloudPtr
Definition: pcl_base.h:76
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:217
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:181
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)
Apply an affine transform defined by an Eigen Transform.
Definition: transforms.hpp:221
pcl::registration::ELCH
ELCH (Explicit Loop Closing Heuristic) class
Definition: elch.h:64
pcl::registration::ELCH::initCompute
virtual bool initCompute()
This method should get called before starting the actual computation.
Definition: elch.hpp:157