Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
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
50namespace Eigen {
51using Vector6f = Eigen::Matrix<float, 6, 1>;
52using Matrix6f = Eigen::Matrix<float, 6, 6>;
53} // namespace Eigen
54
55namespace pcl {
56namespace 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 */
107template <typename PointT>
108class LUM {
109public:
110 using Ptr = shared_ptr<LUM<PointT>>;
111 using ConstPtr = shared_ptr<const LUM<PointT>>;
112
116
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) {}
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
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
331protected:
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
341private:
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_{5};
347
348 /** \brief The convergence threshold for the summed vector lengths of all poses. */
349 float convergence_threshold_{0.0};
350};
351} // namespace registration
352} // namespace pcl
353
354#ifdef PCL_NO_PRECOMPILE
355#include <pcl/registration/impl/lum.hpp>
356#endif
PointCloud represents the base class in PCL for storing collections of 3D points.
shared_ptr< PointCloud< PointT > > Ptr
shared_ptr< const PointCloud< PointT > > ConstPtr
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:86
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