Point Cloud Library (PCL)  1.8.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 <list>
45 #include <algorithm>
46 
47 #include <pcl/common/transforms.h>
48 #include <pcl/registration/eigen.h>
49 #include <pcl/registration/boost.h>
50 #include <pcl/registration/registration.h>
51 
52 //////////////////////////////////////////////////////////////////////////////////////////////
53 template <typename PointT> void
55 {
56  std::list<int> crossings, branches;
57  crossings.push_back (static_cast<int> (loop_start_));
58  crossings.push_back (static_cast<int> (loop_end_));
59  weights[loop_start_] = 0;
60  weights[loop_end_] = 1;
61 
62  int *p = new int[num_vertices (g)];
63  int *p_min = new int[num_vertices (g)];
64  double *d = new double[num_vertices (g)];
65  double *d_min = new double[num_vertices (g)];
66  double dist;
67  bool do_swap = false;
68  std::list<int>::iterator crossings_it, end_it, start_min, end_min;
69 
70  // process all junctions
71  while (!crossings.empty ())
72  {
73  dist = -1;
74  // find shortest crossing for all vertices on the loop
75  for (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  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  int s;
139 
140  // error propagation
141  while (!branches.empty ())
142  {
143  s = branches.front ();
144  branches.pop_front ();
145 
146  for (boost::tuples::tie (adjacent_it, adjacent_it_end) = adjacent_vertices (s, g); adjacent_it != adjacent_it_end; ++adjacent_it)
147  {
148  weights[*adjacent_it] = weights[s];
149  if (degree (*adjacent_it, g) > 1)
150  branches.push_back (static_cast<int> (*adjacent_it));
151  }
152  clear_vertex (s, g);
153  }
154 }
155 
156 //////////////////////////////////////////////////////////////////////////////////////////////
157 template <typename PointT> bool
159 {
160  /*if (!PCLBase<PointT>::initCompute ())
161  {
162  PCL_ERROR ("[pcl::registration:ELCH::initCompute] Init failed.\n");
163  return (false);
164  }*/ //TODO
165 
166  if (loop_end_ == 0)
167  {
168  PCL_ERROR ("[pcl::registration::ELCH::initCompute] no end of loop defined!\n");
169  deinitCompute ();
170  return (false);
171  }
172 
173  //compute transformation if it's not given
174  if (compute_loop_)
175  {
176  PointCloudPtr meta_start (new PointCloud);
177  PointCloudPtr meta_end (new PointCloud);
178  *meta_start = *(*loop_graph_)[loop_start_].cloud;
179  *meta_end = *(*loop_graph_)[loop_end_].cloud;
180 
181  typename boost::graph_traits<LoopGraph>::adjacency_iterator si, si_end;
182  for (boost::tuples::tie (si, si_end) = adjacent_vertices (loop_start_, *loop_graph_); si != si_end; si++)
183  *meta_start += *(*loop_graph_)[*si].cloud;
184 
185  for (boost::tuples::tie (si, si_end) = adjacent_vertices (loop_end_, *loop_graph_); si != si_end; si++)
186  *meta_end += *(*loop_graph_)[*si].cloud;
187 
188  //TODO use real pose instead of centroid
189  //Eigen::Vector4f pose_start;
190  //pcl::compute3DCentroid (*(*loop_graph_)[loop_start_].cloud, pose_start);
191 
192  //Eigen::Vector4f pose_end;
193  //pcl::compute3DCentroid (*(*loop_graph_)[loop_end_].cloud, pose_end);
194 
195  PointCloudPtr tmp (new PointCloud);
196  //Eigen::Vector4f diff = pose_start - pose_end;
197  //Eigen::Translation3f translation (diff.head (3));
198  //Eigen::Affine3f trans = translation * Eigen::Quaternionf::Identity ();
199  //pcl::transformPointCloud (*(*loop_graph_)[loop_end_].cloud, *tmp, trans);
200 
201  reg_->setInputTarget (meta_start);
202 
203  reg_->setInputSource (meta_end);
204 
205  reg_->align (*tmp);
206 
207  loop_transform_ = reg_->getFinalTransformation ();
208  //TODO hack
209  //loop_transform_ *= trans.matrix ();
210 
211  }
212 
213  return (true);
214 }
215 
216 //////////////////////////////////////////////////////////////////////////////////////////////
217 template <typename PointT> void
219 {
220  if (!initCompute ())
221  {
222  return;
223  }
224 
225  LOAGraph grb[4];
226 
227  typename boost::graph_traits<LoopGraph>::edge_iterator edge_it, edge_it_end;
228  for (boost::tuples::tie (edge_it, edge_it_end) = edges (*loop_graph_); edge_it != edge_it_end; edge_it++)
229  {
230  for (int j = 0; j < 4; j++)
231  add_edge (source (*edge_it, *loop_graph_), target (*edge_it, *loop_graph_), 1, grb[j]); //TODO add variance
232  }
233 
234  double *weights[4];
235  for (int i = 0; i < 4; i++)
236  {
237  weights[i] = new double[num_vertices (*loop_graph_)];
238  loopOptimizerAlgorithm (grb[i], weights[i]);
239  }
240 
241  //TODO use pose
242  //Eigen::Vector4f cend;
243  //pcl::compute3DCentroid (*((*loop_graph_)[loop_end_].cloud), cend);
244  //Eigen::Translation3f tend (cend.head (3));
245  //Eigen::Affine3f aend (tend);
246  //Eigen::Affine3f aendI = aend.inverse ();
247 
248  //TODO iterate ovr loop_graph_
249  //typename boost::graph_traits<LoopGraph>::vertex_iterator vertex_it, vertex_it_end;
250  //for (boost::tuples::tie (vertex_it, vertex_it_end) = vertices (*loop_graph_); vertex_it != vertex_it_end; vertex_it++)
251  for (size_t i = 0; i < num_vertices (*loop_graph_); i++)
252  {
253  Eigen::Vector3f t2;
254  t2[0] = loop_transform_ (0, 3) * static_cast<float> (weights[0][i]);
255  t2[1] = loop_transform_ (1, 3) * static_cast<float> (weights[1][i]);
256  t2[2] = loop_transform_ (2, 3) * static_cast<float> (weights[2][i]);
257 
258  Eigen::Affine3f bl (loop_transform_);
259  Eigen::Quaternionf q (bl.rotation ());
260  Eigen::Quaternionf q2;
261  q2 = Eigen::Quaternionf::Identity ().slerp (static_cast<float> (weights[3][i]), q);
262 
263  //TODO use rotation from branch start
264  Eigen::Translation3f t3 (t2);
265  Eigen::Affine3f a (t3 * q2);
266  //a = aend * a * aendI;
267 
268  pcl::transformPointCloud (*(*loop_graph_)[i].cloud, *(*loop_graph_)[i].cloud, a);
269  (*loop_graph_)[i].transform = a;
270  }
271 
272  add_edge (loop_start_, loop_end_, *loop_graph_);
273 
274  deinitCompute ();
275 }
276 
277 #endif // PCL_REGISTRATION_IMPL_ELCH_H_
void compute()
Computes new poses for all point clouds by closing the loop between start and end point cloud.
Definition: elch.hpp:218
ELCH (Explicit Loop Closing Heuristic) class
Definition: elch.h:62
PointCloud::Ptr PointCloudPtr
Definition: pcl_base.h:72
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:42
virtual bool initCompute()
This method should get called before starting the actual computation.
Definition: elch.hpp:158
PointCloud represents the base class in PCL for storing collections of 3D points.