Point Cloud Library (PCL)  1.10.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/memory.h>
45 #include <pcl/sample_consensus/ransac.h>
46 #include <pcl/sample_consensus/sac_model_registration.h>
47 #include <pcl/registration/registration.h>
48 #include <pcl/registration/transformation_estimation_svd.h>
49 #include <pcl/registration/transformation_estimation_point_to_plane_lls.h>
50 #include <pcl/registration/transformation_estimation_symmetric_point_to_plane_lls.h>
51 #include <pcl/registration/correspondence_estimation.h>
52 #include <pcl/registration/default_convergence_criteria.h>
53 
54 namespace pcl
55 {
56  /** \brief @b IterativeClosestPoint provides a base implementation of the Iterative Closest Point algorithm.
57  * The transformation is estimated based on Singular Value Decomposition (SVD).
58  *
59  * The algorithm has several termination criteria:
60  *
61  * <ol>
62  * <li>Number of iterations has reached the maximum user imposed number of iterations (via \ref setMaximumIterations)</li>
63  * <li>The epsilon (difference) between the previous transformation and the current estimated transformation is smaller than an user imposed value (via \ref setTransformationEpsilon)</li>
64  * <li>The sum of Euclidean squared errors is smaller than a user defined threshold (via \ref setEuclideanFitnessEpsilon)</li>
65  * </ol>
66  *
67  *
68  * Usage example:
69  * \code
70  * IterativeClosestPoint<PointXYZ, PointXYZ> icp;
71  * // Set the input source and target
72  * icp.setInputCloud (cloud_source);
73  * icp.setInputTarget (cloud_target);
74  *
75  * // Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored)
76  * icp.setMaxCorrespondenceDistance (0.05);
77  * // Set the maximum number of iterations (criterion 1)
78  * icp.setMaximumIterations (50);
79  * // Set the transformation epsilon (criterion 2)
80  * icp.setTransformationEpsilon (1e-8);
81  * // Set the euclidean distance difference epsilon (criterion 3)
82  * icp.setEuclideanFitnessEpsilon (1);
83  *
84  * // Perform the alignment
85  * icp.align (cloud_source_registered);
86  *
87  * // Obtain the transformation that aligned cloud_source to cloud_source_registered
88  * Eigen::Matrix4f transformation = icp.getFinalTransformation ();
89  * \endcode
90  *
91  * \author Radu B. Rusu, Michael Dixon
92  * \ingroup registration
93  */
94  template <typename PointSource, typename PointTarget, typename Scalar = float>
95  class IterativeClosestPoint : public Registration<PointSource, PointTarget, Scalar>
96  {
97  public:
99  using PointCloudSourcePtr = typename PointCloudSource::Ptr;
100  using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
101 
103  using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
104  using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
105 
108 
111 
134 
137 
138  /** \brief Empty constructor. */
140  : x_idx_offset_ (0)
141  , y_idx_offset_ (0)
142  , z_idx_offset_ (0)
143  , nx_idx_offset_ (0)
144  , ny_idx_offset_ (0)
145  , nz_idx_offset_ (0)
147  , source_has_normals_ (false)
148  , target_has_normals_ (false)
149  {
150  reg_name_ = "IterativeClosestPoint";
154  };
155 
156  /** \brief Empty destructor */
158 
159  /** \brief Returns a pointer to the DefaultConvergenceCriteria used by the IterativeClosestPoint class.
160  * This allows to check the convergence state after the align() method as well as to configure
161  * DefaultConvergenceCriteria's parameters not available through the ICP API before the align()
162  * method is called. Please note that the align method sets max_iterations_,
163  * euclidean_fitness_epsilon_ and transformation_epsilon_ and therefore overrides the default / set
164  * values of the DefaultConvergenceCriteria instance.
165  * \return Pointer to the IterativeClosestPoint's DefaultConvergenceCriteria.
166  */
169  {
170  return convergence_criteria_;
171  }
172 
173  /** \brief Provide a pointer to the input source
174  * (e.g., the point cloud that we want to align to the target)
175  *
176  * \param[in] cloud the input point cloud source
177  */
178  void
179  setInputSource (const PointCloudSourceConstPtr &cloud) override
180  {
182  const auto fields = pcl::getFields<PointSource> ();
183  source_has_normals_ = false;
184  for (const auto &field : fields)
185  {
186  if (field.name == "x") x_idx_offset_ = field.offset;
187  else if (field.name == "y") y_idx_offset_ = field.offset;
188  else if (field.name == "z") z_idx_offset_ = field.offset;
189  else if (field.name == "normal_x")
190  {
191  source_has_normals_ = true;
192  nx_idx_offset_ = field.offset;
193  }
194  else if (field.name == "normal_y")
195  {
196  source_has_normals_ = true;
197  ny_idx_offset_ = field.offset;
198  }
199  else if (field.name == "normal_z")
200  {
201  source_has_normals_ = true;
202  nz_idx_offset_ = field.offset;
203  }
204  }
205  }
206 
207  /** \brief Provide a pointer to the input target
208  * (e.g., the point cloud that we want to align to the target)
209  *
210  * \param[in] cloud the input point cloud target
211  */
212  void
213  setInputTarget (const PointCloudTargetConstPtr &cloud) override
214  {
216  const auto fields = pcl::getFields<PointSource> ();
217  target_has_normals_ = false;
218  for (const auto &field : fields)
219  {
220  if (field.name == "normal_x" || field.name == "normal_y" || field.name == "normal_z")
221  {
222  target_has_normals_ = true;
223  break;
224  }
225  }
226  }
227 
228  /** \brief Set whether to use reciprocal correspondence or not
229  *
230  * \param[in] use_reciprocal_correspondence whether to use reciprocal correspondence or not
231  */
232  inline void
233  setUseReciprocalCorrespondences (bool use_reciprocal_correspondence)
234  {
235  use_reciprocal_correspondence_ = use_reciprocal_correspondence;
236  }
237 
238  /** \brief Obtain whether reciprocal correspondence are used or not */
239  inline bool
241  {
243  }
244 
245  protected:
246 
247  /** \brief Apply a rigid transform to a given dataset. Here we check whether whether
248  * the dataset has surface normals in addition to XYZ, and rotate normals as well.
249  * \param[in] input the input point cloud
250  * \param[out] output the resultant output point cloud
251  * \param[in] transform a 4x4 rigid transformation
252  * \note Can be used with cloud_in equal to cloud_out
253  */
254  virtual void
255  transformCloud (const PointCloudSource &input,
256  PointCloudSource &output,
257  const Matrix4 &transform);
258 
259  /** \brief Rigid transformation computation method with initial guess.
260  * \param output the transformed input point cloud dataset using the rigid transformation found
261  * \param guess the initial guess of the transformation to compute
262  */
263  void
264  computeTransformation (PointCloudSource &output, const Matrix4 &guess) override;
265 
266  /** \brief Looks at the Estimators and Rejectors and determines whether their blob-setter methods need to be called */
267  virtual void
269 
270  /** \brief XYZ fields offset. */
272 
273  /** \brief Normal fields offset. */
275 
276  /** \brief The correspondence type used for correspondence estimation. */
278 
279  /** \brief Internal check whether source dataset has normals or not. */
281  /** \brief Internal check whether target dataset has normals or not. */
283 
284  /** \brief Checks for whether estimators and rejectors need various data */
286  };
287 
288  /** \brief @b IterativeClosestPointWithNormals is a special case of
289  * IterativeClosestPoint, that uses a transformation estimated based on
290  * Point to Plane distances by default.
291  *
292  * By default, this implementation uses the traditional point to plane objective
293  * and computes point to plane distances using the normals of the target point
294  * cloud. It also provides the option (through setUseSymmetricObjective) of
295  * using the symmetric objective function of [Rusinkiewicz 2019]. This objective
296  * uses the normals of both the source and target point cloud and has a similar
297  * computational cost to the traditional point to plane objective while also
298  * offering improved convergence speed and a wider basin of convergence.
299  *
300  * Note that this implementation not demean the point clouds which can lead
301  * to increased numerical error. If desired, a user can demean the point cloud,
302  * run iterative closest point, and composite the resulting ICP transformation
303  * with the translations from demeaning to obtain a transformation between
304  * the original point clouds.
305  *
306  * \author Radu B. Rusu, Matthew Cong
307  * \ingroup registration
308  */
309  template <typename PointSource, typename PointTarget, typename Scalar = float>
310  class IterativeClosestPointWithNormals : public IterativeClosestPoint<PointSource, PointTarget, Scalar>
311  {
312  public:
316 
320 
323 
324  /** \brief Empty constructor. */
326  {
327  reg_name_ = "IterativeClosestPointWithNormals";
328  setUseSymmetricObjective (false);
329  setEnforceSameDirectionNormals (true);
330  //correspondence_rejectors_.add
331  };
332 
333  /** \brief Empty destructor */
335 
336  /** \brief Set whether to use a symmetric objective function or not
337  *
338  * \param[in] use_symmetric_objective whether to use a symmetric objective function or not
339  */
340  inline void
341  setUseSymmetricObjective (bool use_symmetric_objective)
342  {
343  use_symmetric_objective_ = use_symmetric_objective;
344  if (use_symmetric_objective_)
345  {
346  auto symmetric_transformation_estimation = pcl::make_shared<pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar> > ();
347  symmetric_transformation_estimation->setEnforceSameDirectionNormals (enforce_same_direction_normals_);
348  transformation_estimation_ = symmetric_transformation_estimation;
349  }
350  else
351  {
353  }
354  }
355 
356  /** \brief Obtain whether a symmetric objective is used or not */
357  inline bool
359  {
360  return use_symmetric_objective_;
361  }
362 
363  /** \brief Set whether or not to negate source or target normals on a per-point basis such that they point in the same direction. Only applicable to the symmetric objective function.
364  *
365  * \param[in] enforce_same_direction_normals whether to negate source or target normals on a per-point basis such that they point in the same direction.
366  */
367  inline void
368  setEnforceSameDirectionNormals (bool enforce_same_direction_normals)
369  {
370  enforce_same_direction_normals_ = enforce_same_direction_normals;
372  if (symmetric_transformation_estimation)
373  symmetric_transformation_estimation->setEnforceSameDirectionNormals (enforce_same_direction_normals_);
374  }
375 
376  /** \brief Obtain whether source or target normals are negated on a per-point basis such that they point in the same direction or not */
377  inline bool
379  {
380  return enforce_same_direction_normals_;
381  }
382 
383  protected:
384 
385  /** \brief Apply a rigid transform to a given dataset
386  * \param[in] input the input point cloud
387  * \param[out] output the resultant output point cloud
388  * \param[in] transform a 4x4 rigid transformation
389  * \note Can be used with cloud_in equal to cloud_out
390  */
391  virtual void
392  transformCloud (const PointCloudSource &input,
393  PointCloudSource &output,
394  const Matrix4 &transform);
395 
396  /** \brief Type of objective function (asymmetric vs. symmetric) used for transform estimation */
398  /** \brief Whether or not to negate source and/or target normals such that they point in the same direction in the symmetric objective function */
400  };
401 
402 }
403 
404 #include <pcl/registration/impl/icp.hpp>
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:399
typename Registration< PointSource, PointTarget, float >::PointCloudTarget PointCloudTarget
Definition: icp.h:102
Defines functions, macros and traits for allocating and using memory.
DefaultConvergenceCriteria represents an instantiation of ConvergenceCriteria, and implements the fol...
TransformationEstimationPtr transformation_estimation_
A TransformationEstimation object, used to calculate the 4x4 rigid transformation.
Definition: registration.h:560
IterativeClosestPointWithNormals is a special case of IterativeClosestPoint, that uses a transformati...
Definition: icp.h:310
bool target_has_normals_
Internal check whether target dataset has normals or not.
Definition: icp.h:282
typename Registration< PointSource, PointTarget, float >::Matrix4 Matrix4
Definition: icp.h:136
shared_ptr< const IterativeClosestPoint< PointSource, PointTarget, float > > ConstPtr
Definition: icp.h:110
bool getUseReciprocalCorrespondences() const
Obtain whether reciprocal correspondence are used or not.
Definition: icp.h:240
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) ...
Definition: registration.h:191
PointIndices::Ptr PointIndicesPtr
Definition: pcl_base.h:77
CorrespondenceEstimationPtr correspondence_estimation_
A CorrespondenceEstimation object, used to estimate correspondences between the source and the target...
Definition: registration.h:563
int nr_iterations_
The number of iterations the internal optimization ran for (used internally).
Definition: registration.h:499
std::size_t nx_idx_offset_
Normal fields offset.
Definition: icp.h:274
CorrespondencesPtr correspondences_
The set of correspondences determined at this ICP step.
Definition: registration.h:557
TransformationEstimationSymmetricPointToPlaneLLS implements a Linear Least Squares (LLS) approximatio...
virtual void determineRequiredBlobData()
Looks at the Estimators and Rejectors and determines whether their blob-setter methods need to be cal...
Definition: icp.hpp:254
Eigen::Matrix< Scalar, 4, 4 > Matrix4
Definition: registration.h:64
typename PointCloudSource::ConstPtr PointCloudSourceConstPtr
Definition: icp.h:100
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...
shared_ptr< ::pcl::PointIndices > Ptr
Definition: PointIndices.h:15
virtual void transformCloud(const PointCloudSource &input, PointCloudSource &output, const Matrix4 &transform)
Apply a rigid transform to a given dataset.
Definition: icp.hpp:49
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:368
Matrix4 transformation_
The transformation matrix estimated by the registration method.
Definition: registration.h:516
IterativeClosestPointWithNormals()
Empty constructor.
Definition: icp.h:325
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:179
TransformationEstimationSVD implements SVD-based estimation of the transformation aligning the given ...
typename Registration< PointSource, PointTarget, float >::PointCloudSource PointCloudSource
Definition: icp.h:98
shared_ptr< IterativeClosestPoint< PointSource, PointTarget, float > > Ptr
Definition: icp.h:109
virtual ~IterativeClosestPointWithNormals()
Empty destructor.
Definition: icp.h:334
~IterativeClosestPoint()
Empty destructor.
Definition: icp.h:157
IterativeClosestPoint provides a base implementation of the Iterative Closest Point algorithm...
Definition: icp.h:95
shared_ptr< DefaultConvergenceCriteria< Scalar > > Ptr
Registration represents the base registration class for general purpose, ICP-like methods...
Definition: registration.h:61
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
Definition: icp.h:104
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...
std::size_t y_idx_offset_
Definition: icp.h:271
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:378
bool source_has_normals_
Internal check whether source dataset has normals or not.
Definition: icp.h:280
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:213
shared_ptr< const ::pcl::PointIndices > ConstPtr
Definition: PointIndices.h:16
pcl::registration::DefaultConvergenceCriteria< Scalar >::Ptr getConvergeCriteria()
Returns a pointer to the DefaultConvergenceCriteria used by the IterativeClosestPoint class...
Definition: icp.h:168
pcl::registration::DefaultConvergenceCriteria< Scalar >::Ptr convergence_criteria_
Definition: icp.h:135
TransformationEstimationPointToPlaneLLS implements a Linear Least Squares (LLS) approximation for min...
std::string reg_name_
The registration method name.
Definition: registration.h:490
void computeTransformation(PointCloudSource &output, const Matrix4 &guess) override
Rigid transformation computation method with initial guess.
Definition: icp.hpp:119
bool need_source_blob_
Checks for whether estimators and rejectors need various data.
Definition: icp.h:285
bool use_reciprocal_correspondence_
The correspondence type used for correspondence estimation.
Definition: icp.h:277
void setUseReciprocalCorrespondences(bool use_reciprocal_correspondence)
Set whether to use reciprocal correspondence or not.
Definition: icp.h:233
bool getUseSymmetricObjective() const
Obtain whether a symmetric objective is used or not.
Definition: icp.h:358
IterativeClosestPoint()
Empty constructor.
Definition: icp.h:139
bool use_symmetric_objective_
Type of objective function (asymmetric vs.
Definition: icp.h:397
CorrespondenceEstimation represents the base class for determining correspondences between target and...
typename PointCloudTarget::Ptr PointCloudTargetPtr
Definition: icp.h:103
std::size_t z_idx_offset_
Definition: icp.h:271
boost::shared_ptr< T > shared_ptr
Alias for boost::shared_ptr.
Definition: memory.h:81
PointIndices::ConstPtr PointIndicesConstPtr
Definition: pcl_base.h:78
std::size_t x_idx_offset_
XYZ fields offset.
Definition: icp.h:271
typename PointCloudSource::Ptr PointCloudSourcePtr
Definition: icp.h:99
std::size_t nz_idx_offset_
Definition: icp.h:274
void setUseSymmetricObjective(bool use_symmetric_objective)
Set whether to use a symmetric objective function or not.
Definition: icp.h:341
std::size_t ny_idx_offset_
Definition: icp.h:274