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