Point Cloud Library (PCL)
1.14.1-dev
|
IterativeClosestPointWithNormals is a special case of IterativeClosestPoint, that uses a transformation estimated based on Point to Plane distances by default. More...
#include <pcl/registration/icp.h>
Public Types | |
using | PointCloudSource = typename IterativeClosestPoint< PointSource, PointTarget, Scalar >::PointCloudSource |
using | PointCloudTarget = typename IterativeClosestPoint< PointSource, PointTarget, Scalar >::PointCloudTarget |
using | Matrix4 = typename IterativeClosestPoint< PointSource, PointTarget, Scalar >::Matrix4 |
using | Ptr = shared_ptr< IterativeClosestPointWithNormals< PointSource, PointTarget, Scalar > > |
using | ConstPtr = shared_ptr< const IterativeClosestPointWithNormals< PointSource, PointTarget, Scalar > > |
Public Types inherited from pcl::IterativeClosestPoint< PointSource, PointTarget, float > | |
using | PointCloudSource = typename Registration< PointSource, PointTarget, float >::PointCloudSource |
using | PointCloudSourcePtr = typename PointCloudSource::Ptr |
using | PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr |
using | PointCloudTarget = typename Registration< PointSource, PointTarget, float >::PointCloudTarget |
using | PointCloudTargetPtr = typename PointCloudTarget::Ptr |
using | PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr |
using | PointIndicesPtr = PointIndices::Ptr |
using | PointIndicesConstPtr = PointIndices::ConstPtr |
using | Ptr = shared_ptr< IterativeClosestPoint< PointSource, PointTarget, float > > |
using | ConstPtr = shared_ptr< const IterativeClosestPoint< PointSource, PointTarget, float > > |
using | Matrix4 = typename Registration< PointSource, PointTarget, float >::Matrix4 |
Public Types inherited from pcl::Registration< PointSource, PointTarget, float > | |
using | Matrix4 = Eigen::Matrix< float, 4, 4 > |
using | Ptr = shared_ptr< Registration< PointSource, PointTarget, float > > |
using | ConstPtr = shared_ptr< const Registration< PointSource, PointTarget, float > > |
using | CorrespondenceRejectorPtr = pcl::registration::CorrespondenceRejector::Ptr |
using | KdTree = pcl::search::KdTree< PointTarget > |
using | KdTreePtr = typename KdTree::Ptr |
using | KdTreeReciprocal = pcl::search::KdTree< PointSource > |
using | KdTreeReciprocalPtr = typename KdTreeReciprocal::Ptr |
using | PointCloudSource = pcl::PointCloud< PointSource > |
using | PointCloudSourcePtr = typename PointCloudSource::Ptr |
using | PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr |
using | PointCloudTarget = pcl::PointCloud< PointTarget > |
using | PointCloudTargetPtr = typename PointCloudTarget::Ptr |
using | PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr |
using | PointRepresentationConstPtr = typename KdTree::PointRepresentationConstPtr |
using | TransformationEstimation = typename pcl::registration::TransformationEstimation< PointSource, PointTarget, float > |
using | TransformationEstimationPtr = typename TransformationEstimation::Ptr |
using | TransformationEstimationConstPtr = typename TransformationEstimation::ConstPtr |
using | CorrespondenceEstimation = pcl::registration::CorrespondenceEstimationBase< PointSource, PointTarget, float > |
using | CorrespondenceEstimationPtr = typename CorrespondenceEstimation::Ptr |
using | CorrespondenceEstimationConstPtr = typename CorrespondenceEstimation::ConstPtr |
using | UpdateVisualizerCallbackSignature = void(const pcl::PointCloud< PointSource > &, const pcl::Indices &, const pcl::PointCloud< PointTarget > &, const pcl::Indices &) |
The callback signature to the function updating intermediate source point cloud position during it's registration to the target point cloud. More... | |
Public Types inherited from pcl::PCLBase< PointSource > | |
using | PointCloud = pcl::PointCloud< PointSource > |
using | PointCloudPtr = typename PointCloud::Ptr |
using | PointCloudConstPtr = typename PointCloud::ConstPtr |
using | PointIndicesPtr = PointIndices::Ptr |
using | PointIndicesConstPtr = PointIndices::ConstPtr |
Public Member Functions | |
IterativeClosestPointWithNormals () | |
Empty constructor. More... | |
~IterativeClosestPointWithNormals () override=default | |
Empty destructor. More... | |
void | setUseSymmetricObjective (bool use_symmetric_objective) |
Set whether to use a symmetric objective function or not. More... | |
bool | getUseSymmetricObjective () const |
Obtain whether a symmetric objective is used or not. More... | |
void | setEnforceSameDirectionNormals (bool enforce_same_direction_normals) |
Set whether or not to negate source or target normals on a per-point basis such that they point in the same direction. More... | |
bool | getEnforceSameDirectionNormals () const |
Obtain whether source or target normals are negated on a per-point basis such that they point in the same direction or not. More... | |
Public Member Functions inherited from pcl::IterativeClosestPoint< PointSource, PointTarget, float > | |
IterativeClosestPoint () | |
Empty constructor. More... | |
IterativeClosestPoint (const IterativeClosestPoint &)=delete | |
Due to convergence_criteria_ holding references to the class members, it is tricky to correctly implement its copy and move operations correctly. More... | |
IterativeClosestPoint (IterativeClosestPoint &&)=delete | |
IterativeClosestPoint & | operator= (const IterativeClosestPoint &)=delete |
IterativeClosestPoint & | operator= (IterativeClosestPoint &&)=delete |
~IterativeClosestPoint () override=default | |
Empty destructor. More... | |
pcl::registration::DefaultConvergenceCriteria< float >::Ptr | getConvergeCriteria () |
Returns a pointer to the DefaultConvergenceCriteria used by the IterativeClosestPoint class. More... | |
void | setInputSource (const PointCloudSourceConstPtr &cloud) override |
Provide a pointer to the input source (e.g., the point cloud that we want to align to the target) More... | |
void | setInputTarget (const PointCloudTargetConstPtr &cloud) override |
Provide a pointer to the input target (e.g., the point cloud that we want to align the input source to) More... | |
void | setUseReciprocalCorrespondences (bool use_reciprocal_correspondence) |
Set whether to use reciprocal correspondence or not. More... | |
bool | getUseReciprocalCorrespondences () const |
Obtain whether reciprocal correspondence are used or not. More... | |
void | setNumberOfThreads (unsigned int nr_threads) |
Set the number of threads to use. More... | |
Public Member Functions inherited from pcl::Registration< PointSource, PointTarget, float > | |
Registration () | |
Empty constructor. More... | |
~Registration () override=default | |
destructor. More... | |
void | setTransformationEstimation (const TransformationEstimationPtr &te) |
Provide a pointer to the transformation estimation object. More... | |
void | setCorrespondenceEstimation (const CorrespondenceEstimationPtr &ce) |
Provide a pointer to the correspondence estimation object. More... | |
PointCloudSourceConstPtr const | getInputSource () |
Get a pointer to the input point cloud dataset target. More... | |
PointCloudTargetConstPtr const | getInputTarget () |
Get a pointer to the input point cloud dataset target. More... | |
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. More... | |
KdTreePtr | getSearchMethodTarget () const |
Get a pointer to the search method used to find correspondences in the target cloud. More... | |
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 by reciprocal correspondence finding). More... | |
KdTreeReciprocalPtr | getSearchMethodSource () const |
Get a pointer to the search method used to find correspondences in the source cloud. More... | |
Matrix4 | getFinalTransformation () |
Get the final transformation matrix estimated by the registration method. More... | |
Matrix4 | getLastIncrementalTransformation () |
Get the last incremental transformation matrix estimated by the registration method. More... | |
void | setMaximumIterations (int nr_iterations) |
Set the maximum number of iterations the internal optimization should run for. More... | |
int | getMaximumIterations () |
Get the maximum number of iterations the internal optimization should run for, as set by the user. More... | |
void | setRANSACIterations (int ransac_iterations) |
Set the number of iterations RANSAC should run for. More... | |
double | getRANSACIterations () |
Get the number of iterations RANSAC should run for, as set by the user. More... | |
void | setRANSACOutlierRejectionThreshold (double inlier_threshold) |
Set the inlier distance threshold for the internal RANSAC outlier rejection loop. More... | |
double | getRANSACOutlierRejectionThreshold () |
Get the inlier distance threshold for the internal outlier rejection loop as set by the user. More... | |
void | setMaxCorrespondenceDistance (double distance_threshold) |
Set the maximum distance threshold between two correspondent points in source <-> target. More... | |
double | getMaxCorrespondenceDistance () |
Get the maximum distance threshold between two correspondent points in source <-> target. More... | |
void | setTransformationEpsilon (double epsilon) |
Set the transformation epsilon (maximum allowable translation squared difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution. More... | |
double | getTransformationEpsilon () |
Get the transformation epsilon (maximum allowable translation squared difference between two consecutive transformations) as set by the user. More... | |
void | setTransformationRotationEpsilon (double epsilon) |
Set the transformation rotation epsilon (maximum allowable rotation difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution. More... | |
double | getTransformationRotationEpsilon () |
Get the transformation rotation epsilon (maximum allowable difference between two consecutive transformations) as set by the user (epsilon is the cos(angle) in a axis-angle representation). More... | |
void | setEuclideanFitnessEpsilon (double epsilon) |
Set the maximum allowed Euclidean error between two consecutive steps in the ICP loop, before the algorithm is considered to have converged. More... | |
double | getEuclideanFitnessEpsilon () |
Get the maximum allowed distance error before the algorithm will be considered to have converged, as set by the user. More... | |
void | setPointRepresentation (const PointRepresentationConstPtr &point_representation) |
Provide a boost shared pointer to the PointRepresentation to be used when comparing points. More... | |
bool | registerVisualizationCallback (std::function< UpdateVisualizerCallbackSignature > &visualizerCallback) |
Register the user callback function which will be called from registration thread in order to update point cloud obtained after each iteration. More... | |
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) More... | |
double | getFitnessScore (const std::vector< float > &distances_a, const std::vector< float > &distances_b) |
Obtain the Euclidean fitness score (e.g., mean of squared distances from the source to the target) from two sets of correspondence distances (distances between source and target points) More... | |
bool | hasConverged () const |
Return the state of convergence after the last align run. More... | |
void | align (PointCloudSource &output) |
Call the registration algorithm which estimates the transformation and returns the transformed source (input) as output. More... | |
void | align (PointCloudSource &output, const Matrix4 &guess) |
Call the registration algorithm which estimates the transformation and returns the transformed source (input) as output. More... | |
const std::string & | getClassName () const |
Abstract class get name method. More... | |
bool | initCompute () |
Internal computation initialization. More... | |
bool | initComputeReciprocal () |
Internal computation when reciprocal lookup is needed. More... | |
void | addCorrespondenceRejector (const CorrespondenceRejectorPtr &rejector) |
Add a new correspondence rejector to the list. More... | |
std::vector< CorrespondenceRejectorPtr > | getCorrespondenceRejectors () |
Get the list of correspondence rejectors. More... | |
bool | removeCorrespondenceRejector (unsigned int i) |
Remove the i-th correspondence rejector in the list. More... | |
void | clearCorrespondenceRejectors () |
Clear the list of correspondence rejectors. More... | |
Public Member Functions inherited from pcl::PCLBase< PointSource > | |
PCLBase () | |
Empty constructor. More... | |
PCLBase (const PCLBase &base) | |
Copy constructor. More... | |
virtual | ~PCLBase ()=default |
Destructor. More... | |
virtual void | setInputCloud (const PointCloudConstPtr &cloud) |
Provide a pointer to the input dataset. More... | |
PointCloudConstPtr const | getInputCloud () const |
Get a pointer to the input point cloud dataset. More... | |
virtual void | setIndices (const IndicesPtr &indices) |
Provide a pointer to the vector of indices that represents the input data. More... | |
virtual void | setIndices (const IndicesConstPtr &indices) |
Provide a pointer to the vector of indices that represents the input data. More... | |
virtual void | setIndices (const PointIndicesConstPtr &indices) |
Provide a pointer to the vector of indices that represents the input data. More... | |
virtual void | setIndices (std::size_t row_start, std::size_t col_start, std::size_t nb_rows, std::size_t nb_cols) |
Set the indices for the points laying within an interest region of the point cloud. More... | |
IndicesPtr | getIndices () |
Get a pointer to the vector of indices used. More... | |
IndicesConstPtr const | getIndices () const |
Get a pointer to the vector of indices used. More... | |
const PointSource & | operator[] (std::size_t pos) const |
Override PointCloud operator[] to shorten code. More... | |
Protected Member Functions | |
void | transformCloud (const PointCloudSource &input, PointCloudSource &output, const Matrix4 &transform) override |
Apply a rigid transform to a given dataset. More... | |
Protected Member Functions inherited from pcl::IterativeClosestPoint< PointSource, PointTarget, float > | |
void | computeTransformation (PointCloudSource &output, const Matrix4 &guess) override |
Rigid transformation computation method with initial guess. More... | |
virtual void | determineRequiredBlobData () |
Looks at the Estimators and Rejectors and determines whether their blob-setter methods need to be called. More... | |
Protected Member Functions inherited from pcl::Registration< PointSource, PointTarget, float > | |
bool | searchForNeighbors (const PointCloudSource &cloud, int index, pcl::Indices &indices, std::vector< float > &distances) |
Search for the closest nearest neighbor of a given point. More... | |
virtual void | computeTransformation (PointCloudSource &output, const Matrix4 &guess)=0 |
Abstract transformation computation method with initial guess. More... | |
Protected Member Functions inherited from pcl::PCLBase< PointSource > | |
bool | initCompute () |
This method should get called before starting the actual computation. More... | |
bool | deinitCompute () |
This method should get called after finishing the actual computation. More... | |
Protected Attributes | |
bool | use_symmetric_objective_ |
Type of objective function (asymmetric vs. More... | |
bool | enforce_same_direction_normals_ |
Whether or not to negate source and/or target normals such that they point in the same direction in the symmetric objective function. More... | |
Protected Attributes inherited from pcl::IterativeClosestPoint< PointSource, PointTarget, float > | |
std::size_t | x_idx_offset_ |
XYZ fields offset. More... | |
std::size_t | y_idx_offset_ |
std::size_t | z_idx_offset_ |
std::size_t | nx_idx_offset_ |
Normal fields offset. More... | |
std::size_t | ny_idx_offset_ |
std::size_t | nz_idx_offset_ |
bool | use_reciprocal_correspondence_ |
The correspondence type used for correspondence estimation. More... | |
bool | source_has_normals_ |
Internal check whether source dataset has normals or not. More... | |
bool | target_has_normals_ |
Internal check whether target dataset has normals or not. More... | |
bool | need_source_blob_ |
Checks for whether estimators and rejectors need various data. More... | |
bool | need_target_blob_ |
Protected Attributes inherited from pcl::Registration< PointSource, PointTarget, float > | |
std::string | reg_name_ |
The registration method name. More... | |
KdTreePtr | tree_ |
A pointer to the spatial search object. More... | |
KdTreeReciprocalPtr | tree_reciprocal_ |
A pointer to the spatial search object of the source. More... | |
int | nr_iterations_ |
The number of iterations the internal optimization ran for (used internally). More... | |
int | max_iterations_ |
The maximum number of iterations the internal optimization should run for. More... | |
int | ransac_iterations_ |
The number of iterations RANSAC should run for. More... | |
PointCloudTargetConstPtr | target_ |
The input point cloud dataset target. More... | |
Matrix4 | final_transformation_ |
The final transformation matrix estimated by the registration method after N iterations. More... | |
Matrix4 | transformation_ |
The transformation matrix estimated by the registration method. More... | |
Matrix4 | previous_transformation_ |
The previous transformation matrix estimated by the registration method (used internally). More... | |
double | transformation_epsilon_ |
The maximum difference between two consecutive transformations in order to consider convergence (user defined). More... | |
double | transformation_rotation_epsilon_ |
The maximum rotation difference between two consecutive transformations in order to consider convergence (user defined). More... | |
double | euclidean_fitness_epsilon_ |
The maximum allowed Euclidean error between two consecutive steps in the ICP loop, before the algorithm is considered to have converged. More... | |
double | corr_dist_threshold_ |
The maximum distance threshold between two correspondent points in source <-> target. More... | |
double | inlier_threshold_ |
The inlier distance threshold for the internal RANSAC outlier rejection loop. More... | |
bool | converged_ |
Holds internal convergence state, given user parameters. More... | |
unsigned int | min_number_correspondences_ |
The minimum number of correspondences that the algorithm needs before attempting to estimate the transformation. More... | |
CorrespondencesPtr | correspondences_ |
The set of correspondences determined at this ICP step. More... | |
TransformationEstimationPtr | transformation_estimation_ |
A TransformationEstimation object, used to calculate the 4x4 rigid transformation. More... | |
CorrespondenceEstimationPtr | correspondence_estimation_ |
A CorrespondenceEstimation object, used to estimate correspondences between the source and the target cloud. More... | |
std::vector< CorrespondenceRejectorPtr > | correspondence_rejectors_ |
The list of correspondence rejectors to use. More... | |
bool | target_cloud_updated_ |
Variable that stores whether we have a new target cloud, meaning we need to pre-process it again. More... | |
bool | source_cloud_updated_ |
Variable that stores whether we have a new source cloud, meaning we need to pre-process it again. More... | |
bool | force_no_recompute_ |
A flag which, if set, means the tree operating on the target cloud will never be recomputed. More... | |
bool | force_no_recompute_reciprocal_ |
A flag which, if set, means the tree operating on the source cloud will never be recomputed. More... | |
std::function< UpdateVisualizerCallbackSignature > | update_visualizer_ |
Callback function to update intermediate source point cloud position during it's registration to the target point cloud. More... | |
Protected Attributes inherited from pcl::PCLBase< PointSource > | |
PointCloudConstPtr | input_ |
The input point cloud dataset. More... | |
IndicesPtr | indices_ |
A pointer to the vector of point indices to use. More... | |
bool | use_indices_ |
Set to true if point indices are used. More... | |
bool | fake_indices_ |
If no set of indices are given, we construct a set of fake indices that mimic the input PointCloud. More... | |
Additional Inherited Members | |
Public Attributes inherited from pcl::IterativeClosestPoint< PointSource, PointTarget, float > | |
pcl::registration::DefaultConvergenceCriteria< float >::Ptr | convergence_criteria_ |
IterativeClosestPointWithNormals is a special case of IterativeClosestPoint, that uses a transformation estimated based on Point to Plane distances by default.
By default, this implementation uses the traditional point to plane objective and computes point to plane distances using the normals of the target point cloud. It also provides the option (through setUseSymmetricObjective) of using the symmetric objective function of [Rusinkiewicz 2019]. This objective uses the normals of both the source and target point cloud and has a similar computational cost to the traditional point to plane objective while also offering improved convergence speed and a wider basin of convergence.
Note that this implementation not demean the point clouds which can lead to increased numerical error. If desired, a user can demean the point cloud, run iterative closest point, and composite the resulting ICP transformation with the translations from demeaning to obtain a transformation between the original point clouds.
using pcl::IterativeClosestPointWithNormals< PointSource, PointTarget, Scalar >::ConstPtr = shared_ptr< const IterativeClosestPointWithNormals<PointSource, PointTarget, Scalar> > |
using pcl::IterativeClosestPointWithNormals< PointSource, PointTarget, Scalar >::Matrix4 = typename IterativeClosestPoint<PointSource, PointTarget, Scalar>::Matrix4 |
using pcl::IterativeClosestPointWithNormals< PointSource, PointTarget, Scalar >::PointCloudSource = typename IterativeClosestPoint<PointSource, PointTarget, Scalar>::PointCloudSource |
using pcl::IterativeClosestPointWithNormals< PointSource, PointTarget, Scalar >::PointCloudTarget = typename IterativeClosestPoint<PointSource, PointTarget, Scalar>::PointCloudTarget |
using pcl::IterativeClosestPointWithNormals< PointSource, PointTarget, Scalar >::Ptr = shared_ptr<IterativeClosestPointWithNormals<PointSource, PointTarget, Scalar> > |
|
inline |
Empty constructor.
Definition at line 364 of file icp.h.
References pcl::Registration< PointSource, PointTarget, float >::reg_name_, pcl::IterativeClosestPointWithNormals< PointSource, PointTarget, Scalar >::setEnforceSameDirectionNormals(), and pcl::IterativeClosestPointWithNormals< PointSource, PointTarget, Scalar >::setUseSymmetricObjective().
|
overridedefault |
Empty destructor.
|
inline |
Obtain whether source or target normals are negated on a per-point basis such that they point in the same direction or not.
Definition at line 433 of file icp.h.
References pcl::IterativeClosestPointWithNormals< PointSource, PointTarget, Scalar >::enforce_same_direction_normals_.
|
inline |
Obtain whether a symmetric objective is used or not.
Definition at line 404 of file icp.h.
References pcl::IterativeClosestPointWithNormals< PointSource, PointTarget, Scalar >::use_symmetric_objective_.
|
inline |
Set whether or not to negate source or target normals on a per-point basis such that they point in the same direction.
Only applicable to the symmetric objective function.
[in] | enforce_same_direction_normals | whether to negate source or target normals on a per-point basis such that they point in the same direction. |
Definition at line 417 of file icp.h.
References pcl::IterativeClosestPointWithNormals< PointSource, PointTarget, Scalar >::enforce_same_direction_normals_, and pcl::Registration< PointSource, PointTarget, float >::transformation_estimation_.
Referenced by pcl::IterativeClosestPointWithNormals< PointSource, PointTarget, Scalar >::IterativeClosestPointWithNormals().
|
inline |
Set whether to use a symmetric objective function or not.
[in] | use_symmetric_objective | whether to use a symmetric objective function or not |
Definition at line 381 of file icp.h.
References pcl::IterativeClosestPointWithNormals< PointSource, PointTarget, Scalar >::enforce_same_direction_normals_, pcl::make_shared(), pcl::Registration< PointSource, PointTarget, float >::transformation_estimation_, and pcl::IterativeClosestPointWithNormals< PointSource, PointTarget, Scalar >::use_symmetric_objective_.
Referenced by pcl::IterativeClosestPointWithNormals< PointSource, PointTarget, Scalar >::IterativeClosestPointWithNormals().
|
overrideprotectedvirtual |
Apply a rigid transform to a given dataset.
[in] | input | the input point cloud |
[out] | output | the resultant output point cloud |
[in] | transform | a 4x4 rigid transformation |
Reimplemented from pcl::IterativeClosestPoint< PointSource, PointTarget, float >.
Definition at line 314 of file icp.hpp.
References pcl::transformPointCloudWithNormals().
|
protected |
Whether or not to negate source and/or target normals such that they point in the same direction in the symmetric objective function.
Definition at line 455 of file icp.h.
Referenced by pcl::IterativeClosestPointWithNormals< PointSource, PointTarget, Scalar >::getEnforceSameDirectionNormals(), pcl::IterativeClosestPointWithNormals< PointSource, PointTarget, Scalar >::setEnforceSameDirectionNormals(), and pcl::IterativeClosestPointWithNormals< PointSource, PointTarget, Scalar >::setUseSymmetricObjective().
|
protected |
Type of objective function (asymmetric vs.
symmetric) used for transform estimation
Definition at line 452 of file icp.h.
Referenced by pcl::IterativeClosestPointWithNormals< PointSource, PointTarget, Scalar >::getUseSymmetricObjective(), and pcl::IterativeClosestPointWithNormals< PointSource, PointTarget, Scalar >::setUseSymmetricObjective().