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