Point Cloud Library (PCL)  1.14.1-dev
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 #include <tuple>
45 
46 namespace pcl {
47 
48 namespace registration {
49 
50 template <typename PointT>
51 inline void
53 {
54  slam_graph_ = slam_graph;
55 }
56 
57 template <typename PointT>
58 inline typename LUM<PointT>::SLAMGraphPtr
60 {
61  return (slam_graph_);
62 }
63 
64 template <typename PointT>
67 {
68  return (num_vertices(*slam_graph_));
69 }
70 
71 template <typename PointT>
72 void
73 LUM<PointT>::setMaxIterations(int max_iterations)
74 {
75  max_iterations_ = max_iterations;
76 }
77 
78 template <typename PointT>
79 inline int
81 {
82  return (max_iterations_);
83 }
84 
85 template <typename PointT>
86 void
87 LUM<PointT>::setConvergenceThreshold(float convergence_threshold)
88 {
89  convergence_threshold_ = convergence_threshold;
90 }
91 
92 template <typename PointT>
93 inline float
95 {
96  return (convergence_threshold_);
97 }
98 
99 template <typename PointT>
100 typename LUM<PointT>::Vertex
102 {
103  Vertex v = add_vertex(*slam_graph_);
104  (*slam_graph_)[v].cloud_ = cloud;
105  if (v == 0 && pose != Eigen::Vector6f::Zero()) {
106  PCL_WARN(
107  "[pcl::registration::LUM::addPointCloud] The pose estimate is ignored for the "
108  "first cloud in the graph since that will become the reference pose.\n");
109  (*slam_graph_)[v].pose_ = Eigen::Vector6f::Zero();
110  return (v);
111  }
112  (*slam_graph_)[v].pose_ = pose;
113  return (v);
114 }
115 
116 template <typename PointT>
117 inline void
118 LUM<PointT>::setPointCloud(const Vertex& vertex, const PointCloudPtr& cloud)
119 {
120  if (vertex >= getNumVertices()) {
121  PCL_ERROR("[pcl::registration::LUM::setPointCloud] You are attempting to set a "
122  "point cloud to a non-existing graph vertex.\n");
123  return;
124  }
125  (*slam_graph_)[vertex].cloud_ = cloud;
126 }
127 
128 template <typename PointT>
129 inline typename LUM<PointT>::PointCloudPtr
130 LUM<PointT>::getPointCloud(const Vertex& vertex) const
131 {
132  if (vertex >= getNumVertices()) {
133  PCL_ERROR("[pcl::registration::LUM::getPointCloud] You are attempting to get a "
134  "point cloud from a non-existing graph vertex.\n");
135  return (PointCloudPtr());
136  }
137  return ((*slam_graph_)[vertex].cloud_);
138 }
139 
140 template <typename PointT>
141 inline void
142 LUM<PointT>::setPose(const Vertex& vertex, const Eigen::Vector6f& pose)
143 {
144  if (vertex >= getNumVertices()) {
145  PCL_ERROR("[pcl::registration::LUM::setPose] You are attempting to set a pose "
146  "estimate to a non-existing graph vertex.\n");
147  return;
148  }
149  if (vertex == 0) {
150  PCL_ERROR("[pcl::registration::LUM::setPose] The pose estimate is ignored for the "
151  "first cloud in the graph since that will become the reference pose.\n");
152  return;
153  }
154  (*slam_graph_)[vertex].pose_ = pose;
155 }
156 
157 template <typename PointT>
158 inline Eigen::Vector6f
159 LUM<PointT>::getPose(const Vertex& vertex) const
160 {
161  if (vertex >= getNumVertices()) {
162  PCL_ERROR("[pcl::registration::LUM::getPose] You are attempting to get a pose "
163  "estimate from a non-existing graph vertex.\n");
164  return (Eigen::Vector6f::Zero());
165  }
166  return ((*slam_graph_)[vertex].pose_);
167 }
168 
169 template <typename PointT>
170 inline Eigen::Affine3f
172 {
173  Eigen::Vector6f pose = getPose(vertex);
174  return (pcl::getTransformation(pose(0), pose(1), pose(2), pose(3), pose(4), pose(5)));
175 }
176 
177 template <typename PointT>
178 void
180  const Vertex& target_vertex,
181  const pcl::CorrespondencesPtr& corrs)
182 {
183  if (source_vertex >= getNumVertices() || target_vertex >= getNumVertices() ||
184  source_vertex == target_vertex) {
185  PCL_ERROR(
186  "[pcl::registration::LUM::setCorrespondences] You are attempting to set a set "
187  "of correspondences between non-existing or identical graph vertices.\n");
188  return;
189  }
190  Edge e;
191  bool present;
192  std::tie(e, present) = edge(source_vertex, target_vertex, *slam_graph_);
193  if (!present)
194  std::tie(e, present) = add_edge(source_vertex, target_vertex, *slam_graph_);
195  (*slam_graph_)[e].corrs_ = corrs;
196 }
197 
198 template <typename PointT>
201  const Vertex& target_vertex) const
202 {
203  if (source_vertex >= getNumVertices() || target_vertex >= getNumVertices()) {
204  PCL_ERROR("[pcl::registration::LUM::getCorrespondences] You are attempting to get "
205  "a set of correspondences between non-existing graph vertices.\n");
206  return {};
207  }
208  Edge e;
209  bool present;
210  std::tie(e, present) = edge(source_vertex, target_vertex, *slam_graph_);
211  if (!present) {
212  PCL_ERROR("[pcl::registration::LUM::getCorrespondences] You are attempting to get "
213  "a set of correspondences from a non-existing graph edge.\n");
214  return {};
215  }
216  return ((*slam_graph_)[e].corrs_);
217 }
218 
219 template <typename PointT>
220 void
222 {
223  int n = static_cast<int>(getNumVertices());
224  if (n < 2) {
225  PCL_ERROR("[pcl::registration::LUM::compute] The slam graph needs at least 2 "
226  "vertices.\n");
227  return;
228  }
229  for (int i = 0; i < max_iterations_; ++i) {
230  // Linearized computation of C^-1 and C^-1*D and convergence checking for all edges
231  // in the graph (results stored in slam_graph_)
232  typename SLAMGraph::edge_iterator e, e_end;
233  for (std::tie(e, e_end) = edges(*slam_graph_); e != e_end; ++e)
234  computeEdge(*e);
235 
236  // Declare matrices G and B
237  Eigen::MatrixXf G = Eigen::MatrixXf::Zero(6 * (n - 1), 6 * (n - 1));
238  Eigen::VectorXf B = Eigen::VectorXf::Zero(6 * (n - 1));
239 
240  // Start at 1 because 0 is the reference pose
241  for (int vi = 1; vi != n; ++vi) {
242  for (int vj = 0; vj != n; ++vj) {
243  // Attempt to use the forward edge, otherwise use backward edge, otherwise there
244  // was no edge
245  Edge e;
246  bool present1;
247  std::tie(e, present1) = edge(vi, vj, *slam_graph_);
248  if (!present1) {
249  bool present2;
250  std::tie(e, present2) = edge(vj, vi, *slam_graph_);
251  if (!present2)
252  continue;
253  }
254 
255  // Fill in elements of G and B
256  if (vj > 0)
257  G.block<6, 6>(6 * (vi - 1), 6 * (vj - 1)) = -(*slam_graph_)[e].cinv_;
258  G.block<6, 6>(6 * (vi - 1), 6 * (vi - 1)) += (*slam_graph_)[e].cinv_;
259  B.segment(6 * (vi - 1), 6) += (present1 ? 1 : -1) * (*slam_graph_)[e].cinvd_;
260  }
261  }
262 
263  // Computation of the linear equation system: GX = B
264  // TODO investigate accuracy vs. speed tradeoff and find the best solving method for
265  // our type of linear equation (sparse)
266  Eigen::VectorXf X = G.colPivHouseholderQr().solve(B);
267 
268  // Update the poses
269  float sum = 0.0;
270  for (int vi = 1; vi != n; ++vi) {
271  Eigen::Vector6f difference_pose = static_cast<Eigen::Vector6f>(
272  -incidenceCorrection(getPose(vi)).inverse() * X.segment(6 * (vi - 1), 6));
273  sum += difference_pose.norm();
274  setPose(vi, getPose(vi) + difference_pose);
275  }
276 
277  // Convergence check
278  if (sum <= convergence_threshold_ * static_cast<float>(n - 1))
279  return;
280  }
281 }
282 
283 template <typename PointT>
286 {
287  PointCloudPtr out(new PointCloud);
288  pcl::transformPointCloud(*getPointCloud(vertex), *out, getTransformation(vertex));
289  return (out);
290 }
291 
292 template <typename PointT>
295 {
296  PointCloudPtr out(new PointCloud);
297  typename SLAMGraph::vertex_iterator v, v_end;
298  for (std::tie(v, v_end) = vertices(*slam_graph_); v != v_end; ++v) {
299  PointCloud temp;
300  pcl::transformPointCloud(*getPointCloud(*v), temp, getTransformation(*v));
301  *out += temp;
302  }
303  return (out);
304 }
305 
306 template <typename PointT>
307 void
309 {
310  // Get necessary local data from graph
311  PointCloudPtr source_cloud = (*slam_graph_)[source(e, *slam_graph_)].cloud_;
312  PointCloudPtr target_cloud = (*slam_graph_)[target(e, *slam_graph_)].cloud_;
313  Eigen::Vector6f source_pose = (*slam_graph_)[source(e, *slam_graph_)].pose_;
314  Eigen::Vector6f target_pose = (*slam_graph_)[target(e, *slam_graph_)].pose_;
315  pcl::CorrespondencesPtr corrs = (*slam_graph_)[e].corrs_;
316 
317  // Build the average and difference vectors for all correspondences
318  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>> corrs_aver(
319  corrs->size());
320  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>> corrs_diff(
321  corrs->size());
322  int oci = 0; // oci = output correspondence iterator
323  for (const auto& icorr : (*corrs)) // icorr = input correspondence
324  {
325  // Compound the point pair onto the current pose
326  Eigen::Vector3f source_compounded =
327  pcl::getTransformation(source_pose(0),
328  source_pose(1),
329  source_pose(2),
330  source_pose(3),
331  source_pose(4),
332  source_pose(5)) *
333  (*source_cloud)[icorr.index_query].getVector3fMap();
334  Eigen::Vector3f target_compounded =
335  pcl::getTransformation(target_pose(0),
336  target_pose(1),
337  target_pose(2),
338  target_pose(3),
339  target_pose(4),
340  target_pose(5)) *
341  (*target_cloud)[icorr.index_match].getVector3fMap();
342 
343  // NaN points can not be passed to the remaining computational pipeline
344  if (!std::isfinite(source_compounded(0)) || !std::isfinite(source_compounded(1)) ||
345  !std::isfinite(source_compounded(2)) || !std::isfinite(target_compounded(0)) ||
346  !std::isfinite(target_compounded(1)) || !std::isfinite(target_compounded(2)))
347  continue;
348 
349  // Compute the point pair average and difference and store for later use
350  corrs_aver[oci] = 0.5 * (source_compounded + target_compounded);
351  corrs_diff[oci] = source_compounded - target_compounded;
352  oci++;
353  }
354  corrs_aver.resize(oci);
355  corrs_diff.resize(oci);
356 
357  // Need enough valid correspondences to get a good triangulation
358  if (oci < 3) {
359  PCL_WARN("[pcl::registration::LUM::compute] The correspondences between vertex %d "
360  "and %d do not contain enough valid correspondences to be considered for "
361  "computation.\n",
362  source(e, *slam_graph_),
363  target(e, *slam_graph_));
364  (*slam_graph_)[e].cinv_ = Eigen::Matrix6f::Zero();
365  (*slam_graph_)[e].cinvd_ = Eigen::Vector6f::Zero();
366  return;
367  }
368 
369  // Build the matrices M'M and M'Z
370  Eigen::Matrix6f MM = Eigen::Matrix6f::Zero();
371  Eigen::Vector6f MZ = Eigen::Vector6f::Zero();
372  for (int ci = 0; ci != oci; ++ci) // ci = correspondence iterator
373  {
374  // Fast computation of summation elements of M'M
375  MM(0, 4) -= corrs_aver[ci](1);
376  MM(0, 5) += corrs_aver[ci](2);
377  MM(1, 3) -= corrs_aver[ci](2);
378  MM(1, 4) += corrs_aver[ci](0);
379  MM(2, 3) += corrs_aver[ci](1);
380  MM(2, 5) -= corrs_aver[ci](0);
381  MM(3, 4) -= corrs_aver[ci](0) * corrs_aver[ci](2);
382  MM(3, 5) -= corrs_aver[ci](0) * corrs_aver[ci](1);
383  MM(4, 5) -= corrs_aver[ci](1) * corrs_aver[ci](2);
384  MM(3, 3) +=
385  corrs_aver[ci](1) * corrs_aver[ci](1) + corrs_aver[ci](2) * corrs_aver[ci](2);
386  MM(4, 4) +=
387  corrs_aver[ci](0) * corrs_aver[ci](0) + corrs_aver[ci](1) * corrs_aver[ci](1);
388  MM(5, 5) +=
389  corrs_aver[ci](0) * corrs_aver[ci](0) + corrs_aver[ci](2) * corrs_aver[ci](2);
390 
391  // Fast computation of M'Z
392  MZ(0) += corrs_diff[ci](0);
393  MZ(1) += corrs_diff[ci](1);
394  MZ(2) += corrs_diff[ci](2);
395  MZ(3) +=
396  corrs_aver[ci](1) * corrs_diff[ci](2) - corrs_aver[ci](2) * corrs_diff[ci](1);
397  MZ(4) +=
398  corrs_aver[ci](0) * corrs_diff[ci](1) - corrs_aver[ci](1) * corrs_diff[ci](0);
399  MZ(5) +=
400  corrs_aver[ci](2) * corrs_diff[ci](0) - corrs_aver[ci](0) * corrs_diff[ci](2);
401  }
402  // Remaining elements of M'M
403  MM(0, 0) = MM(1, 1) = MM(2, 2) = static_cast<float>(oci);
404  MM(4, 0) = MM(0, 4);
405  MM(5, 0) = MM(0, 5);
406  MM(3, 1) = MM(1, 3);
407  MM(4, 1) = MM(1, 4);
408  MM(3, 2) = MM(2, 3);
409  MM(5, 2) = MM(2, 5);
410  MM(4, 3) = MM(3, 4);
411  MM(5, 3) = MM(3, 5);
412  MM(5, 4) = MM(4, 5);
413 
414  // Compute pose difference estimation
415  Eigen::Vector6f D = static_cast<Eigen::Vector6f>(MM.inverse() * MZ);
416 
417  // Compute s^2
418  float ss = 0.0f;
419  for (int ci = 0; ci != oci; ++ci) // ci = correspondence iterator
420  ss += static_cast<float>(
421  std::pow(corrs_diff[ci](0) -
422  (D(0) + corrs_aver[ci](2) * D(5) - corrs_aver[ci](1) * D(4)),
423  2.0f) +
424  std::pow(corrs_diff[ci](1) -
425  (D(1) + corrs_aver[ci](0) * D(4) - corrs_aver[ci](2) * D(3)),
426  2.0f) +
427  std::pow(corrs_diff[ci](2) -
428  (D(2) + corrs_aver[ci](1) * D(3) - corrs_aver[ci](0) * D(5)),
429  2.0f));
430 
431  // When reaching the limitations of computation due to linearization
432  if (ss < 0.0000000000001 || !std::isfinite(ss)) {
433  (*slam_graph_)[e].cinv_ = Eigen::Matrix6f::Zero();
434  (*slam_graph_)[e].cinvd_ = Eigen::Vector6f::Zero();
435  return;
436  }
437 
438  // Store the results in the slam graph
439  (*slam_graph_)[e].cinv_ = MM * (1.0f / ss);
440  (*slam_graph_)[e].cinvd_ = MZ * (1.0f / ss);
441 }
442 
443 template <typename PointT>
444 inline Eigen::Matrix6f
446 {
447  Eigen::Matrix6f out = Eigen::Matrix6f::Identity();
448  float cx = std::cos(pose(3)), sx = sinf(pose(3)), cy = std::cos(pose(4)),
449  sy = sinf(pose(4));
450  out(0, 4) = pose(1) * sx - pose(2) * cx;
451  out(0, 5) = pose(1) * cx * cy + pose(2) * sx * cy;
452  out(1, 3) = pose(2);
453  out(1, 4) = -pose(0) * sx;
454  out(1, 5) = -pose(0) * cx * cy + pose(2) * sy;
455  out(2, 3) = -pose(1);
456  out(2, 4) = pose(0) * cx;
457  out(2, 5) = -pose(0) * sx * cy - pose(1) * sy;
458  out(3, 5) = sy;
459  out(4, 4) = sx;
460  out(4, 5) = cx * cy;
461  out(5, 4) = cx;
462  out(5, 5) = -sx * cy;
463  return (out);
464 }
465 
466 } // namespace registration
467 } // namespace pcl
468 
469 #define PCL_INSTANTIATE_LUM(T) template class PCL_EXPORTS pcl::registration::LUM<T>;
470 
471 #endif // PCL_REGISTRATION_IMPL_LUM_HPP_
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
Globally Consistent Scan Matching based on an algorithm by Lu and Milios.
Definition: lum.h:108
void compute()
Perform LUM's globally consistent scan matching.
Definition: lum.hpp:221
float getConvergenceThreshold() const
Get the convergence threshold for the compute() method.
Definition: lum.hpp:94
Eigen::Matrix6f incidenceCorrection(const Eigen::Vector6f &pose)
Returns a pose corrected 6DoF incidence matrix.
Definition: lum.hpp:445
void computeEdge(const Edge &e)
Linearized computation of C^-1 and C^-1*D (results stored in slam_graph_).
Definition: lum.hpp:308
shared_ptr< SLAMGraph > SLAMGraphPtr
Definition: lum.h:136
PointCloudPtr getPointCloud(const Vertex &vertex) const
Return a point cloud from one of the SLAM graph's vertices.
Definition: lum.hpp:130
SLAMGraphPtr getLoopGraph() const
Get the internal SLAM graph structure.
Definition: lum.hpp:59
SLAMGraph::vertices_size_type getNumVertices() const
Get the number of vertices in the SLAM graph.
Definition: lum.hpp:66
typename SLAMGraph::vertex_descriptor Vertex
Definition: lum.h:137
void setConvergenceThreshold(float convergence_threshold)
Set the convergence threshold for the compute() method.
Definition: lum.hpp:87
typename PointCloud::Ptr PointCloudPtr
Definition: lum.h:114
void setMaxIterations(int max_iterations)
Set the maximum number of iterations for the compute() method.
Definition: lum.hpp:73
PointCloudPtr getTransformedCloud(const Vertex &vertex) const
Return a point cloud from one of the SLAM graph's vertices compounded onto its current pose estimate.
Definition: lum.hpp:285
PointCloudPtr getConcatenatedCloud() const
Return a concatenated point cloud of all the SLAM graph's point clouds compounded onto their current ...
Definition: lum.hpp:294
int getMaxIterations() const
Get the maximum number of iterations for the compute() method.
Definition: lum.hpp:80
void setPose(const Vertex &vertex, const Eigen::Vector6f &pose)
Change a pose estimate on one of the SLAM graph's vertices.
Definition: lum.hpp:142
pcl::CorrespondencesPtr getCorrespondences(const Vertex &source_vertex, const Vertex &target_vertex) const
Return a set of correspondences from one of the SLAM graph's edges.
Definition: lum.hpp:200
Vertex addPointCloud(const PointCloudPtr &cloud, const Eigen::Vector6f &pose=Eigen::Vector6f::Zero())
Add a new point cloud to the SLAM graph.
Definition: lum.hpp:101
void setPointCloud(const Vertex &vertex, const PointCloudPtr &cloud)
Change a point cloud on one of the SLAM graph's vertices.
Definition: lum.hpp:118
Eigen::Vector6f getPose(const Vertex &vertex) const
Return a pose estimate from one of the SLAM graph's vertices.
Definition: lum.hpp:159
Eigen::Affine3f getTransformation(const Vertex &vertex) const
Return a pose estimate from one of the SLAM graph's vertices as an affine transformation matrix.
Definition: lum.hpp:171
void setCorrespondences(const Vertex &source_vertex, const Vertex &target_vertex, const pcl::CorrespondencesPtr &corrs)
Add/change a set of correspondences for one of the SLAM graph's edges.
Definition: lum.hpp:179
void setLoopGraph(const SLAMGraphPtr &slam_graph)
Set the internal SLAM graph structure.
Definition: lum.hpp:52
typename SLAMGraph::edge_descriptor Edge
Definition: lum.h:138
void getTransformation(Scalar x, Scalar y, Scalar z, Scalar roll, Scalar pitch, Scalar yaw, Eigen::Transform< Scalar, 3, Eigen::Affine > &t)
Create a transformation from the given translation and Euler angles (intrinsic rotations,...
Definition: eigen.hpp:618
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
@ B
Definition: norms.h:54
Eigen::Matrix< float, 6, 1 > Vector6f
Definition: lum.h:51
Eigen::Matrix< float, 6, 6 > Matrix6f
Definition: lum.h:52
shared_ptr< Correspondences > CorrespondencesPtr