Point Cloud sampling based on the 6D covariances. It selects the points such that the resulting cloud is as stable as possible for being registered (against a copy of itself) with ICP. The algorithm adds points to the resulting cloud incrementally, while trying to keep all the 6 eigenvalues of the covariance matrix as close to each other as possible. This class also comes with the computeConditionNumber method that returns a number which shows how stable a point cloud will be when used as input for ICP (the closer the value it is to 1.0, the better). More...
#include <covariance_sampling.h>
Public Types | |
typedef boost::shared_ptr < const CovarianceSampling < PointT, PointNT > > | ConstPtr |
typedef boost::shared_ptr < CovarianceSampling< PointT, PointNT > > | Ptr |
Public Member Functions | |
double | computeConditionNumber () |
Compute the condition number of the input point cloud. The condition number is the ratio between the largest and smallest eigenvalues of the 6x6 covariance matrix of the cloud. The closer this number is to 1.0, the more stable the cloud is for ICP registration. | |
bool | computeCovarianceMatrix (Eigen::Matrix< double, 6, 6 > &covariance_matrix) |
Computes the covariance matrix of the input cloud. | |
CovarianceSampling () | |
Empty constructor. | |
NormalsConstPtr | getNormals () const |
Get the normals computed on the input point cloud. | |
unsigned int | getNumberOfSamples () const |
Get the value of the internal num_samples_ parameter. | |
void | setNormals (const NormalsConstPtr &normals) |
Set the normals computed on the input point cloud. | |
void | setNumberOfSamples (unsigned int samples) |
Set number of indices to be sampled. | |
Static Public Member Functions | |
static double | computeConditionNumber (const Eigen::Matrix< double, 6, 6 > &covariance_matrix) |
Compute the condition number of the input point cloud. The condition number is the ratio between the largest and smallest eigenvalues of the 6x6 covariance matrix of the cloud. The closer this number is to 1.0, the more stable the cloud is for ICP registration. | |
Protected Member Functions | |
void | applyFilter (Cloud &output) |
Sample of point indices into a separate PointCloud. | |
void | applyFilter (std::vector< int > &indices) |
Sample of point indices. | |
bool | initCompute () |
This method should get called before starting the actual computation. | |
Static Protected Member Functions | |
static bool | sort_dot_list_function (std::pair< int, double > a, std::pair< int, double > b) |
Protected Attributes | |
NormalsConstPtr | input_normals_ |
The normals computed at each point in the input cloud. | |
unsigned int | num_samples_ |
Number of indices that will be returned. | |
std::vector< Eigen::Vector3f > | scaled_points_ |
Private Types | |
typedef FilterIndices< PointT > ::PointCloud | Cloud |
typedef Cloud::ConstPtr | CloudConstPtr |
typedef Cloud::Ptr | CloudPtr |
typedef pcl::PointCloud < PointNT >::ConstPtr | NormalsConstPtr |
Point Cloud sampling based on the 6D covariances. It selects the points such that the resulting cloud is as stable as possible for being registered (against a copy of itself) with ICP. The algorithm adds points to the resulting cloud incrementally, while trying to keep all the 6 eigenvalues of the covariance matrix as close to each other as possible. This class also comes with the computeConditionNumber method that returns a number which shows how stable a point cloud will be when used as input for ICP (the closer the value it is to 1.0, the better).
Based on the following publication: * "Geometrically Stable Sampling for the ICP Algorithm" - N. Gelfand, L. Ikemoto, S. Rusinkiewicz, M. Levoy
Definition at line 62 of file covariance_sampling.h.
typedef FilterIndices<PointT>::PointCloud pcl::CovarianceSampling< PointT, PointNT >::Cloud [private] |
Definition at line 70 of file covariance_sampling.h.
typedef Cloud::ConstPtr pcl::CovarianceSampling< PointT, PointNT >::CloudConstPtr [private] |
Definition at line 72 of file covariance_sampling.h.
typedef Cloud::Ptr pcl::CovarianceSampling< PointT, PointNT >::CloudPtr [private] |
Definition at line 71 of file covariance_sampling.h.
typedef boost::shared_ptr< const CovarianceSampling<PointT, PointNT> > pcl::CovarianceSampling< PointT, PointNT >::ConstPtr |
Reimplemented from pcl::FilterIndices< PointT >.
Definition at line 77 of file covariance_sampling.h.
typedef pcl::PointCloud<PointNT>::ConstPtr pcl::CovarianceSampling< PointT, PointNT >::NormalsConstPtr [private] |
Definition at line 73 of file covariance_sampling.h.
typedef boost::shared_ptr< CovarianceSampling<PointT, PointNT> > pcl::CovarianceSampling< PointT, PointNT >::Ptr |
Reimplemented from pcl::FilterIndices< PointT >.
Definition at line 76 of file covariance_sampling.h.
pcl::CovarianceSampling< PointT, PointNT >::CovarianceSampling | ( | ) | [inline] |
Empty constructor.
Definition at line 80 of file covariance_sampling.h.
void pcl::CovarianceSampling< PointT, PointNT >::applyFilter | ( | Cloud & | output | ) | [protected, virtual] |
Sample of point indices into a separate PointCloud.
[out] | output | the resultant point cloud |
Implements pcl::Filter< PointT >.
Definition at line 259 of file covariance_sampling.hpp.
void pcl::CovarianceSampling< PointT, PointNT >::applyFilter | ( | std::vector< int > & | indices | ) | [protected, virtual] |
Sample of point indices.
[out] | indices | the resultant point cloud indices |
TODO figure out how to fill the candidate_indices - see subsequent paper paragraphs
Implements pcl::FilterIndices< PointT >.
Definition at line 159 of file covariance_sampling.hpp.
double pcl::CovarianceSampling< PointT, PointNT >::computeConditionNumber | ( | ) |
Compute the condition number of the input point cloud. The condition number is the ratio between the largest and smallest eigenvalues of the 6x6 covariance matrix of the cloud. The closer this number is to 1.0, the more stable the cloud is for ICP registration.
Definition at line 85 of file covariance_sampling.hpp.
double pcl::CovarianceSampling< PointT, PointNT >::computeConditionNumber | ( | const Eigen::Matrix< double, 6, 6 > & | covariance_matrix | ) | [static] |
Compute the condition number of the input point cloud. The condition number is the ratio between the largest and smallest eigenvalues of the 6x6 covariance matrix of the cloud. The closer this number is to 1.0, the more stable the cloud is for ICP registration.
[in] | covariance_matrix | user given covariance matrix |
Definition at line 113 of file covariance_sampling.hpp.
bool pcl::CovarianceSampling< PointT, PointNT >::computeCovarianceMatrix | ( | Eigen::Matrix< double, 6, 6 > & | covariance_matrix | ) |
Computes the covariance matrix of the input cloud.
[out] | covariance_matrix | the computed covariance matrix. |
Definition at line 137 of file covariance_sampling.hpp.
NormalsConstPtr pcl::CovarianceSampling< PointT, PointNT >::getNormals | ( | ) | const [inline] |
Get the normals computed on the input point cloud.
Definition at line 104 of file covariance_sampling.h.
unsigned int pcl::CovarianceSampling< PointT, PointNT >::getNumberOfSamples | ( | ) | const [inline] |
Get the value of the internal num_samples_ parameter.
Definition at line 92 of file covariance_sampling.h.
bool pcl::CovarianceSampling< PointT, PointNT >::initCompute | ( | ) | [protected] |
This method should get called before starting the actual computation.
Internally, initCompute() does the following:
Reimplemented from pcl::PCLBase< PointT >.
Definition at line 50 of file covariance_sampling.hpp.
void pcl::CovarianceSampling< PointT, PointNT >::setNormals | ( | const NormalsConstPtr & | normals | ) | [inline] |
Set the normals computed on the input point cloud.
[in] | normals | the normals computed for the input cloud |
Definition at line 99 of file covariance_sampling.h.
void pcl::CovarianceSampling< PointT, PointNT >::setNumberOfSamples | ( | unsigned int | samples | ) | [inline] |
Set number of indices to be sampled.
[in] | sample | the number of sample indices |
Definition at line 87 of file covariance_sampling.h.
static bool pcl::CovarianceSampling< PointT, PointNT >::sort_dot_list_function | ( | std::pair< int, double > | a, |
std::pair< int, double > | b | ||
) | [inline, static, protected] |
Definition at line 158 of file covariance_sampling.h.
NormalsConstPtr pcl::CovarianceSampling< PointT, PointNT >::input_normals_ [protected] |
The normals computed at each point in the input cloud.
Definition at line 138 of file covariance_sampling.h.
unsigned int pcl::CovarianceSampling< PointT, PointNT >::num_samples_ [protected] |
Number of indices that will be returned.
Definition at line 135 of file covariance_sampling.h.
std::vector<Eigen::Vector3f> pcl::CovarianceSampling< PointT, PointNT >::scaled_points_ [protected] |
Definition at line 140 of file covariance_sampling.h.