Classes | Public Member Functions | Protected Attributes | Private Types | Private Member Functions | Private 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. in http://www.stanford.edu/~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. More...

#include <gicp.h>

Inheritance diagram for pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >:
Inheritance graph
[legend]

List of all members.

Classes

struct  OptimizationFunctorWithIndices
 optimization functor structure More...

Public Member Functions

void 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].
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.
 GeneralizedIterativeClosestPoint ()
 Empty constructor.
int getCorrespondenceRandomness ()
 Get the number of neighbors used when computing covariances as set by the user.
int getMaximumOptimizerIterations ()
double getRotationEpsilon ()
 Get the rotation epsilon (maximum allowable difference between two consecutive rotations) as set by the user.
const Eigen::Matrix3d & mahalanobis (size_t index) const
void setCorrespondenceRandomness (int k)
 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.
void setInputCloud (const PointCloudSourceConstPtr &cloud)
 Provide a pointer to the input dataset.
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)
void setMaximumOptimizerIterations (int max)
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.

Protected Attributes

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_

Private Types

typedef pcl::KdTree< PointSource > InputKdTree
typedef pcl::KdTree
< PointSource >::Ptr 		InputKdTreePtr
typedef pcl::PointCloud
< PointSource > 		PointCloudSource
typedef PointCloudSource::ConstPtr PointCloudSourceConstPtr
typedef PointCloudSource::Ptr PointCloudSourcePtr
typedef pcl::PointCloud
< PointTarget > 		PointCloudTarget
typedef PointCloudTarget::ConstPtr PointCloudTargetConstPtr
typedef PointCloudTarget::Ptr PointCloudTargetPtr
typedef PointIndices::ConstPtr PointIndicesConstPtr
typedef PointIndices::Ptr PointIndicesPtr
typedef Eigen::Matrix< double, 6, 1 > Vector6d

Private Member Functions

void applyState (Eigen::Matrix4f &t, const Vector6d &x) const
 compute transformation matrix from transformation matrix
template<typename PointT >
void computeCovariances (typename pcl::PointCloud< PointT >::ConstPtr cloud, const typename pcl::KdTree< PointT >::Ptr tree, std::vector< Eigen::Matrix3d > &cloud_covariances)
 compute points covariances matrices according to the K nearest neighbors. K is set via setCorrespondenceRandomness() methode.
void computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess)
 Rigid transformation computation method with initial guess.
double matricesInnerProd (const Eigen::MatrixXd &mat1, const Eigen::MatrixXd &mat2) const
bool searchForNeighbors (const PointSource &query, std::vector< int > &index, std::vector< float > &distance)
 Search for the closest nearest neighbor of a given point.

Private Attributes

Eigen::Matrix4f base_transformation_
 base transformation
double gicp_epsilon_
 The epsilon constant for gicp paper; this is NOT the convergence tolerence 0.001.
std::vector< Eigen::Matrix3d > input_covariances_
 Input cloud points covariances.
InputKdTreePtr input_tree_
 KD tree pointer of the input cloud.
int k_correspondences_
 The number of neighbors used for covariances computation. 20.
std::vector< Eigen::Matrix3d > mahalanobis_
 Mahalanobis matrices holder.
int max_inner_iterations_
 maximum number of optimizations
double rotation_epsilon_
std::vector< Eigen::Matrix3d > target_covariances_
 Target cloud points covariances.
const std::vector< int > * tmp_idx_src_
 Temporary pointer to the source dataset indices.
const std::vector< int > * tmp_idx_tgt_
 Temporary pointer to the target dataset indices.
const PointCloudSource * tmp_src_
 Temporary pointer to the source dataset.
const PointCloudTarget * tmp_tgt_
 Temporary pointer to the target dataset.

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.stanford.edu/~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 57 of file gicp.h.

Member Typedef Documentation

template<typename PointSource , typename PointTarget >
typedef pcl::KdTree<PointSource> pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::InputKdTree [private]

Definition at line 88 of file gicp.h.

template<typename PointSource , typename PointTarget >
typedef pcl::KdTree<PointSource>::Ptr pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::InputKdTreePtr [private]

Definition at line 89 of file gicp.h.

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

Reimplemented from pcl::IterativeClosestPoint< PointSource, PointTarget >.

Definition at line 77 of file gicp.h.

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

Reimplemented from pcl::IterativeClosestPoint< PointSource, PointTarget >.

Definition at line 79 of file gicp.h.

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

Reimplemented from pcl::IterativeClosestPoint< PointSource, PointTarget >.

Definition at line 78 of file gicp.h.

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

Reimplemented from pcl::IterativeClosestPoint< PointSource, PointTarget >.

Definition at line 81 of file gicp.h.

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

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

Definition at line 83 of file gicp.h.

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

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

Definition at line 82 of file gicp.h.

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

Reimplemented from pcl::IterativeClosestPoint< PointSource, PointTarget >.

Definition at line 86 of file gicp.h.

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

Reimplemented from pcl::IterativeClosestPoint< PointSource, PointTarget >.

Definition at line 85 of file gicp.h.

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

Definition at line 91 of file gicp.h.

Constructor & Destructor Documentation

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

Empty constructor.

Definition at line 95 of file gicp.h.

Member Function Documentation

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

compute transformation matrix from transformation matrix

Definition at line 455 of file gicp.hpp.

template<typename PointSource , typename PointTarget >
template<typename PointT >
void pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::computeCovariances ( typename pcl::PointCloud< PointT >::ConstPtr  cloud,
const typename pcl::KdTree< PointT >::Ptr  tree,
std::vector< Eigen::Matrix3d > &  cloud_covariances 
) [private]

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
Returns:
cloud_covariance covariances matrices for each point in the cloud

Definition at line 44 of file gicp.hpp.

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 122 of file gicp.hpp.

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

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 339 of file gicp.hpp.

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 177 of file gicp.hpp.

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 205 of file gicp.h.

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

Definition at line 215 of file gicp.h.

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 190 of file gicp.h.

template<typename PointSource , typename PointTarget >
const Eigen::Matrix3d& pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::mahalanobis ( size_t  index) const [inline]
Returns:
Mahalanobis distance matrix for the given point index

Definition at line 162 of file gicp.h.

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

Definition at line 282 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 
) [inline, private]

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 306 of file gicp.h.

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 199 of file gicp.h.

template<typename PointSource , typename PointTarget >
void pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::setInputCloud ( const PointCloudSourceConstPtr &  cloud) [inline]

Provide a pointer to the input dataset.

Parameters:
cloudthe const boost shared pointer to a PointCloud message

Definition at line 119 of file gicp.h.

template<typename PointSource , typename PointTarget >
void pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::setInputTarget ( const PointCloudTargetConstPtr &  target) [inline]

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

Definition at line 140 of file gicp.h.

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 211 of file gicp.h.

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 184 of file gicp.h.

Member Data Documentation

template<typename PointSource , typename PointTarget >
Eigen::Matrix4f pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::base_transformation_ [private]

base transformation

Definition at line 237 of file gicp.h.

template<typename PointSource , typename PointTarget >
double pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::gicp_epsilon_ [private]

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

Definition at line 228 of file gicp.h.

template<typename PointSource , typename PointTarget >
std::vector<Eigen::Matrix3d> pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::input_covariances_ [private]

Input cloud points covariances.

Definition at line 255 of file gicp.h.

template<typename PointSource , typename PointTarget >
InputKdTreePtr pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::input_tree_ [private]

KD tree pointer of the input cloud.

Definition at line 252 of file gicp.h.

template<typename PointSource , typename PointTarget >
int pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::k_correspondences_ [private]

The number of neighbors used for covariances computation. 20.

Definition at line 222 of file gicp.h.

template<typename PointSource , typename PointTarget >
std::vector<Eigen::Matrix3d> pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::mahalanobis_ [private]

Mahalanobis matrices holder.

Definition at line 261 of file gicp.h.

template<typename PointSource , typename PointTarget >
int pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::max_inner_iterations_ [private]

maximum number of optimizations

Definition at line 264 of file gicp.h.

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]

Definition at line 334 of file gicp.h.

template<typename PointSource , typename PointTarget >
double pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::rotation_epsilon_ [private]

The epsilon constant for rotation error. (In GICP the transformation epsilon is split in rotation part and translation part). 2e-3

Definition at line 234 of file gicp.h.

template<typename PointSource , typename PointTarget >
std::vector<Eigen::Matrix3d> pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::target_covariances_ [private]

Target cloud points covariances.

Definition at line 258 of file gicp.h.

template<typename PointSource , typename PointTarget >
const std::vector<int>* pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::tmp_idx_src_ [private]

Temporary pointer to the source dataset indices.

Definition at line 246 of file gicp.h.

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

Temporary pointer to the target dataset indices.

Definition at line 249 of file gicp.h.

template<typename PointSource , typename PointTarget >
const PointCloudSource* pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::tmp_src_ [private]

Temporary pointer to the source dataset.

Definition at line 240 of file gicp.h.

template<typename PointSource , typename PointTarget >
const PointCloudTarget* pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::tmp_tgt_ [private]

Temporary pointer to the target dataset.

Definition at line 243 of file gicp.h.

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

pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:19:30