Point Cloud Library (PCL)  1.7.0
registration.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, 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_REGISTRATION_H_
42 #define PCL_REGISTRATION_H_
43 
44 // PCL includes
45 #include <pcl/pcl_base.h>
46 #include <pcl/common/transforms.h>
47 #include <pcl/pcl_macros.h>
48 #include <pcl/search/kdtree.h>
49 #include <pcl/kdtree/kdtree_flann.h>
50 #include <pcl/registration/boost.h>
51 #include <pcl/registration/transformation_estimation.h>
52 #include <pcl/registration/correspondence_estimation.h>
53 #include <pcl/registration/correspondence_rejection.h>
54 
55 namespace pcl
56 {
57  /** \brief @b Registration represents the base registration class for general purpose, ICP-like methods.
58  * \author Radu B. Rusu, Michael Dixon
59  * \ingroup registration
60  */
61  template <typename PointSource, typename PointTarget, typename Scalar = float>
62  class Registration : public PCLBase<PointSource>
63  {
64  public:
65  typedef Eigen::Matrix<Scalar, 4, 4> Matrix4;
66 
67  // using PCLBase<PointSource>::initCompute;
71 
72  typedef boost::shared_ptr< Registration<PointSource, PointTarget, Scalar> > Ptr;
73  typedef boost::shared_ptr< const Registration<PointSource, PointTarget, Scalar> > ConstPtr;
74 
78 
80  typedef typename KdTree::Ptr KdTreeReciprocalPtr;
81 
85 
89 
91 
95 
99 
100  /** \brief Empty constructor. */
102  : reg_name_ ()
103  , tree_ (new KdTree)
105  , nr_iterations_ (0)
106  , max_iterations_ (10)
107  , ransac_iterations_ (0)
108  , target_ ()
109  , final_transformation_ (Matrix4::Identity ())
110  , transformation_ (Matrix4::Identity ())
111  , previous_transformation_ (Matrix4::Identity ())
113  , euclidean_fitness_epsilon_ (-std::numeric_limits<double>::max ())
114  , corr_dist_threshold_ (std::sqrt (std::numeric_limits<double>::max ()))
115  , inlier_threshold_ (0.05)
116  , converged_ (false)
122  , target_cloud_updated_ (true)
123  , source_cloud_updated_ (true)
124  , force_no_recompute_ (false)
126  , update_visualizer_ (NULL)
127  , point_representation_ ()
128  {
129  }
130 
131  /** \brief destructor. */
132  virtual ~Registration () {}
133 
134  /** \brief Provide a pointer to the transformation estimation object.
135  * (e.g., SVD, point to plane etc.)
136  *
137  * \param[in] te is the pointer to the corresponding transformation estimation object
138  *
139  * Code example:
140  *
141  * \code
142  * TransformationEstimationPointToPlaneLLS<PointXYZ, PointXYZ>::Ptr trans_lls (new TransformationEstimationPointToPlaneLLS<PointXYZ, PointXYZ>);
143  * icp.setTransformationEstimation (trans_lls);
144  * // or...
145  * TransformationEstimationSVD<PointXYZ, PointXYZ>::Ptr trans_svd (new TransformationEstimationSVD<PointXYZ, PointXYZ>);
146  * icp.setTransformationEstimation (trans_svd);
147  * \endcode
148  */
149  void
151 
152  /** \brief Provide a pointer to the correspondence estimation object.
153  * (e.g., regular, reciprocal, normal shooting etc.)
154  *
155  * \param[in] ce is the pointer to the corresponding correspondence estimation object
156  *
157  * Code example:
158  *
159  * \code
160  * CorrespondenceEstimation<PointXYZ, PointXYZ>::Ptr ce (new CorrespondenceEstimation<PointXYZ, PointXYZ>);
161  * ce->setInputSource (source);
162  * ce->setInputTarget (target);
163  * icp.setCorrespondenceEstimation (ce);
164  * // or...
165  * CorrespondenceEstimationNormalShooting<PointNormal, PointNormal, PointNormal>::Ptr cens (new CorrespondenceEstimationNormalShooting<PointNormal, PointNormal>);
166  * ce->setInputSource (source);
167  * ce->setInputTarget (target);
168  * ce->setSourceNormals (source);
169  * ce->setTargetNormals (target);
170  * icp.setCorrespondenceEstimation (cens);
171  * \endcode
172  */
173  void
175 
176  /** \brief Provide a pointer to the input source
177  * (e.g., the point cloud that we want to align to the target)
178  *
179  * \param[in] cloud the input point cloud source
180  */
181  PCL_DEPRECATED (void setInputCloud (const PointCloudSourceConstPtr &cloud), "[pcl::registration::Registration::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead.");
182 
183  /** \brief Get a pointer to the input point cloud dataset target. */
184  PCL_DEPRECATED (PointCloudSourceConstPtr const getInputCloud (), "[pcl::registration::Registration::getInputCloud] getInputCloud is deprecated. Please use getInputSource instead.");
185 
186  /** \brief Provide a pointer to the input source
187  * (e.g., the point cloud that we want to align to the target)
188  *
189  * \param[in] cloud the input point cloud source
190  */
191  virtual void
193  {
194  source_cloud_updated_ = true;
196  }
197 
198  /** \brief Get a pointer to the input point cloud dataset target. */
199  inline PointCloudSourceConstPtr const
200  getInputSource () { return (input_ ); }
201 
202  /** \brief Provide a pointer to the input target (e.g., the point cloud that we want to align the input source to)
203  * \param[in] cloud the input point cloud target
204  */
205  virtual inline void
207 
208  /** \brief Get a pointer to the input point cloud dataset target. */
209  inline PointCloudTargetConstPtr const
210  getInputTarget () { return (target_ ); }
211 
212 
213  /** \brief Provide a pointer to the search object used to find correspondences in
214  * the target cloud.
215  * \param[in] tree a pointer to the spatial search object.
216  * \param[in] force_no_recompute If set to true, this tree will NEVER be
217  * recomputed, regardless of calls to setInputTarget. Only use if you are
218  * confident that the tree will be set correctly.
219  */
220  inline void
222  bool force_no_recompute = false)
223  {
224  tree_ = tree;
225  if (force_no_recompute)
226  {
227  force_no_recompute_ = true;
228  }
229  // Since we just set a new tree, we need to check for updates
230  target_cloud_updated_ = true;
231  }
232 
233  /** \brief Get a pointer to the search method used to find correspondences in the
234  * target cloud. */
235  inline KdTreePtr
237  {
238  return (tree_);
239  }
240 
241  /** \brief Provide a pointer to the search object used to find correspondences in
242  * the source cloud (usually used by reciprocal correspondence finding).
243  * \param[in] tree a pointer to the spatial search object.
244  * \param[in] force_no_recompute If set to true, this tree will NEVER be
245  * recomputed, regardless of calls to setInputSource. Only use if you are
246  * extremely confident that the tree will be set correctly.
247  */
248  inline void
250  bool force_no_recompute = false)
251  {
252  tree_reciprocal_ = tree;
253  if ( force_no_recompute )
254  {
256  }
257  // Since we just set a new tree, we need to check for updates
258  source_cloud_updated_ = true;
259  }
260 
261  /** \brief Get a pointer to the search method used to find correspondences in the
262  * source cloud. */
263  inline KdTreeReciprocalPtr
265  {
266  return (tree_reciprocal_);
267  }
268 
269  /** \brief Get the final transformation matrix estimated by the registration method. */
270  inline Matrix4
272 
273  /** \brief Get the last incremental transformation matrix estimated by the registration method. */
274  inline Matrix4
276 
277  /** \brief Set the maximum number of iterations the internal optimization should run for.
278  * \param[in] nr_iterations the maximum number of iterations the internal optimization should run for
279  */
280  inline void
281  setMaximumIterations (int nr_iterations) { max_iterations_ = nr_iterations; }
282 
283  /** \brief Get the maximum number of iterations the internal optimization should run for, as set by the user. */
284  inline int
286 
287  /** \brief Set the number of iterations RANSAC should run for.
288  * \param[in] ransac_iterations is the number of iterations RANSAC should run for
289  */
290  inline void
291  setRANSACIterations (int ransac_iterations) { ransac_iterations_ = ransac_iterations; }
292 
293  /** \brief Get the number of iterations RANSAC should run for, as set by the user. */
294  inline double
296 
297  /** \brief Set the inlier distance threshold for the internal RANSAC outlier rejection loop.
298  *
299  * The method considers a point to be an inlier, if the distance between the target data index and the transformed
300  * source index is smaller than the given inlier distance threshold.
301  * The value is set by default to 0.05m.
302  * \param[in] inlier_threshold the inlier distance threshold for the internal RANSAC outlier rejection loop
303  */
304  inline void
305  setRANSACOutlierRejectionThreshold (double inlier_threshold) { inlier_threshold_ = inlier_threshold; }
306 
307  /** \brief Get the inlier distance threshold for the internal outlier rejection loop as set by the user. */
308  inline double
310 
311  /** \brief Set the maximum distance threshold between two correspondent points in source <-> target. If the
312  * distance is larger than this threshold, the points will be ignored in the alignment process.
313  * \param[in] distance_threshold the maximum distance threshold between a point and its nearest neighbor
314  * correspondent in order to be considered in the alignment process
315  */
316  inline void
317  setMaxCorrespondenceDistance (double distance_threshold) { corr_dist_threshold_ = distance_threshold; }
318 
319  /** \brief Get the maximum distance threshold between two correspondent points in source <-> target. If the
320  * distance is larger than this threshold, the points will be ignored in the alignment process.
321  */
322  inline double
324 
325  /** \brief Set the transformation epsilon (maximum allowable difference between two consecutive
326  * transformations) in order for an optimization to be considered as having converged to the final
327  * solution.
328  * \param[in] epsilon the transformation epsilon in order for an optimization to be considered as having
329  * converged to the final solution.
330  */
331  inline void
332  setTransformationEpsilon (double epsilon) { transformation_epsilon_ = epsilon; }
333 
334  /** \brief Get the transformation epsilon (maximum allowable difference between two consecutive
335  * transformations) as set by the user.
336  */
337  inline double
339 
340  /** \brief Set the maximum allowed Euclidean error between two consecutive steps in the ICP loop, before
341  * the algorithm is considered to have converged.
342  * The error is estimated as the sum of the differences between correspondences in an Euclidean sense,
343  * divided by the number of correspondences.
344  * \param[in] epsilon the maximum allowed distance error before the algorithm will be considered to have
345  * converged
346  */
347 
348  inline void
350 
351  /** \brief Get the maximum allowed distance error before the algorithm will be considered to have converged,
352  * as set by the user. See \ref setEuclideanFitnessEpsilon
353  */
354  inline double
356 
357  /** \brief Provide a boost shared pointer to the PointRepresentation to be used when comparing points
358  * \param[in] point_representation the PointRepresentation to be used by the k-D tree
359  */
360  inline void
362  {
363  point_representation_ = point_representation;
364  }
365 
366  /** \brief Register the user callback function which will be called from registration thread
367  * in order to update point cloud obtained after each iteration
368  * \param[in] visualizerCallback reference of the user callback function
369  */
370  template<typename FunctionSignature> inline bool
371  registerVisualizationCallback (boost::function<FunctionSignature> &visualizerCallback)
372  {
373  if (visualizerCallback != NULL)
374  {
375  update_visualizer_ = visualizerCallback;
376  return (true);
377  }
378  else
379  return (false);
380  }
381 
382  /** \brief Obtain the Euclidean fitness score (e.g., sum of squared distances from the source to the target)
383  * \param[in] max_range maximum allowable distance between a point and its correspondence in the target
384  * (default: double::max)
385  */
386  inline double
387  getFitnessScore (double max_range = std::numeric_limits<double>::max ());
388 
389  /** \brief Obtain the Euclidean fitness score (e.g., sum of squared distances from the source to the target)
390  * from two sets of correspondence distances (distances between source and target points)
391  * \param[in] distances_a the first set of distances between correspondences
392  * \param[in] distances_b the second set of distances between correspondences
393  */
394  inline double
395  getFitnessScore (const std::vector<float> &distances_a, const std::vector<float> &distances_b);
396 
397  /** \brief Return the state of convergence after the last align run */
398  inline bool
399  hasConverged () { return (converged_); }
400 
401  /** \brief Call the registration algorithm which estimates the transformation and returns the transformed source
402  * (input) as \a output.
403  * \param[out] output the resultant input transfomed point cloud dataset
404  */
405  inline void
406  align (PointCloudSource &output);
407 
408  /** \brief Call the registration algorithm which estimates the transformation and returns the transformed source
409  * (input) as \a output.
410  * \param[out] output the resultant input transfomed point cloud dataset
411  * \param[in] guess the initial gross estimation of the transformation
412  */
413  inline void
414  align (PointCloudSource &output, const Matrix4& guess);
415 
416  /** \brief Abstract class get name method. */
417  inline const std::string&
418  getClassName () const { return (reg_name_); }
419 
420  /** \brief Internal computation initalization. */
421  bool
422  initCompute ();
423 
424  /** \brief Internal computation when reciprocal lookup is needed */
425  bool
427 
428  /** \brief Add a new correspondence rejector to the list
429  * \param[in] rejector the new correspondence rejector to concatenate
430  *
431  * Code example:
432  *
433  * \code
434  * CorrespondenceRejectorDistance rej;
435  * rej.setInputCloud<PointXYZ> (keypoints_src);
436  * rej.setInputTarget<PointXYZ> (keypoints_tgt);
437  * rej.setMaximumDistance (1);
438  * rej.setInputCorrespondences (all_correspondences);
439  *
440  * // or...
441  *
442  * \endcode
443  */
444  inline void
446  {
447  correspondence_rejectors_.push_back (rejector);
448  }
449 
450  /** \brief Get the list of correspondence rejectors. */
451  inline std::vector<CorrespondenceRejectorPtr>
453  {
454  return (correspondence_rejectors_);
455  }
456 
457  /** \brief Remove the i-th correspondence rejector in the list
458  * \param[in] i the position of the correspondence rejector in the list to remove
459  */
460  inline bool
462  {
463  if (i >= correspondence_rejectors_.size ())
464  return (false);
466  return (true);
467  }
468 
469  /** \brief Clear the list of correspondence rejectors. */
470  inline void
472  {
473  correspondence_rejectors_.clear ();
474  }
475 
476  protected:
477  /** \brief The registration method name. */
478  std::string reg_name_;
479 
480  /** \brief A pointer to the spatial search object. */
482 
483  /** \brief A pointer to the spatial search object of the source. */
485 
486  /** \brief The number of iterations the internal optimization ran for (used internally). */
488 
489  /** \brief The maximum number of iterations the internal optimization should run for.
490  * The default value is 10.
491  */
493 
494  /** \brief The number of iterations RANSAC should run for. */
496 
497  /** \brief The input point cloud dataset target. */
499 
500  /** \brief The final transformation matrix estimated by the registration method after N iterations. */
502 
503  /** \brief The transformation matrix estimated by the registration method. */
505 
506  /** \brief The previous transformation matrix estimated by the registration method (used internally). */
508 
509  /** \brief The maximum difference between two consecutive transformations in order to consider convergence
510  * (user defined).
511  */
513 
514  /** \brief The maximum allowed Euclidean error between two consecutive steps in the ICP loop, before the
515  * algorithm is considered to have converged. The error is estimated as the sum of the differences between
516  * correspondences in an Euclidean sense, divided by the number of correspondences.
517  */
519 
520  /** \brief The maximum distance threshold between two correspondent points in source <-> target. If the
521  * distance is larger than this threshold, the points will be ignored in the alignement process.
522  */
524 
525  /** \brief The inlier distance threshold for the internal RANSAC outlier rejection loop.
526  * The method considers a point to be an inlier, if the distance between the target data index and the transformed
527  * source index is smaller than the given inlier distance threshold. The default value is 0.05.
528  */
530 
531  /** \brief Holds internal convergence state, given user parameters. */
533 
534  /** \brief The minimum number of correspondences that the algorithm needs before attempting to estimate the
535  * transformation. The default value is 3.
536  */
538 
539  /** \brief The set of correspondences determined at this ICP step. */
541 
542  /** \brief A TransformationEstimation object, used to calculate the 4x4 rigid transformation. */
544 
545  /** \brief A CorrespondenceEstimation object, used to estimate correspondences between the source and the target cloud. */
547 
548  /** \brief The list of correspondence rejectors to use. */
549  std::vector<CorrespondenceRejectorPtr> correspondence_rejectors_;
550 
551  /** \brief Variable that stores whether we have a new target cloud, meaning we need to pre-process it again.
552  * This way, we avoid rebuilding the kd-tree for the target cloud every time the determineCorrespondences () method
553  * is called. */
555  /** \brief Variable that stores whether we have a new source cloud, meaning we need to pre-process it again.
556  * This way, we avoid rebuilding the reciprocal kd-tree for the source cloud every time the determineCorrespondences () method
557  * is called. */
559  /** \brief A flag which, if set, means the tree operating on the target cloud
560  * will never be recomputed*/
562 
563  /** \brief A flag which, if set, means the tree operating on the source cloud
564  * will never be recomputed*/
566 
567  /** \brief Callback function to update intermediate source point cloud position during it's registration
568  * to the target point cloud.
569  */
570  boost::function<void(const pcl::PointCloud<PointSource> &cloud_src,
571  const std::vector<int> &indices_src,
572  const pcl::PointCloud<PointTarget> &cloud_tgt,
573  const std::vector<int> &indices_tgt)> update_visualizer_;
574 
575  /** \brief Search for the closest nearest neighbor of a given point.
576  * \param cloud the point cloud dataset to use for nearest neighbor search
577  * \param index the index of the query point
578  * \param indices the resultant vector of indices representing the k-nearest neighbors
579  * \param distances the resultant distances from the query point to the k-nearest neighbors
580  */
581  inline bool
582  searchForNeighbors (const PointCloudSource &cloud, int index,
583  std::vector<int> &indices, std::vector<float> &distances)
584  {
585  int k = tree_->nearestKSearch (cloud, index, 1, indices, distances);
586  if (k == 0)
587  return (false);
588  return (true);
589  }
590 
591  /** \brief Abstract transformation computation method with initial guess */
592  virtual void
593  computeTransformation (PointCloudSource &output, const Matrix4& guess) = 0;
594 
595  private:
596  /** \brief The point representation used (internal). */
597  PointRepresentationConstPtr point_representation_;
598  public:
599  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
600  };
601 }
602 
603 #include <pcl/registration/impl/registration.hpp>
604 
605 #endif //#ifndef PCL_REGISTRATION_H_