|
using |
PointCloudSource = typename Registration< PointSource, PointTarget, Scalar >::PointCloudSource
|
|
using |
PointCloudSourcePtr = typename PointCloudSource::Ptr |
|
using |
PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr |
|
using |
PointCloudTarget = typename Registration< PointSource, PointTarget, Scalar >::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, Scalar > > |
|
using |
ConstPtr = shared_ptr< const IterativeClosestPoint< PointSource, PointTarget, Scalar > > |
|
using |
Matrix4 = typename Registration< PointSource, PointTarget, Scalar >::Matrix4
|
|
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...
|
|
using |
PointCloud = pcl::PointCloud< PointSource > |
|
using |
PointCloudPtr = typename PointCloud::Ptr
|
|
using |
PointCloudConstPtr = typename PointCloud::ConstPtr
|
|
using |
PointIndicesPtr = PointIndices::Ptr
|
|
using |
PointIndicesConstPtr = PointIndices::ConstPtr
|
|
|
|
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 () |
|
Empty destructor. More...
|
|
pcl::registration::DefaultConvergenceCriteria< Scalar >::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 to the target) 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...
|
|
|
Registration () |
|
Empty constructor. More...
|
|
|
~Registration () |
|
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...
|
|
const PointCloudSourceConstPtr |
getInputSource () |
|
Get a pointer to the input point cloud dataset target. More...
|
|
const PointCloudTargetConstPtr |
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...
|
|
|
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...
|
|
const PointCloudConstPtr |
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...
|
|
const IndicesConstPtr |
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...
|
|
|
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_ |
|
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...
|
|
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...
|
|
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...
|
|
template<typename PointSource, typename PointTarget, typename Scalar = float>
class pcl::IterativeClosestPoint< PointSource, PointTarget, Scalar >
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:
- Number of iterations has reached the maximum user imposed number of iterations (via setMaximumIterations)
- The epsilon (difference) between the previous transformation and the current estimated transformation is smaller than an user imposed value (via setTransformationEpsilon)
- The sum of Euclidean squared errors is smaller than a user defined threshold (via setEuclideanFitnessEpsilon)
Usage example:
IterativeClosestPoint<PointXYZ, PointXYZ> icp;
icp.setInputSource (cloud_source);
icp.setInputTarget (cloud_target);
icp.setMaxCorrespondenceDistance (0.05);
icp.setMaximumIterations (50);
icp.setTransformationEpsilon (1e-8);
icp.setEuclideanFitnessEpsilon (1);
icp.align (cloud_source_registered);
Eigen::Matrix4f transformation = icp.getFinalTransformation ();
- Author
- Radu B. Rusu, Michael Dixon
Definition at line 97 of file icp.h.