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$
*
*/
#ifndef PCL_REGISTRATION_SAMPLE_CONSENSUS_PREREJECTIVE_H_
#define PCL_REGISTRATION_SAMPLE_CONSENSUS_PREREJECTIVE_H_
#include <pcl/registration/registration.h>
#include <pcl/registration/transformation_estimation_svd.h>
#include <pcl/registration/transformation_validation.h>
#include <pcl/registration/correspondence_rejection_poly.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:
typedef typename Registration<PointSource, PointTarget>::Matrix4 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_;
typedef typename Registration<PointSource, PointTarget>::PointCloudSource PointCloudSource;
typedef typename PointCloudSource::Ptr PointCloudSourcePtr;
typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr;
typedef typename Registration<PointSource, PointTarget>::PointCloudTarget PointCloudTarget;
typedef PointIndices::Ptr PointIndicesPtr;
typedef PointIndices::ConstPtr PointIndicesConstPtr;
typedef pcl::PointCloud<FeatureT> FeatureCloud;
typedef typename FeatureCloud::Ptr FeatureCloudPtr;
typedef typename FeatureCloud::ConstPtr FeatureCloudConstPtr;
typedef boost::shared_ptr<SampleConsensusPrerejective<PointSource, PointTarget, FeatureT> > Ptr;
typedef boost::shared_ptr<const SampleConsensusPrerejective<PointSource, PointTarget, FeatureT> > ConstPtr;
typedef typename KdTreeFLANN<FeatureT>::Ptr FeatureKdTreePtr;
typedef pcl::registration::CorrespondenceRejectorPoly<PointSource, PointTarget> CorrespondenceRejectorPoly;
typedef typename CorrespondenceRejectorPoly::Ptr CorrespondenceRejectorPolyPtr;
typedef typename CorrespondenceRejectorPoly::ConstPtr CorrespondenceRejectorPolyConstPtr;
/** \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 */
virtual ~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 std::vector<int>&
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, std::vector<int> &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 std::vector<int> &sample_indices,
std::vector<std::vector<int> >& similar_features,
std::vector<int> &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);
/** \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 (std::vector<int>& 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 */
std::vector<int> inliers_;
};
}
#include <pcl/registration/impl/sample_consensus_prerejective.hpp>
#endif
pcl::SampleConsensusPrerejective::FeatureCloudPtr
FeatureCloud::Ptr FeatureCloudPtr
Definition: sample_consensus_prerejective.h:105
pcl::KdTreeFLANN
KdTreeFLANN is a generic type of 3D spatial locator using kD-tree structures.
Definition: kdtree_flann.h:69
pcl::SampleConsensusPrerejective::nr_samples_
int nr_samples_
The number of samples to use during each iteration.
Definition: sample_consensus_prerejective.h:299
pcl::SampleConsensusPrerejective::~SampleConsensusPrerejective
virtual ~SampleConsensusPrerejective()
Destructor.
Definition: sample_consensus_prerejective.h:134
pcl::Registration< PointSource, PointTarget >::transformation_estimation_
TransformationEstimationPtr transformation_estimation_
A TransformationEstimation object, used to calculate the 4x4 rigid transformation.
Definition: registration.h:547
pcl::SampleConsensusPrerejective::input_features_
FeatureCloudConstPtr input_features_
The source point cloud&#39;s feature descriptors.
Definition: sample_consensus_prerejective.h:293
pcl
Definition: convolution.h:45
pcl::SampleConsensusPrerejective::getFitness
void getFitness(std::vector< int > &inliers, float &fitness_score)
Obtain the fitness of a transformation The following metrics are calculated, based on final_transform...
Definition: sample_consensus_prerejective.hpp:294
pcl::SampleConsensusPrerejective::getInlierFraction
float getInlierFraction() const
Get the required inlier fraction.
Definition: sample_consensus_prerejective.h:229
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:192
pcl::SampleConsensusPrerejective::computeTransformation
void computeTransformation(PointCloudSource &output, const Eigen::Matrix4f &guess)
Rigid transformation computation method.
Definition: sample_consensus_prerejective.hpp:144
pcl::SampleConsensusPrerejective::getSourceFeatures
const FeatureCloudConstPtr getSourceFeatures() const
Get a pointer to the source point cloud&#39;s features.
Definition: sample_consensus_prerejective.h:146
pcl::SampleConsensusPrerejective::getSimilarityThreshold
float getSimilarityThreshold() const
Get the similarity threshold between edge lengths of the underlying polygonal correspondence rejector...
Definition: sample_consensus_prerejective.h:211
pcl::SampleConsensusPrerejective::PointCloudSource
Registration< PointSource, PointTarget >::PointCloudSource PointCloudSource
Definition: sample_consensus_prerejective.h:95
pcl::SampleConsensusPrerejective::getRandomIndex
int getRandomIndex(int n) const
Choose a random index between 0 and n-1.
Definition: sample_consensus_prerejective.h:248
pcl::SampleConsensusPrerejective::inlier_fraction_
float inlier_fraction_
The fraction [0,1] of inlier points required for accepting a transformation.
Definition: sample_consensus_prerejective.h:311
pcl::SampleConsensusPrerejective::FeatureKdTreePtr
KdTreeFLANN< FeatureT >::Ptr FeatureKdTreePtr
Definition: sample_consensus_prerejective.h:111
pcl::SampleConsensusPrerejective::feature_tree_
FeatureKdTreePtr feature_tree_
The KdTree used to compare feature descriptors.
Definition: sample_consensus_prerejective.h:305
pcl::SampleConsensusPrerejective::setInlierFraction
void setInlierFraction(float inlier_fraction)
Set the required inlier fraction (of the input)
Definition: sample_consensus_prerejective.h:220
pcl::SampleConsensusPrerejective::PointIndicesPtr
PointIndices::Ptr PointIndicesPtr
Definition: sample_consensus_prerejective.h:101
pcl::PointCloud< FeatureT >::Ptr
boost::shared_ptr< PointCloud< FeatureT > > Ptr
Definition: point_cloud.h:428
pcl::SampleConsensusPrerejective::Matrix4
Registration< PointSource, PointTarget >::Matrix4 Matrix4
Definition: sample_consensus_prerejective.h:80
pcl::SampleConsensusPrerejective
Pose estimation and alignment class using a prerejective RANSAC routine.
Definition: sample_consensus_prerejective.h:77
pcl::SampleConsensusPrerejective::Ptr
boost::shared_ptr< SampleConsensusPrerejective< PointSource, PointTarget, FeatureT > > Ptr
Definition: sample_consensus_prerejective.h:108
pcl::Registration< PointSource, PointTarget >::max_iterations_
int max_iterations_
The maximum number of iterations the internal optimization should run for.
Definition: registration.h:496
pcl::registration::TransformationEstimationSVD
TransformationEstimationSVD implements SVD-based estimation of the transformation aligning the given ...
Definition: transformation_estimation_svd.h:58
pcl::SampleConsensusPrerejective::FeatureCloudConstPtr
FeatureCloud::ConstPtr FeatureCloudConstPtr
Definition: sample_consensus_prerejective.h:106
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:185
pcl::PointCloud::ConstPtr
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
pcl::SampleConsensusPrerejective::PointIndicesConstPtr
PointIndices::ConstPtr PointIndicesConstPtr
Definition: sample_consensus_prerejective.h:102
pcl::SampleConsensusPrerejective::SampleConsensusPrerejective
SampleConsensusPrerejective()
Constructor.
Definition: sample_consensus_prerejective.h:118
pcl::SampleConsensusPrerejective::setTargetFeatures
void setTargetFeatures(const FeatureCloudConstPtr &features)
Provide a boost shared pointer to the target point cloud&#39;s feature descriptors.
Definition: sample_consensus_prerejective.hpp:58
pcl::Registration
Registration represents the base registration class for general purpose, ICP-like methods...
Definition: registration.h:62
pcl::SampleConsensusPrerejective::setNumberOfSamples
void setNumberOfSamples(int nr_samples)
Set the number of samples to use during each iteration.
Definition: sample_consensus_prerejective.h:168
pcl::PointCloud< FeatureT >
pcl::SampleConsensusPrerejective::PointCloudTarget
Registration< PointSource, PointTarget >::PointCloudTarget PointCloudTarget
Definition: sample_consensus_prerejective.h:99
pcl::SampleConsensusPrerejective::PointCloudSourceConstPtr
PointCloudSource::ConstPtr PointCloudSourceConstPtr
Definition: sample_consensus_prerejective.h:97
pcl::SampleConsensusPrerejective::target_features_
FeatureCloudConstPtr target_features_
The target point cloud&#39;s feature descriptors.
Definition: sample_consensus_prerejective.h:296
pcl::registration::CorrespondenceRejectorPoly
CorrespondenceRejectorPoly implements a correspondence rejection method that exploits low-level and p...
Definition: correspondence_rejection_poly.h:64
pcl::SampleConsensusPrerejective::CorrespondenceRejectorPolyConstPtr
CorrespondenceRejectorPoly::ConstPtr CorrespondenceRejectorPolyConstPtr
Definition: sample_consensus_prerejective.h:115
pcl::SampleConsensusPrerejective::findSimilarFeatures
void findSimilarFeatures(const std::vector< int > &sample_indices, std::vector< std::vector< int > > &similar_features, std::vector< int > &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:115
pcl::PointIndices::Ptr
boost::shared_ptr< ::pcl::PointIndices > Ptr
Definition: PointIndices.h:22
pcl::SampleConsensusPrerejective::k_correspondences_
int k_correspondences_
The number of neighbors to use when selecting a random feature correspondence.
Definition: sample_consensus_prerejective.h:302
pcl::registration::CorrespondenceRejectorPoly::ConstPtr
boost::shared_ptr< const CorrespondenceRejectorPoly > ConstPtr
Definition: correspondence_rejection_poly.h:72
pcl::SampleConsensusPrerejective::FeatureCloud
pcl::PointCloud< FeatureT > FeatureCloud
Definition: sample_consensus_prerejective.h:104
pcl::SampleConsensusPrerejective::getTargetFeatures
const FeatureCloudConstPtr getTargetFeatures() const
Get a pointer to the target point cloud&#39;s features.
Definition: sample_consensus_prerejective.h:159
pcl::SampleConsensusPrerejective::CorrespondenceRejectorPolyPtr
CorrespondenceRejectorPoly::Ptr CorrespondenceRejectorPolyPtr
Definition: sample_consensus_prerejective.h:114
pcl::Registration< PointSource, PointTarget >::reg_name_
std::string reg_name_
The registration method name.
Definition: registration.h:482
pcl::SampleConsensusPrerejective::CorrespondenceRejectorPoly
pcl::registration::CorrespondenceRejectorPoly< PointSource, PointTarget > CorrespondenceRejectorPoly
Definition: sample_consensus_prerejective.h:113
pcl::SampleConsensusPrerejective::ConstPtr
boost::shared_ptr< const SampleConsensusPrerejective< PointSource, PointTarget, FeatureT > > ConstPtr
Definition: sample_consensus_prerejective.h:109
pcl::SampleConsensusPrerejective::selectSamples
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 greate...
Definition: sample_consensus_prerejective.hpp:71
pcl::SampleConsensusPrerejective::setSourceFeatures
void setSourceFeatures(const FeatureCloudConstPtr &features)
Provide a boost shared pointer to the source point cloud&#39;s feature descriptors.
Definition: sample_consensus_prerejective.hpp:46
pcl::SampleConsensusPrerejective::inliers_
std::vector< int > inliers_
Inlier points of final transformation as indices into source.
Definition: sample_consensus_prerejective.h:314
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:202
pcl::SampleConsensusPrerejective::correspondence_rejector_poly_
CorrespondenceRejectorPolyPtr correspondence_rejector_poly_
The polygonal correspondence rejector used for prerejection.
Definition: sample_consensus_prerejective.h:308
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:175
pcl::SampleConsensusPrerejective::PointCloudSourcePtr
PointCloudSource::Ptr PointCloudSourcePtr
Definition: sample_consensus_prerejective.h:96
pcl::PointIndices::ConstPtr
boost::shared_ptr< ::pcl::PointIndices const > ConstPtr
Definition: PointIndices.h:23
pcl::SampleConsensusPrerejective::getInliers
const std::vector< int > & getInliers() const
Get the inlier indices of the source point cloud under the final transformation.
Definition: sample_consensus_prerejective.h:238
pcl::registration::CorrespondenceRejectorPoly::Ptr
boost::shared_ptr< CorrespondenceRejectorPoly > Ptr
Definition: correspondence_rejection_poly.h:71