Point Cloud Library (PCL) 1.12.0
icp.h
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010, 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$
38 *
39 */
40
41#pragma once
42
43// PCL includes
44#include <pcl/registration/correspondence_estimation.h>
45#include <pcl/registration/default_convergence_criteria.h>
46#include <pcl/registration/registration.h>
47#include <pcl/registration/transformation_estimation_point_to_plane_lls.h>
48#include <pcl/registration/transformation_estimation_svd.h>
49#include <pcl/registration/transformation_estimation_symmetric_point_to_plane_lls.h>
50#include <pcl/memory.h> // for dynamic_pointer_cast, pcl::make_shared, shared_ptr
51
52namespace pcl {
53/** \brief @b IterativeClosestPoint provides a base implementation of the Iterative
54 * Closest Point algorithm. The transformation is estimated based on Singular Value
55 * Decomposition (SVD).
56 *
57 * The algorithm has several termination criteria:
58 *
59 * <ol>
60 * <li>Number of iterations has reached the maximum user imposed number of iterations
61 * (via \ref setMaximumIterations)</li> <li>The epsilon (difference) between the
62 * previous transformation and the current estimated transformation is smaller than an
63 * user imposed value (via \ref setTransformationEpsilon)</li> <li>The sum of Euclidean
64 * squared errors is smaller than a user defined threshold (via \ref
65 * setEuclideanFitnessEpsilon)</li>
66 * </ol>
67 *
68 *
69 * Usage example:
70 * \code
71 * IterativeClosestPoint<PointXYZ, PointXYZ> icp;
72 * // Set the input source and target
73 * icp.setInputSource (cloud_source);
74 * icp.setInputTarget (cloud_target);
75 *
76 * // Set the max correspondence distance to 5cm (e.g., correspondences with higher
77 * // distances will be ignored)
78 * icp.setMaxCorrespondenceDistance (0.05);
79 * // Set the maximum number of iterations (criterion 1)
80 * icp.setMaximumIterations (50);
81 * // Set the transformation epsilon (criterion 2)
82 * icp.setTransformationEpsilon (1e-8);
83 * // Set the euclidean distance difference epsilon (criterion 3)
84 * icp.setEuclideanFitnessEpsilon (1);
85 *
86 * // Perform the alignment
87 * icp.align (cloud_source_registered);
88 *
89 * // Obtain the transformation that aligned cloud_source to cloud_source_registered
90 * Eigen::Matrix4f transformation = icp.getFinalTransformation ();
91 * \endcode
92 *
93 * \author Radu B. Rusu, Michael Dixon
94 * \ingroup registration
95 */
96template <typename PointSource, typename PointTarget, typename Scalar = float>
97class IterativeClosestPoint : public Registration<PointSource, PointTarget, Scalar> {
98public:
101 using PointCloudSourcePtr = typename PointCloudSource::Ptr;
102 using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
103
106 using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
107 using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
108
111
112 using Ptr = shared_ptr<IterativeClosestPoint<PointSource, PointTarget, Scalar>>;
113 using ConstPtr =
114 shared_ptr<const IterativeClosestPoint<PointSource, PointTarget, Scalar>>;
115
139
143
144 /** \brief Empty constructor. */
146 : x_idx_offset_(0)
147 , y_idx_offset_(0)
148 , z_idx_offset_(0)
149 , nx_idx_offset_(0)
150 , ny_idx_offset_(0)
151 , nz_idx_offset_(0)
153 , source_has_normals_(false)
154 , target_has_normals_(false)
155 {
156 reg_name_ = "IterativeClosestPoint";
159 TransformationEstimationSVD<PointSource, PointTarget, Scalar>());
166 };
167
168 /**
169 * \brief Due to `convergence_criteria_` holding references to the class members,
170 * it is tricky to correctly implement its copy and move operations correctly. This
171 * can result in subtle bugs and to prevent them, these operations for ICP have
172 * been disabled.
173 *
174 * \todo: remove deleted ctors and assignments operations after resolving the issue
175 */
182
183 /** \brief Empty destructor */
185
186 /** \brief Returns a pointer to the DefaultConvergenceCriteria used by the
187 * IterativeClosestPoint class. This allows to check the convergence state after the
188 * align() method as well as to configure DefaultConvergenceCriteria's parameters not
189 * available through the ICP API before the align() method is called. Please note that
190 * the align method sets max_iterations_, euclidean_fitness_epsilon_ and
191 * transformation_epsilon_ and therefore overrides the default / set values of the
192 * DefaultConvergenceCriteria instance. \return Pointer to the IterativeClosestPoint's
193 * DefaultConvergenceCriteria.
194 */
197 {
199 }
200
201 /** \brief Provide a pointer to the input source
202 * (e.g., the point cloud that we want to align to the target)
203 *
204 * \param[in] cloud the input point cloud source
205 */
206 void
208 {
210 const auto fields = pcl::getFields<PointSource>();
211 source_has_normals_ = false;
212 for (const auto& field : fields) {
213 if (field.name == "x")
214 x_idx_offset_ = field.offset;
215 else if (field.name == "y")
216 y_idx_offset_ = field.offset;
217 else if (field.name == "z")
218 z_idx_offset_ = field.offset;
219 else if (field.name == "normal_x") {
220 source_has_normals_ = true;
221 nx_idx_offset_ = field.offset;
222 }
223 else if (field.name == "normal_y") {
224 source_has_normals_ = true;
225 ny_idx_offset_ = field.offset;
226 }
227 else if (field.name == "normal_z") {
228 source_has_normals_ = true;
229 nz_idx_offset_ = field.offset;
230 }
231 }
232 }
233
234 /** \brief Provide a pointer to the input target
235 * (e.g., the point cloud that we want to align to the target)
236 *
237 * \param[in] cloud the input point cloud target
238 */
239 void
241 {
243 const auto fields = pcl::getFields<PointSource>();
244 target_has_normals_ = false;
245 for (const auto& field : fields) {
246 if (field.name == "normal_x" || field.name == "normal_y" ||
247 field.name == "normal_z") {
248 target_has_normals_ = true;
249 break;
250 }
251 }
252 }
253
254 /** \brief Set whether to use reciprocal correspondence or not
255 *
256 * \param[in] use_reciprocal_correspondence whether to use reciprocal correspondence
257 * or not
258 */
259 inline void
260 setUseReciprocalCorrespondences(bool use_reciprocal_correspondence)
261 {
262 use_reciprocal_correspondence_ = use_reciprocal_correspondence;
263 }
264
265 /** \brief Obtain whether reciprocal correspondence are used or not */
266 inline bool
268 {
270 }
271
272protected:
273 /** \brief Apply a rigid transform to a given dataset. Here we check whether
274 * the dataset has surface normals in addition to XYZ, and rotate normals as well.
275 * \param[in] input the input point cloud
276 * \param[out] output the resultant output point cloud
277 * \param[in] transform a 4x4 rigid transformation
278 * \note Can be used with cloud_in equal to cloud_out
279 */
280 virtual void
281 transformCloud(const PointCloudSource& input,
282 PointCloudSource& output,
283 const Matrix4& transform);
284
285 /** \brief Rigid transformation computation method with initial guess.
286 * \param output the transformed input point cloud dataset using the rigid
287 * transformation found \param guess the initial guess of the transformation to
288 * compute
289 */
290 void
291 computeTransformation(PointCloudSource& output, const Matrix4& guess) override;
292
293 /** \brief Looks at the Estimators and Rejectors and determines whether their
294 * blob-setter methods need to be called */
295 virtual void
297
298 /** \brief XYZ fields offset. */
300
301 /** \brief Normal fields offset. */
303
304 /** \brief The correspondence type used for correspondence estimation. */
306
307 /** \brief Internal check whether source dataset has normals or not. */
309 /** \brief Internal check whether target dataset has normals or not. */
311
312 /** \brief Checks for whether estimators and rejectors need various data */
314};
315
316/** \brief @b IterativeClosestPointWithNormals is a special case of
317 * IterativeClosestPoint, that uses a transformation estimated based on
318 * Point to Plane distances by default.
319 *
320 * By default, this implementation uses the traditional point to plane objective
321 * and computes point to plane distances using the normals of the target point
322 * cloud. It also provides the option (through setUseSymmetricObjective) of
323 * using the symmetric objective function of [Rusinkiewicz 2019]. This objective
324 * uses the normals of both the source and target point cloud and has a similar
325 * computational cost to the traditional point to plane objective while also
326 * offering improved convergence speed and a wider basin of convergence.
327 *
328 * Note that this implementation not demean the point clouds which can lead
329 * to increased numerical error. If desired, a user can demean the point cloud,
330 * run iterative closest point, and composite the resulting ICP transformation
331 * with the translations from demeaning to obtain a transformation between
332 * the original point clouds.
333 *
334 * \author Radu B. Rusu, Matthew Cong
335 * \ingroup registration
336 */
337template <typename PointSource, typename PointTarget, typename Scalar = float>
339: public IterativeClosestPoint<PointSource, PointTarget, Scalar> {
340public:
341 using PointCloudSource = typename IterativeClosestPoint<PointSource,
342 PointTarget,
343 Scalar>::PointCloudSource;
344 using PointCloudTarget = typename IterativeClosestPoint<PointSource,
345 PointTarget,
346 Scalar>::PointCloudTarget;
347 using Matrix4 =
349
355
356 using Ptr = shared_ptr<IterativeClosestPoint<PointSource, PointTarget, Scalar>>;
357 using ConstPtr =
358 shared_ptr<const IterativeClosestPoint<PointSource, PointTarget, Scalar>>;
359
360 /** \brief Empty constructor. */
362 {
363 reg_name_ = "IterativeClosestPointWithNormals";
366 // correspondence_rejectors_.add
367 };
368
369 /** \brief Empty destructor */
371
372 /** \brief Set whether to use a symmetric objective function or not
373 *
374 * \param[in] use_symmetric_objective whether to use a symmetric objective function or
375 * not
376 */
377 inline void
378 setUseSymmetricObjective(bool use_symmetric_objective)
379 {
380 use_symmetric_objective_ = use_symmetric_objective;
382 auto symmetric_transformation_estimation = pcl::make_shared<
384 PointSource,
385 PointTarget,
386 Scalar>>();
387 symmetric_transformation_estimation->setEnforceSameDirectionNormals(
389 transformation_estimation_ = symmetric_transformation_estimation;
390 }
391 else {
394 PointTarget,
395 Scalar>());
396 }
397 }
398
399 /** \brief Obtain whether a symmetric objective is used or not */
400 inline bool
402 {
404 }
405
406 /** \brief Set whether or not to negate source or target normals on a per-point basis
407 * such that they point in the same direction. Only applicable to the symmetric
408 * objective function.
409 *
410 * \param[in] enforce_same_direction_normals whether to negate source or target
411 * normals on a per-point basis such that they point in the same direction.
412 */
413 inline void
414 setEnforceSameDirectionNormals(bool enforce_same_direction_normals)
415 {
416 enforce_same_direction_normals_ = enforce_same_direction_normals;
417 auto symmetric_transformation_estimation = dynamic_pointer_cast<
419 PointTarget,
420 Scalar>>(
422 if (symmetric_transformation_estimation)
423 symmetric_transformation_estimation->setEnforceSameDirectionNormals(
425 }
426
427 /** \brief Obtain whether source or target normals are negated on a per-point basis
428 * such that they point in the same direction or not */
429 inline bool
431 {
433 }
434
435protected:
436 /** \brief Apply a rigid transform to a given dataset
437 * \param[in] input the input point cloud
438 * \param[out] output the resultant output point cloud
439 * \param[in] transform a 4x4 rigid transformation
440 * \note Can be used with cloud_in equal to cloud_out
441 */
442 virtual void
443 transformCloud(const PointCloudSource& input,
444 PointCloudSource& output,
445 const Matrix4& transform);
446
447 /** \brief Type of objective function (asymmetric vs. symmetric) used for transform
448 * estimation */
450 /** \brief Whether or not to negate source and/or target normals such that they point
451 * in the same direction in the symmetric objective function */
453};
454
455} // namespace pcl
456
457#include <pcl/registration/impl/icp.hpp>
IterativeClosestPoint provides a base implementation of the Iterative Closest Point algorithm.
Definition: icp.h:97
typename PointCloudSource::ConstPtr PointCloudSourceConstPtr
Definition: icp.h:102
typename Registration< PointSource, PointTarget, Scalar >::Matrix4 Matrix4
Definition: icp.h:142
std::size_t y_idx_offset_
Definition: icp.h:299
std::size_t nz_idx_offset_
Definition: icp.h:302
bool use_reciprocal_correspondence_
The correspondence type used for correspondence estimation.
Definition: icp.h:305
void setUseReciprocalCorrespondences(bool use_reciprocal_correspondence)
Set whether to use reciprocal correspondence or not.
Definition: icp.h:260
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
Definition: icp.h:107
typename Registration< PointSource, PointTarget, Scalar >::PointCloudTarget PointCloudTarget
Definition: icp.h:105
std::size_t ny_idx_offset_
Definition: icp.h:302
shared_ptr< IterativeClosestPoint< PointSource, PointTarget, Scalar > > Ptr
Definition: icp.h:112
bool need_source_blob_
Checks for whether estimators and rejectors need various data.
Definition: icp.h:313
typename PointCloudTarget::Ptr PointCloudTargetPtr
Definition: icp.h:106
typename Registration< PointSource, PointTarget, Scalar >::PointCloudSource PointCloudSource
Definition: icp.h:100
pcl::registration::DefaultConvergenceCriteria< Scalar >::Ptr convergence_criteria_
Definition: icp.h:141
bool getUseReciprocalCorrespondences() const
Obtain whether reciprocal correspondence are used or not.
Definition: icp.h:267
IterativeClosestPoint()
Empty constructor.
Definition: icp.h:145
void setInputTarget(const PointCloudTargetConstPtr &cloud) override
Provide a pointer to the input target (e.g., the point cloud that we want to align to the target)
Definition: icp.h:240
PointIndices::ConstPtr PointIndicesConstPtr
Definition: icp.h:110
bool source_has_normals_
Internal check whether source dataset has normals or not.
Definition: icp.h:308
virtual void transformCloud(const PointCloudSource &input, PointCloudSource &output, const Matrix4 &transform)
Apply a rigid transform to a given dataset.
Definition: icp.hpp:50
virtual void determineRequiredBlobData()
Looks at the Estimators and Rejectors and determines whether their blob-setter methods need to be cal...
Definition: icp.hpp:267
std::size_t nx_idx_offset_
Normal fields offset.
Definition: icp.h:302
~IterativeClosestPoint()
Empty destructor.
Definition: icp.h:184
IterativeClosestPoint & operator=(const IterativeClosestPoint &)=delete
IterativeClosestPoint(IterativeClosestPoint &&)=delete
void computeTransformation(PointCloudSource &output, const Matrix4 &guess) override
Rigid transformation computation method with initial guess.
Definition: icp.hpp:114
IterativeClosestPoint & operator=(IterativeClosestPoint &&)=delete
PointIndices::Ptr PointIndicesPtr
Definition: icp.h:109
IterativeClosestPoint(const IterativeClosestPoint &)=delete
Due to convergence_criteria_ holding references to the class members, it is tricky to correctly imple...
void setInputSource(const PointCloudSourceConstPtr &cloud) override
Provide a pointer to the input source (e.g., the point cloud that we want to align to the target)
Definition: icp.h:207
pcl::registration::DefaultConvergenceCriteria< Scalar >::Ptr getConvergeCriteria()
Returns a pointer to the DefaultConvergenceCriteria used by the IterativeClosestPoint class.
Definition: icp.h:196
bool target_has_normals_
Internal check whether target dataset has normals or not.
Definition: icp.h:310
typename PointCloudSource::Ptr PointCloudSourcePtr
Definition: icp.h:101
shared_ptr< const IterativeClosestPoint< PointSource, PointTarget, Scalar > > ConstPtr
Definition: icp.h:114
std::size_t z_idx_offset_
Definition: icp.h:299
std::size_t x_idx_offset_
XYZ fields offset.
Definition: icp.h:299
IterativeClosestPointWithNormals is a special case of IterativeClosestPoint, that uses a transformati...
Definition: icp.h:339
shared_ptr< const IterativeClosestPoint< PointSource, PointTarget, Scalar > > ConstPtr
Definition: icp.h:358
IterativeClosestPointWithNormals()
Empty constructor.
Definition: icp.h:361
void setUseSymmetricObjective(bool use_symmetric_objective)
Set whether to use a symmetric objective function or not.
Definition: icp.h:378
shared_ptr< IterativeClosestPoint< PointSource, PointTarget, Scalar > > Ptr
Definition: icp.h:356
typename IterativeClosestPoint< PointSource, PointTarget, Scalar >::PointCloudSource PointCloudSource
Definition: icp.h:343
bool getUseSymmetricObjective() const
Obtain whether a symmetric objective is used or not.
Definition: icp.h:401
bool use_symmetric_objective_
Type of objective function (asymmetric vs.
Definition: icp.h:449
void setEnforceSameDirectionNormals(bool enforce_same_direction_normals)
Set whether or not to negate source or target normals on a per-point basis such that they point in th...
Definition: icp.h:414
virtual void transformCloud(const PointCloudSource &input, PointCloudSource &output, const Matrix4 &transform)
Apply a rigid transform to a given dataset.
Definition: icp.hpp:309
bool getEnforceSameDirectionNormals() const
Obtain whether source or target normals are negated on a per-point basis such that they point in the ...
Definition: icp.h:430
typename IterativeClosestPoint< PointSource, PointTarget, Scalar >::Matrix4 Matrix4
Definition: icp.h:348
virtual ~IterativeClosestPointWithNormals()
Empty destructor.
Definition: icp.h:370
bool enforce_same_direction_normals_
Whether or not to negate source and/or target normals such that they point in the same direction in t...
Definition: icp.h:452
typename IterativeClosestPoint< PointSource, PointTarget, Scalar >::PointCloudTarget PointCloudTarget
Definition: icp.h:346
Registration represents the base registration class for general purpose, ICP-like methods.
Definition: registration.h:57
virtual void setInputSource(const PointCloudSourceConstPtr &cloud)
Provide a pointer to the input source (e.g., the point cloud that we want to align to the target)
std::string reg_name_
The registration method name.
Definition: registration.h:558
Matrix4 transformation_
The transformation matrix estimated by the registration method.
Definition: registration.h:586
virtual void setInputTarget(const PointCloudTargetConstPtr &cloud)
Provide a pointer to the input target (e.g., the point cloud that we want to align the input source t...
CorrespondenceEstimationPtr correspondence_estimation_
A CorrespondenceEstimation object, used to estimate correspondences between the source and the target...
Definition: registration.h:639
int nr_iterations_
The number of iterations the internal optimization ran for (used internally).
Definition: registration.h:568
Eigen::Matrix< Scalar, 4, 4 > Matrix4
Definition: registration.h:59
CorrespondencesPtr correspondences_
The set of correspondences determined at this ICP step.
Definition: registration.h:631
TransformationEstimationPtr transformation_estimation_
A TransformationEstimation object, used to calculate the 4x4 rigid transformation.
Definition: registration.h:635
double transformation_rotation_epsilon_
The maximum rotation difference between two consecutive transformations in order to consider converge...
Definition: registration.h:600
std::vector< CorrespondenceRejectorPtr > correspondence_rejectors_
The list of correspondence rejectors to use.
Definition: registration.h:642
DefaultConvergenceCriteria represents an instantiation of ConvergenceCriteria, and implements the fol...
shared_ptr< DefaultConvergenceCriteria< Scalar > > Ptr
TransformationEstimationPointToPlaneLLS implements a Linear Least Squares (LLS) approximation for min...
TransformationEstimationSymmetricPointToPlaneLLS implements a Linear Least Squares (LLS) approximatio...
Defines functions, macros and traits for allocating and using memory.
shared_ptr< T > make_shared(Args &&... args)
Returns a pcl::shared_ptr compliant with type T's allocation policy.
shared_ptr< ::pcl::PointIndices > Ptr
Definition: PointIndices.h:13
shared_ptr< const ::pcl::PointIndices > ConstPtr
Definition: PointIndices.h:14