Loading...
Searching...
No Matches
ia_ransac.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/registration.h>
#include <pcl/registration/transformation_estimation_svd.h>
#include <pcl/memory.h>
namespace pcl {
/** \brief @b SampleConsensusInitialAlignment is an implementation of the initial
* alignment algorithm described in section IV of "Fast Point Feature Histograms (FPFH)
* for 3D Registration," Rusu et al. \author Michael Dixon, Radu B. Rusu
* \ingroup registration
*/
template <typename PointSource, typename PointTarget, typename FeatureT>
class SampleConsensusInitialAlignment : public Registration<PointSource, PointTarget> {
public:
using Registration<PointSource, PointTarget>::reg_name_;
using Registration<PointSource, PointTarget>::input_;
using Registration<PointSource, PointTarget>::indices_;
using Registration<PointSource, PointTarget>::target_;
using Registration<PointSource, PointTarget>::final_transformation_;
using Registration<PointSource, PointTarget>::transformation_;
using Registration<PointSource, PointTarget>::corr_dist_threshold_;
using Registration<PointSource, PointTarget>::min_number_correspondences_;
using Registration<PointSource, PointTarget>::max_iterations_;
using Registration<PointSource, PointTarget>::tree_;
using Registration<PointSource, PointTarget>::transformation_estimation_;
using Registration<PointSource, PointTarget>::converged_;
using Registration<PointSource, PointTarget>::getClassName;
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<SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>>;
using ConstPtr = shared_ptr<
const SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>>;
class ErrorFunctor {
public:
using Ptr = shared_ptr<ErrorFunctor>;
using ConstPtr = shared_ptr<const ErrorFunctor>;
virtual ~ErrorFunctor() = default;
virtual float
operator()(float d) const = 0;
};
class HuberPenalty : public ErrorFunctor {
private:
HuberPenalty() = default;
public:
HuberPenalty(float threshold) : threshold_(threshold) {}
float
operator()(float e) const override
{
if (e <= threshold_)
return (0.5 * e * e);
return (0.5 * threshold_ * (2.0 * std::fabs(e) - threshold_));
}
protected:
float threshold_{0.0f};
};
class TruncatedError : public ErrorFunctor {
private:
TruncatedError() = default;
public:
~TruncatedError() override = default;
TruncatedError(float threshold) : threshold_(threshold) {}
float
operator()(float e) const override
{
if (e <= threshold_)
return (e / threshold_);
return (1.0);
}
protected:
float threshold_{0.0f};
};
using ErrorFunctorPtr = typename ErrorFunctor::Ptr;
using FeatureKdTreePtr = typename KdTreeFLANN<FeatureT>::Ptr;
/** \brief Constructor. */
SampleConsensusInitialAlignment()
: input_features_()
, target_features_()
, feature_tree_(new pcl::KdTreeFLANN<FeatureT>)
, error_functor_()
{
reg_name_ = "SampleConsensusInitialAlignment";
max_iterations_ = 1000;
// Setting a non-std::numeric_limits<double>::max () value to corr_dist_threshold_
// to make it play nicely with TruncatedError
corr_dist_threshold_ = 100.0f;
transformation_estimation_.reset(
new pcl::registration::TransformationEstimationSVD<PointSource, PointTarget>);
};
/** \brief Provide a 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 FeatureCloudConstPtr const
getSourceFeatures()
{
return (input_features_);
}
/** \brief Provide a 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 FeatureCloudConstPtr const
getTargetFeatures()
{
return (target_features_);
}
/** \brief Set the minimum distances between samples
* \param min_sample_distance the minimum distances between samples
*/
void
setMinSampleDistance(float min_sample_distance)
{
min_sample_distance_ = min_sample_distance;
}
/** \brief Get the minimum distances between samples, as set by the user */
float
getMinSampleDistance()
{
return (min_sample_distance_);
}
/** \brief Set the number of samples to use during each iteration
* \param nr_samples the number of samples to use during each iteration
*/
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
*/
int
getNumberOfSamples()
{
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.
*/
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 */
int
getCorrespondenceRandomness()
{
return (k_correspondences_);
}
/** \brief Specify the error function to minimize
* \note This call is optional. TruncatedError will be used by default
* \param[in] error_functor a shared pointer to a subclass of
* SampleConsensusInitialAlignment::ErrorFunctor
*/
void
setErrorFunction(const ErrorFunctorPtr& error_functor)
{
error_functor_ = error_functor;
}
/** \brief Get a shared pointer to the ErrorFunctor that is to be minimized
* \return A shared pointer to a subclass of
* SampleConsensusInitialAlignment::ErrorFunctor
*/
ErrorFunctorPtr
getErrorFunction()
{
return (error_functor_);
}
protected:
/** \brief Choose a random index between 0 and n-1
* \param n the number of possible indices to choose from
*/
inline pcl::index_t
getRandomIndex(int n)
{
return (static_cast<pcl::index_t>(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 min_sample_distance the minimum distance between
* any two samples \param sample_indices the resulting sample indices
*/
void
selectSamples(const PointCloudSource& cloud,
unsigned int nr_samples,
float min_sample_distance,
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
* input_features a cloud of feature descriptors \param sample_indices the indices of
* each sample point \param corresponding_indices the resulting indices of each
* sample's corresponding point in the target cloud
*/
void
findSimilarFeatures(const FeatureCloud& input_features,
const pcl::Indices& sample_indices,
pcl::Indices& corresponding_indices);
/** \brief An error metric for that computes the quality of the alignment between the
* given cloud and the target. \param cloud the input cloud \param threshold distances
* greater than this value are capped
*/
float
computeErrorMetric(const PointCloudSource& cloud, float threshold);
/** \brief Rigid transformation computation method.
* \param output the transformed input point cloud dataset using the rigid
* transformation found \param guess The computed transforamtion
*/
void
computeTransformation(PointCloudSource& output,
const Eigen::Matrix4f& guess) override;
/** \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_{3};
/** \brief The minimum distances between samples. */
float min_sample_distance_{0.0f};
/** \brief The number of neighbors to use when selecting a random feature
* correspondence. */
int k_correspondences_{10};
/** \brief The KdTree used to compare feature descriptors. */
FeatureKdTreePtr feature_tree_;
ErrorFunctorPtr error_functor_;
public:
PCL_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace pcl
#include <pcl/registration/impl/ia_ransac.hpp>
pcl::KdTreeFLANN
KdTreeFLANN is a generic type of 3D spatial locator using kD-tree structures.
Definition kdtree_flann.h:132
pcl::KdTreeFLANN::Ptr
shared_ptr< KdTreeFLANN< PointT, Dist > > Ptr
Definition kdtree_flann.h:151
pcl::PCLBase< PointSource >::input_
PointCloudConstPtr input_
The input point cloud dataset.
Definition pcl_base.h:147
pcl::PCLBase< PointSource >::indices_
IndicesPtr indices_
A pointer to the vector of point indices to use.
Definition pcl_base.h:150
pcl::PointCloud< PointSource >
pcl::PointCloud< FeatureT >::Ptr
shared_ptr< PointCloud< FeatureT > > Ptr
Definition point_cloud.h:413
pcl::PointCloud< FeatureT >::ConstPtr
shared_ptr< const PointCloud< FeatureT > > 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::final_transformation_
Matrix4 final_transformation_
The final transformation matrix estimated by the registration method after N iterations.
Definition registration.h:573
pcl::Registration::corr_dist_threshold_
double corr_dist_threshold_
The maximum distance threshold between two correspondent points in source <-> target.
Definition registration.h:603
pcl::Registration::reg_name_
std::string reg_name_
The registration method name.
Definition registration.h:548
pcl::Registration::transformation_
Matrix4 transformation_
The transformation matrix estimated by the registration method.
Definition registration.h:576
pcl::Registration::tree_
KdTreePtr tree_
A pointer to the spatial search object.
Definition registration.h:551
pcl::Registration::converged_
bool converged_
Holds internal convergence state, given user parameters.
Definition registration.h:613
pcl::Registration::transformation_estimation_
TransformationEstimationPtr transformation_estimation_
A TransformationEstimation object, used to calculate the 4x4 rigid transformation.
Definition registration.h:625
pcl::Registration::max_iterations_
int max_iterations_
The maximum number of iterations the internal optimization should run for.
Definition registration.h:563
pcl::Registration::min_number_correspondences_
unsigned int min_number_correspondences_
The minimum number of correspondences that the algorithm needs before attempting to estimate the tran...
Definition registration.h:618
pcl::Registration::target_
PointCloudTargetConstPtr target_
The input point cloud dataset target.
Definition registration.h:569
pcl::Registration::getClassName
const std::string & getClassName() const
Abstract class get name method.
Definition registration.h:485
pcl::SampleConsensusInitialAlignment::ErrorFunctor
Definition ia_ransac.h:90
pcl::SampleConsensusInitialAlignment::ErrorFunctor::operator()
virtual float operator()(float d) const =0
pcl::SampleConsensusInitialAlignment::ErrorFunctor::ConstPtr
shared_ptr< const ErrorFunctor > ConstPtr
Definition ia_ransac.h:93
pcl::SampleConsensusInitialAlignment::ErrorFunctor::~ErrorFunctor
virtual ~ErrorFunctor()=default
pcl::SampleConsensusInitialAlignment::ErrorFunctor::Ptr
shared_ptr< ErrorFunctor > Ptr
Definition ia_ransac.h:92
pcl::SampleConsensusInitialAlignment::HuberPenalty
Definition ia_ransac.h:100
pcl::SampleConsensusInitialAlignment::HuberPenalty::operator()
float operator()(float e) const override
Definition ia_ransac.h:107
pcl::SampleConsensusInitialAlignment::HuberPenalty::HuberPenalty
HuberPenalty(float threshold)
Definition ia_ransac.h:105
pcl::SampleConsensusInitialAlignment::HuberPenalty::threshold_
float threshold_
Definition ia_ransac.h:115
pcl::SampleConsensusInitialAlignment::TruncatedError
Definition ia_ransac.h:118
pcl::SampleConsensusInitialAlignment::TruncatedError::TruncatedError
TruncatedError(float threshold)
Definition ia_ransac.h:125
pcl::SampleConsensusInitialAlignment::TruncatedError::operator()
float operator()(float e) const override
Definition ia_ransac.h:127
pcl::SampleConsensusInitialAlignment::TruncatedError::~TruncatedError
~TruncatedError() override=default
pcl::SampleConsensusInitialAlignment::TruncatedError::threshold_
float threshold_
Definition ia_ransac.h:135
pcl::SampleConsensusInitialAlignment
SampleConsensusInitialAlignment is an implementation of the initial alignment algorithm described in ...
Definition ia_ransac.h:54
pcl::SampleConsensusInitialAlignment::getTargetFeatures
FeatureCloudConstPtr const getTargetFeatures()
Get a pointer to the target point cloud's features.
Definition ia_ransac.h:179
pcl::SampleConsensusInitialAlignment::PointIndicesConstPtr
PointIndices::ConstPtr PointIndicesConstPtr
Definition ia_ransac.h:79
pcl::SampleConsensusInitialAlignment::computeTransformation
void computeTransformation(PointCloudSource &output, const Eigen::Matrix4f &guess) override
Rigid transformation computation method.
Definition ia_ransac.hpp:189
pcl::SampleConsensusInitialAlignment::ConstPtr
shared_ptr< const SampleConsensusInitialAlignment< PointSource, PointTarget, FeatureT > > ConstPtr
Definition ia_ransac.h:88
pcl::SampleConsensusInitialAlignment::getNumberOfSamples
int getNumberOfSamples()
Get the number of samples to use during each iteration, as set by the user.
Definition ia_ransac.h:212
pcl::SampleConsensusInitialAlignment::PointIndicesPtr
PointIndices::Ptr PointIndicesPtr
Definition ia_ransac.h:78
pcl::SampleConsensusInitialAlignment::computeErrorMetric
float computeErrorMetric(const PointCloudSource &cloud, float threshold)
An error metric for that computes the quality of the alignment between the given cloud and the target...
Definition ia_ransac.hpp:166
pcl::SampleConsensusInitialAlignment::PointCloudSourceConstPtr
typename PointCloudSource::ConstPtr PointCloudSourceConstPtr
Definition ia_ransac.h:73
pcl::SampleConsensusInitialAlignment::setMinSampleDistance
void setMinSampleDistance(float min_sample_distance)
Set the minimum distances between samples.
Definition ia_ransac.h:188
pcl::SampleConsensusInitialAlignment::PointCloudSource
typename Registration< PointSource, PointTarget >::PointCloudSource PointCloudSource
Definition ia_ransac.h:71
pcl::SampleConsensusInitialAlignment::feature_tree_
FeatureKdTreePtr feature_tree_
The KdTree used to compare feature descriptors.
Definition ia_ransac.h:323
pcl::SampleConsensusInitialAlignment::getRandomIndex
pcl::index_t getRandomIndex(int n)
Choose a random index between 0 and n-1.
Definition ia_ransac.h:262
pcl::SampleConsensusInitialAlignment::FeatureCloud
pcl::PointCloud< FeatureT > FeatureCloud
Definition ia_ransac.h:81
pcl::SampleConsensusInitialAlignment::setTargetFeatures
void setTargetFeatures(const FeatureCloudConstPtr &features)
Provide a shared pointer to the target point cloud's feature descriptors.
Definition ia_ransac.hpp:64
pcl::SampleConsensusInitialAlignment::FeatureKdTreePtr
typename KdTreeFLANN< FeatureT >::Ptr FeatureKdTreePtr
Definition ia_ransac.h:140
pcl::SampleConsensusInitialAlignment::setNumberOfSamples
void setNumberOfSamples(int nr_samples)
Set the number of samples to use during each iteration.
Definition ia_ransac.h:204
pcl::SampleConsensusInitialAlignment::target_features_
FeatureCloudConstPtr target_features_
The target point cloud's feature descriptors.
Definition ia_ransac.h:310
pcl::SampleConsensusInitialAlignment::getMinSampleDistance
float getMinSampleDistance()
Get the minimum distances between samples, as set by the user.
Definition ia_ransac.h:195
pcl::SampleConsensusInitialAlignment::Ptr
shared_ptr< SampleConsensusInitialAlignment< PointSource, PointTarget, FeatureT > > Ptr
Definition ia_ransac.h:86
pcl::SampleConsensusInitialAlignment::min_sample_distance_
float min_sample_distance_
The minimum distances between samples.
Definition ia_ransac.h:316
pcl::SampleConsensusInitialAlignment::ErrorFunctorPtr
typename ErrorFunctor::Ptr ErrorFunctorPtr
Definition ia_ransac.h:138
pcl::SampleConsensusInitialAlignment::FeatureCloudPtr
typename FeatureCloud::Ptr FeatureCloudPtr
Definition ia_ransac.h:82
pcl::SampleConsensusInitialAlignment::setErrorFunction
void setErrorFunction(const ErrorFunctorPtr &error_functor)
Specify the error function to minimize.
Definition ia_ransac.h:242
pcl::SampleConsensusInitialAlignment::input_features_
FeatureCloudConstPtr input_features_
The source point cloud's feature descriptors.
Definition ia_ransac.h:307
pcl::SampleConsensusInitialAlignment::nr_samples_
int nr_samples_
The number of samples to use during each iteration.
Definition ia_ransac.h:313
pcl::SampleConsensusInitialAlignment::selectSamples
void selectSamples(const PointCloudSource &cloud, unsigned int nr_samples, float min_sample_distance, pcl::Indices &sample_indices)
Select nr_samples sample points from cloud while making sure that their pairwise distances are greate...
Definition ia_ransac.hpp:79
pcl::SampleConsensusInitialAlignment::PointCloudSourcePtr
typename PointCloudSource::Ptr PointCloudSourcePtr
Definition ia_ransac.h:72
pcl::SampleConsensusInitialAlignment::getCorrespondenceRandomness
int getCorrespondenceRandomness()
Get the number of neighbors used when selecting a random feature correspondence, as set by the user.
Definition ia_ransac.h:231
pcl::SampleConsensusInitialAlignment::findSimilarFeatures
void findSimilarFeatures(const FeatureCloud &input_features, const pcl::Indices &sample_indices, 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 ia_ransac.hpp:142
pcl::SampleConsensusInitialAlignment::setCorrespondenceRandomness
void setCorrespondenceRandomness(int k)
Set the number of neighbors to use when selecting a random feature correspondence.
Definition ia_ransac.h:223
pcl::SampleConsensusInitialAlignment::FeatureCloudConstPtr
typename FeatureCloud::ConstPtr FeatureCloudConstPtr
Definition ia_ransac.h:83
pcl::SampleConsensusInitialAlignment::SampleConsensusInitialAlignment
SampleConsensusInitialAlignment()
Constructor.
Definition ia_ransac.h:142
pcl::SampleConsensusInitialAlignment::k_correspondences_
int k_correspondences_
The number of neighbors to use when selecting a random feature correspondence.
Definition ia_ransac.h:320
pcl::SampleConsensusInitialAlignment::error_functor_
ErrorFunctorPtr error_functor_
Definition ia_ransac.h:325
pcl::SampleConsensusInitialAlignment::getErrorFunction
ErrorFunctorPtr getErrorFunction()
Get a shared pointer to the ErrorFunctor that is to be minimized.
Definition ia_ransac.h:252
pcl::SampleConsensusInitialAlignment::getSourceFeatures
FeatureCloudConstPtr const getSourceFeatures()
Get a pointer to the source point cloud's features.
Definition ia_ransac.h:166
pcl::SampleConsensusInitialAlignment::PointCloudTarget
typename Registration< PointSource, PointTarget >::PointCloudTarget PointCloudTarget
Definition ia_ransac.h:76
pcl::SampleConsensusInitialAlignment::setSourceFeatures
void setSourceFeatures(const FeatureCloudConstPtr &features)
Provide a shared pointer to the source point cloud's feature descriptors.
Definition ia_ransac.hpp:50
pcl::registration::TransformationEstimationSVD
TransformationEstimationSVD implements SVD-based estimation of the transformation aligning the given ...
Definition transformation_estimation_svd.h:59
PCL_MAKE_ALIGNED_OPERATOR_NEW
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition memory.h:63
memory.h
Defines functions, macros and traits for allocating and using memory.
pcl
Definition convolution.h:46
pcl::index_t
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
Definition types.h:112
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