Point Cloud Library (PCL)  1.7.1
lum.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, 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: lum.hpp 5663 2012-05-02 13:49:39Z florentinus $
38  *
39  */
40 
41 #ifndef PCL_REGISTRATION_IMPL_LUM_HPP_
42 #define PCL_REGISTRATION_IMPL_LUM_HPP_
43 
44 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
45 template<typename PointT> inline void
47 {
48  slam_graph_ = slam_graph;
49 }
50 
51 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
52 template<typename PointT> inline typename pcl::registration::LUM<PointT>::SLAMGraphPtr
54 {
55  return (slam_graph_);
56 }
57 
58 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
61 {
62  return (num_vertices (*slam_graph_));
63 }
64 
65 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
66 template<typename PointT> void
68 {
69  max_iterations_ = max_iterations;
70 }
71 
72 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
73 template<typename PointT> inline int
75 {
76  return (max_iterations_);
77 }
78 
79 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
80 template<typename PointT> void
82 {
83  convergence_threshold_ = convergence_threshold;
84 }
85 
86 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
87 template<typename PointT> inline float
89 {
90  return (convergence_threshold_);
91 }
92 
93 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
94 template<typename PointT> typename pcl::registration::LUM<PointT>::Vertex
96 {
97  Vertex v = add_vertex (*slam_graph_);
98  (*slam_graph_)[v].cloud_ = cloud;
99  if (v == 0 && pose != Eigen::Vector6f::Zero ())
100  {
101  PCL_WARN("[pcl::registration::LUM::addPointCloud] The pose estimate is ignored for the first cloud in the graph since that will become the reference pose.\n");
102  (*slam_graph_)[v].pose_ = Eigen::Vector6f::Zero ();
103  return (v);
104  }
105  (*slam_graph_)[v].pose_ = pose;
106  return (v);
107 }
108 
109 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
110 template<typename PointT> inline void
112 {
113  if (vertex >= getNumVertices ())
114  {
115  PCL_ERROR("[pcl::registration::LUM::setPointCloud] You are attempting to set a point cloud to a non-existing graph vertex.\n");
116  return;
117  }
118  (*slam_graph_)[vertex].cloud_ = cloud;
119 }
120 
121 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
122 template<typename PointT> inline typename pcl::registration::LUM<PointT>::PointCloudPtr
124 {
125  if (vertex >= getNumVertices ())
126  {
127  PCL_ERROR("[pcl::registration::LUM::getPointCloud] You are attempting to get a point cloud from a non-existing graph vertex.\n");
128  return (PointCloudPtr ());
129  }
130  return ((*slam_graph_)[vertex].cloud_);
131 }
132 
133 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
134 template<typename PointT> inline void
136 {
137  if (vertex >= getNumVertices ())
138  {
139  PCL_ERROR("[pcl::registration::LUM::setPose] You are attempting to set a pose estimate to a non-existing graph vertex.\n");
140  return;
141  }
142  if (vertex == 0)
143  {
144  PCL_ERROR("[pcl::registration::LUM::setPose] The pose estimate is ignored for the first cloud in the graph since that will become the reference pose.\n");
145  return;
146  }
147  (*slam_graph_)[vertex].pose_ = pose;
148 }
149 
150 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
151 template<typename PointT> inline Eigen::Vector6f
153 {
154  if (vertex >= getNumVertices ())
155  {
156  PCL_ERROR("[pcl::registration::LUM::getPose] You are attempting to get a pose estimate from a non-existing graph vertex.\n");
157  return (Eigen::Vector6f::Zero ());
158  }
159  return ((*slam_graph_)[vertex].pose_);
160 }
161 
162 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
163 template<typename PointT> inline Eigen::Affine3f
165 {
166  Eigen::Vector6f pose = getPose (vertex);
167  return (pcl::getTransformation (pose (0), pose (1), pose (2), pose (3), pose (4), pose (5)));
168 }
169 
170 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
171 template<typename PointT> void
172 pcl::registration::LUM<PointT>::setCorrespondences (const Vertex &source_vertex, const Vertex &target_vertex, const pcl::CorrespondencesPtr &corrs)
173 {
174  if (source_vertex >= getNumVertices () || target_vertex >= getNumVertices () || source_vertex == target_vertex)
175  {
176  PCL_ERROR("[pcl::registration::LUM::setCorrespondences] You are attempting to set a set of correspondences between non-existing or identical graph vertices.\n");
177  return;
178  }
179  Edge e;
180  bool present;
181  boost::tuples::tie (e, present) = edge (source_vertex, target_vertex, *slam_graph_);
182  if (!present)
183  boost::tuples::tie (e, present) = add_edge (source_vertex, target_vertex, *slam_graph_);
184  (*slam_graph_)[e].corrs_ = corrs;
185 }
186 
187 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
188 template<typename PointT> inline pcl::CorrespondencesPtr
189 pcl::registration::LUM<PointT>::getCorrespondences (const Vertex &source_vertex, const Vertex &target_vertex) const
190 {
191  if (source_vertex >= getNumVertices () || target_vertex >= getNumVertices ())
192  {
193  PCL_ERROR("[pcl::registration::LUM::getCorrespondences] You are attempting to get a set of correspondences between non-existing graph vertices.\n");
194  return (pcl::CorrespondencesPtr ());
195  }
196  Edge e;
197  bool present;
198  boost::tuples::tie (e, present) = edge (source_vertex, target_vertex, *slam_graph_);
199  if (!present)
200  {
201  PCL_ERROR("[pcl::registration::LUM::getCorrespondences] You are attempting to get a set of correspondences from a non-existing graph edge.\n");
202  return (pcl::CorrespondencesPtr ());
203  }
204  return ((*slam_graph_)[e].corrs_);
205 }
206 
207 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
208 template<typename PointT> void
210 {
211  int n = static_cast<int> (getNumVertices ());
212  if (n < 2)
213  {
214  PCL_ERROR("[pcl::registration::LUM::compute] The slam graph needs at least 2 vertices.\n");
215  return;
216  }
217  for (int i = 0; i < max_iterations_; ++i)
218  {
219  // Linearized computation of C^-1 and C^-1*D and convergence checking for all edges in the graph (results stored in slam_graph_)
220  typename SLAMGraph::edge_iterator e, e_end;
221  for (boost::tuples::tie (e, e_end) = edges (*slam_graph_); e != e_end; ++e)
222  computeEdge (*e);
223 
224  // Declare matrices G and B
225  Eigen::MatrixXf G = Eigen::MatrixXf::Zero (6 * (n - 1), 6 * (n - 1));
226  Eigen::VectorXf B = Eigen::VectorXf::Zero (6 * (n - 1));
227 
228  // Start at 1 because 0 is the reference pose
229  for (int vi = 1; vi != n; ++vi)
230  {
231  for (int vj = 0; vj != n; ++vj)
232  {
233  // Attempt to use the forward edge, otherwise use backward edge, otherwise there was no edge
234  Edge e;
235  bool present1, present2;
236  boost::tuples::tie (e, present1) = edge (vi, vj, *slam_graph_);
237  if (!present1)
238  {
239  boost::tuples::tie (e, present2) = edge (vj, vi, *slam_graph_);
240  if (!present2)
241  continue;
242  }
243 
244  // Fill in elements of G and B
245  if (vj > 0)
246  G.block (6 * (vi - 1), 6 * (vj - 1), 6, 6) = -(*slam_graph_)[e].cinv_;
247  G.block (6 * (vi - 1), 6 * (vi - 1), 6, 6) += (*slam_graph_)[e].cinv_;
248  B.segment (6 * (vi - 1), 6) += (present1 ? 1 : -1) * (*slam_graph_)[e].cinvd_;
249  }
250  }
251 
252  // Computation of the linear equation system: GX = B
253  // TODO investigate accuracy vs. speed tradeoff and find the best solving method for our type of linear equation (sparse)
254  Eigen::VectorXf X = G.colPivHouseholderQr ().solve (B);
255 
256  // Update the poses
257  float sum = 0.0;
258  for (int vi = 1; vi != n; ++vi)
259  {
260  Eigen::Vector6f difference_pose = static_cast<Eigen::Vector6f> (-incidenceCorrection (getPose (vi)).inverse () * X.segment (6 * (vi - 1), 6));
261  sum += difference_pose.norm ();
262  setPose (vi, getPose (vi) + difference_pose);
263  }
264 
265  // Convergence check
266  if (sum <= convergence_threshold_ * static_cast<float> (n - 1))
267  return;
268  }
269 }
270 
271 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
272 template<typename PointT> typename pcl::registration::LUM<PointT>::PointCloudPtr
274 {
275  PointCloudPtr out (new PointCloud);
276  pcl::transformPointCloud (*getPointCloud (vertex), *out, getTransformation (vertex));
277  return (out);
278 }
279 
280 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
281 template<typename PointT> typename pcl::registration::LUM<PointT>::PointCloudPtr
283 {
284  PointCloudPtr out (new PointCloud);
285  typename SLAMGraph::vertex_iterator v, v_end;
286  for (boost::tuples::tie (v, v_end) = vertices (*slam_graph_); v != v_end; ++v)
287  {
288  PointCloud temp;
289  pcl::transformPointCloud (*getPointCloud (*v), temp, getTransformation (*v));
290  *out += temp;
291  }
292  return (out);
293 }
294 
295 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
296 template<typename PointT> void
298 {
299  // Get necessary local data from graph
300  PointCloudPtr source_cloud = (*slam_graph_)[source (e, *slam_graph_)].cloud_;
301  PointCloudPtr target_cloud = (*slam_graph_)[target (e, *slam_graph_)].cloud_;
302  Eigen::Vector6f source_pose = (*slam_graph_)[source (e, *slam_graph_)].pose_;
303  Eigen::Vector6f target_pose = (*slam_graph_)[target (e, *slam_graph_)].pose_;
304  pcl::CorrespondencesPtr corrs = (*slam_graph_)[e].corrs_;
305 
306  // Build the average and difference vectors for all correspondences
307  std::vector <Eigen::Vector3f> corrs_aver (corrs->size ());
308  std::vector <Eigen::Vector3f> corrs_diff (corrs->size ());
309  int oci = 0; // oci = output correspondence iterator
310  for (int ici = 0; ici != static_cast<int> (corrs->size ()); ++ici) // ici = input correspondence iterator
311  {
312  // Compound the point pair onto the current pose
313  Eigen::Vector3f source_compounded = pcl::getTransformation (source_pose (0), source_pose (1), source_pose (2), source_pose (3), source_pose (4), source_pose (5)) * source_cloud->points[(*corrs)[ici].index_query].getVector3fMap ();
314  Eigen::Vector3f target_compounded = pcl::getTransformation (target_pose (0), target_pose (1), target_pose (2), target_pose (3), target_pose (4), target_pose (5)) * target_cloud->points[(*corrs)[ici].index_match].getVector3fMap ();
315 
316  // NaN points can not be passed to the remaining computational pipeline
317  if (!pcl_isfinite (source_compounded (0)) || !pcl_isfinite (source_compounded (1)) || !pcl_isfinite (source_compounded (2)) || !pcl_isfinite (target_compounded (0)) || !pcl_isfinite (target_compounded (1)) || !pcl_isfinite (target_compounded (2)))
318  continue;
319 
320  // Compute the point pair average and difference and store for later use
321  corrs_aver[oci] = 0.5 * (source_compounded + target_compounded);
322  corrs_diff[oci] = source_compounded - target_compounded;
323  oci++;
324  }
325  corrs_aver.resize (oci);
326  corrs_diff.resize (oci);
327 
328  // Need enough valid correspondences to get a good triangulation
329  if (oci < 3)
330  {
331  PCL_WARN("[pcl::registration::LUM::compute] The correspondences between vertex %d and %d do not contain enough valid correspondences to be considered for computation.\n", source (e, *slam_graph_), target (e, *slam_graph_));
332  (*slam_graph_)[e].cinv_ = Eigen::Matrix6f::Zero ();
333  (*slam_graph_)[e].cinvd_ = Eigen::Vector6f::Zero ();
334  return;
335  }
336 
337  // Build the matrices M'M and M'Z
338  Eigen::Matrix6f MM = Eigen::Matrix6f::Zero ();
339  Eigen::Vector6f MZ = Eigen::Vector6f::Zero ();
340  for (int ci = 0; ci != oci; ++ci) // ci = correspondence iterator
341  {
342  // Fast computation of summation elements of M'M
343  MM (0, 4) -= corrs_aver[ci] (1);
344  MM (0, 5) += corrs_aver[ci] (2);
345  MM (1, 3) -= corrs_aver[ci] (2);
346  MM (1, 4) += corrs_aver[ci] (0);
347  MM (2, 3) += corrs_aver[ci] (1);
348  MM (2, 5) -= corrs_aver[ci] (0);
349  MM (3, 4) -= corrs_aver[ci] (0) * corrs_aver[ci] (2);
350  MM (3, 5) -= corrs_aver[ci] (0) * corrs_aver[ci] (1);
351  MM (4, 5) -= corrs_aver[ci] (1) * corrs_aver[ci] (2);
352  MM (3, 3) += corrs_aver[ci] (1) * corrs_aver[ci] (1) + corrs_aver[ci] (2) * corrs_aver[ci] (2);
353  MM (4, 4) += corrs_aver[ci] (0) * corrs_aver[ci] (0) + corrs_aver[ci] (1) * corrs_aver[ci] (1);
354  MM (5, 5) += corrs_aver[ci] (0) * corrs_aver[ci] (0) + corrs_aver[ci] (2) * corrs_aver[ci] (2);
355 
356  // Fast computation of M'Z
357  MZ (0) += corrs_diff[ci] (0);
358  MZ (1) += corrs_diff[ci] (1);
359  MZ (2) += corrs_diff[ci] (2);
360  MZ (3) += corrs_aver[ci] (1) * corrs_diff[ci] (2) - corrs_aver[ci] (2) * corrs_diff[ci] (1);
361  MZ (4) += corrs_aver[ci] (0) * corrs_diff[ci] (1) - corrs_aver[ci] (1) * corrs_diff[ci] (0);
362  MZ (5) += corrs_aver[ci] (2) * corrs_diff[ci] (0) - corrs_aver[ci] (0) * corrs_diff[ci] (2);
363  }
364  // Remaining elements of M'M
365  MM (0, 0) = MM (1, 1) = MM (2, 2) = static_cast<float> (oci);
366  MM (4, 0) = MM (0, 4);
367  MM (5, 0) = MM (0, 5);
368  MM (3, 1) = MM (1, 3);
369  MM (4, 1) = MM (1, 4);
370  MM (3, 2) = MM (2, 3);
371  MM (5, 2) = MM (2, 5);
372  MM (4, 3) = MM (3, 4);
373  MM (5, 3) = MM (3, 5);
374  MM (5, 4) = MM (4, 5);
375 
376  // Compute pose difference estimation
377  Eigen::Vector6f D = static_cast<Eigen::Vector6f> (MM.inverse () * MZ);
378 
379  // Compute s^2
380  float ss = 0.0f;
381  for (int ci = 0; ci != oci; ++ci) // ci = correspondence iterator
382  ss += static_cast<float> (pow (corrs_diff[ci] (0) - (D (0) + corrs_aver[ci] (2) * D (5) - corrs_aver[ci] (1) * D (4)), 2.0f)
383  + pow (corrs_diff[ci] (1) - (D (1) + corrs_aver[ci] (0) * D (4) - corrs_aver[ci] (2) * D (3)), 2.0f)
384  + pow (corrs_diff[ci] (2) - (D (2) + corrs_aver[ci] (1) * D (3) - corrs_aver[ci] (0) * D (5)), 2.0f));
385 
386  // When reaching the limitations of computation due to linearization
387  if (ss < 0.0000000000001 || !pcl_isfinite (ss))
388  {
389  (*slam_graph_)[e].cinv_ = Eigen::Matrix6f::Zero ();
390  (*slam_graph_)[e].cinvd_ = Eigen::Vector6f::Zero ();
391  return;
392  }
393 
394  // Store the results in the slam graph
395  (*slam_graph_)[e].cinv_ = MM * (1.0f / ss);
396  (*slam_graph_)[e].cinvd_ = MZ * (1.0f / ss);
397 }
398 
399 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
400 template<typename PointT> inline Eigen::Matrix6f
402 {
403  Eigen::Matrix6f out = Eigen::Matrix6f::Identity ();
404  float cx = cosf (pose (3)), sx = sinf (pose (3)), cy = cosf (pose (4)), sy = sinf (pose (4));
405  out (0, 4) = pose (1) * sx - pose (2) * cx;
406  out (0, 5) = pose (1) * cx * cy + pose (2) * sx * cy;
407  out (1, 3) = pose (2);
408  out (1, 4) = -pose (0) * sx;
409  out (1, 5) = -pose (0) * cx * cy + pose (2) * sy;
410  out (2, 3) = -pose (1);
411  out (2, 4) = pose (0) * cx;
412  out (2, 5) = -pose (0) * sx * cy - pose (1) * sy;
413  out (3, 5) = sy;
414  out (4, 4) = sx;
415  out (4, 5) = cx * cy;
416  out (5, 4) = cx;
417  out (5, 5) = -sx * cy;
418  return (out);
419 }
420 
421 #define PCL_INSTANTIATE_LUM(T) template class PCL_EXPORTS pcl::registration::LUM<T>;
422 
423 #endif // PCL_REGISTRATION_IMPL_LUM_HPP_
424