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