sample_consensus_prerejective.h
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2010-2012, Willow Garage, Inc.
* Copyright (c) 2012-, Open Perception, Inc.
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder(s) nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id$
*
*/
#pragma once
#include <pcl/registration/correspondence_rejection_poly.h>
#include <pcl/registration/registration.h>
#include <pcl/registration/transformation_estimation_svd.h>
#include <pcl/registration/transformation_validation.h>
namespace pcl {
/** \brief 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
* \ref registration::CorrespondenceRejectorPoly "CorrespondenceRejectorPoly".
*
* In order to robustly align partial/occluded models, this routine performs
* fit error evaluation using only inliers, i.e. points closer than a
* Euclidean threshold, which is specifiable using \ref setInlierFraction().
*
* The amount of prerejection or "greedyness" of the algorithm can be specified
* using \ref 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.
*
* \author Anders Glent Buch (andersgb1@gmail.com)
* \ingroup registration
*/
template <typename PointSource, typename PointTarget, typename FeatureT>
class SampleConsensusPrerejective : public Registration<PointSource, PointTarget> {
public:
using Matrix4 = typename Registration<PointSource, PointTarget>::Matrix4;
using Registration<PointSource, PointTarget>::reg_name_;
using Registration<PointSource, PointTarget>::getClassName;
using Registration<PointSource, PointTarget>::input_;
using Registration<PointSource, PointTarget>::target_;
using Registration<PointSource, PointTarget>::tree_;
using Registration<PointSource, PointTarget>::max_iterations_;
using Registration<PointSource, PointTarget>::corr_dist_threshold_;
using Registration<PointSource, PointTarget>::transformation_;
using Registration<PointSource, PointTarget>::final_transformation_;
using Registration<PointSource, PointTarget>::transformation_estimation_;
using Registration<PointSource, PointTarget>::getFitnessScore;
using Registration<PointSource, PointTarget>::converged_;
using PointCloudSource =
typename Registration<PointSource, PointTarget>::PointCloudSource;
using PointCloudSourcePtr = typename PointCloudSource::Ptr;
using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
using PointCloudTarget =
typename Registration<PointSource, PointTarget>::PointCloudTarget;
using PointIndicesPtr = PointIndices::Ptr;
using PointIndicesConstPtr = PointIndices::ConstPtr;
using FeatureCloud = pcl::PointCloud<FeatureT>;
using FeatureCloudPtr = typename FeatureCloud::Ptr;
using FeatureCloudConstPtr = typename FeatureCloud::ConstPtr;
using Ptr =
shared_ptr<SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>>;
using ConstPtr =
shared_ptr<const SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>>;
using FeatureKdTreePtr = typename KdTreeFLANN<FeatureT>::Ptr;
using CorrespondenceRejectorPoly =
pcl::registration::CorrespondenceRejectorPoly<PointSource, PointTarget>;
using CorrespondenceRejectorPolyPtr = typename CorrespondenceRejectorPoly::Ptr;
using CorrespondenceRejectorPolyConstPtr =
typename CorrespondenceRejectorPoly::ConstPtr;
/** \brief Constructor */
SampleConsensusPrerejective()
: input_features_()
, target_features_()
, nr_samples_(3)
, k_correspondences_(2)
, feature_tree_(new pcl::KdTreeFLANN<FeatureT>)
, correspondence_rejector_poly_(new CorrespondenceRejectorPoly)
, inlier_fraction_(0.0f)
{
reg_name_ = "SampleConsensusPrerejective";
correspondence_rejector_poly_->setSimilarityThreshold(0.6f);
max_iterations_ = 5000;
transformation_estimation_.reset(
new pcl::registration::TransformationEstimationSVD<PointSource, PointTarget>);
};
/** \brief Destructor */
~SampleConsensusPrerejective() {}
/** \brief Provide a boost shared pointer to the source point cloud's feature
* descriptors \param features the source point cloud's features
*/
void
setSourceFeatures(const FeatureCloudConstPtr& features);
/** \brief Get a pointer to the source point cloud's features */
inline const FeatureCloudConstPtr
getSourceFeatures() const
{
return (input_features_);
}
/** \brief Provide a boost shared pointer to the target point cloud's feature
* descriptors \param features the target point cloud's features
*/
void
setTargetFeatures(const FeatureCloudConstPtr& features);
/** \brief Get a pointer to the target point cloud's features */
inline const FeatureCloudConstPtr
getTargetFeatures() const
{
return (target_features_);
}
/** \brief Set the number of samples to use during each iteration
* \param nr_samples the number of samples to use during each iteration
*/
inline void
setNumberOfSamples(int nr_samples)
{
nr_samples_ = nr_samples;
}
/** \brief Get the number of samples to use during each iteration, as set by the user
*/
inline int
getNumberOfSamples() const
{
return (nr_samples_);
}
/** \brief Set the number of neighbors to use when selecting a random feature
* correspondence. A higher value will add more randomness to the feature matching.
* \param k the number of neighbors to use when selecting a random feature
* correspondence.
*/
inline void
setCorrespondenceRandomness(int k)
{
k_correspondences_ = k;
}
/** \brief Get the number of neighbors used when selecting a random feature
* correspondence, as set by the user */
inline int
getCorrespondenceRandomness() const
{
return (k_correspondences_);
}
/** \brief Set the similarity threshold in [0,1[ between edge lengths of the
* underlying polygonal correspondence rejector object, where 1 is a perfect match
* \param similarity_threshold edge length similarity threshold
*/
inline void
setSimilarityThreshold(float similarity_threshold)
{
correspondence_rejector_poly_->setSimilarityThreshold(similarity_threshold);
}
/** \brief Get the similarity threshold between edge lengths of the underlying
* polygonal correspondence rejector object, \return edge length similarity threshold
*/
inline float
getSimilarityThreshold() const
{
return correspondence_rejector_poly_->getSimilarityThreshold();
}
/** \brief Set the required inlier fraction (of the input)
* \param inlier_fraction required inlier fraction, must be in [0,1]
*/
inline void
setInlierFraction(float inlier_fraction)
{
inlier_fraction_ = inlier_fraction;
}
/** \brief Get the required inlier fraction
* \return required inlier fraction in [0,1]
*/
inline float
getInlierFraction() const
{
return inlier_fraction_;
}
/** \brief Get the inlier indices of the source point cloud under the final
* transformation
* @return inlier indices
*/
inline const pcl::Indices&
getInliers() const
{
return inliers_;
}
protected:
/** \brief Choose a random index between 0 and n-1
* \param n the number of possible indices to choose from
*/
inline int
getRandomIndex(int n) const
{
return (static_cast<int>(n * (rand() / (RAND_MAX + 1.0))));
};
/** \brief Select \a nr_samples sample points from cloud while making sure that their
* pairwise distances are greater than a user-defined minimum distance, \a
* min_sample_distance. \param cloud the input point cloud \param nr_samples the
* number of samples to select \param sample_indices the resulting sample indices
*/
void
selectSamples(const PointCloudSource& cloud,
int nr_samples,
pcl::Indices& sample_indices);
/** \brief 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. \param
* sample_indices the indices of each sample point \param similar_features
* correspondence cache, which is used to read/write already computed correspondences
* \param corresponding_indices the resulting indices of each sample's corresponding
* point in the target cloud
*/
void
findSimilarFeatures(const pcl::Indices& sample_indices,
std::vector<pcl::Indices>& similar_features,
pcl::Indices& corresponding_indices);
/** \brief Rigid transformation computation method.
* \param output the transformed input point cloud dataset using the rigid
* transformation found \param guess The computed transformation
*/
void
computeTransformation(PointCloudSource& output,
const Eigen::Matrix4f& guess) override;
/** \brief Obtain the fitness of a transformation
* The following metrics are calculated, based on
* \b final_transformation_ and \b corr_dist_threshold_:
* - Inliers: the number of transformed points which are closer than threshold to NN
* - Error score: the MSE of the inliers
* \param inliers indices of source point cloud inliers
* \param fitness_score output fitness score as RMSE
*/
void
getFitness(pcl::Indices& inliers, float& fitness_score);
/** \brief The source point cloud's feature descriptors. */
FeatureCloudConstPtr input_features_;
/** \brief The target point cloud's feature descriptors. */
FeatureCloudConstPtr target_features_;
/** \brief The number of samples to use during each iteration. */
int nr_samples_;
/** \brief The number of neighbors to use when selecting a random feature
* correspondence. */
int k_correspondences_;
/** \brief The KdTree used to compare feature descriptors. */
FeatureKdTreePtr feature_tree_;
/** \brief The polygonal correspondence rejector used for prerejection */
CorrespondenceRejectorPolyPtr correspondence_rejector_poly_;
/** \brief The fraction [0,1] of inlier points required for accepting a transformation
*/
float inlier_fraction_;
/** \brief Inlier points of final transformation as indices into source */
pcl::Indices inliers_;
};
} // namespace pcl
#include <pcl/registration/impl/sample_consensus_prerejective.hpp>
pcl::KdTreeFLANN
KdTreeFLANN is a generic type of 3D spatial locator using kD-tree structures.
Definition: kdtree_flann.h:132
pcl::PointCloud< PointSource >
pcl::PointCloud< PointSource >::Ptr
shared_ptr< PointCloud< PointSource > > Ptr
Definition: point_cloud.h:413
pcl::PointCloud< PointSource >::ConstPtr
shared_ptr< const PointCloud< PointSource > > ConstPtr
Definition: point_cloud.h:414
pcl::Registration
Registration represents the base registration class for general purpose, ICP-like methods.
Definition: registration.h:57
pcl::Registration::reg_name_
std::string reg_name_
The registration method name.
Definition: registration.h:558
pcl::Registration::ConstPtr
shared_ptr< const Registration< PointSource, PointTarget, Scalar > > ConstPtr
Definition: registration.h:67
pcl::Registration::PointCloudSourceConstPtr
typename PointCloudSource::ConstPtr PointCloudSourceConstPtr
Definition: registration.h:78
pcl::Registration::PointCloudSourcePtr
typename PointCloudSource::Ptr PointCloudSourcePtr
Definition: registration.h:77
pcl::Registration::Ptr
shared_ptr< Registration< PointSource, PointTarget, Scalar > > Ptr
Definition: registration.h:66
pcl::Registration::Matrix4
Eigen::Matrix< Scalar, 4, 4 > Matrix4
Definition: registration.h:59
pcl::Registration::transformation_estimation_
TransformationEstimationPtr transformation_estimation_
A TransformationEstimation object, used to calculate the 4x4 rigid transformation.
Definition: registration.h:635
pcl::Registration::max_iterations_
int max_iterations_
The maximum number of iterations the internal optimization should run for.
Definition: registration.h:573
pcl::SampleConsensusPrerejective
Pose estimation and alignment class using a prerejective RANSAC routine.
Definition: sample_consensus_prerejective.h:75
pcl::SampleConsensusPrerejective::getSimilarityThreshold
float getSimilarityThreshold() const
Get the similarity threshold between edge lengths of the underlying polygonal correspondence rejector...
Definition: sample_consensus_prerejective.h:216
pcl::SampleConsensusPrerejective::inlier_fraction_
float inlier_fraction_
The fraction [0,1] of inlier points required for accepting a transformation.
Definition: sample_consensus_prerejective.h:322
pcl::SampleConsensusPrerejective::inliers_
pcl::Indices inliers_
Inlier points of final transformation as indices into source.
Definition: sample_consensus_prerejective.h:325
pcl::SampleConsensusPrerejective::getInlierFraction
float getInlierFraction() const
Get the required inlier fraction.
Definition: sample_consensus_prerejective.h:234
pcl::SampleConsensusPrerejective::setCorrespondenceRandomness
void setCorrespondenceRandomness(int k)
Set the number of neighbors to use when selecting a random feature correspondence.
Definition: sample_consensus_prerejective.h:189
pcl::SampleConsensusPrerejective::SampleConsensusPrerejective
SampleConsensusPrerejective()
Constructor.
Definition: sample_consensus_prerejective.h:121
pcl::SampleConsensusPrerejective::getSourceFeatures
const FeatureCloudConstPtr getSourceFeatures() const
Get a pointer to the source point cloud's features.
Definition: sample_consensus_prerejective.h:148
pcl::SampleConsensusPrerejective::input_features_
FeatureCloudConstPtr input_features_
The source point cloud's feature descriptors.
Definition: sample_consensus_prerejective.h:302
pcl::SampleConsensusPrerejective::feature_tree_
FeatureKdTreePtr feature_tree_
The KdTree used to compare feature descriptors.
Definition: sample_consensus_prerejective.h:315
pcl::SampleConsensusPrerejective::setTargetFeatures
void setTargetFeatures(const FeatureCloudConstPtr &features)
Provide a boost shared pointer to the target point cloud's feature descriptors.
Definition: sample_consensus_prerejective.hpp:62
pcl::SampleConsensusPrerejective::setInlierFraction
void setInlierFraction(float inlier_fraction)
Set the required inlier fraction (of the input)
Definition: sample_consensus_prerejective.h:225
pcl::SampleConsensusPrerejective::correspondence_rejector_poly_
CorrespondenceRejectorPolyPtr correspondence_rejector_poly_
The polygonal correspondence rejector used for prerejection.
Definition: sample_consensus_prerejective.h:318
pcl::SampleConsensusPrerejective::computeTransformation
void computeTransformation(PointCloudSource &output, const Eigen::Matrix4f &guess) override
Rigid transformation computation method.
Definition: sample_consensus_prerejective.hpp:154
pcl::SampleConsensusPrerejective::getCorrespondenceRandomness
int getCorrespondenceRandomness() const
Get the number of neighbors used when selecting a random feature correspondence, as set by the user.
Definition: sample_consensus_prerejective.h:197
pcl::SampleConsensusPrerejective::CorrespondenceRejectorPolyConstPtr
typename CorrespondenceRejectorPoly::ConstPtr CorrespondenceRejectorPolyConstPtr
Definition: sample_consensus_prerejective.h:118
pcl::SampleConsensusPrerejective::getTargetFeatures
const FeatureCloudConstPtr getTargetFeatures() const
Get a pointer to the target point cloud's features.
Definition: sample_consensus_prerejective.h:161
pcl::SampleConsensusPrerejective::PointCloudSource
typename Registration< PointSource, PointTarget >::PointCloudSource PointCloudSource
Definition: sample_consensus_prerejective.h:93
pcl::SampleConsensusPrerejective::FeatureCloudConstPtr
typename FeatureCloud::ConstPtr FeatureCloudConstPtr
Definition: sample_consensus_prerejective.h:105
pcl::SampleConsensusPrerejective::PointIndicesPtr
PointIndices::Ptr PointIndicesPtr
Definition: sample_consensus_prerejective.h:100
pcl::SampleConsensusPrerejective::getInliers
const pcl::Indices & getInliers() const
Get the inlier indices of the source point cloud under the final transformation.
Definition: sample_consensus_prerejective.h:244
pcl::SampleConsensusPrerejective::PointIndicesConstPtr
PointIndices::ConstPtr PointIndicesConstPtr
Definition: sample_consensus_prerejective.h:101
pcl::SampleConsensusPrerejective::setSimilarityThreshold
void setSimilarityThreshold(float similarity_threshold)
Set the similarity threshold in [0,1[ between edge lengths of the underlying polygonal correspondence...
Definition: sample_consensus_prerejective.h:207
pcl::SampleConsensusPrerejective::findSimilarFeatures
void findSimilarFeatures(const pcl::Indices &sample_indices, std::vector< pcl::Indices > &similar_features, pcl::Indices &corresponding_indices)
For each of the sample points, find a list of points in the target cloud whose features are similar t...
Definition: sample_consensus_prerejective.hpp:120
pcl::SampleConsensusPrerejective::~SampleConsensusPrerejective
~SampleConsensusPrerejective()
Destructor.
Definition: sample_consensus_prerejective.h:138
pcl::SampleConsensusPrerejective::setSourceFeatures
void setSourceFeatures(const FeatureCloudConstPtr &features)
Provide a boost shared pointer to the source point cloud's feature descriptors.
Definition: sample_consensus_prerejective.hpp:48
pcl::SampleConsensusPrerejective::target_features_
FeatureCloudConstPtr target_features_
The target point cloud's feature descriptors.
Definition: sample_consensus_prerejective.h:305
pcl::SampleConsensusPrerejective::setNumberOfSamples
void setNumberOfSamples(int nr_samples)
Set the number of samples to use during each iteration.
Definition: sample_consensus_prerejective.h:170
pcl::SampleConsensusPrerejective::FeatureCloudPtr
typename FeatureCloud::Ptr FeatureCloudPtr
Definition: sample_consensus_prerejective.h:104
pcl::SampleConsensusPrerejective::FeatureKdTreePtr
typename KdTreeFLANN< FeatureT >::Ptr FeatureKdTreePtr
Definition: sample_consensus_prerejective.h:112
pcl::SampleConsensusPrerejective::getNumberOfSamples
int getNumberOfSamples() const
Get the number of samples to use during each iteration, as set by the user.
Definition: sample_consensus_prerejective.h:178
pcl::SampleConsensusPrerejective::selectSamples
void selectSamples(const PointCloudSource &cloud, int nr_samples, pcl::Indices &sample_indices)
Select nr_samples sample points from cloud while making sure that their pairwise distances are greate...
Definition: sample_consensus_prerejective.hpp:77
pcl::SampleConsensusPrerejective::getRandomIndex
int getRandomIndex(int n) const
Choose a random index between 0 and n-1.
Definition: sample_consensus_prerejective.h:254
pcl::SampleConsensusPrerejective::nr_samples_
int nr_samples_
The number of samples to use during each iteration.
Definition: sample_consensus_prerejective.h:308
pcl::SampleConsensusPrerejective::CorrespondenceRejectorPolyPtr
typename CorrespondenceRejectorPoly::Ptr CorrespondenceRejectorPolyPtr
Definition: sample_consensus_prerejective.h:116
pcl::SampleConsensusPrerejective::k_correspondences_
int k_correspondences_
The number of neighbors to use when selecting a random feature correspondence.
Definition: sample_consensus_prerejective.h:312
pcl::SampleConsensusPrerejective::getFitness
void getFitness(pcl::Indices &inliers, float &fitness_score)
Obtain the fitness of a transformation The following metrics are calculated, based on final_transform...
Definition: sample_consensus_prerejective.hpp:307
pcl::registration::CorrespondenceRejectorPoly
CorrespondenceRejectorPoly implements a correspondence rejection method that exploits low-level and p...
Definition: correspondence_rejection_poly.h:63
pcl::registration::CorrespondenceRejectorPoly::ConstPtr
shared_ptr< const CorrespondenceRejectorPoly< SourceT, TargetT > > ConstPtr
Definition: correspondence_rejection_poly.h:70
pcl::registration::CorrespondenceRejectorPoly::Ptr
shared_ptr< CorrespondenceRejectorPoly< SourceT, TargetT > > Ptr
Definition: correspondence_rejection_poly.h:69
pcl::registration::TransformationEstimationSVD
TransformationEstimationSVD implements SVD-based estimation of the transformation aligning the given ...
Definition: transformation_estimation_svd.h:58
pcl
Definition: convolution.h:46
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
pcl::PointIndices::Ptr
shared_ptr< ::pcl::PointIndices > Ptr
Definition: PointIndices.h:13
pcl::PointIndices::ConstPtr
shared_ptr< const ::pcl::PointIndices > ConstPtr
Definition: PointIndices.h:14