Point Cloud Library (PCL)  1.11.0
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Modules Pages
lum.h
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.h 5663 2012-05-02 13:49:39Z florentinus $
38  *
39  */
40 
41 #pragma once
42 
43 #include <pcl/memory.h>
44 #include <pcl/pcl_macros.h>
45 #include <pcl/pcl_base.h>
46 #include <pcl/registration/eigen.h>
47 #include <pcl/registration/boost.h>
48 #include <pcl/common/transforms.h>
49 #include <pcl/correspondence.h>
50 #include <pcl/registration/boost_graph.h>
51 
52 namespace Eigen
53 {
54  using Vector6f = Eigen::Matrix<float, 6, 1>;
55  using Matrix6f = Eigen::Matrix<float, 6, 6>;
56 }
57 
58 namespace pcl
59 {
60  namespace registration
61  {
62  /** \brief Globally Consistent Scan Matching based on an algorithm by Lu and Milios.
63  * \details A GraphSLAM algorithm where registration data is managed in a graph:
64  * <ul>
65  * <li>Vertices represent poses and hold the point cloud data and relative transformations.</li>
66  * <li>Edges represent pose constraints and hold the correspondence data between two point clouds.</li>
67  * </ul>
68  * Computation uses the first point cloud in the SLAM graph as a reference pose and attempts to align all other point clouds to it simultaneously.
69  * For more information:
70  * <ul><li>
71  * F. Lu, E. Milios,
72  * Globally Consistent Range Scan Alignment for Environment Mapping,
73  * Autonomous Robots 4, April 1997
74  * </li><li>
75  * Dorit Borrmann, Jan Elseberg, Kai Lingemann, Andreas Nüchter, and Joachim Hertzberg,
76  * The Efficient Extension of Globally Consistent Scan Matching to 6 DoF,
77  * In Proceedings of the 4th International Symposium on 3D Data Processing, Visualization and Transmission (3DPVT '08), June 2008
78  * </li></ul>
79  * Usage example:
80  * \code
81  * pcl::registration::LUM<pcl::PointXYZ> lum;
82  * // Add point clouds as vertices to the SLAM graph
83  * lum.addPointCloud (cloud_0);
84  * lum.addPointCloud (cloud_1);
85  * lum.addPointCloud (cloud_2);
86  * // Use your favorite pairwise correspondence estimation algorithm(s)
87  * corrs_0_to_1 = someAlgo (cloud_0, cloud_1);
88  * corrs_1_to_2 = someAlgo (cloud_1, cloud_2);
89  * corrs_2_to_0 = someAlgo (lum.getPointCloud (2), lum.getPointCloud (0));
90  * // Add the correspondence results as edges to the SLAM graph
91  * lum.setCorrespondences (0, 1, corrs_0_to_1);
92  * lum.setCorrespondences (1, 2, corrs_1_to_2);
93  * lum.setCorrespondences (2, 0, corrs_2_to_0);
94  * // Change the computation parameters
95  * lum.setMaxIterations (5);
96  * lum.setConvergenceThreshold (0.0);
97  * // Perform the actual LUM computation
98  * lum.compute ();
99  * // Return the concatenated point cloud result
100  * cloud_out = lum.getConcatenatedCloud ();
101  * // Return the separate point cloud transformations
102  * for(int i = 0; i < lum.getNumVertices (); i++)
103  * {
104  * transforms_out[i] = lum.getTransformation (i);
105  * }
106  * \endcode
107  * \author Frits Florentinus, Jochen Sprickerhof
108  * \ingroup registration
109  */
110  template<typename PointT>
111  class LUM
112  {
113  public:
114  using Ptr = shared_ptr<LUM<PointT> >;
115  using ConstPtr = shared_ptr<const LUM<PointT> >;
116 
118  using PointCloudPtr = typename PointCloud::Ptr;
120 
122  {
126  };
128  {
133  };
134 
135  using SLAMGraph = boost::adjacency_list<boost::eigen_vecS, boost::eigen_vecS, boost::bidirectionalS, VertexProperties, EdgeProperties, boost::no_property, boost::eigen_listS>;
136  using SLAMGraphPtr = shared_ptr<SLAMGraph>;
137  using Vertex = typename SLAMGraph::vertex_descriptor;
138  using Edge = typename SLAMGraph::edge_descriptor;
139 
140  /** \brief Empty constructor.
141  */
142  LUM ()
143  : slam_graph_ (new SLAMGraph)
144  , max_iterations_ (5)
145  , convergence_threshold_ (0.0)
146  {
147  }
148 
149  /** \brief Set the internal SLAM graph structure.
150  * \details All data used and produced by LUM is stored in this boost::adjacency_list.
151  * It is recommended to use the LUM class itself to build the graph.
152  * This method could otherwise be useful for managing several SLAM graphs in one instance of LUM.
153  * \param[in] slam_graph The new SLAM graph.
154  */
155  inline void
156  setLoopGraph (const SLAMGraphPtr &slam_graph);
157 
158  /** \brief Get the internal SLAM graph structure.
159  * \details All data used and produced by LUM is stored in this boost::adjacency_list.
160  * It is recommended to use the LUM class itself to build the graph.
161  * This method could otherwise be useful for managing several SLAM graphs in one instance of LUM.
162  * \return The current SLAM graph.
163  */
164  inline SLAMGraphPtr
165  getLoopGraph () const;
166 
167  /** \brief Get the number of vertices in the SLAM graph.
168  * \return The current number of vertices in the SLAM graph.
169  */
170  typename SLAMGraph::vertices_size_type
171  getNumVertices () const;
172 
173  /** \brief Set the maximum number of iterations for the compute() method.
174  * \details The compute() method finishes when max_iterations are met or when the convergence criteria is met.
175  * \param[in] max_iterations The new maximum number of iterations (default = 5).
176  */
177  void
178  setMaxIterations (int max_iterations);
179 
180  /** \brief Get the maximum number of iterations for the compute() method.
181  * \details The compute() method finishes when max_iterations are met or when the convergence criteria is met.
182  * \return The current maximum number of iterations (default = 5).
183  */
184  inline int
185  getMaxIterations () const;
186 
187  /** \brief Set the convergence threshold for the compute() method.
188  * \details When the compute() method computes the new poses relative to the old poses, it will determine the length of the difference vector.
189  * When the average length of all difference vectors becomes less than the convergence_threshold the convergence is assumed to be met.
190  * \param[in] convergence_threshold The new convergence threshold (default = 0.0).
191  */
192  void
193  setConvergenceThreshold (float convergence_threshold);
194 
195  /** \brief Get the convergence threshold for the compute() method.
196  * \details When the compute() method computes the new poses relative to the old poses, it will determine the length of the difference vector.
197  * When the average length of all difference vectors becomes less than the convergence_threshold the convergence is assumed to be met.
198  * \return The current convergence threshold (default = 0.0).
199  */
200  inline float
201  getConvergenceThreshold () const;
202 
203  /** \brief Add a new point cloud to the SLAM graph.
204  * \details This method will add a new vertex to the SLAM graph and attach a point cloud to that vertex.
205  * Optionally you can specify a pose estimate for this point cloud.
206  * A vertex' pose is always relative to the first vertex in the SLAM graph, i.e. the first point cloud that was added.
207  * Because this first vertex is the reference, you can not set a pose estimate for this vertex.
208  * Providing pose estimates to the vertices in the SLAM graph will reduce overall computation time of LUM.
209  * \note Vertex descriptors are typecastable to int.
210  * \param[in] cloud The new point cloud.
211  * \param[in] pose (optional) The pose estimate relative to the reference pose (first point cloud that was added).
212  * \return The vertex descriptor of the newly created vertex.
213  */
214  Vertex
215  addPointCloud (const PointCloudPtr &cloud, const Eigen::Vector6f &pose = Eigen::Vector6f::Zero ());
216 
217  /** \brief Change a point cloud on one of the SLAM graph's vertices.
218  * \details This method will change the point cloud attached to an existing vertex and will not alter the SLAM graph structure.
219  * Note that the correspondences attached to this vertex will not change and may need to be updated manually.
220  * \note Vertex descriptors are typecastable to int.
221  * \param[in] vertex The vertex descriptor of which to change the point cloud.
222  * \param[in] cloud The new point cloud for that vertex.
223  */
224  inline void
225  setPointCloud (const Vertex &vertex, const PointCloudPtr &cloud);
226 
227  /** \brief Return a point cloud from one of the SLAM graph's vertices.
228  * \note Vertex descriptors are typecastable to int.
229  * \param[in] vertex The vertex descriptor of which to return the point cloud.
230  * \return The current point cloud for that vertex.
231  */
232  inline PointCloudPtr
233  getPointCloud (const Vertex &vertex) const;
234 
235  /** \brief Change a pose estimate on one of the SLAM graph's vertices.
236  * \details A vertex' pose is always relative to the first vertex in the SLAM graph, i.e. the first point cloud that was added.
237  * Because this first vertex is the reference, you can not set a pose estimate for this vertex.
238  * Providing pose estimates to the vertices in the SLAM graph will reduce overall computation time of LUM.
239  * \note Vertex descriptors are typecastable to int.
240  * \param[in] vertex The vertex descriptor of which to set the pose estimate.
241  * \param[in] pose The new pose estimate for that vertex.
242  */
243  inline void
244  setPose (const Vertex &vertex, const Eigen::Vector6f &pose);
245 
246  /** \brief Return a pose estimate from one of the SLAM graph's vertices.
247  * \note Vertex descriptors are typecastable to int.
248  * \param[in] vertex The vertex descriptor of which to return the pose estimate.
249  * \return The current pose estimate of that vertex.
250  */
251  inline Eigen::Vector6f
252  getPose (const Vertex &vertex) const;
253 
254  /** \brief Return a pose estimate from one of the SLAM graph's vertices as an affine transformation matrix.
255  * \note Vertex descriptors are typecastable to int.
256  * \param[in] vertex The vertex descriptor of which to return the transformation matrix.
257  * \return The current transformation matrix of that vertex.
258  */
259  inline Eigen::Affine3f
260  getTransformation (const Vertex &vertex) const;
261 
262  /** \brief Add/change a set of correspondences for one of the SLAM graph's edges.
263  * \details The edges in the SLAM graph are directional and point from source vertex to target vertex.
264  * The query indices of the correspondences, index the points at the source vertex' point cloud.
265  * The matching indices of the correspondences, index the points at the target vertex' point cloud.
266  * If no edge was present at the specified location, this method will add a new edge to the SLAM graph and attach the correspondences to that edge.
267  * If the edge was already present, this method will overwrite the correspondence information of that edge and will not alter the SLAM graph structure.
268  * \note Vertex descriptors are typecastable to int.
269  * \param[in] source_vertex The vertex descriptor of the correspondences' source point cloud.
270  * \param[in] target_vertex The vertex descriptor of the correspondences' target point cloud.
271  * \param[in] corrs The new set of correspondences for that edge.
272  */
273  void
274  setCorrespondences (const Vertex &source_vertex,
275  const Vertex &target_vertex,
276  const pcl::CorrespondencesPtr &corrs);
277 
278  /** \brief Return a set of correspondences from one of the SLAM graph's edges.
279  * \note Vertex descriptors are typecastable to int.
280  * \param[in] source_vertex The vertex descriptor of the correspondences' source point cloud.
281  * \param[in] target_vertex The vertex descriptor of the correspondences' target point cloud.
282  * \return The current set of correspondences of that edge.
283  */
285  getCorrespondences (const Vertex &source_vertex, const Vertex &target_vertex) const;
286 
287  /** \brief Perform LUM's globally consistent scan matching.
288  * \details Computation uses the first point cloud in the SLAM graph as a reference pose and attempts to align all other point clouds to it simultaneously.
289  * <br>
290  * Things to keep in mind:
291  * <ul>
292  * <li>Only those parts of the graph connected to the reference pose will properly align to it.</li>
293  * <li>All sets of correspondences should span the same space and need to be sufficient to determine a rigid transformation.</li>
294  * <li>The algorithm draws it strength from loops in the graph because it will distribute errors evenly amongst those loops.</li>
295  * </ul>
296  * Computation ends when either of the following conditions hold:
297  * <ul>
298  * <li>The number of iterations reaches max_iterations. Use setMaxIterations() to change.</li>
299  * <li>The convergence criteria is met. Use setConvergenceThreshold() to change.</li>
300  * </ul>
301  * Computation will change the pose estimates for the vertices of the SLAM graph, not the point clouds attached to them.
302  * The results can be retrieved with getPose(), getTransformation(), getTransformedCloud() or getConcatenatedCloud().
303  */
304  void
305  compute ();
306 
307  /** \brief Return a point cloud from one of the SLAM graph's vertices compounded onto its current pose estimate.
308  * \note Vertex descriptors are typecastable to int.
309  * \param[in] vertex The vertex descriptor of which to return the transformed point cloud.
310  * \return The transformed point cloud of that vertex.
311  */
312  PointCloudPtr
313  getTransformedCloud (const Vertex &vertex) const;
314 
315  /** \brief Return a concatenated point cloud of all the SLAM graph's point clouds compounded onto their current pose estimates.
316  * \return The concatenated transformed point clouds of the entire SLAM graph.
317  */
318  PointCloudPtr
319  getConcatenatedCloud () const;
320 
321  protected:
322  /** \brief Linearized computation of C^-1 and C^-1*D (results stored in slam_graph_). */
323  void
324  computeEdge (const Edge &e);
325 
326  /** \brief Returns a pose corrected 6DoF incidence matrix. */
327  inline Eigen::Matrix6f
328  incidenceCorrection (const Eigen::Vector6f &pose);
329 
330  private:
331  /** \brief The internal SLAM graph structure. */
332  SLAMGraphPtr slam_graph_;
333 
334  /** \brief The maximum number of iterations for the compute() method. */
335  int max_iterations_;
336 
337  /** \brief The convergence threshold for the summed vector lengths of all poses. */
338  float convergence_threshold_;
339  };
340  }
341 }
342 
343 #ifdef PCL_NO_PRECOMPILE
344 #include <pcl/registration/impl/lum.hpp>
345 #endif
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:180
pcl::CorrespondencesPtr corrs_
Definition: lum.h:129
boost::adjacency_list< boost::eigen_vecS, boost::eigen_vecS, boost::bidirectionalS, VertexProperties, EdgeProperties, boost::no_property, boost::eigen_listS > SLAMGraph
Definition: lum.h:135
typename PointCloud::Ptr PointCloudPtr
Definition: lum.h:118
LUM()
Empty constructor.
Definition: lum.h:142
int getMaxIterations() const
Get the maximum number of iterations for the compute() method.
Definition: lum.hpp:82
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:282
Eigen::Matrix< float, 6, 6 > Matrix6f
Definition: lum.h:55
void compute()
Perform LUM's globally consistent scan matching.
Definition: lum.hpp:217
Globally Consistent Scan Matching based on an algorithm by Lu and Milios.
Definition: lum.h:111
shared_ptr< const LUM< PointT > > ConstPtr
Definition: lum.h:115
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: memory.h:63
PointCloudPtr getPointCloud(const Vertex &vertex) const
Return a point cloud from one of the SLAM graph's vertices.
Definition: lum.hpp:131
typename SLAMGraph::edge_descriptor Edge
Definition: lum.h:138
void setLoopGraph(const SLAMGraphPtr &slam_graph)
Set the internal SLAM graph structure.
Definition: lum.hpp:54
typename SLAMGraph::vertex_descriptor Vertex
Definition: lum.h:137
Definition: bfgs.h:9
Definition: edge.h:49
Eigen::Matrix6f incidenceCorrection(const Eigen::Vector6f &pose)
Returns a pose corrected 6DoF incidence matrix.
Definition: lum.hpp:410
PointCloudPtr getConcatenatedCloud() const
Return a concatenated point cloud of all the SLAM graph's point clouds compounded onto their current ...
Definition: lum.hpp:291
Eigen::Matrix< float, 6, 1 > Vector6f
Definition: lum.h:54
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
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:197
Eigen::Vector6f getPose(const Vertex &vertex) const
Return a pose estimate from one of the SLAM graph's vertices.
Definition: lum.hpp:160
shared_ptr< LUM< PointT > > Ptr
Definition: lum.h:114
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:172
SLAMGraph::vertices_size_type getNumVertices() const
Get the number of vertices in the SLAM graph.
Definition: lum.hpp:68
Vertex addPointCloud(const PointCloudPtr &cloud, const Eigen::Vector6f &pose=Eigen::Vector6f::Zero())
Add a new point cloud to the SLAM graph.
Definition: lum.hpp:103
SLAMGraphPtr getLoopGraph() const
Get the internal SLAM graph structure.
Definition: lum.hpp:61
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: lum.h:119
void setPointCloud(const Vertex &vertex, const PointCloudPtr &cloud)
Change a point cloud on one of the SLAM graph's vertices.
Definition: lum.hpp:119
shared_ptr< SLAMGraph > SLAMGraphPtr
Definition: lum.h:136
PointCloud represents the base class in PCL for storing collections of 3D points. ...
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
void setMaxIterations(int max_iterations)
Set the maximum number of iterations for the compute() method.
Definition: lum.hpp:75
void computeEdge(const Edge &e)
Linearized computation of C^-1 and C^-1*D (results stored in slam_graph_).
Definition: lum.hpp:306
void setConvergenceThreshold(float convergence_threshold)
Set the convergence threshold for the compute() method.
Definition: lum.hpp:89
float getConvergenceThreshold() const
Get the convergence threshold for the compute() method.
Definition: lum.hpp:96
shared_ptr< Correspondences > CorrespondencesPtr
void setPose(const Vertex &vertex, const Eigen::Vector6f &pose)
Change a pose estimate on one of the SLAM graph's vertices.
Definition: lum.hpp:143