Point Cloud Library (PCL)  1.14.0-dev
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 #pragma once
42 
43 // PCL includes
44 #include <pcl/registration/correspondence_estimation.h>
45 #include <pcl/registration/correspondence_rejection.h>
46 #include <pcl/registration/transformation_estimation.h>
47 #include <pcl/search/kdtree.h>
48 #include <pcl/memory.h>
49 #include <pcl/pcl_base.h>
50 #include <pcl/pcl_macros.h>
51 
52 namespace pcl {
53 /** \brief @b Registration represents the base registration class for general purpose,
54  * ICP-like methods. \author Radu B. Rusu, Michael Dixon \ingroup registration
55  */
56 template <typename PointSource, typename PointTarget, typename Scalar = float>
57 class Registration : public PCLBase<PointSource> {
58 public:
59  using Matrix4 = Eigen::Matrix<Scalar, 4, 4>;
60 
61  // using PCLBase<PointSource>::initCompute;
65 
66  using Ptr = shared_ptr<Registration<PointSource, PointTarget, Scalar>>;
67  using ConstPtr = shared_ptr<const Registration<PointSource, PointTarget, Scalar>>;
68 
71  using KdTreePtr = typename KdTree::Ptr;
72 
75 
79 
83 
85 
86  using TransformationEstimation = typename pcl::registration::
87  TransformationEstimation<PointSource, PointTarget, Scalar>;
88  using TransformationEstimationPtr = typename TransformationEstimation::Ptr;
89  using TransformationEstimationConstPtr = typename TransformationEstimation::ConstPtr;
90 
95 
96  /** \brief The callback signature to the function updating intermediate source point
97  * cloud position during it's registration to the target point cloud. \param[in]
98  * cloud_src - the point cloud which will be updated to match target \param[in]
99  * indices_src - a selector of points in cloud_src \param[in] cloud_tgt - the point
100  * cloud we want to register against \param[in] indices_tgt - a selector of points in
101  * cloud_tgt
102  */
104  const pcl::Indices&,
106  const pcl::Indices&);
107 
108  /** \brief Empty constructor. */
110  : tree_(new KdTree)
112  , target_()
113  , final_transformation_(Matrix4::Identity())
114  , transformation_(Matrix4::Identity())
115  , previous_transformation_(Matrix4::Identity())
116  , euclidean_fitness_epsilon_(-std::numeric_limits<double>::max())
117  , corr_dist_threshold_(std::sqrt(std::numeric_limits<double>::max()))
121  , point_representation_()
122  {}
123 
124  /** \brief destructor. */
125  ~Registration() override = default;
126 
127  /** \brief Provide a pointer to the transformation estimation object.
128  * (e.g., SVD, point to plane etc.)
129  *
130  * \param[in] te is the pointer to the corresponding transformation estimation object
131  *
132  * Code example:
133  *
134  * \code
135  * TransformationEstimationPointToPlaneLLS<PointXYZ, PointXYZ>::Ptr trans_lls
136  * (new TransformationEstimationPointToPlaneLLS<PointXYZ, PointXYZ>);
137  * icp.setTransformationEstimation (trans_lls);
138  * // or...
139  * TransformationEstimationSVD<PointXYZ, PointXYZ>::Ptr trans_svd
140  * (new TransformationEstimationSVD<PointXYZ, PointXYZ>);
141  * icp.setTransformationEstimation (trans_svd);
142  * \endcode
143  */
144  void
146  {
148  }
149 
150  /** \brief Provide a pointer to the correspondence estimation object.
151  * (e.g., regular, reciprocal, normal shooting etc.)
152  *
153  * \param[in] ce is the pointer to the corresponding correspondence estimation object
154  *
155  * Code example:
156  *
157  * \code
158  * CorrespondenceEstimation<PointXYZ, PointXYZ>::Ptr
159  * ce (new CorrespondenceEstimation<PointXYZ, PointXYZ>);
160  * ce->setInputSource (source);
161  * ce->setInputTarget (target);
162  * icp.setCorrespondenceEstimation (ce);
163  * // or...
164  * CorrespondenceEstimationNormalShooting<PointNormal, PointNormal, PointNormal>::Ptr
165  * 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  {
177  }
178 
179  /** \brief Provide a pointer to the input source
180  * (e.g., the point cloud that we want to align to the target)
181  *
182  * \param[in] cloud the input point cloud source
183  */
184  virtual void
186 
187  /** \brief Get a pointer to the input point cloud dataset target. */
188  inline PointCloudSourceConstPtr const
190  {
191  return (input_);
192  }
193 
194  /** \brief Provide a pointer to the input target (e.g., the point cloud that we want
195  * to align the input source to) \param[in] cloud the input point cloud target
196  */
197  virtual inline void
199 
200  /** \brief Get a pointer to the input point cloud dataset target. */
201  inline PointCloudTargetConstPtr const
203  {
204  return (target_);
205  }
206 
207  /** \brief Provide a pointer to the search object used to find correspondences in
208  * the target cloud.
209  * \param[in] tree a pointer to the spatial search object.
210  * \param[in] force_no_recompute If set to true, this tree will NEVER be
211  * recomputed, regardless of calls to setInputTarget. Only use if you are
212  * confident that the tree will be set correctly.
213  */
214  inline void
215  setSearchMethodTarget(const KdTreePtr& tree, bool force_no_recompute = false)
216  {
217  tree_ = tree;
218  force_no_recompute_ = force_no_recompute;
219  // Since we just set a new tree, we need to check for updates
220  target_cloud_updated_ = true;
221  }
222 
223  /** \brief Get a pointer to the search method used to find correspondences in the
224  * target cloud. */
225  inline KdTreePtr
227  {
228  return (tree_);
229  }
230 
231  /** \brief Provide a pointer to the search object used to find correspondences in
232  * the source cloud (usually used by reciprocal correspondence finding).
233  * \param[in] tree a pointer to the spatial search object.
234  * \param[in] force_no_recompute If set to true, this tree will NEVER be
235  * recomputed, regardless of calls to setInputSource. Only use if you are
236  * extremely confident that the tree will be set correctly.
237  */
238  inline void
240  bool force_no_recompute = false)
241  {
242  tree_reciprocal_ = tree;
243  force_no_recompute_reciprocal_ = force_no_recompute;
244  // Since we just set a new tree, we need to check for updates
245  source_cloud_updated_ = true;
246  }
247 
248  /** \brief Get a pointer to the search method used to find correspondences in the
249  * source cloud. */
250  inline KdTreeReciprocalPtr
252  {
253  return (tree_reciprocal_);
254  }
255 
256  /** \brief Get the final transformation matrix estimated by the registration method.
257  */
258  inline Matrix4
260  {
261  return (final_transformation_);
262  }
263 
264  /** \brief Get the last incremental transformation matrix estimated by the
265  * registration method. */
266  inline Matrix4
268  {
269  return (transformation_);
270  }
271 
272  /** \brief Set the maximum number of iterations the internal optimization should run
273  * for. \param[in] nr_iterations the maximum number of iterations the internal
274  * optimization should run for
275  */
276  inline void
277  setMaximumIterations(int nr_iterations)
278  {
279  max_iterations_ = nr_iterations;
280  }
281 
282  /** \brief Get the maximum number of iterations the internal optimization should run
283  * for, as set by the user. */
284  inline int
286  {
287  return (max_iterations_);
288  }
289 
290  /** \brief Set the number of iterations RANSAC should run for.
291  * \param[in] ransac_iterations is the number of iterations RANSAC should run for
292  */
293  inline void
294  setRANSACIterations(int ransac_iterations)
295  {
296  ransac_iterations_ = ransac_iterations;
297  }
298 
299  /** \brief Get the number of iterations RANSAC should run for, as set by the user. */
300  inline double
302  {
303  return (ransac_iterations_);
304  }
305 
306  /** \brief Set the inlier distance threshold for the internal RANSAC outlier rejection
307  * loop.
308  *
309  * The method considers a point to be an inlier, if the distance between the target
310  * data index and the transformed source index is smaller than the given inlier
311  * distance threshold. The value is set by default to 0.05m. \param[in]
312  * inlier_threshold the inlier distance threshold for the internal RANSAC outlier
313  * rejection loop
314  */
315  inline void
316  setRANSACOutlierRejectionThreshold(double inlier_threshold)
317  {
318  inlier_threshold_ = inlier_threshold;
319  }
320 
321  /** \brief Get the inlier distance threshold for the internal outlier rejection loop
322  * as set by the user. */
323  inline double
325  {
326  return (inlier_threshold_);
327  }
328 
329  /** \brief Set the maximum distance threshold between two correspondent points in
330  * source <-> target. If the distance is larger than this threshold, the points will
331  * be ignored in the alignment process. \param[in] distance_threshold the maximum
332  * distance threshold between a point and its nearest neighbor correspondent in order
333  * to be considered in the alignment process
334  */
335  inline void
336  setMaxCorrespondenceDistance(double distance_threshold)
337  {
338  corr_dist_threshold_ = distance_threshold;
339  }
340 
341  /** \brief Get the maximum distance threshold between two correspondent points in
342  * source <-> target. If the distance is larger than this threshold, the points will
343  * be ignored in the alignment process.
344  */
345  inline double
347  {
348  return (corr_dist_threshold_);
349  }
350 
351  /** \brief Set the transformation epsilon (maximum allowable translation squared
352  * difference between two consecutive transformations) in order for an optimization to
353  * be considered as having converged to the final solution. \param[in] epsilon the
354  * transformation epsilon in order for an optimization to be considered as having
355  * converged to the final solution.
356  */
357  inline void
358  setTransformationEpsilon(double epsilon)
359  {
360  transformation_epsilon_ = epsilon;
361  }
362 
363  /** \brief Get the transformation epsilon (maximum allowable translation squared
364  * difference between two consecutive transformations) as set by the user.
365  */
366  inline double
368  {
369  return (transformation_epsilon_);
370  }
371 
372  /** \brief Set the transformation rotation epsilon (maximum allowable rotation
373  * difference between two consecutive transformations) in order for an optimization to
374  * be considered as having converged to the final solution. \param[in] epsilon the
375  * transformation rotation epsilon in order for an optimization to be considered as
376  * having converged to the final solution (epsilon is the cos(angle) in a axis-angle
377  * representation).
378  */
379  inline void
381  {
383  }
384 
385  /** \brief Get the transformation rotation epsilon (maximum allowable difference
386  * between two consecutive transformations) as set by the user (epsilon is the
387  * cos(angle) in a axis-angle representation).
388  */
389  inline double
391  {
393  }
394 
395  /** \brief Set the maximum allowed Euclidean error between two consecutive steps in
396  * the ICP loop, before the algorithm is considered to have converged. The error is
397  * estimated as the sum of the differences between correspondences in an Euclidean
398  * sense, divided by the number of correspondences. \param[in] epsilon the maximum
399  * allowed distance error before the algorithm will be considered to have converged
400  */
401  inline void
403  {
404  euclidean_fitness_epsilon_ = epsilon;
405  }
406 
407  /** \brief Get the maximum allowed distance error before the algorithm will be
408  * considered to have converged, as set by the user. See \ref
409  * setEuclideanFitnessEpsilon
410  */
411  inline double
413  {
415  }
416 
417  /** \brief Provide a boost shared pointer to the PointRepresentation to be used when
418  * comparing points \param[in] point_representation the PointRepresentation to be used
419  * by the k-D tree
420  */
421  inline void
423  {
424  point_representation_ = point_representation;
425  }
426 
427  /** \brief Register the user callback function which will be called from registration
428  * thread in order to update point cloud obtained after each iteration \param[in]
429  * visualizerCallback reference of the user callback function
430  */
431  inline bool
433  std::function<UpdateVisualizerCallbackSignature>& visualizerCallback)
434  {
435  if (visualizerCallback) {
436  update_visualizer_ = visualizerCallback;
437  pcl::Indices indices;
438  update_visualizer_(*input_, indices, *target_, indices);
439  return (true);
440  }
441  return (false);
442  }
443 
444  /** \brief Obtain the Euclidean fitness score (e.g., mean of squared distances from
445  * the source to the target) \param[in] max_range maximum allowable distance between a
446  * point and its correspondence in the target (default: double::max)
447  */
448  inline double
449  getFitnessScore(double max_range = std::numeric_limits<double>::max());
450 
451  /** \brief Obtain the Euclidean fitness score (e.g., mean of squared distances from
452  * the source to the target) from two sets of correspondence distances (distances
453  * between source and target points) \param[in] distances_a the first set of distances
454  * between correspondences \param[in] distances_b the second set of distances between
455  * correspondences
456  */
457  inline double
458  getFitnessScore(const std::vector<float>& distances_a,
459  const std::vector<float>& distances_b);
460 
461  /** \brief Return the state of convergence after the last align run */
462  inline bool
463  hasConverged() const
464  {
465  return (converged_);
466  }
467 
468  /** \brief Call the registration algorithm which estimates the transformation and
469  * returns the transformed source (input) as \a output. \param[out] output the
470  * resultant input transformed point cloud dataset
471  */
472  inline void
473  align(PointCloudSource& output);
474 
475  /** \brief Call the registration algorithm which estimates the transformation and
476  * returns the transformed source (input) as \a output. \param[out] output the
477  * resultant input transformed point cloud dataset \param[in] guess the initial gross
478  * estimation of the transformation
479  */
480  inline void
481  align(PointCloudSource& output, const Matrix4& guess);
482 
483  /** \brief Abstract class get name method. */
484  inline const std::string&
485  getClassName() const
486  {
487  return (reg_name_);
488  }
489 
490  /** \brief Internal computation initialization. */
491  bool
492  initCompute();
493 
494  /** \brief Internal computation when reciprocal lookup is needed */
495  bool
497 
498  /** \brief Add a new correspondence rejector to the list
499  * \param[in] rejector the new correspondence rejector to concatenate
500  *
501  * Code example:
502  *
503  * \code
504  * CorrespondenceRejectorDistance rej;
505  * rej.setInputCloud<PointXYZ> (keypoints_src);
506  * rej.setInputTarget<PointXYZ> (keypoints_tgt);
507  * rej.setMaximumDistance (1);
508  * rej.setInputCorrespondences (all_correspondences);
509  *
510  * // or...
511  *
512  * \endcode
513  */
514  inline void
516  {
517  correspondence_rejectors_.push_back(rejector);
518  }
519 
520  /** \brief Get the list of correspondence rejectors. */
521  inline std::vector<CorrespondenceRejectorPtr>
523  {
524  return (correspondence_rejectors_);
525  }
526 
527  /** \brief Remove the i-th correspondence rejector in the list
528  * \param[in] i the position of the correspondence rejector in the list to remove
529  */
530  inline bool
532  {
533  if (i >= correspondence_rejectors_.size())
534  return (false);
536  return (true);
537  }
538 
539  /** \brief Clear the list of correspondence rejectors. */
540  inline void
542  {
544  }
545 
546 protected:
547  /** \brief The registration method name. */
548  std::string reg_name_;
549 
550  /** \brief A pointer to the spatial search object. */
552 
553  /** \brief A pointer to the spatial search object of the source. */
555 
556  /** \brief The number of iterations the internal optimization ran for (used
557  * internally). */
559 
560  /** \brief The maximum number of iterations the internal optimization should run for.
561  * The default value is 10.
562  */
564 
565  /** \brief The number of iterations RANSAC should run for. */
567 
568  /** \brief The input point cloud dataset target. */
570 
571  /** \brief The final transformation matrix estimated by the registration method after
572  * N iterations. */
574 
575  /** \brief The transformation matrix estimated by the registration method. */
577 
578  /** \brief The previous transformation matrix estimated by the registration method
579  * (used internally). */
581 
582  /** \brief The maximum difference between two consecutive transformations in order to
583  * consider convergence (user defined).
584  */
586 
587  /** \brief The maximum rotation difference between two consecutive transformations in
588  * order to consider convergence (user defined).
589  */
591 
592  /** \brief The maximum allowed Euclidean error between two consecutive steps in the
593  * ICP loop, before the algorithm is considered to have converged. The error is
594  * estimated as the sum of the differences between correspondences in an Euclidean
595  * sense, divided by the number of correspondences.
596  */
598 
599  /** \brief The maximum distance threshold between two correspondent points in source
600  * <-> target. If the distance is larger than this threshold, the points will be
601  * ignored in the alignment process.
602  */
604 
605  /** \brief The inlier distance threshold for the internal RANSAC outlier rejection
606  * loop. The method considers a point to be an inlier, if the distance between the
607  * target data index and the transformed source index is smaller than the given inlier
608  * distance threshold. The default value is 0.05.
609  */
610  double inlier_threshold_{0.05};
611 
612  /** \brief Holds internal convergence state, given user parameters. */
613  bool converged_{false};
614 
615  /** \brief The minimum number of correspondences that the algorithm needs before
616  * attempting to estimate the transformation. The default value is 3.
617  */
619 
620  /** \brief The set of correspondences determined at this ICP step. */
622 
623  /** \brief A TransformationEstimation object, used to calculate the 4x4 rigid
624  * transformation. */
626 
627  /** \brief A CorrespondenceEstimation object, used to estimate correspondences between
628  * the source and the target cloud. */
630 
631  /** \brief The list of correspondence rejectors to use. */
632  std::vector<CorrespondenceRejectorPtr> correspondence_rejectors_;
633 
634  /** \brief Variable that stores whether we have a new target cloud, meaning we need to
635  * pre-process it again. This way, we avoid rebuilding the kd-tree for the target
636  * cloud every time the determineCorrespondences () method is called. */
638  /** \brief Variable that stores whether we have a new source cloud, meaning we need to
639  * pre-process it again. This way, we avoid rebuilding the reciprocal kd-tree for the
640  * source cloud every time the determineCorrespondences () method is called. */
642  /** \brief A flag which, if set, means the tree operating on the target cloud
643  * will never be recomputed*/
644  bool force_no_recompute_{false};
645 
646  /** \brief A flag which, if set, means the tree operating on the source cloud
647  * will never be recomputed*/
649 
650  /** \brief Callback function to update intermediate source point cloud position during
651  * it's registration to the target point cloud.
652  */
653  std::function<UpdateVisualizerCallbackSignature> update_visualizer_;
654 
655  /** \brief Search for the closest nearest neighbor of a given point.
656  * \param cloud the point cloud dataset to use for nearest neighbor search
657  * \param index the index of the query point
658  * \param indices the resultant vector of indices representing the k-nearest neighbors
659  * \param distances the resultant distances from the query point to the k-nearest
660  * neighbors
661  */
662  inline bool
664  int index,
665  pcl::Indices& indices,
666  std::vector<float>& distances)
667  {
668  int k = tree_->nearestKSearch(cloud, index, 1, indices, distances);
669  if (k == 0)
670  return (false);
671  return (true);
672  }
673 
674  /** \brief Abstract transformation computation method with initial guess */
675  virtual void
676  computeTransformation(PointCloudSource& output, const Matrix4& guess) = 0;
677 
678 private:
679  /** \brief The point representation used (internal). */
680  PointRepresentationConstPtr point_representation_;
681 
682  /**
683  * \brief Remove from public API in favor of \ref setInputSource
684  *
685  * Still gives the correct result (with a warning)
686  */
687  void
688  setInputCloud(const PointCloudSourceConstPtr& cloud) override
689  {
690  PCL_WARN("[pcl::registration::Registration] setInputCloud is deprecated."
691  "Please use setInputSource instead.\n");
692  setInputSource(cloud);
693  }
694 
695 public:
697 };
698 } // namespace pcl
699 
700 #include <pcl/registration/impl/registration.hpp>
PCL base class.
Definition: pcl_base.h:70
PointCloudConstPtr input_
The input point cloud dataset.
Definition: pcl_base.h:147
shared_ptr< PointCloud< PointSource > > Ptr
Definition: point_cloud.h:413
shared_ptr< const PointCloud< PointSource > > ConstPtr
Definition: point_cloud.h:414
Registration represents the base registration class for general purpose, ICP-like methods.
Definition: registration.h:57
bool target_cloud_updated_
Variable that stores whether we have a new target cloud, meaning we need to pre-process it again.
Definition: registration.h:637
Matrix4 final_transformation_
The final transformation matrix estimated by the registration method after N iterations.
Definition: registration.h:573
typename KdTreeReciprocal::Ptr KdTreeReciprocalPtr
Definition: registration.h:74
bool initCompute()
Internal computation initialization.
std::function< UpdateVisualizerCallbackSignature > update_visualizer_
Callback function to update intermediate source point cloud position during it's registration to the ...
Definition: registration.h:653
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)
double corr_dist_threshold_
The maximum distance threshold between two correspondent points in source <-> target.
Definition: registration.h:603
void clearCorrespondenceRejectors()
Clear the list of correspondence rejectors.
Definition: registration.h:541
double getTransformationEpsilon()
Get the transformation epsilon (maximum allowable translation squared difference between two consecut...
Definition: registration.h:367
void(const pcl::PointCloud< PointSource > &, const pcl::Indices &, const pcl::PointCloud< PointTarget > &, const pcl::Indices &) UpdateVisualizerCallbackSignature
The callback signature to the function updating intermediate source point cloud position during it's ...
Definition: registration.h:106
std::string reg_name_
The registration method name.
Definition: registration.h:548
Matrix4 getFinalTransformation()
Get the final transformation matrix estimated by the registration method.
Definition: registration.h:259
void setRANSACIterations(int ransac_iterations)
Set the number of iterations RANSAC should run for.
Definition: registration.h:294
const std::string & getClassName() const
Abstract class get name method.
Definition: registration.h:485
shared_ptr< const Registration< PointSource, PointTarget, Scalar > > ConstPtr
Definition: registration.h:67
Matrix4 transformation_
The transformation matrix estimated by the registration method.
Definition: registration.h:576
pcl::PointCloud< PointSource > PointCloudSource
Definition: registration.h:76
KdTreeReciprocalPtr tree_reciprocal_
A pointer to the spatial search object of the source.
Definition: registration.h:554
bool removeCorrespondenceRejector(unsigned int i)
Remove the i-th correspondence rejector in the list.
Definition: registration.h:531
void setCorrespondenceEstimation(const CorrespondenceEstimationPtr &ce)
Provide a pointer to the correspondence estimation object.
Definition: registration.h:174
typename CorrespondenceEstimation::ConstPtr CorrespondenceEstimationConstPtr
Definition: registration.h:94
void setMaximumIterations(int nr_iterations)
Set the maximum number of iterations the internal optimization should run for.
Definition: registration.h:277
void setSearchMethodSource(const KdTreeReciprocalPtr &tree, bool force_no_recompute=false)
Provide a pointer to the search object used to find correspondences in the source cloud (usually used...
Definition: registration.h:239
void setSearchMethodTarget(const KdTreePtr &tree, bool force_no_recompute=false)
Provide a pointer to the search object used to find correspondences in the target cloud.
Definition: registration.h:215
typename PointCloudSource::ConstPtr PointCloudSourceConstPtr
Definition: registration.h:78
typename KdTree::Ptr KdTreePtr
Definition: registration.h:71
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...
bool registerVisualizationCallback(std::function< UpdateVisualizerCallbackSignature > &visualizerCallback)
Register the user callback function which will be called from registration thread in order to update ...
Definition: registration.h:432
typename TransformationEstimation::Ptr TransformationEstimationPtr
Definition: registration.h:88
typename PointCloudSource::Ptr PointCloudSourcePtr
Definition: registration.h:77
void setPointRepresentation(const PointRepresentationConstPtr &point_representation)
Provide a boost shared pointer to the PointRepresentation to be used when comparing points.
Definition: registration.h:422
CorrespondenceEstimationPtr correspondence_estimation_
A CorrespondenceEstimation object, used to estimate correspondences between the source and the target...
Definition: registration.h:629
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
Definition: registration.h:82
double getTransformationRotationEpsilon()
Get the transformation rotation epsilon (maximum allowable difference between two consecutive transfo...
Definition: registration.h:390
bool force_no_recompute_
A flag which, if set, means the tree operating on the target cloud will never be recomputed.
Definition: registration.h:644
void setRANSACOutlierRejectionThreshold(double inlier_threshold)
Set the inlier distance threshold for the internal RANSAC outlier rejection loop.
Definition: registration.h:316
void setMaxCorrespondenceDistance(double distance_threshold)
Set the maximum distance threshold between two correspondent points in source <-> target.
Definition: registration.h:336
void addCorrespondenceRejector(const CorrespondenceRejectorPtr &rejector)
Add a new correspondence rejector to the list.
Definition: registration.h:515
int nr_iterations_
The number of iterations the internal optimization ran for (used internally).
Definition: registration.h:558
shared_ptr< Registration< PointSource, PointTarget, Scalar > > Ptr
Definition: registration.h:66
double getRANSACOutlierRejectionThreshold()
Get the inlier distance threshold for the internal outlier rejection loop as set by the user.
Definition: registration.h:324
double getEuclideanFitnessEpsilon()
Get the maximum allowed distance error before the algorithm will be considered to have converged,...
Definition: registration.h:412
double getMaxCorrespondenceDistance()
Get the maximum distance threshold between two correspondent points in source <-> target.
Definition: registration.h:346
KdTreePtr getSearchMethodTarget() const
Get a pointer to the search method used to find correspondences in the target cloud.
Definition: registration.h:226
KdTreePtr tree_
A pointer to the spatial search object.
Definition: registration.h:551
Registration()
Empty constructor.
Definition: registration.h:109
Matrix4 getLastIncrementalTransformation()
Get the last incremental transformation matrix estimated by the registration method.
Definition: registration.h:267
typename PointCloudTarget::Ptr PointCloudTargetPtr
Definition: registration.h:81
Matrix4 previous_transformation_
The previous transformation matrix estimated by the registration method (used internally).
Definition: registration.h:580
bool converged_
Holds internal convergence state, given user parameters.
Definition: registration.h:613
int ransac_iterations_
The number of iterations RANSAC should run for.
Definition: registration.h:566
Eigen::Matrix< Scalar, 4, 4 > Matrix4
Definition: registration.h:59
void align(PointCloudSource &output)
Call the registration algorithm which estimates the transformation and returns the transformed source...
PointCloudTargetConstPtr const getInputTarget()
Get a pointer to the input point cloud dataset target.
Definition: registration.h:202
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
bool searchForNeighbors(const PointCloudSource &cloud, int index, pcl::Indices &indices, std::vector< float > &distances)
Search for the closest nearest neighbor of a given point.
Definition: registration.h:663
int max_iterations_
The maximum number of iterations the internal optimization should run for.
Definition: registration.h:563
double transformation_rotation_epsilon_
The maximum rotation difference between two consecutive transformations in order to consider converge...
Definition: registration.h:590
unsigned int min_number_correspondences_
The minimum number of correspondences that the algorithm needs before attempting to estimate the tran...
Definition: registration.h:618
double getFitnessScore(double max_range=std::numeric_limits< double >::max())
Obtain the Euclidean fitness score (e.g., mean of squared distances from the source to the target)
double euclidean_fitness_epsilon_
The maximum allowed Euclidean error between two consecutive steps in the ICP loop,...
Definition: registration.h:597
double inlier_threshold_
The inlier distance threshold for the internal RANSAC outlier rejection loop.
Definition: registration.h:610
pcl::registration::CorrespondenceRejector::Ptr CorrespondenceRejectorPtr
Definition: registration.h:69
std::vector< CorrespondenceRejectorPtr > getCorrespondenceRejectors()
Get the list of correspondence rejectors.
Definition: registration.h:522
PointCloudSourceConstPtr const getInputSource()
Get a pointer to the input point cloud dataset target.
Definition: registration.h:189
typename TransformationEstimation::ConstPtr TransformationEstimationConstPtr
Definition: registration.h:89
typename pcl::registration::TransformationEstimation< PointSource, PointTarget, Scalar > TransformationEstimation
Definition: registration.h:87
bool hasConverged() const
Return the state of convergence after the last align run.
Definition: registration.h:463
typename KdTree::PointRepresentationConstPtr PointRepresentationConstPtr
Definition: registration.h:84
void setTransformationEstimation(const TransformationEstimationPtr &te)
Provide a pointer to the transformation estimation object.
Definition: registration.h:145
bool force_no_recompute_reciprocal_
A flag which, if set, means the tree operating on the source cloud will never be recomputed.
Definition: registration.h:648
double transformation_epsilon_
The maximum difference between two consecutive transformations in order to consider convergence (user...
Definition: registration.h:585
std::vector< CorrespondenceRejectorPtr > correspondence_rejectors_
The list of correspondence rejectors to use.
Definition: registration.h:632
KdTreeReciprocalPtr getSearchMethodSource() const
Get a pointer to the search method used to find correspondences in the source cloud.
Definition: registration.h:251
~Registration() override=default
destructor.
int getMaximumIterations()
Get the maximum number of iterations the internal optimization should run for, as set by the user.
Definition: registration.h:285
void setTransformationRotationEpsilon(double epsilon)
Set the transformation rotation epsilon (maximum allowable rotation difference between two consecutiv...
Definition: registration.h:380
bool initComputeReciprocal()
Internal computation when reciprocal lookup is needed.
virtual void computeTransformation(PointCloudSource &output, const Matrix4 &guess)=0
Abstract transformation computation method with initial guess.
typename CorrespondenceEstimation::Ptr CorrespondenceEstimationPtr
Definition: registration.h:93
void setEuclideanFitnessEpsilon(double epsilon)
Set the maximum allowed Euclidean error between two consecutive steps in the ICP loop,...
Definition: registration.h:402
void setTransformationEpsilon(double epsilon)
Set the transformation epsilon (maximum allowable translation squared difference between two consecut...
Definition: registration.h:358
bool source_cloud_updated_
Variable that stores whether we have a new source cloud, meaning we need to pre-process it again.
Definition: registration.h:641
PointCloudTargetConstPtr target_
The input point cloud dataset target.
Definition: registration.h:569
double getRANSACIterations()
Get the number of iterations RANSAC should run for, as set by the user.
Definition: registration.h:301
Abstract CorrespondenceEstimationBase class.
shared_ptr< const CorrespondenceEstimationBase< PointSource, PointTarget, Scalar > > ConstPtr
shared_ptr< CorrespondenceEstimationBase< PointSource, PointTarget, Scalar > > Ptr
shared_ptr< CorrespondenceRejector > Ptr
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition: kdtree.h:62
shared_ptr< KdTree< PointT, Tree > > Ptr
Definition: kdtree.h:75
typename PointRepresentation< PointT >::ConstPtr PointRepresentationConstPtr
Definition: kdtree.h:80
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: memory.h:63
Defines functions, macros and traits for allocating and using memory.
shared_ptr< Correspondences > CorrespondencesPtr
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
Defines all the PCL and non-PCL macros used.