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 >:

[legend]

List of all members.

Classes
struct	OptimizationFunctorWithIndices
	optimization functor structure More...
Public Types
typedef boost::shared_ptr < const GeneralizedIterativeClosestPoint < PointSource, PointTarget > >	ConstPtr
typedef Registration < PointSource, PointTarget > ::KdTree	InputKdTree
typedef Registration < PointSource, PointTarget > ::KdTreePtr	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 boost::shared_ptr < GeneralizedIterativeClosestPoint < PointSource, PointTarget > >	Ptr
typedef Eigen::Matrix< double, 6, 1 >	Vector6d
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
	PCL_DEPRECATED (void setInputCloud(const PointCloudSourceConstPtr &cloud),"[pcl::registration::GeneralizedIterativeClosestPoint::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead.")
	Provide a pointer to the input dataset.
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	setInputSource (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 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::search::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.
Protected Attributes
Eigen::Matrix4f	base_transformation_
	base transformation
double	gicp_epsilon_
	The epsilon constant for gicp paper; this is NOT the convergence tolerence default: 0.001.
std::vector< Eigen::Matrix3d >	input_covariances_
	Input cloud points covariances.
int	k_correspondences_
	The number of neighbors used for covariances computation. default: 20.
std::vector< Eigen::Matrix3d >	mahalanobis_
	Mahalanobis matrices holder.
int	max_inner_iterations_
	maximum number of optimizations
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_
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 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

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

Definition at line 97 of file gicp.h.

template<typename PointSource, typename PointTarget>

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

Definition at line 93 of file gicp.h.

template<typename PointSource, typename PointTarget>

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

Definition at line 94 of file gicp.h.

template<typename PointSource, typename PointTarget>

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

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

Definition at line 82 of file gicp.h.

template<typename PointSource, typename PointTarget>

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

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

Definition at line 84 of file gicp.h.

template<typename PointSource, typename PointTarget>

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

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

Definition at line 83 of file gicp.h.

template<typename PointSource, typename PointTarget>

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

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

Definition at line 86 of file gicp.h.

template<typename PointSource, typename PointTarget>

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

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

Definition at line 88 of file gicp.h.

template<typename PointSource, typename PointTarget>

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

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

Definition at line 87 of file gicp.h.

template<typename PointSource, typename PointTarget>

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

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

Definition at line 91 of file gicp.h.

template<typename PointSource, typename PointTarget>

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

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

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

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

Definition at line 96 of file gicp.h.

template<typename PointSource, typename PointTarget>

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

Definition at line 100 of file gicp.h.

Constructor & Destructor Documentation

template<typename PointSource, typename PointTarget>

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

Empty constructor.

Definition at line 103 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 `[protected]`

compute transformation matrix from transformation matrix

Definition at line 469 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::search::KdTree< PointT >::Ptr	tree,
		std::vector< Eigen::Matrix3d > &	cloud_covariances
	)		`[protected]`

compute points covariances matrices according to the K nearest neighbors. K is set via setCorrespondenceRandomness() methode.

Parameters:

cloud	pointer to point cloud
tree	KD tree performer for nearest neighbors search

Returns:: cloud_covariance covariances matrices for each point in the cloud

Definition at line 57 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 135 of file gicp.hpp.

template<typename PointSource , typename PointTarget >

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

Rigid transformation computation method with initial guess.

Parameters:

output	the transformed input point cloud dataset using the rigid transformation found
guess	the initial guess of the transformation to compute

Definition at line 352 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_src	the source point cloud dataset
[in]	indices_src	the vector of indices describing the points of interest in cloud_src
[in]	cloud_tgt	the target point cloud dataset
[in]	indices_tgt	the vector of indices describing the correspondences of the interst points from indices_src
[out]	transformation_matrix	the resultant transformation matrix

Definition at line 190 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 217 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 227 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 202 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 174 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, protected]`

Returns:: trace of mat1^t . mat2

Parameters:

mat1	matrix of dimension nxm
mat2	matrix of dimension nxp

Definition at line 292 of file gicp.h.

template<typename PointSource, typename PointTarget>

pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::PCL_DEPRECATED	(	void	setInputCloudconst PointCloudSourceConstPtr &cloud,
		" setInputCloud is deprecated. Please use setInputSource instead."	[pcl::registration::GeneralizedIterativeClosestPoint::setInputCloud]
	)

Provide a pointer to the input dataset.

Parameters:

cloud the const boost shared pointer to a PointCloud message

template<typename PointSource, typename PointTarget>

bool pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::searchForNeighbors	(	const PointSource &	query,
		std::vector< int > &	index,
		std::vector< float > &	distance
	)		`[inline, protected]`

Search for the closest nearest neighbor of a given point.

Parameters:

query	the point to search a nearest neighbour for
index	vector of size 1 to store the index of the nearest neighbour found
distance	vector of size 1 to store the distance to nearest neighbour found

Definition at line 316 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:

k	the number of neighbors to use when computing covariances

Definition at line 211 of file gicp.h.

template<typename PointSource, typename PointTarget>

void pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::setInputSource ( const PointCloudSourceConstPtr & cloud ) [inline, virtual]

Provide a pointer to the input dataset.

Parameters:

cloud the const boost shared pointer to a PointCloud message

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

Definition at line 131 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] target the input point cloud target

Definition at line 152 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] max maximum number of iterations for the optimizer

Definition at line 223 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:

epsilon the rotation epsilon

Definition at line 196 of file gicp.h.

Member Data Documentation

template<typename PointSource, typename PointTarget>

Eigen::Matrix4f pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::base_transformation_ [protected]

base transformation

Definition at line 249 of file gicp.h.

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

template<typename PointSource, typename PointTarget>

std::vector<Eigen::Matrix3d> pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::input_covariances_ [protected]

Input cloud points covariances.

Definition at line 265 of file gicp.h.

template<typename PointSource, typename PointTarget>

int pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::k_correspondences_ [protected]

The number of neighbors used for covariances computation. default: 20.

Definition at line 234 of file gicp.h.

template<typename PointSource, typename PointTarget>

std::vector<Eigen::Matrix3d> pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::mahalanobis_ [protected]

Mahalanobis matrices holder.

Definition at line 271 of file gicp.h.

template<typename PointSource, typename PointTarget>

int pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::max_inner_iterations_ [protected]

maximum number of optimizations

Definition at line 274 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 343 of file gicp.h.

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

template<typename PointSource, typename PointTarget>

std::vector<Eigen::Matrix3d> pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::target_covariances_ [protected]

Target cloud points covariances.

Definition at line 268 of file gicp.h.

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

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

template<typename PointSource, typename PointTarget>

const PointCloudSource* pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::tmp_src_ [protected]

Temporary pointer to the source dataset.

Definition at line 252 of file gicp.h.

template<typename PointSource, typename PointTarget>

const PointCloudTarget* pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >::tmp_tgt_ [protected]

Temporary pointer to the target dataset.

Definition at line 255 of file gicp.h.

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

Classes

Public Types

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

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

Member Typedef Documentation

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation

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