Template Class GeneralizedIterativeClosestPoint

Nested Relationships

Nested Types

Inheritance Relationships

Base Type

  • public pcl::IterativeClosestPoint< PointSource, PointTarget >

Class Documentation

template<typename PointSource, typename PointTarget>
class GeneralizedIterativeClosestPoint : public pcl::IterativeClosestPoint<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 anisotropic 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

Public Types

using PointCloudSource = pcl::PointCloud<PointSource>
using PointCloudSourcePtr = typename PointCloudSource::Ptr
using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr
using PointCloudTarget = pcl::PointCloud<PointTarget>
using PointCloudTargetPtr = typename PointCloudTarget::Ptr
using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr
using PointIndicesPtr = pcl::PointIndices::Ptr
using PointIndicesConstPtr = pcl::PointIndices::ConstPtr
using InputKdTree = typename pcl::Registration<PointSource, PointTarget>::KdTree
using InputKdTreePtr = typename pcl::Registration<PointSource, PointTarget>::KdTreePtr
using MatricesVector = std::vector<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d>>
using MatricesVectorPtr = pcl::shared_ptr<MatricesVector>
using MatricesVectorConstPtr = pcl::shared_ptr<const MatricesVector>
using Ptr = pcl::shared_ptr<GeneralizedIterativeClosestPoint<PointSource, PointTarget>>
using ConstPtr = pcl::shared_ptr<const GeneralizedIterativeClosestPoint<PointSource, PointTarget>>
using Vector6d = Eigen::Matrix<double, 6, 1>

Public Functions

inline GeneralizedIterativeClosestPoint()

Empty constructor.

inline void setInputSource(const PointCloudSourceConstPtr &cloud) override

Provide a pointer to the input dataset.

Parameters:

cloud – the const boost shared pointer to a PointCloud message

inline void setSourceCovariances(const MatricesVectorPtr &covariances)

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:

target – [in] the input point cloud target

inline void setInputTarget(const PointCloudTargetConstPtr &target) override

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

Parameters:

target – [in] the input point cloud target

inline void setTargetCovariances(const MatricesVectorPtr &covariances)

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:

target – [in] the input point cloud target

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.

Parameters:

  • cloud_src – [in] the source point cloud dataset

  • indices_src – [in] the vector of indices describing the points of interest in cloud_src

  • cloud_tgt – [in] the target point cloud dataset

  • indices_tgt – [in] the vector of indices describing the correspondences of the interest points from indices_src

  • transformation_matrix – [out] the resultant transformation matrix

inline const Eigen::Matrix4f &mahalanobis(size_t index) const
Returns:

Mahalanobis distance matrix for the given point index

void computeRDerivative(const Vector6d &x, const Eigen::Matrix3d &R, Vector6d &g) const

Computes rotation matrix derivative. rotation matrix is obtained 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

inline 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.

Parameters:

epsilon – the rotation epsilon

inline double getRotationEpsilon()

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

inline 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.

Parameters:

k – the number of neighbors to use when computing covariances

inline int getCorrespondenceRandomness()

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

inline void setMaximumOptimizerIterations(int max)

set maximum number of iterations at the optimization step

Parameters:

max – [in] maximum number of iterations for the optimizer

inline int getMaximumOptimizerIterations()
Returns:

maximum number of iterations at the optimization step

Protected Functions

template<typename PointT>
void computeCovariances(typename pcl::PointCloud<PointT>::ConstPtr cloud, const typename pcl::search::KdTree<PointT>::ConstPtr tree, MatricesVector &cloud_covariances)

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

Parameters:

  • cloud – pointer to point cloud

  • tree – KD tree performer for nearest neighbors search

  • cloud_covariances – [out] covariances matrices for each point in the cloud

inline double matricesInnerProd(const Eigen::MatrixXd &mat1, const Eigen::MatrixXd &mat2) const
Parameters:

  • mat1 – matrix of dimension nxm

  • mat2 – matrix of dimension nxp

Returns:

trace of mat1^t . mat2

inline void computeTransformation(PointCloudSource &output, const Eigen::Matrix4f &guess) override

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

inline bool searchForNeighbors(const PointSource &query, std::vector<int> &index, std::vector<float> &distance) const

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

void applyState(Eigen::Matrix4f &t, const Vector6d &x) const

compute transformation matrix from transformation matrix

Protected Attributes

int k_correspondences_

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

double gicp_epsilon_

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

double rotation_epsilon_

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

Eigen::Matrix4f base_transformation_

base transformation

const PointCloudSource *tmp_src_

Temporary pointer to the source dataset.

const PointCloudTarget *tmp_tgt_

Temporary pointer to the target dataset.

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.

MatricesVectorPtr input_covariances_

Input cloud points covariances.

MatricesVectorPtr target_covariances_

Target cloud points covariances.

std::vector<Eigen::Matrix4f> mahalanobis_

Mahalanobis matrices holder.

int max_inner_iterations_

maximum number of optimizations

std::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_
struct OptimizationFunctorWithIndices : public BFGSDummyFunctor<double, 6>

optimization functor structure

Public Functions

inline OptimizationFunctorWithIndices(const GeneralizedIterativeClosestPoint *gicp)
inline double operator()(const Vector6d &x) override
inline void df(const Vector6d &x, Vector6d &df) override
inline void fdf(const Vector6d &x, double &f, Vector6d &df) override

Public Members

const GeneralizedIterativeClosestPoint *gicp_