Point Cloud Library (PCL)  1.8.1-dev
List of all members | Classes | Public Types | Public Member Functions | Protected Member Functions | Protected Attributes
pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget > Class Template Reference

GeneralizedIterativeClosestPoint is an ICP variant that implements the generalized iterative closest point algorithm as described by Alex Segal et al. More...

#include <pcl/registration/gicp.h>

+ Inheritance diagram for pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >:

Classes

struct  OptimizationFunctorWithIndices
 optimization functor structure More...
 

Public Types

typedef pcl::PointCloud
< PointSource > 
PointCloudSource
 
typedef PointCloudSource::Ptr PointCloudSourcePtr
 
typedef PointCloudSource::ConstPtr PointCloudSourceConstPtr
 
typedef pcl::PointCloud
< PointTarget > 
PointCloudTarget
 
typedef PointCloudTarget::Ptr PointCloudTargetPtr
 
typedef PointCloudTarget::ConstPtr PointCloudTargetConstPtr
 
typedef PointIndices::Ptr PointIndicesPtr
 
typedef PointIndices::ConstPtr PointIndicesConstPtr
 
typedef std::vector
< Eigen::Matrix3d,
Eigen::aligned_allocator
< Eigen::Matrix3d > > 
MatricesVector
 
typedef boost::shared_ptr
< MatricesVector
MatricesVectorPtr
 
typedef boost::shared_ptr
< const MatricesVector
MatricesVectorConstPtr
 
typedef Registration
< PointSource, PointTarget >
::KdTree 
InputKdTree
 
typedef Registration
< PointSource, PointTarget >
::KdTreePtr 
InputKdTreePtr
 
typedef boost::shared_ptr
< GeneralizedIterativeClosestPoint
< PointSource, PointTarget > > 
Ptr
 
typedef boost::shared_ptr
< const
GeneralizedIterativeClosestPoint
< PointSource, PointTarget > > 
ConstPtr
 
typedef Eigen::Matrix< double, 6, 1 > Vector6d
 
- Public Types inherited from pcl::IterativeClosestPoint< PointSource, PointTarget >
typedef Registration
< PointSource, PointTarget,
float >::PointCloudSource 
PointCloudSource
 
typedef PointCloudSource::Ptr PointCloudSourcePtr
 
typedef PointCloudSource::ConstPtr PointCloudSourceConstPtr
 
typedef Registration
< PointSource, PointTarget,
float >::PointCloudTarget 
PointCloudTarget
 
typedef PointCloudTarget::Ptr PointCloudTargetPtr
 
typedef PointCloudTarget::ConstPtr PointCloudTargetConstPtr
 
typedef PointIndices::Ptr PointIndicesPtr
 
typedef PointIndices::ConstPtr PointIndicesConstPtr
 
typedef boost::shared_ptr
< IterativeClosestPoint
< PointSource, PointTarget,
float > > 
Ptr
 
typedef boost::shared_ptr
< const IterativeClosestPoint
< PointSource, PointTarget,
float > > 
ConstPtr
 
typedef Registration
< PointSource, PointTarget,
float >::Matrix4 
Matrix4
 
- Public Types inherited from pcl::Registration< PointSource, PointTarget, float >
typedef Eigen::Matrix< float, 4, 4 > Matrix4
 
typedef boost::shared_ptr
< Registration< PointSource,
PointTarget, float > > 
Ptr
 
typedef boost::shared_ptr
< const Registration
< PointSource, PointTarget,
float > > 
ConstPtr
 
typedef
pcl::registration::CorrespondenceRejector::Ptr 
CorrespondenceRejectorPtr
 
typedef pcl::search::KdTree
< PointTarget > 
KdTree
 
typedef pcl::search::KdTree
< PointTarget >::Ptr 
KdTreePtr
 
typedef pcl::search::KdTree
< PointSource > 
KdTreeReciprocal
 
typedef KdTreeReciprocal::Ptr KdTreeReciprocalPtr
 
typedef pcl::PointCloud
< PointSource > 
PointCloudSource
 
typedef PointCloudSource::Ptr PointCloudSourcePtr
 
typedef PointCloudSource::ConstPtr PointCloudSourceConstPtr
 
typedef pcl::PointCloud
< PointTarget > 
PointCloudTarget
 
typedef PointCloudTarget::Ptr PointCloudTargetPtr
 
typedef PointCloudTarget::ConstPtr PointCloudTargetConstPtr
 
typedef
KdTree::PointRepresentationConstPtr 
PointRepresentationConstPtr
 
typedef
pcl::registration::TransformationEstimation
< PointSource, PointTarget,
float > 
TransformationEstimation
 
typedef
TransformationEstimation::Ptr 
TransformationEstimationPtr
 
typedef
TransformationEstimation::ConstPtr 
TransformationEstimationConstPtr
 
typedef
pcl::registration::CorrespondenceEstimationBase
< PointSource, PointTarget,
float > 
CorrespondenceEstimation
 
typedef
CorrespondenceEstimation::Ptr 
CorrespondenceEstimationPtr
 
typedef
CorrespondenceEstimation::ConstPtr 
CorrespondenceEstimationConstPtr
 
- Public Types inherited from pcl::PCLBase< PointSource >
typedef pcl::PointCloud
< PointSource > 
PointCloud
 
typedef PointCloud::Ptr PointCloudPtr
 
typedef PointCloud::ConstPtr PointCloudConstPtr
 
typedef boost::shared_ptr
< PointIndices
PointIndicesPtr
 
typedef boost::shared_ptr
< PointIndices const > 
PointIndicesConstPtr
 

Public Member Functions

 GeneralizedIterativeClosestPoint ()
 Empty constructor. More...
 
void setInputSource (const PointCloudSourceConstPtr &cloud)
 Provide a pointer to the input dataset. More...
 
void setSourceCovariances (const MatricesVectorPtr &covariances)
 Provide a pointer to the covariances of the input source (if computed externally!). More...
 
void setInputTarget (const PointCloudTargetConstPtr &target)
 Provide a pointer to the input target (e.g., the point cloud that we want to align the input source to) More...
 
void setTargetCovariances (const MatricesVectorPtr &covariances)
 Provide a pointer to the covariances of the input target (if computed externally!). More...
 
void estimateRigidTransformationBFGS (const PointCloudSource &cloud_src, const std::vector< int > &indices_src, const PointCloudTarget &cloud_tgt, const std::vector< int > &indices_tgt, Eigen::Matrix4f &transformation_matrix)
 Estimate a rigid rotation transformation between a source and a target point cloud using an iterative non-linear Levenberg-Marquardt approach. More...
 
const Eigen::Matrix3d & mahalanobis (size_t index) const
 
void computeRDerivative (const Vector6d &x, const Eigen::Matrix3d &R, Vector6d &g) const
 Computes rotation matrix derivative. More...
 
void setRotationEpsilon (double epsilon)
 Set the rotation epsilon (maximum allowable difference between two consecutive rotations) in order for an optimization to be considered as having converged to the final solution. More...
 
double getRotationEpsilon ()
 Get the rotation epsilon (maximum allowable difference between two consecutive rotations) as set by the user. More...
 
void setCorrespondenceRandomness (int k)
 Set the number of neighbors used when selecting a point neighbourhood to compute covariances. More...
 
int getCorrespondenceRandomness ()
 Get the number of neighbors used when computing covariances as set by the user. More...
 
void setMaximumOptimizerIterations (int max)
 set maximum number of iterations at the optimization step More...
 
int getMaximumOptimizerIterations ()
 
- Public Member Functions inherited from pcl::IterativeClosestPoint< PointSource, PointTarget >
 IterativeClosestPoint ()
 Empty constructor. More...
 
virtual ~IterativeClosestPoint ()
 Empty destructor. More...
 
pcl::registration::DefaultConvergenceCriteria
< float >::Ptr 
getConvergeCriteria ()
 Returns a pointer to the DefaultConvergenceCriteria used by the IterativeClosestPoint class. More...
 
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) More...
 
virtual void setInputTarget (const PointCloudTargetConstPtr &cloud)
 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...
 
- Public Member Functions inherited from pcl::Registration< PointSource, PointTarget, float >
 Registration ()
 Empty constructor. More...
 
virtual ~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...
 
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 (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. More...
 
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) More...
 
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) More...
 
bool hasConverged ()
 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 initalization. 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 ()
 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 (size_t row_start, size_t col_start, size_t nb_rows, size_t nb_cols)
 Set the indices for the points laying within an interest region of the point cloud. More...
 
IndicesPtr const 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[] (size_t pos) const
 Override PointCloud operator[] to shorten code. More...
 

Protected Member Functions

template<typename PointT >
void computeCovariances (typename pcl::PointCloud< PointT >::ConstPtr cloud, const typename pcl::search::KdTree< PointT >::Ptr tree, MatricesVector &cloud_covariances)
 compute points covariances matrices according to the K nearest neighbors. More...
 
double matricesInnerProd (const Eigen::MatrixXd &mat1, const Eigen::MatrixXd &mat2) const
 
void computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess)
 Rigid transformation computation method with initial guess. More...
 
bool searchForNeighbors (const PointSource &query, std::vector< int > &index, std::vector< float > &distance)
 Search for the closest nearest neighbor of a given point. More...
 
void applyState (Eigen::Matrix4f &t, const Vector6d &x) const
 compute transformation matrix from transformation matrix More...
 
- Protected Member Functions inherited from pcl::IterativeClosestPoint< PointSource, PointTarget >
virtual void transformCloud (const PointCloudSource &input, PointCloudSource &output, const Matrix4 &transform)
 Apply a rigid transform to a given dataset. More...
 
virtual void computeTransformation (PointCloudSource &output, const Matrix4 &guess)
 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, std::vector< int > &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

int k_correspondences_
 The number of neighbors used for covariances computation. More...
 
double gicp_epsilon_
 The epsilon constant for gicp paper; this is NOT the convergence tolerence default: 0.001. More...
 
double rotation_epsilon_
 The epsilon constant for rotation error. More...
 
Eigen::Matrix4f base_transformation_
 base transformation More...
 
const PointCloudSourcetmp_src_
 Temporary pointer to the source dataset. More...
 
const PointCloudTargettmp_tgt_
 Temporary pointer to the target dataset. More...
 
const std::vector< int > * tmp_idx_src_
 Temporary pointer to the source dataset indices. More...
 
const std::vector< int > * tmp_idx_tgt_
 Temporary pointer to the target dataset indices. More...
 
MatricesVectorPtr input_covariances_
 Input cloud points covariances. More...
 
MatricesVectorPtr target_covariances_
 Target cloud points covariances. More...
 
std::vector< Eigen::Matrix3d > mahalanobis_
 Mahalanobis matrices holder. More...
 
int max_inner_iterations_
 maximum number of optimizations More...
 
boost::function< void(const
pcl::PointCloud< PointSource >
&cloud_src, const std::vector
< int > &src_indices, const
pcl::PointCloud< PointTarget >
&cloud_tgt, const std::vector
< int > &tgt_indices,
Eigen::Matrix4f
&transformation_matrix)> 
rigid_transformation_estimation_
 
- Protected Attributes inherited from pcl::IterativeClosestPoint< PointSource, PointTarget >
size_t x_idx_offset_
 XYZ fields offset. More...
 
size_t y_idx_offset_
 
size_t z_idx_offset_
 
size_t nx_idx_offset_
 Normal fields offset. More...
 
size_t ny_idx_offset_
 
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...
 
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...
 
boost::function< void(const
pcl::PointCloud< PointSource >
&cloud_src, const std::vector
< int > &indices_src, const
pcl::PointCloud< PointTarget >
&cloud_tgt, const std::vector
< int > &indices_tgt)> 
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 >
pcl::registration::DefaultConvergenceCriteria
< float >::Ptr 
convergence_criteria_
 

Detailed Description

template<typename PointSource, typename PointTarget>
class pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >

GeneralizedIterativeClosestPoint is an ICP variant that implements the generalized iterative closest point algorithm as described by Alex Segal et al.

in http://www.robots.ox.ac.uk/~avsegal/resources/papers/Generalized_ICP.pdf The approach is based on using anistropic cost functions to optimize the alignment after closest point assignments have been made. The original code uses GSL and ANN while in ours we use an eigen mapped BFGS and FLANN.

Author
Nizar Sallem

Definition at line 60 of file gicp.h.

Member Typedef Documentation

template<typename PointSource , typename PointTarget >
typedef boost::shared_ptr< const GeneralizedIterativeClosestPoint<PointSource, PointTarget> > pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::ConstPtr

Definition at line 101 of file gicp.h.

template<typename PointSource , typename PointTarget >
typedef Registration<PointSource, PointTarget>::KdTree pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::InputKdTree

Definition at line 97 of file gicp.h.

template<typename PointSource , typename PointTarget >
typedef Registration<PointSource, PointTarget>::KdTreePtr pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::InputKdTreePtr

Definition at line 98 of file gicp.h.

template<typename PointSource , typename PointTarget >
typedef std::vector< Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d> > pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::MatricesVector

Definition at line 93 of file gicp.h.

template<typename PointSource , typename PointTarget >
typedef boost::shared_ptr< const MatricesVector > pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::MatricesVectorConstPtr

Definition at line 95 of file gicp.h.

template<typename PointSource , typename PointTarget >
typedef boost::shared_ptr< MatricesVector > pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::MatricesVectorPtr

Definition at line 94 of file gicp.h.

template<typename PointSource , typename PointTarget >
typedef pcl::PointCloud<PointSource> pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::PointCloudSource

Definition at line 82 of file gicp.h.

template<typename PointSource , typename PointTarget >
typedef PointCloudSource::ConstPtr pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::PointCloudSourceConstPtr

Definition at line 84 of file gicp.h.

template<typename PointSource , typename PointTarget >
typedef PointCloudSource::Ptr pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::PointCloudSourcePtr

Definition at line 83 of file gicp.h.

template<typename PointSource , typename PointTarget >
typedef pcl::PointCloud<PointTarget> pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::PointCloudTarget

Definition at line 86 of file gicp.h.

template<typename PointSource , typename PointTarget >
typedef PointCloudTarget::ConstPtr pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::PointCloudTargetConstPtr

Definition at line 88 of file gicp.h.

template<typename PointSource , typename PointTarget >
typedef PointCloudTarget::Ptr pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::PointCloudTargetPtr

Definition at line 87 of file gicp.h.

template<typename PointSource , typename PointTarget >
typedef PointIndices::ConstPtr pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::PointIndicesConstPtr

Definition at line 91 of file gicp.h.

template<typename PointSource , typename PointTarget >
typedef PointIndices::Ptr pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::PointIndicesPtr

Definition at line 90 of file gicp.h.

template<typename PointSource , typename PointTarget >
typedef boost::shared_ptr< GeneralizedIterativeClosestPoint<PointSource, PointTarget> > pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::Ptr

Definition at line 100 of file gicp.h.

template<typename PointSource , typename PointTarget >
typedef Eigen::Matrix<double, 6, 1> pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::Vector6d

Definition at line 104 of file gicp.h.

Constructor & Destructor Documentation

template<typename PointSource , typename PointTarget >
pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::GeneralizedIterativeClosestPoint ( )
inline

Member Function Documentation

template<typename PointSource , typename PointTarget >
void pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::applyState ( Eigen::Matrix4f &  t,
const Vector6d x 
) const
protected

compute transformation matrix from transformation matrix

Definition at line 468 of file gicp.hpp.

Referenced by pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::OptimizationFunctorWithIndices::operator()().

template<typename PointSource , typename PointTarget >
template<typename PointT >
void pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::computeCovariances ( typename pcl::PointCloud< PointT >::ConstPtr  cloud,
const typename pcl::search::KdTree< PointT >::Ptr  tree,
MatricesVector cloud_covariances 
)
protected

compute points covariances matrices according to the K nearest neighbors.

K is set via setCorrespondenceRandomness() methode.

Parameters
cloudpointer to point cloud
treeKD tree performer for nearest neighbors search
[out]cloud_covariancescovariances matrices for each point in the cloud

Definition at line 49 of file gicp.hpp.

References pcl::PointCloud< T >::begin(), pcl::PointCloud< T >::end(), pcl::search::KdTree< PointT, Tree >::nearestKSearch(), and pcl::PointCloud< T >::size().

template<typename PointSource , typename PointTarget >
void pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::computeRDerivative ( const Vector6d x,
const Eigen::Matrix3d &  R,
Vector6d g 
) const

Computes rotation matrix derivative.

rotation matrix is obtainded from rotation angles x[3], x[4] and x[5]

Returns
d/d_rx, d/d_ry and d/d_rz respectively in g[3], g[4] and g[5] param x array representing 3D transformation param R rotation matrix param g gradient vector

Definition at line 127 of file gicp.hpp.

template<typename PointSource , typename PointTarget >
void pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::computeTransformation ( PointCloudSource output,
const Eigen::Matrix4f &  guess 
)
inlineprotected

Rigid transformation computation method with initial guess.

Parameters
outputthe transformed input point cloud dataset using the rigid transformation found
guessthe initial guess of the transformation to compute

Definition at line 345 of file gicp.hpp.

References pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::base_transformation_, pcl::Registration< PointSource, PointTarget, float >::converged_, pcl::Registration< PointSource, PointTarget, float >::corr_dist_threshold_, pcl::Registration< PointSource, PointTarget, float >::final_transformation_, pcl::Registration< PointSource, PointTarget, float >::getClassName(), pcl::PCLBase< PointSource >::indices_, pcl::Registration< PointSource, PointTarget, Scalar >::initComputeReciprocal(), pcl::PCLBase< PointSource >::input_, pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::input_covariances_, pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::mahalanobis_, pcl::Registration< PointSource, PointTarget, float >::max_iterations_, pcl::Registration< PointSource, PointTarget, float >::nr_iterations_, pcl::Registration< PointSource, PointTarget, float >::previous_transformation_, pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::rigid_transformation_estimation_, pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::rotation_epsilon_, pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::searchForNeighbors(), pcl::Registration< PointSource, PointTarget, float >::target_, pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::target_covariances_, pcl::Registration< PointSource, PointTarget, float >::transformation_, pcl::Registration< PointSource, PointTarget, float >::transformation_epsilon_, pcl::transformPointCloud(), pcl::Registration< PointSource, PointTarget, float >::tree_, and pcl::Registration< PointSource, PointTarget, float >::tree_reciprocal_.

template<typename PointSource , typename PointTarget >
void pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::estimateRigidTransformationBFGS ( const PointCloudSource cloud_src,
const std::vector< int > &  indices_src,
const PointCloudTarget cloud_tgt,
const std::vector< int > &  indices_tgt,
Eigen::Matrix4f &  transformation_matrix 
)

Estimate a rigid rotation transformation between a source and a target point cloud using an iterative non-linear Levenberg-Marquardt approach.

Parameters
[in]cloud_srcthe source point cloud dataset
[in]indices_srcthe vector of indices describing the points of interest in cloud_src
[in]cloud_tgtthe target point cloud dataset
[in]indices_tgtthe vector of indices describing the correspondences of the interst points from indices_src
[out]transformation_matrixthe resultant transformation matrix

Definition at line 182 of file gicp.hpp.

References BFGS< FunctorType >::minimizeInit(), BFGS< FunctorType >::minimizeOneStep(), BFGSSpace::NoProgress, BFGS< FunctorType >::parameters, BFGSSpace::Running, BFGSSpace::Success, and BFGS< FunctorType >::testGradient().

template<typename PointSource , typename PointTarget >
int pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::getCorrespondenceRandomness ( )
inline

Get the number of neighbors used when computing covariances as set by the user.

Definition at line 236 of file gicp.h.

References pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::k_correspondences_.

template<typename PointSource , typename PointTarget >
int pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::getMaximumOptimizerIterations ( )
inline
Returns
maximum number of iterations at the optimization step

Definition at line 246 of file gicp.h.

References pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::max_inner_iterations_.

template<typename PointSource , typename PointTarget >
double pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::getRotationEpsilon ( )
inline

Get the rotation epsilon (maximum allowable difference between two consecutive rotations) as set by the user.

Definition at line 221 of file gicp.h.

References pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::rotation_epsilon_.

template<typename PointSource , typename PointTarget >
const Eigen::Matrix3d& pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::mahalanobis ( size_t  index) const
inline
template<typename PointSource , typename PointTarget >
double pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::matricesInnerProd ( const Eigen::MatrixXd &  mat1,
const Eigen::MatrixXd &  mat2 
) const
inlineprotected
Returns
trace of mat1^t . mat2
Parameters
mat1matrix of dimension nxm
mat2matrix of dimension nxp

Definition at line 311 of file gicp.h.

template<typename PointSource , typename PointTarget >
bool pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::searchForNeighbors ( const PointSource &  query,
std::vector< int > &  index,
std::vector< float > &  distance 
)
inlineprotected

Search for the closest nearest neighbor of a given point.

Parameters
querythe point to search a nearest neighbour for
indexvector of size 1 to store the index of the nearest neighbour found
distancevector of size 1 to store the distance to nearest neighbour found

Definition at line 335 of file gicp.h.

References pcl::Registration< PointSource, PointTarget, float >::tree_.

Referenced by pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::computeTransformation().

template<typename PointSource , typename PointTarget >
void pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::setCorrespondenceRandomness ( int  k)
inline

Set the number of neighbors used when selecting a point neighbourhood to compute covariances.

A higher value will bring more accurate covariance matrix but will make covariances computation slower.

Parameters
kthe number of neighbors to use when computing covariances

Definition at line 230 of file gicp.h.

References pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::k_correspondences_.

template<typename PointSource , typename PointTarget >
void pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::setInputSource ( const PointCloudSourceConstPtr cloud)
inlinevirtual
template<typename PointSource , typename PointTarget >
void pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::setInputTarget ( const PointCloudTargetConstPtr target)
inlinevirtual

Provide a pointer to the input target (e.g., the point cloud that we want to align the input source to)

Parameters
[in]targetthe input point cloud target

Reimplemented from pcl::Registration< PointSource, PointTarget, float >.

Definition at line 160 of file gicp.h.

References pcl::IterativeClosestPoint< PointSource, PointTarget, Scalar >::setInputTarget(), and pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::target_covariances_.

template<typename PointSource , typename PointTarget >
void pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::setMaximumOptimizerIterations ( int  max)
inline

set maximum number of iterations at the optimization step

Parameters
[in]maxmaximum number of iterations for the optimizer

Definition at line 242 of file gicp.h.

References pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::max_inner_iterations_.

template<typename PointSource , typename PointTarget >
void pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::setRotationEpsilon ( double  epsilon)
inline

Set the rotation epsilon (maximum allowable difference between two consecutive rotations) in order for an optimization to be considered as having converged to the final solution.

Parameters
epsilonthe rotation epsilon

Definition at line 215 of file gicp.h.

References pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::rotation_epsilon_.

template<typename PointSource , typename PointTarget >
void pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::setSourceCovariances ( const MatricesVectorPtr covariances)
inline

Provide a pointer to the covariances of the input source (if computed externally!).

If not set, GeneralizedIterativeClosestPoint will compute the covariances itself. Make sure to set the covariances AFTER setting the input source point cloud (setting the input source point cloud will reset the covariances).

Parameters
[in]targetthe input point cloud target

Definition at line 151 of file gicp.h.

References pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::input_covariances_.

template<typename PointSource , typename PointTarget >
void pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::setTargetCovariances ( const MatricesVectorPtr covariances)
inline

Provide a pointer to the covariances of the input target (if computed externally!).

If not set, GeneralizedIterativeClosestPoint will compute the covariances itself. Make sure to set the covariances AFTER setting the input source point cloud (setting the input source point cloud will reset the covariances).

Parameters
[in]targetthe input point cloud target

Definition at line 172 of file gicp.h.

References pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::target_covariances_.

Member Data Documentation

template<typename PointSource , typename PointTarget >
Eigen::Matrix4f pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::base_transformation_
protected
template<typename PointSource , typename PointTarget >
double pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::gicp_epsilon_
protected

The epsilon constant for gicp paper; this is NOT the convergence tolerence default: 0.001.

Definition at line 259 of file gicp.h.

template<typename PointSource , typename PointTarget >
MatricesVectorPtr pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::input_covariances_
protected
template<typename PointSource , typename PointTarget >
int pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::k_correspondences_
protected
template<typename PointSource , typename PointTarget >
std::vector<Eigen::Matrix3d> pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::mahalanobis_
protected
template<typename PointSource , typename PointTarget >
int pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::max_inner_iterations_
protected
template<typename PointSource , typename PointTarget >
boost::function<void(const pcl::PointCloud<PointSource> &cloud_src, const std::vector<int> &src_indices, const pcl::PointCloud<PointTarget> &cloud_tgt, const std::vector<int> &tgt_indices, Eigen::Matrix4f &transformation_matrix)> pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::rigid_transformation_estimation_
protected
template<typename PointSource , typename PointTarget >
double pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::rotation_epsilon_
protected

The epsilon constant for rotation error.

(In GICP the transformation epsilon is split in rotation part and translation part). default: 2e-3

Definition at line 265 of file gicp.h.

Referenced by pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::computeTransformation(), pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::getRotationEpsilon(), and pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::setRotationEpsilon().

template<typename PointSource , typename PointTarget >
MatricesVectorPtr pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::target_covariances_
protected
template<typename PointSource , typename PointTarget >
const std::vector<int>* pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::tmp_idx_src_
protected

Temporary pointer to the source dataset indices.

Definition at line 277 of file gicp.h.

Referenced by pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::OptimizationFunctorWithIndices::operator()().

template<typename PointSource , typename PointTarget >
const std::vector<int>* pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::tmp_idx_tgt_
protected

Temporary pointer to the target dataset indices.

Definition at line 280 of file gicp.h.

Referenced by pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::OptimizationFunctorWithIndices::operator()().

template<typename PointSource , typename PointTarget >
const PointCloudSource* pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::tmp_src_
protected

Temporary pointer to the source dataset.

Definition at line 271 of file gicp.h.

Referenced by pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::OptimizationFunctorWithIndices::operator()().

template<typename PointSource , typename PointTarget >
const PointCloudTarget* pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::tmp_tgt_
protected

Temporary pointer to the target dataset.

Definition at line 274 of file gicp.h.

Referenced by pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::OptimizationFunctorWithIndices::operator()().


The documentation for this class was generated from the following files: