336 lines
11 KiB
C++
336 lines
11 KiB
C++
/*
|
|
* 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() {}
|
|
|
|
public:
|
|
HuberPenalty(float threshold) : threshold_(threshold) {}
|
|
virtual float
|
|
operator()(float e) const
|
|
{
|
|
if (e <= threshold_)
|
|
return (0.5 * e * e);
|
|
return (0.5 * threshold_ * (2.0 * std::fabs(e) - threshold_));
|
|
}
|
|
|
|
protected:
|
|
float threshold_;
|
|
};
|
|
|
|
class TruncatedError : public ErrorFunctor {
|
|
private:
|
|
TruncatedError() {}
|
|
|
|
public:
|
|
~TruncatedError() {}
|
|
|
|
TruncatedError(float threshold) : threshold_(threshold) {}
|
|
float
|
|
operator()(float e) const override
|
|
{
|
|
if (e <= threshold_)
|
|
return (e / threshold_);
|
|
return (1.0);
|
|
}
|
|
|
|
protected:
|
|
float threshold_;
|
|
};
|
|
|
|
using ErrorFunctorPtr = typename ErrorFunctor::Ptr;
|
|
|
|
using FeatureKdTreePtr = typename KdTreeFLANN<FeatureT>::Ptr;
|
|
/** \brief Constructor. */
|
|
SampleConsensusInitialAlignment()
|
|
: input_features_()
|
|
, target_features_()
|
|
, nr_samples_(3)
|
|
, min_sample_distance_(0.0f)
|
|
, k_correspondences_(10)
|
|
, 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_;
|
|
|
|
/** \brief The minimum distances between samples. */
|
|
float min_sample_distance_;
|
|
|
|
/** \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_;
|
|
|
|
ErrorFunctorPtr error_functor_;
|
|
|
|
public:
|
|
PCL_MAKE_ALIGNED_OPERATOR_NEW
|
|
};
|
|
} // namespace pcl
|
|
|
|
#include <pcl/registration/impl/ia_ransac.hpp>
|