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 
52 namespace 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  */
96 template <typename PointSource, typename PointTarget, typename Scalar = float>
97 class IterativeClosestPoint : public Registration<PointSource, PointTarget, Scalar> {
98 public:
99   using PointCloudSource =
100       typename Registration<PointSource, PointTarget, Scalar>::PointCloudSource;
101   using PointCloudSourcePtr = typename PointCloudSource::Ptr;
102   using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
103 
104   using PointCloudTarget =
105       typename Registration<PointSource, PointTarget, Scalar>::PointCloudTarget;
106   using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
107   using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
108 
109   using PointIndicesPtr = PointIndices::Ptr;
110   using PointIndicesConstPtr = PointIndices::ConstPtr;
111 
112   using Ptr = shared_ptr<IterativeClosestPoint<PointSource, PointTarget, Scalar>>;
113   using ConstPtr =
114       shared_ptr<const IterativeClosestPoint<PointSource, PointTarget, Scalar>>;
115 
116   using Registration<PointSource, PointTarget, Scalar>::reg_name_;
117   using Registration<PointSource, PointTarget, Scalar>::getClassName;
118   using Registration<PointSource, PointTarget, Scalar>::input_;
119   using Registration<PointSource, PointTarget, Scalar>::indices_;
120   using Registration<PointSource, PointTarget, Scalar>::target_;
121   using Registration<PointSource, PointTarget, Scalar>::nr_iterations_;
122   using Registration<PointSource, PointTarget, Scalar>::max_iterations_;
123   using Registration<PointSource, PointTarget, Scalar>::previous_transformation_;
124   using Registration<PointSource, PointTarget, Scalar>::final_transformation_;
125   using Registration<PointSource, PointTarget, Scalar>::transformation_;
126   using Registration<PointSource, PointTarget, Scalar>::transformation_epsilon_;
127   using Registration<PointSource, PointTarget, Scalar>::
128       transformation_rotation_epsilon_;
129   using Registration<PointSource, PointTarget, Scalar>::converged_;
130   using Registration<PointSource, PointTarget, Scalar>::corr_dist_threshold_;
131   using Registration<PointSource, PointTarget, Scalar>::inlier_threshold_;
132   using Registration<PointSource, PointTarget, Scalar>::min_number_correspondences_;
133   using Registration<PointSource, PointTarget, Scalar>::update_visualizer_;
134   using Registration<PointSource, PointTarget, Scalar>::euclidean_fitness_epsilon_;
135   using Registration<PointSource, PointTarget, Scalar>::correspondences_;
136   using Registration<PointSource, PointTarget, Scalar>::transformation_estimation_;
137   using Registration<PointSource, PointTarget, Scalar>::correspondence_estimation_;
138   using Registration<PointSource, PointTarget, Scalar>::correspondence_rejectors_;
139 
140   typename pcl::registration::DefaultConvergenceCriteria<Scalar>::Ptr
141       convergence_criteria_;
142   using Matrix4 = typename Registration<PointSource, PointTarget, Scalar>::Matrix4;
143 
144   /** \brief Empty constructor. */
IterativeClosestPoint()145   IterativeClosestPoint()
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)
152   , use_reciprocal_correspondence_(false)
153   , source_has_normals_(false)
154   , target_has_normals_(false)
155   {
156     reg_name_ = "IterativeClosestPoint";
157     transformation_estimation_.reset(
158         new pcl::registration::
159             TransformationEstimationSVD<PointSource, PointTarget, Scalar>());
160     correspondence_estimation_.reset(
161         new pcl::registration::
162             CorrespondenceEstimation<PointSource, PointTarget, Scalar>);
163     convergence_criteria_.reset(
164         new pcl::registration::DefaultConvergenceCriteria<Scalar>(
165             nr_iterations_, transformation_, *correspondences_));
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    */
176   IterativeClosestPoint(const IterativeClosestPoint&) = delete;
177   IterativeClosestPoint(IterativeClosestPoint&&) = delete;
178   IterativeClosestPoint&
179   operator=(const IterativeClosestPoint&) = delete;
180   IterativeClosestPoint&
181   operator=(IterativeClosestPoint&&) = delete;
182 
183   /** \brief Empty destructor */
~IterativeClosestPoint()184   ~IterativeClosestPoint() {}
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    */
195   inline typename pcl::registration::DefaultConvergenceCriteria<Scalar>::Ptr
getConvergeCriteria()196   getConvergeCriteria()
197   {
198     return convergence_criteria_;
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
setInputSource(const PointCloudSourceConstPtr & cloud)207   setInputSource(const PointCloudSourceConstPtr& cloud) override
208   {
209     Registration<PointSource, PointTarget, Scalar>::setInputSource(cloud);
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
setInputTarget(const PointCloudTargetConstPtr & cloud)240   setInputTarget(const PointCloudTargetConstPtr& cloud) override
241   {
242     Registration<PointSource, PointTarget, Scalar>::setInputTarget(cloud);
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
setUseReciprocalCorrespondences(bool use_reciprocal_correspondence)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
getUseReciprocalCorrespondences()267   getUseReciprocalCorrespondences() const
268   {
269     return (use_reciprocal_correspondence_);
270   }
271 
272 protected:
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
296   determineRequiredBlobData();
297 
298   /** \brief XYZ fields offset. */
299   std::size_t x_idx_offset_, y_idx_offset_, z_idx_offset_;
300 
301   /** \brief Normal fields offset. */
302   std::size_t nx_idx_offset_, ny_idx_offset_, nz_idx_offset_;
303 
304   /** \brief The correspondence type used for correspondence estimation. */
305   bool use_reciprocal_correspondence_;
306 
307   /** \brief Internal check whether source dataset has normals or not. */
308   bool source_has_normals_;
309   /** \brief Internal check whether target dataset has normals or not. */
310   bool target_has_normals_;
311 
312   /** \brief Checks for whether estimators and rejectors need various data */
313   bool need_source_blob_, need_target_blob_;
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  */
337 template <typename PointSource, typename PointTarget, typename Scalar = float>
338 class IterativeClosestPointWithNormals
339 : public IterativeClosestPoint<PointSource, PointTarget, Scalar> {
340 public:
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 =
348       typename IterativeClosestPoint<PointSource, PointTarget, Scalar>::Matrix4;
349 
350   using IterativeClosestPoint<PointSource, PointTarget, Scalar>::reg_name_;
351   using IterativeClosestPoint<PointSource, PointTarget, Scalar>::
352       transformation_estimation_;
353   using IterativeClosestPoint<PointSource, PointTarget, Scalar>::
354       correspondence_rejectors_;
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. */
IterativeClosestPointWithNormals()361   IterativeClosestPointWithNormals()
362   {
363     reg_name_ = "IterativeClosestPointWithNormals";
364     setUseSymmetricObjective(false);
365     setEnforceSameDirectionNormals(true);
366     // correspondence_rejectors_.add
367   };
368 
369   /** \brief Empty destructor */
~IterativeClosestPointWithNormals()370   virtual ~IterativeClosestPointWithNormals() {}
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
setUseSymmetricObjective(bool use_symmetric_objective)378   setUseSymmetricObjective(bool use_symmetric_objective)
379   {
380     use_symmetric_objective_ = use_symmetric_objective;
381     if (use_symmetric_objective_) {
382       auto symmetric_transformation_estimation = pcl::make_shared<
383           pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS<
384               PointSource,
385               PointTarget,
386               Scalar>>();
387       symmetric_transformation_estimation->setEnforceSameDirectionNormals(
388           enforce_same_direction_normals_);
389       transformation_estimation_ = symmetric_transformation_estimation;
390     }
391     else {
392       transformation_estimation_.reset(
393           new pcl::registration::TransformationEstimationPointToPlaneLLS<PointSource,
394                                                                          PointTarget,
395                                                                          Scalar>());
396     }
397   }
398 
399   /** \brief Obtain whether a symmetric objective is used or not */
400   inline bool
getUseSymmetricObjective()401   getUseSymmetricObjective() const
402   {
403     return use_symmetric_objective_;
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
setEnforceSameDirectionNormals(bool enforce_same_direction_normals)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<
418         pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS<PointSource,
419                                                                             PointTarget,
420                                                                             Scalar>>(
421         transformation_estimation_);
422     if (symmetric_transformation_estimation)
423       symmetric_transformation_estimation->setEnforceSameDirectionNormals(
424           enforce_same_direction_normals_);
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
getEnforceSameDirectionNormals()430   getEnforceSameDirectionNormals() const
431   {
432     return enforce_same_direction_normals_;
433   }
434 
435 protected:
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 */
449   bool use_symmetric_objective_;
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 */
452   bool enforce_same_direction_normals_;
453 };
454 
455 } // namespace pcl
456 
457 #include <pcl/registration/impl/icp.hpp>
458