Point Cloud Library (PCL)
1.14.1-dev
|
JointIterativeClosestPoint extends ICP to multiple frames which share the same transform. More...
#include <pcl/registration/joint_icp.h>
Public Member Functions | |
JointIterativeClosestPoint () | |
Empty constructor. More... | |
~JointIterativeClosestPoint () override=default | |
Empty destructor. More... | |
void | setInputSource (const PointCloudSourceConstPtr &) override |
Provide a pointer to the input source (e.g., the point cloud that we want to align to the target) More... | |
void | addInputSource (const PointCloudSourceConstPtr &cloud) |
Add a source cloud to the joint solver. More... | |
void | setInputTarget (const PointCloudTargetConstPtr &) override |
Provide a pointer to the input target (e.g., the point cloud that we want to align to the target) More... | |
void | addInputTarget (const PointCloudTargetConstPtr &cloud) |
Add a target cloud to the joint solver. More... | |
void | addCorrespondenceEstimation (CorrespondenceEstimationPtr ce) |
Add a manual correspondence estimator If you choose to do this, you must add one for each input source / target pair. More... | |
void | clearInputSources () |
Reset my list of input sources. More... | |
void | clearInputTargets () |
Reset my list of input targets. More... | |
void | clearCorrespondenceEstimations () |
Reset my list of correspondence estimation methods. 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 | computeTransformation (PointCloudSource &output, const Matrix4 &guess) override |
Rigid transformation computation method with initial guess. More... | |
void | determineRequiredBlobData () override |
Looks at the Estimators and Rejectors and determines whether their blob-setter methods need to be called. More... | |
Protected Member Functions inherited from pcl::IterativeClosestPoint< PointSource, PointTarget, float > | |
virtual void | transformCloud (const PointCloudSource &input, PointCloudSource &output, const Matrix4 &transform) |
Apply a rigid transform to a given dataset. More... | |
void | computeTransformation (PointCloudSource &output, const Matrix4 &guess) override |
Rigid transformation computation method with initial guess. 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 | |
std::vector< PointCloudSourceConstPtr > | sources_ |
std::vector< PointCloudTargetConstPtr > | targets_ |
std::vector< CorrespondenceEstimationPtr > | correspondence_estimations_ |
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_ |
JointIterativeClosestPoint extends ICP to multiple frames which share the same transform.
This is particularly useful when solving for camera extrinsics using multiple observations. When given a single pair of clouds, this reduces to vanilla ICP.
Definition at line 53 of file joint_icp.h.
using pcl::JointIterativeClosestPoint< PointSource, PointTarget, Scalar >::ConstPtr = shared_ptr<const JointIterativeClosestPoint<PointSource, PointTarget, Scalar> > |
Definition at line 78 of file joint_icp.h.
using pcl::JointIterativeClosestPoint< PointSource, PointTarget, Scalar >::CorrespondenceEstimation = pcl::registration::CorrespondenceEstimationBase<PointSource, PointTarget, Scalar> |
Definition at line 81 of file joint_icp.h.
using pcl::JointIterativeClosestPoint< PointSource, PointTarget, Scalar >::CorrespondenceEstimationConstPtr = typename CorrespondenceEstimation::ConstPtr |
Definition at line 84 of file joint_icp.h.
using pcl::JointIterativeClosestPoint< PointSource, PointTarget, Scalar >::CorrespondenceEstimationPtr = typename CorrespondenceEstimation::Ptr |
Definition at line 83 of file joint_icp.h.
using pcl::JointIterativeClosestPoint< PointSource, PointTarget, Scalar >::KdTree = pcl::search::KdTree<PointTarget> |
Definition at line 68 of file joint_icp.h.
using pcl::JointIterativeClosestPoint< PointSource, PointTarget, Scalar >::KdTreePtr = typename KdTree::Ptr |
Definition at line 69 of file joint_icp.h.
using pcl::JointIterativeClosestPoint< PointSource, PointTarget, Scalar >::KdTreeReciprocal = pcl::search::KdTree<PointSource> |
Definition at line 71 of file joint_icp.h.
using pcl::JointIterativeClosestPoint< PointSource, PointTarget, Scalar >::KdTreeReciprocalPtr = typename KdTree::Ptr |
Definition at line 72 of file joint_icp.h.
using pcl::JointIterativeClosestPoint< PointSource, PointTarget, Scalar >::Matrix4 = typename IterativeClosestPoint<PointSource, PointTarget, Scalar>::Matrix4 |
Definition at line 125 of file joint_icp.h.
using pcl::JointIterativeClosestPoint< PointSource, PointTarget, Scalar >::PointCloudSource = typename IterativeClosestPoint<PointSource, PointTarget, Scalar>::PointCloudSource |
Definition at line 56 of file joint_icp.h.
using pcl::JointIterativeClosestPoint< PointSource, PointTarget, Scalar >::PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr |
Definition at line 60 of file joint_icp.h.
using pcl::JointIterativeClosestPoint< PointSource, PointTarget, Scalar >::PointCloudSourcePtr = typename PointCloudSource::Ptr |
Definition at line 59 of file joint_icp.h.
using pcl::JointIterativeClosestPoint< PointSource, PointTarget, Scalar >::PointCloudTarget = typename IterativeClosestPoint<PointSource, PointTarget, Scalar>::PointCloudTarget |
Definition at line 62 of file joint_icp.h.
using pcl::JointIterativeClosestPoint< PointSource, PointTarget, Scalar >::PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr |
Definition at line 66 of file joint_icp.h.
using pcl::JointIterativeClosestPoint< PointSource, PointTarget, Scalar >::PointCloudTargetPtr = typename PointCloudTarget::Ptr |
Definition at line 65 of file joint_icp.h.
using pcl::JointIterativeClosestPoint< PointSource, PointTarget, Scalar >::PointIndicesConstPtr = PointIndices::ConstPtr |
Definition at line 75 of file joint_icp.h.
using pcl::JointIterativeClosestPoint< PointSource, PointTarget, Scalar >::PointIndicesPtr = PointIndices::Ptr |
Definition at line 74 of file joint_icp.h.
using pcl::JointIterativeClosestPoint< PointSource, PointTarget, Scalar >::Ptr = shared_ptr<JointIterativeClosestPoint<PointSource, PointTarget, Scalar> > |
Definition at line 77 of file joint_icp.h.
|
inline |
Empty constructor.
Definition at line 129 of file joint_icp.h.
References pcl::Registration< PointSource, PointTarget, float >::reg_name_.
|
overridedefault |
Empty destructor.
|
inline |
Add a manual correspondence estimator If you choose to do this, you must add one for each input source / target pair.
They do not need to have trees or input clouds set ahead of time.
[in] | ce | Correspondence estimation |
Definition at line 198 of file joint_icp.h.
References pcl::JointIterativeClosestPoint< PointSource, PointTarget, Scalar >::correspondence_estimations_.
|
inline |
Add a source cloud to the joint solver.
[in] | cloud | source cloud |
Definition at line 155 of file joint_icp.h.
References pcl::IterativeClosestPoint< PointSource, PointTarget, Scalar >::setInputSource(), and pcl::JointIterativeClosestPoint< PointSource, PointTarget, Scalar >::sources_.
|
inline |
Add a target cloud to the joint solver.
[in] | cloud | target cloud |
Definition at line 181 of file joint_icp.h.
References pcl::IterativeClosestPoint< PointSource, PointTarget, Scalar >::setInputTarget(), and pcl::JointIterativeClosestPoint< PointSource, PointTarget, Scalar >::targets_.
|
inline |
Reset my list of correspondence estimation methods.
Definition at line 222 of file joint_icp.h.
References pcl::JointIterativeClosestPoint< PointSource, PointTarget, Scalar >::correspondence_estimations_.
|
inline |
Reset my list of input sources.
Definition at line 206 of file joint_icp.h.
References pcl::JointIterativeClosestPoint< PointSource, PointTarget, Scalar >::sources_.
|
inline |
Reset my list of input targets.
Definition at line 214 of file joint_icp.h.
References pcl::JointIterativeClosestPoint< PointSource, PointTarget, Scalar >::targets_.
|
overrideprotected |
Rigid transformation computation method with initial guess.
output | the transformed input point cloud dataset using the rigid transformation found |
guess | the initial guess of the transformation to compute |
Definition at line 49 of file joint_icp.hpp.
References pcl::Correspondence::index_match, pcl::Correspondence::index_query, pcl::IterativeClosestPoint< PointSource, PointTarget, Scalar >::setInputSource(), pcl::IterativeClosestPoint< PointSource, PointTarget, Scalar >::setInputTarget(), and pcl::toPCLPointCloud2().
|
overrideprotectedvirtual |
Looks at the Estimators and Rejectors and determines whether their blob-setter methods need to be called.
Reimplemented from pcl::IterativeClosestPoint< PointSource, PointTarget, float >.
Definition at line 294 of file joint_icp.hpp.
|
inlineoverridevirtual |
Provide a pointer to the input source (e.g., the point cloud that we want to align to the target)
Reimplemented from pcl::Registration< PointSource, PointTarget, float >.
Definition at line 142 of file joint_icp.h.
References pcl::Registration< PointSource, PointTarget, float >::getClassName().
|
inlineoverridevirtual |
Provide a pointer to the input target (e.g., the point cloud that we want to align to the target)
Reimplemented from pcl::Registration< PointSource, PointTarget, float >.
Definition at line 168 of file joint_icp.h.
References pcl::Registration< PointSource, PointTarget, float >::getClassName().
|
protected |
|
protected |
Definition at line 241 of file joint_icp.h.
Referenced by pcl::JointIterativeClosestPoint< PointSource, PointTarget, Scalar >::addInputSource(), and pcl::JointIterativeClosestPoint< PointSource, PointTarget, Scalar >::clearInputSources().
|
protected |
Definition at line 242 of file joint_icp.h.
Referenced by pcl::JointIterativeClosestPoint< PointSource, PointTarget, Scalar >::addInputTarget(), and pcl::JointIterativeClosestPoint< PointSource, PointTarget, Scalar >::clearInputTargets().