Point Cloud Library (PCL)
1.3.1
|
IterativeClosestPoint provides a base implementation of the Iterative Closest Point algorithm. More...
#include <pcl/registration/icp.h>
Public Types | |
typedef boost::shared_ptr < Registration< PointSource, PointTarget > > | Ptr |
typedef boost::shared_ptr < const Registration < PointSource, PointTarget > > | ConstPtr |
typedef pcl::KdTree< PointTarget > | KdTree |
typedef pcl::KdTree < PointTarget >::Ptr | KdTreePtr |
typedef PointCloudTarget::Ptr | PointCloudTargetPtr |
typedef PointCloudTarget::ConstPtr | PointCloudTargetConstPtr |
typedef KdTree::PointRepresentationConstPtr | PointRepresentationConstPtr |
typedef pcl::registration::TransformationEstimation < PointSource, PointTarget > | TransformationEstimation |
typedef TransformationEstimation::Ptr | TransformationEstimationPtr |
typedef TransformationEstimation::ConstPtr | TransformationEstimationConstPtr |
Public Member Functions | |
IterativeClosestPoint () | |
Empty constructor. | |
void | setTransformationEstimation (const TransformationEstimationPtr &te) |
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 to) | |
PointCloudTargetConstPtr const | getInputTarget () |
Get a pointer to the input point cloud dataset target. | |
Eigen::Matrix4f | getFinalTransformation () |
Get the final transformation matrix estimated by the registration method. | |
Eigen::Matrix4f | getLastIncrementalTransformation () |
Get the last incremental transformation matrix estimated by the registration method. | |
void | setMaximumIterations (int nr_iterations) |
Set the maximum number of iterations the internal optimization should run for. | |
int | getMaximumIterations () |
Get the maximum number of iterations the internal optimization should run for, as set by the user. | |
void | setRANSACOutlierRejectionThreshold (double inlier_threshold) |
Set the inlier distance threshold for the internal RANSAC outlier rejection loop. | |
double | getRANSACOutlierRejectionThreshold () |
Get the inlier distance threshold for the internal outlier rejection loop as set by the user. | |
void | setMaxCorrespondenceDistance (double distance_threshold) |
Set the maximum distance threshold between two correspondent points in source <-> target. | |
double | getMaxCorrespondenceDistance () |
Get the maximum distance threshold between two correspondent points in source <-> target. | |
void | setTransformationEpsilon (double epsilon) |
Set the transformation epsilon (maximum allowable difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution. | |
double | getTransformationEpsilon () |
Get the transformation epsilon (maximum allowable difference between two consecutive transformations) as set by the user. | |
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. | |
double | getEuclideanFitnessEpsilon () |
Get the maximum allowed distance error before the algorithm will be considered to have converged, as set by the user. | |
void | setPointRepresentation (const PointRepresentationConstPtr &point_representation) |
Provide a boost shared pointer to the PointRepresentation to be used when comparing points. | |
bool | registerVisualizationCallback (boost::function< FunctionSignature > &visualizerCallback) |
Register the user callback function which will be called from registration thread in order to update point cloud obtained after each iteration. | |
double | getFitnessScore (double max_range=std::numeric_limits< double >::max()) |
Obtain the Euclidean fitness score (e.g., sum of squared distances from the source to the target) | |
double | getFitnessScore (const std::vector< float > &distances_a, const std::vector< float > &distances_b) |
Obtain the Euclidean fitness score (e.g., sum of squared distances from the source to the target) from two sets of correspondence distances (distances between source and target points) | |
bool | hasConverged () |
Return the state of convergence after the last align run. | |
void | align (PointCloudSource &output) |
Call the registration algorithm which estimates the transformation and returns the transformed source (input) as output. | |
void | align (PointCloudSource &output, const Eigen::Matrix4f &guess) |
Call the registration algorithm which estimates the transformation and returns the transformed source (input) as output. | |
const std::string & | getClassName () const |
Abstract class get name method. |
IterativeClosestPoint provides a base implementation of the Iterative Closest Point algorithm.
The transformation is estimated based on Singular Value Decomposition (SVD).
The algorithm has several termination criteria:
Usage example:
IterativeClosestPoint<PointXYZ, PointXYZ> icp; // Set the input source and target icp.setInputCloud (cloud_source); icp.setInputTarget (cloud_target); // Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored) icp.setMaxCorrespondenceDistance (0.05); // Set the maximum number of iterations (criterion 1) icp.setMaximumIterations (50); // Set the transformation epsilon (criterion 2) icp.setTransformationEpsilon (1e-8); // Set the euclidean distance difference epsilon (criterion 3) icp.setEuclideanFitnessEpsilon (1); // Perform the alignment icp.align (cloud_source_registered); // Obtain the transformation that aligned cloud_source to cloud_source_registered Eigen::Matrix4f transformation = icp.getFinalTransformation ();
typedef boost::shared_ptr< const Registration<PointSource, PointTarget> > pcl::Registration::ConstPtr [inherited] |
Definition at line 70 of file registration.h.
typedef pcl::KdTree<PointTarget> pcl::Registration::KdTree [inherited] |
Definition at line 72 of file registration.h.
typedef pcl::KdTree<PointTarget>::Ptr pcl::Registration::KdTreePtr [inherited] |
Definition at line 73 of file registration.h.
typedef PointCloudTarget::ConstPtr pcl::Registration::PointCloudTargetConstPtr [inherited] |
Reimplemented in pcl::PPFRegistration.
Definition at line 81 of file registration.h.
typedef PointCloudTarget::Ptr pcl::Registration::PointCloudTargetPtr [inherited] |
Reimplemented in pcl::PPFRegistration.
Definition at line 80 of file registration.h.
typedef KdTree::PointRepresentationConstPtr pcl::Registration::PointRepresentationConstPtr [inherited] |
Definition at line 83 of file registration.h.
typedef boost::shared_ptr< Registration<PointSource, PointTarget> > pcl::Registration::Ptr [inherited] |
Definition at line 69 of file registration.h.
typedef pcl::registration::TransformationEstimation<PointSource, PointTarget> pcl::Registration::TransformationEstimation [inherited] |
Definition at line 85 of file registration.h.
typedef TransformationEstimation::ConstPtr pcl::Registration::TransformationEstimationConstPtr [inherited] |
Definition at line 87 of file registration.h.
typedef TransformationEstimation::Ptr pcl::Registration::TransformationEstimationPtr [inherited] |
Definition at line 86 of file registration.h.
pcl::IterativeClosestPoint::IterativeClosestPoint | ( | ) | [inline] |
void pcl::Registration::align | ( | PointCloudSource & | output | ) | [inline, inherited] |
Call the registration algorithm which estimates the transformation and returns the transformed source (input) as output.
output | the resultant input transfomed point cloud dataset |
void pcl::Registration::align | ( | PointCloudSource & | output, |
const Eigen::Matrix4f & | guess | ||
) | [inline, inherited] |
Call the registration algorithm which estimates the transformation and returns the transformed source (input) as output.
output | the resultant input transfomed point cloud dataset |
guess | the initial gross estimation of the transformation |
const std::string& pcl::Registration::getClassName | ( | ) | const [inline, inherited] |
Abstract class get name method.
Definition at line 263 of file registration.h.
double pcl::Registration::getEuclideanFitnessEpsilon | ( | ) | [inline, inherited] |
Get the maximum allowed distance error before the algorithm will be considered to have converged, as set by the user.
See setEuclideanFitnessEpsilon
Definition at line 200 of file registration.h.
Eigen::Matrix4f pcl::Registration::getFinalTransformation | ( | ) | [inline, inherited] |
Get the final transformation matrix estimated by the registration method.
Definition at line 126 of file registration.h.
double pcl::Registration::getFitnessScore | ( | double | max_range = std::numeric_limits<double>::max () | ) | [inline, inherited] |
Obtain the Euclidean fitness score (e.g., sum of squared distances from the source to the target)
max_range | maximum allowable distance between a point and its correspondence in the target (default: double::max) |
double pcl::Registration::getFitnessScore | ( | const std::vector< float > & | distances_a, |
const std::vector< float > & | distances_b | ||
) | [inline, inherited] |
Obtain the Euclidean fitness score (e.g., sum of squared distances from the source to the target) from two sets of correspondence distances (distances between source and target points)
[in] | distances_a | the first set of distances between correspondences |
[in] | distances_b | the second set of distances between correspondences |
PointCloudTargetConstPtr const pcl::Registration::getInputTarget | ( | ) | [inline, inherited] |
Get a pointer to the input point cloud dataset target.
Definition at line 122 of file registration.h.
Eigen::Matrix4f pcl::Registration::getLastIncrementalTransformation | ( | ) | [inline, inherited] |
Get the last incremental transformation matrix estimated by the registration method.
Definition at line 130 of file registration.h.
double pcl::Registration::getMaxCorrespondenceDistance | ( | ) | [inline, inherited] |
Get the maximum distance threshold between two correspondent points in source <-> target.
If the distance is larger than this threshold, the points will be ignored in the alignment process.
Definition at line 168 of file registration.h.
int pcl::Registration::getMaximumIterations | ( | ) | [inline, inherited] |
Get the maximum number of iterations the internal optimization should run for, as set by the user.
Definition at line 140 of file registration.h.
double pcl::Registration::getRANSACOutlierRejectionThreshold | ( | ) | [inline, inherited] |
Get the inlier distance threshold for the internal outlier rejection loop as set by the user.
Definition at line 154 of file registration.h.
double pcl::Registration::getTransformationEpsilon | ( | ) | [inline, inherited] |
Get the transformation epsilon (maximum allowable difference between two consecutive transformations) as set by the user.
Definition at line 183 of file registration.h.
bool pcl::Registration::hasConverged | ( | ) | [inline, inherited] |
Return the state of convergence after the last align run.
Definition at line 244 of file registration.h.
bool pcl::Registration::registerVisualizationCallback | ( | boost::function< FunctionSignature > & | visualizerCallback | ) | [inline, inherited] |
Register the user callback function which will be called from registration thread in order to update point cloud obtained after each iteration.
refference | of the user callback function |
Definition at line 216 of file registration.h.
void pcl::Registration::setEuclideanFitnessEpsilon | ( | double | epsilon | ) | [inline, inherited] |
Set the maximum allowed Euclidean error between two consecutive steps in the ICP loop, before the algorithm is considered to have converged.
The error is estimated as the sum of the differences between correspondences in an Euclidean sense, divided by the number of correspondences.
epsilon | the maximum allowed distance error before the algorithm will be considered to have converged |
Definition at line 194 of file registration.h.
virtual void pcl::Registration::setInputTarget | ( | const PointCloudTargetConstPtr & | cloud | ) | [inline, virtual, inherited] |
Provide a pointer to the input target (e.g., the point cloud that we want to align the input source to)
cloud | the input point cloud target |
Reimplemented in pcl::PPFRegistration, and pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >.
void pcl::Registration::setMaxCorrespondenceDistance | ( | double | distance_threshold | ) | [inline, inherited] |
Set the maximum distance threshold between two correspondent points in source <-> target.
If the distance is larger than this threshold, the points will be ignored in the alignment process.
distance_threshold | the maximum distance threshold between a point and its nearest neighbor correspondent in order to be considered in the alignment process |
Definition at line 162 of file registration.h.
void pcl::Registration::setMaximumIterations | ( | int | nr_iterations | ) | [inline, inherited] |
Set the maximum number of iterations the internal optimization should run for.
nr_iterations | the maximum number of iterations the internal optimization should run for |
Definition at line 136 of file registration.h.
void pcl::Registration::setPointRepresentation | ( | const PointRepresentationConstPtr & | point_representation | ) | [inline, inherited] |
Provide a boost shared pointer to the PointRepresentation to be used when comparing points.
point_representation | the PointRepresentation to be used by the k-D tree |
Definition at line 206 of file registration.h.
void pcl::Registration::setRANSACOutlierRejectionThreshold | ( | double | inlier_threshold | ) | [inline, inherited] |
Set the inlier distance threshold for the internal RANSAC outlier rejection loop.
The method considers a point to be an inlier, if the distance between the target data index and the transformed source index is smaller than the given inlier distance threshold. The value is set by default to 0.05m.
inlier_threshold | the inlier distance threshold for the internal RANSAC outlier rejection loop |
Definition at line 150 of file registration.h.
void pcl::Registration::setTransformationEpsilon | ( | double | epsilon | ) | [inline, inherited] |
Set the transformation epsilon (maximum allowable difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution.
epsilon | the transformation epsilon in order for an optimization to be considered as having converged to the final solution. |
Definition at line 177 of file registration.h.
void pcl::Registration::setTransformationEstimation | ( | const TransformationEstimationPtr & | te | ) | [inline, inherited] |
Definition at line 112 of file registration.h.