Point Cloud Library (PCL)  1.7.1
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 #ifndef PCL_ICP_H_
42 #define PCL_ICP_H_
43 
44 // PCL includes
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/correspondence_estimation.h>
51 #include <pcl/registration/default_convergence_criteria.h>
52 
53 namespace pcl
54 {
55  /** \brief @b IterativeClosestPoint provides a base implementation of the Iterative Closest Point algorithm.
56  * The transformation is estimated based on Singular Value 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 (via \ref setMaximumIterations)</li>
62  * <li>The epsilon (difference) between the previous transformation and the current estimated transformation is smaller than an user imposed value (via \ref setTransformationEpsilon)</li>
63  * <li>The sum of Euclidean squared errors is smaller than a user defined threshold (via \ref setEuclideanFitnessEpsilon)</li>
64  * </ol>
65  *
66  *
67  * Usage example:
68  * \code
69  * IterativeClosestPoint<PointXYZ, PointXYZ> icp;
70  * // Set the input source and target
71  * icp.setInputCloud (cloud_source);
72  * icp.setInputTarget (cloud_target);
73  *
74  * // Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored)
75  * icp.setMaxCorrespondenceDistance (0.05);
76  * // Set the maximum number of iterations (criterion 1)
77  * icp.setMaximumIterations (50);
78  * // Set the transformation epsilon (criterion 2)
79  * icp.setTransformationEpsilon (1e-8);
80  * // Set the euclidean distance difference epsilon (criterion 3)
81  * icp.setEuclideanFitnessEpsilon (1);
82  *
83  * // Perform the alignment
84  * icp.align (cloud_source_registered);
85  *
86  * // Obtain the transformation that aligned cloud_source to cloud_source_registered
87  * Eigen::Matrix4f transformation = icp.getFinalTransformation ();
88  * \endcode
89  *
90  * \author Radu B. Rusu, Michael Dixon
91  * \ingroup registration
92  */
93  template <typename PointSource, typename PointTarget, typename Scalar = float>
94  class IterativeClosestPoint : public Registration<PointSource, PointTarget, Scalar>
95  {
96  public:
100 
104 
107 
108  typedef boost::shared_ptr<IterativeClosestPoint<PointSource, PointTarget, Scalar> > Ptr;
109  typedef boost::shared_ptr<const IterativeClosestPoint<PointSource, PointTarget, Scalar> > ConstPtr;
110 
133 
136 
137  /** \brief Empty constructor. */
139  : x_idx_offset_ (0)
140  , y_idx_offset_ (0)
141  , z_idx_offset_ (0)
142  , nx_idx_offset_ (0)
143  , ny_idx_offset_ (0)
144  , nz_idx_offset_ (0)
146  , source_has_normals_ (false)
147  , target_has_normals_ (false)
148  {
149  reg_name_ = "IterativeClosestPoint";
153  };
154 
155  /** \brief Empty destructor */
157 
158  /** \brief Returns a pointer to the DefaultConvergenceCriteria used by the IterativeClosestPoint class.
159  * This allows to check the convergence state after the align() method as well as to configure
160  * DefaultConvergenceCriteria's parameters not available through the ICP API before the align()
161  * method is called. Please note that the align method sets max_iterations_,
162  * euclidean_fitness_epsilon_ and transformation_epsilon_ and therefore overrides the default / set
163  * values of the DefaultConvergenceCriteria instance.
164  * \param[out] Pointer to the IterativeClosestPoint's DefaultConvergenceCriteria.
165  */
168  {
169  return convergence_criteria_;
170  }
171 
172  /** \brief Provide a pointer to the input source
173  * (e.g., the point cloud that we want to align to the target)
174  *
175  * \param[in] cloud the input point cloud source
176  */
177  virtual void
179  {
181  std::vector<pcl::PCLPointField> fields;
182  pcl::getFields (*cloud, fields);
183  source_has_normals_ = false;
184  for (size_t i = 0; i < fields.size (); ++i)
185  {
186  if (fields[i].name == "x") x_idx_offset_ = fields[i].offset;
187  else if (fields[i].name == "y") y_idx_offset_ = fields[i].offset;
188  else if (fields[i].name == "z") z_idx_offset_ = fields[i].offset;
189  else if (fields[i].name == "normal_x")
190  {
191  source_has_normals_ = true;
192  nx_idx_offset_ = fields[i].offset;
193  }
194  else if (fields[i].name == "normal_y")
195  {
196  source_has_normals_ = true;
197  ny_idx_offset_ = fields[i].offset;
198  }
199  else if (fields[i].name == "normal_z")
200  {
201  source_has_normals_ = true;
202  nz_idx_offset_ = fields[i].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  virtual void
214  {
216  std::vector<pcl::PCLPointField> fields;
217  pcl::getFields (*cloud, fields);
218  target_has_normals_ = false;
219  for (size_t i = 0; i < fields.size (); ++i)
220  {
221  if (fields[i].name == "normal_x" || fields[i].name == "normal_y" || fields[i].name == "normal_z")
222  {
223  target_has_normals_ = true;
224  break;
225  }
226  }
227  }
228 
229  /** \brief Set whether to use reciprocal correspondence or not
230  *
231  * \param[in] use_reciprocal_correspondence whether to use reciprocal correspondence or not
232  */
233  inline void
234  setUseReciprocalCorrespondences (bool use_reciprocal_correspondence)
235  {
236  use_reciprocal_correspondence_ = use_reciprocal_correspondence;
237  }
238 
239  /** \brief Obtain whether reciprocal correspondence are used or not */
240  inline bool
242  {
244  }
245 
246  protected:
247 
248  /** \brief Apply a rigid transform to a given dataset. Here we check whether whether
249  * the dataset has surface normals in addition to XYZ, and rotate normals as well.
250  * \param[in] input the input point cloud
251  * \param[out] output the resultant output point cloud
252  * \param[in] transform a 4x4 rigid transformation
253  * \note Can be used with cloud_in equal to cloud_out
254  */
255  virtual void
256  transformCloud (const PointCloudSource &input,
257  PointCloudSource &output,
258  const Matrix4 &transform);
259 
260  /** \brief Rigid transformation computation method with initial guess.
261  * \param output the transformed input point cloud dataset using the rigid transformation found
262  * \param guess the initial guess of the transformation to compute
263  */
264  virtual void
265  computeTransformation (PointCloudSource &output, const Matrix4 &guess);
266 
267  /** \brief XYZ fields offset. */
269 
270  /** \brief Normal fields offset. */
272 
273  /** \brief The correspondence type used for correspondence estimation. */
275 
276  /** \brief Internal check whether source dataset has normals or not. */
278  /** \brief Internal check whether target dataset has normals or not. */
280  };
281 
282  /** \brief @b IterativeClosestPointWithNormals is a special case of
283  * IterativeClosestPoint, that uses a transformation estimated based on
284  * Point to Plane distances by default.
285  *
286  * \author Radu B. Rusu
287  * \ingroup registration
288  */
289  template <typename PointSource, typename PointTarget, typename Scalar = float>
290  class IterativeClosestPointWithNormals : public IterativeClosestPoint<PointSource, PointTarget, Scalar>
291  {
292  public:
296 
300 
301  typedef boost::shared_ptr<IterativeClosestPoint<PointSource, PointTarget, Scalar> > Ptr;
302  typedef boost::shared_ptr<const IterativeClosestPoint<PointSource, PointTarget, Scalar> > ConstPtr;
303 
304  /** \brief Empty constructor. */
306  {
307  reg_name_ = "IterativeClosestPointWithNormals";
309  //correspondence_rejectors_.add
310  };
311 
312  /** \brief Empty destructor */
314 
315  protected:
316 
317  /** \brief Apply a rigid transform to a given dataset
318  * \param[in] input the input point cloud
319  * \param[out] output the resultant output point cloud
320  * \param[in] transform a 4x4 rigid transformation
321  * \note Can be used with cloud_in equal to cloud_out
322  */
323  virtual void
324  transformCloud (const PointCloudSource &input,
325  PointCloudSource &output,
326  const Matrix4 &transform);
327  };
328 }
329 
330 #include <pcl/registration/impl/icp.hpp>
331 
332 #endif //#ifndef PCL_ICP_H_
virtual void setInputTarget(const PointCloudTargetConstPtr &cloud)
Provide a pointer to the input target (e.g., the point cloud that we want to align to the target) ...
Definition: icp.h:213
boost::shared_ptr< IterativeClosestPoint< PointSource, PointTarget, Scalar > > Ptr
Definition: icp.h:301
virtual void transformCloud(const PointCloudSource &input, PointCloudSource &output, const Matrix4 &transform)
Apply a rigid transform to a given dataset.
Definition: icp.hpp:232
DefaultConvergenceCriteria represents an instantiation of ConvergenceCriteria, and implements the fol...
boost::shared_ptr< const IterativeClosestPoint< PointSource, PointTarget, Scalar > > ConstPtr
Definition: icp.h:302
IterativeClosestPoint< PointSource, PointTarget, Scalar >::Matrix4 Matrix4
Definition: icp.h:295
PointIndices::Ptr PointIndicesPtr
Definition: icp.h:105
PointCloudSource::ConstPtr PointCloudSourceConstPtr
Definition: icp.h:99
TransformationEstimationPtr transformation_estimation_
A TransformationEstimation object, used to calculate the 4x4 rigid transformation.
Definition: registration.h:543
Registration< PointSource, PointTarget, Scalar >::PointCloudSource PointCloudSource
Definition: icp.h:97
boost::shared_ptr< ::pcl::PointIndices const > ConstPtr
Definition: PointIndices.h:23
CorrespondencesPtr correspondences_
The set of correspondences determined at this ICP step.
Definition: registration.h:540
CorrespondenceEstimation represents the base class for determining correspondences between target and...
Registration< PointSource, PointTarget, Scalar >::PointCloudTarget PointCloudTarget
Definition: icp.h:101
boost::shared_ptr< ::pcl::PointIndices > Ptr
Definition: PointIndices.h:22
IterativeClosestPoint< PointSource, PointTarget, Scalar >::PointCloudTarget PointCloudTarget
Definition: icp.h:294
PointCloudTarget::ConstPtr PointCloudTargetConstPtr
Definition: icp.h:103
virtual ~IterativeClosestPoint()
Empty destructor.
Definition: icp.h:156
boost::shared_ptr< const IterativeClosestPoint< PointSource, PointTarget, Scalar > > ConstPtr
Definition: icp.h:109
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:192
IterativeClosestPointWithNormals()
Empty constructor.
Definition: icp.h:305
IterativeClosestPointWithNormals is a special case of IterativeClosestPoint, that uses a transformati...
Definition: icp.h:290
size_t nx_idx_offset_
Normal fields offset.
Definition: icp.h:271
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: icp.h:178
bool target_has_normals_
Internal check whether target dataset has normals or not.
Definition: icp.h:279
std::string reg_name_
The registration method name.
Definition: registration.h:478
virtual ~IterativeClosestPointWithNormals()
Empty destructor.
Definition: icp.h:313
Registration< PointSource, PointTarget, Scalar >::Matrix4 Matrix4
Definition: icp.h:135
boost::shared_ptr< IterativeClosestPoint< PointSource, PointTarget, Scalar > > Ptr
Definition: icp.h:108
virtual void transformCloud(const PointCloudSource &input, PointCloudSource &output, const Matrix4 &transform)
Apply a rigid transform to a given dataset.
Definition: icp.hpp:49
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...
PointIndices::ConstPtr PointIndicesConstPtr
Definition: icp.h:106
int nr_iterations_
The number of iterations the internal optimization ran for (used internally).
Definition: registration.h:487
IterativeClosestPoint()
Empty constructor.
Definition: icp.h:138
pcl::registration::DefaultConvergenceCriteria< Scalar >::Ptr getConvergeCriteria()
Returns a pointer to the DefaultConvergenceCriteria used by the IterativeClosestPoint class...
Definition: icp.h:167
size_t x_idx_offset_
XYZ fields offset.
Definition: icp.h:268
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
bool source_has_normals_
Internal check whether source dataset has normals or not.
Definition: icp.h:277
void getFields(const pcl::PointCloud< PointT > &cloud, std::vector< pcl::PCLPointField > &fields)
Get the list of available fields (i.e., dimension/channel)
Definition: io.hpp:78
TransformationEstimationSVD implements SVD-based estimation of the transformation aligning the given ...
pcl::registration::DefaultConvergenceCriteria< Scalar >::Ptr convergence_criteria_
Definition: icp.h:134
void setUseReciprocalCorrespondences(bool use_reciprocal_correspondence)
Set whether to use reciprocal correspondence or not.
Definition: icp.h:234
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
TransformationEstimationPointToPlaneLLS implements a Linear Least Squares (LLS) approximation for min...
Matrix4 transformation_
The transformation matrix estimated by the registration method.
Definition: registration.h:504
PointCloudSource::Ptr PointCloudSourcePtr
Definition: icp.h:98
CorrespondenceEstimationPtr correspondence_estimation_
A CorrespondenceEstimation object, used to estimate correspondences between the source and the target...
Definition: registration.h:546
virtual void computeTransformation(PointCloudSource &output, const Matrix4 &guess)
Rigid transformation computation method with initial guess.
Definition: icp.hpp:119
Registration represents the base registration class for general purpose, ICP-like methods...
Definition: registration.h:62
IterativeClosestPoint< PointSource, PointTarget, Scalar >::PointCloudSource PointCloudSource
Definition: icp.h:293
bool getUseReciprocalCorrespondences() const
Obtain whether reciprocal correspondence are used or not.
Definition: icp.h:241
PointCloudTarget::Ptr PointCloudTargetPtr
Definition: icp.h:102
Eigen::Matrix< Scalar, 4, 4 > Matrix4
Definition: registration.h:65
boost::shared_ptr< DefaultConvergenceCriteria< Scalar > > Ptr
IterativeClosestPoint provides a base implementation of the Iterative Closest Point algorithm...
Definition: icp.h:94
bool use_reciprocal_correspondence_
The correspondence type used for correspondence estimation.
Definition: icp.h:274