Pose estimation and alignment class using a prerejective RANSAC routine. More...
#include <sample_consensus_prerejective.h>
Public Types | |
typedef boost::shared_ptr < const SampleConsensusPrerejective < PointSource, PointTarget, FeatureT > > | ConstPtr |
typedef pcl::registration::CorrespondenceRejectorPoly < PointSource, PointTarget > | CorrespondenceRejectorPoly |
typedef CorrespondenceRejectorPoly::ConstPtr | CorrespondenceRejectorPolyConstPtr |
typedef CorrespondenceRejectorPoly::Ptr | CorrespondenceRejectorPolyPtr |
typedef pcl::PointCloud< FeatureT > | FeatureCloud |
typedef FeatureCloud::ConstPtr | FeatureCloudConstPtr |
typedef FeatureCloud::Ptr | FeatureCloudPtr |
typedef KdTreeFLANN< FeatureT > ::Ptr | FeatureKdTreePtr |
typedef Registration < PointSource, PointTarget > ::Matrix4 | Matrix4 |
typedef Registration < PointSource, PointTarget > ::PointCloudSource | PointCloudSource |
typedef PointCloudSource::ConstPtr | PointCloudSourceConstPtr |
typedef PointCloudSource::Ptr | PointCloudSourcePtr |
typedef Registration < PointSource, PointTarget > ::PointCloudTarget | PointCloudTarget |
typedef PointIndices::ConstPtr | PointIndicesConstPtr |
typedef PointIndices::Ptr | PointIndicesPtr |
typedef boost::shared_ptr < SampleConsensusPrerejective < PointSource, PointTarget, FeatureT > > | Ptr |
Public Member Functions | |
int | getCorrespondenceRandomness () const |
Get the number of neighbors used when selecting a random feature correspondence, as set by the user. | |
float | getInlierFraction () const |
Get the required inlier fraction. | |
const std::vector< int > & | getInliers () const |
Get the inlier indices of the source point cloud under the final transformation. | |
int | getNumberOfSamples () const |
Get the number of samples to use during each iteration, as set by the user. | |
float | getSimilarityThreshold () const |
Get the similarity threshold between edge lengths of the underlying polygonal correspondence rejector object,. | |
const FeatureCloudConstPtr | getSourceFeatures () const |
Get a pointer to the source point cloud's features. | |
const FeatureCloudConstPtr | getTargetFeatures () const |
Get a pointer to the target point cloud's features. | |
SampleConsensusPrerejective () | |
Constructor. | |
void | setCorrespondenceRandomness (int k) |
Set the number of neighbors to use when selecting a random feature correspondence. A higher value will add more randomness to the feature matching. | |
void | setInlierFraction (float inlier_fraction) |
Set the required inlier fraction (of the input) | |
void | setNumberOfSamples (int nr_samples) |
Set the number of samples to use during each iteration. | |
void | setSimilarityThreshold (float similarity_threshold) |
Set the similarity threshold in [0,1[ between edge lengths of the underlying polygonal correspondence rejector object, where 1 is a perfect match. | |
void | setSourceFeatures (const FeatureCloudConstPtr &features) |
Provide a boost shared pointer to the source point cloud's feature descriptors. | |
void | setTargetFeatures (const FeatureCloudConstPtr &features) |
Provide a boost shared pointer to the target point cloud's feature descriptors. | |
virtual | ~SampleConsensusPrerejective () |
Destructor. | |
Protected Member Functions | |
void | computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess) |
Rigid transformation computation method. | |
void | findSimilarFeatures (const FeatureCloud &input_features, const std::vector< int > &sample_indices, std::vector< int > &corresponding_indices) |
For each of the sample points, find a list of points in the target cloud whose features are similar to the sample points' features. From these, select one randomly which will be considered that sample point's correspondence. | |
void | getFitness (std::vector< int > &inliers, float &fitness_score) |
Obtain the fitness of a transformation The following metrics are calculated, based on final_transformation_ and corr_dist_threshold_: | |
int | getRandomIndex (int n) const |
Choose a random index between 0 and n-1. | |
void | selectSamples (const PointCloudSource &cloud, int nr_samples, std::vector< int > &sample_indices) |
Select nr_samples sample points from cloud while making sure that their pairwise distances are greater than a user-defined minimum distance, min_sample_distance. | |
Protected Attributes | |
CorrespondenceRejectorPolyPtr | correspondence_rejector_poly_ |
The polygonal correspondence rejector used for prerejection. | |
FeatureKdTreePtr | feature_tree_ |
The KdTree used to compare feature descriptors. | |
float | inlier_fraction_ |
The fraction [0,1] of inlier points required for accepting a transformation. | |
std::vector< int > | inliers_ |
Inlier points of final transformation as indices into source. | |
FeatureCloudConstPtr | input_features_ |
The source point cloud's feature descriptors. | |
int | k_correspondences_ |
The number of neighbors to use when selecting a random feature correspondence. | |
int | nr_samples_ |
The number of samples to use during each iteration. | |
FeatureCloudConstPtr | target_features_ |
The target point cloud's feature descriptors. |
Pose estimation and alignment class using a prerejective RANSAC routine.
This class inserts a simple, yet effective "prerejection" step into the standard RANSAC pose estimation loop in order to avoid verification of pose hypotheses that are likely to be wrong. This is achieved by local pose-invariant geometric constraints, as also implemented in the class CorrespondenceRejectorPoly.
In order to robustly align partial/occluded models, this routine does not try to minimize the fit error, but instead tries to maximize the inlier rate, above a threshold specifiable using setInlierFraction().
The amount of prerejection or "greedyness" of the algorithm can be specified using setSimilarityThreshold() in [0,1[, where a value of 0 means disabled, and 1 is maximally rejective.
If you use this in academic work, please cite:
A. G. Buch, D. Kraft, J.-K. Kämäräinen, H. G. Petersen and N. Krüger. Pose Estimation using Local Structure-Specific Shape and Appearance Context. International Conference on Robotics and Automation (ICRA), 2013.
Definition at line 77 of file sample_consensus_prerejective.h.
typedef boost::shared_ptr<const SampleConsensusPrerejective<PointSource, PointTarget, FeatureT> > pcl::SampleConsensusPrerejective< PointSource, PointTarget, FeatureT >::ConstPtr |
Reimplemented from pcl::Registration< PointSource, PointTarget >.
Definition at line 109 of file sample_consensus_prerejective.h.
typedef pcl::registration::CorrespondenceRejectorPoly<PointSource, PointTarget> pcl::SampleConsensusPrerejective< PointSource, PointTarget, FeatureT >::CorrespondenceRejectorPoly |
Definition at line 113 of file sample_consensus_prerejective.h.
typedef CorrespondenceRejectorPoly::ConstPtr pcl::SampleConsensusPrerejective< PointSource, PointTarget, FeatureT >::CorrespondenceRejectorPolyConstPtr |
Definition at line 115 of file sample_consensus_prerejective.h.
typedef CorrespondenceRejectorPoly::Ptr pcl::SampleConsensusPrerejective< PointSource, PointTarget, FeatureT >::CorrespondenceRejectorPolyPtr |
Definition at line 114 of file sample_consensus_prerejective.h.
typedef pcl::PointCloud<FeatureT> pcl::SampleConsensusPrerejective< PointSource, PointTarget, FeatureT >::FeatureCloud |
Definition at line 104 of file sample_consensus_prerejective.h.
typedef FeatureCloud::ConstPtr pcl::SampleConsensusPrerejective< PointSource, PointTarget, FeatureT >::FeatureCloudConstPtr |
Definition at line 106 of file sample_consensus_prerejective.h.
typedef FeatureCloud::Ptr pcl::SampleConsensusPrerejective< PointSource, PointTarget, FeatureT >::FeatureCloudPtr |
Definition at line 105 of file sample_consensus_prerejective.h.
typedef KdTreeFLANN<FeatureT>::Ptr pcl::SampleConsensusPrerejective< PointSource, PointTarget, FeatureT >::FeatureKdTreePtr |
Definition at line 111 of file sample_consensus_prerejective.h.
typedef Registration<PointSource, PointTarget>::Matrix4 pcl::SampleConsensusPrerejective< PointSource, PointTarget, FeatureT >::Matrix4 |
Reimplemented from pcl::Registration< PointSource, PointTarget >.
Definition at line 80 of file sample_consensus_prerejective.h.
typedef Registration<PointSource, PointTarget>::PointCloudSource pcl::SampleConsensusPrerejective< PointSource, PointTarget, FeatureT >::PointCloudSource |
Reimplemented from pcl::Registration< PointSource, PointTarget >.
Definition at line 95 of file sample_consensus_prerejective.h.
typedef PointCloudSource::ConstPtr pcl::SampleConsensusPrerejective< PointSource, PointTarget, FeatureT >::PointCloudSourceConstPtr |
Reimplemented from pcl::Registration< PointSource, PointTarget >.
Definition at line 97 of file sample_consensus_prerejective.h.
typedef PointCloudSource::Ptr pcl::SampleConsensusPrerejective< PointSource, PointTarget, FeatureT >::PointCloudSourcePtr |
Reimplemented from pcl::Registration< PointSource, PointTarget >.
Definition at line 96 of file sample_consensus_prerejective.h.
typedef Registration<PointSource, PointTarget>::PointCloudTarget pcl::SampleConsensusPrerejective< PointSource, PointTarget, FeatureT >::PointCloudTarget |
Reimplemented from pcl::Registration< PointSource, PointTarget >.
Definition at line 99 of file sample_consensus_prerejective.h.
typedef PointIndices::ConstPtr pcl::SampleConsensusPrerejective< PointSource, PointTarget, FeatureT >::PointIndicesConstPtr |
Reimplemented from pcl::PCLBase< PointSource >.
Definition at line 102 of file sample_consensus_prerejective.h.
typedef PointIndices::Ptr pcl::SampleConsensusPrerejective< PointSource, PointTarget, FeatureT >::PointIndicesPtr |
Reimplemented from pcl::PCLBase< PointSource >.
Definition at line 101 of file sample_consensus_prerejective.h.
typedef boost::shared_ptr<SampleConsensusPrerejective<PointSource, PointTarget, FeatureT> > pcl::SampleConsensusPrerejective< PointSource, PointTarget, FeatureT >::Ptr |
Reimplemented from pcl::Registration< PointSource, PointTarget >.
Definition at line 108 of file sample_consensus_prerejective.h.
pcl::SampleConsensusPrerejective< PointSource, PointTarget, FeatureT >::SampleConsensusPrerejective | ( | ) | [inline] |
Constructor.
Definition at line 118 of file sample_consensus_prerejective.h.
virtual pcl::SampleConsensusPrerejective< PointSource, PointTarget, FeatureT >::~SampleConsensusPrerejective | ( | ) | [inline, virtual] |
Destructor.
Definition at line 134 of file sample_consensus_prerejective.h.
void pcl::SampleConsensusPrerejective< PointSource, PointTarget, FeatureT >::computeTransformation | ( | PointCloudSource & | output, |
const Eigen::Matrix4f & | guess | ||
) | [protected] |
Rigid transformation computation method.
output | the transformed input point cloud dataset using the rigid transformation found |
Definition at line 134 of file sample_consensus_prerejective.hpp.
void pcl::SampleConsensusPrerejective< PointSource, PointTarget, FeatureT >::findSimilarFeatures | ( | const FeatureCloud & | input_features, |
const std::vector< int > & | sample_indices, | ||
std::vector< int > & | corresponding_indices | ||
) | [protected] |
For each of the sample points, find a list of points in the target cloud whose features are similar to the sample points' features. From these, select one randomly which will be considered that sample point's correspondence.
input_features | a cloud of feature descriptors |
sample_indices | the indices of each sample point |
corresponding_indices | the resulting indices of each sample's corresponding point in the target cloud |
Definition at line 106 of file sample_consensus_prerejective.hpp.
int pcl::SampleConsensusPrerejective< PointSource, PointTarget, FeatureT >::getCorrespondenceRandomness | ( | ) | const [inline] |
Get the number of neighbors used when selecting a random feature correspondence, as set by the user.
Definition at line 192 of file sample_consensus_prerejective.h.
void pcl::SampleConsensusPrerejective< PointSource, PointTarget, FeatureT >::getFitness | ( | std::vector< int > & | inliers, |
float & | fitness_score | ||
) | [protected] |
Obtain the fitness of a transformation The following metrics are calculated, based on final_transformation_ and corr_dist_threshold_:
inliers | indices of source point cloud inliers |
fitness_score | output fitness score as RMSE |
Definition at line 271 of file sample_consensus_prerejective.hpp.
float pcl::SampleConsensusPrerejective< PointSource, PointTarget, FeatureT >::getInlierFraction | ( | ) | const [inline] |
Get the required inlier fraction.
Definition at line 229 of file sample_consensus_prerejective.h.
const std::vector<int>& pcl::SampleConsensusPrerejective< PointSource, PointTarget, FeatureT >::getInliers | ( | ) | const [inline] |
Get the inlier indices of the source point cloud under the final transformation.
Definition at line 238 of file sample_consensus_prerejective.h.
int pcl::SampleConsensusPrerejective< PointSource, PointTarget, FeatureT >::getNumberOfSamples | ( | ) | const [inline] |
Get the number of samples to use during each iteration, as set by the user.
Definition at line 175 of file sample_consensus_prerejective.h.
int pcl::SampleConsensusPrerejective< PointSource, PointTarget, FeatureT >::getRandomIndex | ( | int | n | ) | const [inline, protected] |
Choose a random index between 0 and n-1.
n | the number of possible indices to choose from |
Definition at line 248 of file sample_consensus_prerejective.h.
float pcl::SampleConsensusPrerejective< PointSource, PointTarget, FeatureT >::getSimilarityThreshold | ( | ) | const [inline] |
Get the similarity threshold between edge lengths of the underlying polygonal correspondence rejector object,.
Definition at line 211 of file sample_consensus_prerejective.h.
const FeatureCloudConstPtr pcl::SampleConsensusPrerejective< PointSource, PointTarget, FeatureT >::getSourceFeatures | ( | ) | const [inline] |
Get a pointer to the source point cloud's features.
Definition at line 146 of file sample_consensus_prerejective.h.
const FeatureCloudConstPtr pcl::SampleConsensusPrerejective< PointSource, PointTarget, FeatureT >::getTargetFeatures | ( | ) | const [inline] |
Get a pointer to the target point cloud's features.
Definition at line 159 of file sample_consensus_prerejective.h.
void pcl::SampleConsensusPrerejective< PointSource, PointTarget, FeatureT >::selectSamples | ( | const PointCloudSource & | cloud, |
int | nr_samples, | ||
std::vector< int > & | sample_indices | ||
) | [protected] |
Select nr_samples sample points from cloud while making sure that their pairwise distances are greater than a user-defined minimum distance, min_sample_distance.
cloud | the input point cloud |
nr_samples | the number of samples to select |
sample_indices | the resulting sample indices |
Definition at line 71 of file sample_consensus_prerejective.hpp.
void pcl::SampleConsensusPrerejective< PointSource, PointTarget, FeatureT >::setCorrespondenceRandomness | ( | int | k | ) | [inline] |
Set the number of neighbors to use when selecting a random feature correspondence. A higher value will add more randomness to the feature matching.
k | the number of neighbors to use when selecting a random feature correspondence. |
Definition at line 185 of file sample_consensus_prerejective.h.
void pcl::SampleConsensusPrerejective< PointSource, PointTarget, FeatureT >::setInlierFraction | ( | float | inlier_fraction | ) | [inline] |
Set the required inlier fraction (of the input)
inlier_fraction | required inlier fraction, must be in [0,1] |
Definition at line 220 of file sample_consensus_prerejective.h.
void pcl::SampleConsensusPrerejective< PointSource, PointTarget, FeatureT >::setNumberOfSamples | ( | int | nr_samples | ) | [inline] |
Set the number of samples to use during each iteration.
nr_samples | the number of samples to use during each iteration |
Definition at line 168 of file sample_consensus_prerejective.h.
void pcl::SampleConsensusPrerejective< PointSource, PointTarget, FeatureT >::setSimilarityThreshold | ( | float | similarity_threshold | ) | [inline] |
Set the similarity threshold in [0,1[ between edge lengths of the underlying polygonal correspondence rejector object, where 1 is a perfect match.
similarity_threshold | edge length similarity threshold |
Definition at line 202 of file sample_consensus_prerejective.h.
void pcl::SampleConsensusPrerejective< PointSource, PointTarget, FeatureT >::setSourceFeatures | ( | const FeatureCloudConstPtr & | features | ) |
Provide a boost shared pointer to the source point cloud's feature descriptors.
features | the source point cloud's features |
Definition at line 46 of file sample_consensus_prerejective.hpp.
void pcl::SampleConsensusPrerejective< PointSource, PointTarget, FeatureT >::setTargetFeatures | ( | const FeatureCloudConstPtr & | features | ) |
Provide a boost shared pointer to the target point cloud's feature descriptors.
features | the target point cloud's features |
Definition at line 58 of file sample_consensus_prerejective.hpp.
CorrespondenceRejectorPolyPtr pcl::SampleConsensusPrerejective< PointSource, PointTarget, FeatureT >::correspondence_rejector_poly_ [protected] |
The polygonal correspondence rejector used for prerejection.
Definition at line 307 of file sample_consensus_prerejective.h.
FeatureKdTreePtr pcl::SampleConsensusPrerejective< PointSource, PointTarget, FeatureT >::feature_tree_ [protected] |
The KdTree used to compare feature descriptors.
Definition at line 304 of file sample_consensus_prerejective.h.
float pcl::SampleConsensusPrerejective< PointSource, PointTarget, FeatureT >::inlier_fraction_ [protected] |
The fraction [0,1] of inlier points required for accepting a transformation.
Definition at line 310 of file sample_consensus_prerejective.h.
std::vector<int> pcl::SampleConsensusPrerejective< PointSource, PointTarget, FeatureT >::inliers_ [protected] |
Inlier points of final transformation as indices into source.
Definition at line 313 of file sample_consensus_prerejective.h.
FeatureCloudConstPtr pcl::SampleConsensusPrerejective< PointSource, PointTarget, FeatureT >::input_features_ [protected] |
The source point cloud's feature descriptors.
Definition at line 292 of file sample_consensus_prerejective.h.
int pcl::SampleConsensusPrerejective< PointSource, PointTarget, FeatureT >::k_correspondences_ [protected] |
The number of neighbors to use when selecting a random feature correspondence.
Definition at line 301 of file sample_consensus_prerejective.h.
int pcl::SampleConsensusPrerejective< PointSource, PointTarget, FeatureT >::nr_samples_ [protected] |
The number of samples to use during each iteration.
Definition at line 298 of file sample_consensus_prerejective.h.
FeatureCloudConstPtr pcl::SampleConsensusPrerejective< PointSource, PointTarget, FeatureT >::target_features_ [protected] |
The target point cloud's feature descriptors.
Definition at line 295 of file sample_consensus_prerejective.h.