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