Loading...
Searching...
No Matches
sac.h
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2010-2011, 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/sample_consensus/sac_model.h>
#include <pcl/pcl_base.h>
#include <boost/random/mersenne_twister.hpp> // for mt19937
#include <boost/random/uniform_01.hpp> // for uniform_01
#include <ctime>
#include <memory>
#include <set>
namespace pcl
{
/** \brief SampleConsensus represents the base class. All sample consensus methods must inherit from this class.
* \author Radu Bogdan Rusu
* \ingroup sample_consensus
*/
template <typename T>
class SampleConsensus
{
using SampleConsensusModelPtr = typename SampleConsensusModel<T>::Ptr;
private:
/** \brief Constructor for base SAC. */
SampleConsensus () = default;
public:
using Ptr = shared_ptr<SampleConsensus<T> >;
using ConstPtr = shared_ptr<const SampleConsensus<T> >;
/** \brief Constructor for base SAC.
* \param[in] model a Sample Consensus model
* \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
*/
SampleConsensus (const SampleConsensusModelPtr &model, bool random = false)
: sac_model_ (model)
, probability_ (0.99)
, iterations_ (0)
, threshold_ (std::numeric_limits<double>::max ())
, max_iterations_ (1000)
, threads_ (-1)
, rng_ (new boost::uniform_01<boost::mt19937> (rng_alg_))
{
// Create a random number generator object
if (random)
rng_->base ().seed (static_cast<unsigned> (std::time (nullptr)));
else
rng_->base ().seed (12345u);
};
/** \brief Constructor for base SAC.
* \param[in] model a Sample Consensus model
* \param[in] threshold distance to model threshold
* \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
*/
SampleConsensus (const SampleConsensusModelPtr &model,
double threshold,
bool random = false)
: sac_model_ (model)
, probability_ (0.99)
, iterations_ (0)
, threshold_ (threshold)
, max_iterations_ (1000)
, threads_ (-1)
, rng_ (new boost::uniform_01<boost::mt19937> (rng_alg_))
{
// Create a random number generator object
if (random)
rng_->base ().seed (static_cast<unsigned> (std::time (nullptr)));
else
rng_->base ().seed (12345u);
};
/** \brief Set the Sample Consensus model to use.
* \param[in] model a Sample Consensus model
*/
void
setSampleConsensusModel (const SampleConsensusModelPtr &model)
{
sac_model_ = model;
}
/** \brief Get the Sample Consensus model used. */
SampleConsensusModelPtr
getSampleConsensusModel () const
{
return (sac_model_);
}
/** \brief Destructor for base SAC. */
virtual ~SampleConsensus () = default;
/** \brief Set the distance to model threshold.
* \param[in] threshold distance to model threshold
*/
inline void
setDistanceThreshold (double threshold) { threshold_ = threshold; }
/** \brief Get the distance to model threshold, as set by the user. */
inline double
getDistanceThreshold () const { return (threshold_); }
/** \brief Set the maximum number of iterations.
* \param[in] max_iterations maximum number of iterations
*/
inline void
setMaxIterations (int max_iterations) { max_iterations_ = max_iterations; }
/** \brief Get the maximum number of iterations, as set by the user. */
inline int
getMaxIterations () const { return (max_iterations_); }
/** \brief Set the desired probability of choosing at least one sample free from outliers.
* \param[in] probability the desired probability of choosing at least one sample free from outliers
* \note internally, the probability is set to 99% (0.99) by default.
*/
inline void
setProbability (double probability) { probability_ = probability; }
/** \brief Obtain the probability of choosing at least one sample free from outliers, as set by the user. */
inline double
getProbability () const { return (probability_); }
/** \brief Set the number of threads to use or turn off parallelization.
* \param[in] nr_threads the number of hardware threads to use (0 sets the value automatically, a negative number turns parallelization off)
* \note Not all SAC methods have a parallel implementation. Some will ignore this setting.
*/
inline void
setNumberOfThreads (const int nr_threads = -1) { threads_ = nr_threads; }
/** \brief Get the number of threads, as set by the user. */
inline int
getNumberOfThreads () const { return (threads_); }
/** \brief Compute the actual model. Pure virtual. */
virtual bool
computeModel (int debug_verbosity_level = 0) = 0;
/** \brief Refine the model found.
* This loops over the model coefficients and optimizes them together
* with the set of inliers, until the change in the set of inliers is
* minimal.
* \param[in] sigma standard deviation multiplier for considering a sample as inlier (Mahalanobis distance)
* \param[in] max_iterations the maxim number of iterations to try to refine in case the inliers keep on changing
*/
virtual bool
refineModel (const double sigma = 3.0, const unsigned int max_iterations = 1000)
{
if (!sac_model_)
{
PCL_ERROR ("[pcl::SampleConsensus::refineModel] Critical error: NULL model!\n");
return (false);
}
double inlier_distance_threshold_sqr = threshold_ * threshold_,
error_threshold = threshold_;
double sigma_sqr = sigma * sigma;
unsigned int refine_iterations = 0;
bool inlier_changed = false, oscillating = false;
Indices new_inliers, prev_inliers = inliers_;
std::vector<std::size_t> inliers_sizes;
Eigen::VectorXf new_model_coefficients = model_coefficients_;
do
{
// Optimize the model coefficients
sac_model_->optimizeModelCoefficients (prev_inliers, new_model_coefficients, new_model_coefficients);
inliers_sizes.push_back (prev_inliers.size ());
// Select the new inliers based on the optimized coefficients and new threshold
sac_model_->selectWithinDistance (new_model_coefficients, error_threshold, new_inliers);
PCL_DEBUG ("[pcl::SampleConsensus::refineModel] Number of inliers found (before/after): %lu/%lu, with an error threshold of %g.\n", prev_inliers.size (), new_inliers.size (), error_threshold);
if (new_inliers.empty ())
{
refine_iterations++;
if (refine_iterations >= max_iterations)
break;
continue;
//return (false);
}
// Estimate the variance and the new threshold
double variance = sac_model_->computeVariance ();
error_threshold = sqrt (std::min (inlier_distance_threshold_sqr, sigma_sqr * variance));
PCL_DEBUG ("[pcl::SampleConsensus::refineModel] New estimated error threshold: %g on iteration %d out of %d.\n", error_threshold, refine_iterations, max_iterations);
inlier_changed = false;
std::swap (prev_inliers, new_inliers);
// If the number of inliers changed, then we are still optimizing
if (new_inliers.size () != prev_inliers.size ())
{
// Check if the number of inliers is oscillating in between two values
if (inliers_sizes.size () >= 4)
{
if (inliers_sizes[inliers_sizes.size () - 1] == inliers_sizes[inliers_sizes.size () - 3] &&
inliers_sizes[inliers_sizes.size () - 2] == inliers_sizes[inliers_sizes.size () - 4])
{
oscillating = true;
break;
}
}
inlier_changed = true;
continue;
}
// Check the values of the inlier set
for (std::size_t i = 0; i < prev_inliers.size (); ++i)
{
// If the value of the inliers changed, then we are still optimizing
if (prev_inliers[i] != new_inliers[i])
{
inlier_changed = true;
break;
}
}
}
while (inlier_changed && ++refine_iterations < max_iterations);
// If the new set of inliers is empty, we didn't do a good job refining
if (new_inliers.empty ())
{
PCL_ERROR ("[pcl::SampleConsensus::refineModel] Refinement failed: got an empty set of inliers!\n");
return (false);
}
if (oscillating)
{
PCL_DEBUG ("[pcl::SampleConsensus::refineModel] Detected oscillations in the model refinement.\n");
return (true);
}
// If no inliers have been changed anymore, then the refinement was successful
if (!inlier_changed)
{
std::swap (inliers_, new_inliers);
model_coefficients_ = new_model_coefficients;
return (true);
}
return (false);
}
/** \brief Get a set of randomly selected indices.
* \param[in] indices the input indices vector
* \param[in] nr_samples the desired number of point indices to randomly select
* \param[out] indices_subset the resultant output set of randomly selected indices
*/
inline void
getRandomSamples (const IndicesPtr &indices,
std::size_t nr_samples,
std::set<index_t> &indices_subset)
{
indices_subset.clear ();
while (indices_subset.size () < nr_samples)
//indices_subset.insert ((*indices)[(index_t) (indices->size () * (rand () / (RAND_MAX + 1.0)))]);
indices_subset.insert ((*indices)[static_cast<index_t> (static_cast<double>(indices->size ()) * rnd ())]);
}
/** \brief Return the best model found so far.
* \param[out] model the resultant model
*/
inline void
getModel (Indices &model) const { model = model_; }
/** \brief Return the best set of inliers found so far for this model.
* \param[out] inliers the resultant set of inliers
*/
inline void
getInliers (Indices &inliers) const { inliers = inliers_; }
/** \brief Return the model coefficients of the best model found so far.
* \param[out] model_coefficients the resultant model coefficients, as documented in \ref sample_consensus
*/
inline void
getModelCoefficients (Eigen::VectorXf &model_coefficients) const { model_coefficients = model_coefficients_; }
protected:
/** \brief The underlying data model used (i.e. what is it that we attempt to search for). */
SampleConsensusModelPtr sac_model_;
/** \brief The model found after the last computeModel () as point cloud indices. */
Indices model_;
/** \brief The indices of the points that were chosen as inliers after the last computeModel () call. */
Indices inliers_;
/** \brief The coefficients of our model computed directly from the model found. */
Eigen::VectorXf model_coefficients_;
/** \brief Desired probability of choosing at least one sample free from outliers. */
double probability_;
/** \brief Total number of internal loop iterations that we've done so far. */
int iterations_;
/** \brief Distance to model threshold. */
double threshold_;
/** \brief Maximum number of iterations before giving up. */
int max_iterations_;
/** \brief The number of threads the scheduler should use, or a negative number if no parallelization is wanted. */
int threads_;
/** \brief Boost-based random number generator algorithm. */
boost::mt19937 rng_alg_;
/** \brief Boost-based random number generator distribution. */
std::shared_ptr<boost::uniform_01<boost::mt19937> > rng_;
/** \brief Boost-based random number generator. */
inline double
rnd ()
{
return ((*rng_) ());
}
};
}
pcl::SampleConsensus
SampleConsensus represents the base class.
Definition sac.h:61
pcl::SampleConsensus::probability_
double probability_
Desired probability of choosing at least one sample free from outliers.
Definition sac.h:332
pcl::SampleConsensus::rng_
std::shared_ptr< boost::uniform_01< boost::mt19937 > > rng_
Boost-based random number generator distribution.
Definition sac.h:350
pcl::SampleConsensus::getSampleConsensusModel
SampleConsensusModelPtr getSampleConsensusModel() const
Get the Sample Consensus model used.
Definition sac.h:127
pcl::SampleConsensus::Ptr
shared_ptr< SampleConsensus< T > > Ptr
Definition sac.h:69
pcl::SampleConsensus::getDistanceThreshold
double getDistanceThreshold() const
Get the distance to model threshold, as set by the user.
Definition sac.h:143
pcl::SampleConsensus::rng_alg_
boost::mt19937 rng_alg_
Boost-based random number generator algorithm.
Definition sac.h:347
pcl::SampleConsensus::getNumberOfThreads
int getNumberOfThreads() const
Get the number of threads, as set by the user.
Definition sac.h:175
pcl::SampleConsensus::inliers_
Indices inliers_
The indices of the points that were chosen as inliers after the last computeModel () call.
Definition sac.h:326
pcl::SampleConsensus::getMaxIterations
int getMaxIterations() const
Get the maximum number of iterations, as set by the user.
Definition sac.h:153
pcl::SampleConsensus::getInliers
void getInliers(Indices &inliers) const
Return the best set of inliers found so far for this model.
Definition sac.h:310
pcl::SampleConsensus::iterations_
int iterations_
Total number of internal loop iterations that we've done so far.
Definition sac.h:335
pcl::SampleConsensus::getRandomSamples
void getRandomSamples(const IndicesPtr &indices, std::size_t nr_samples, std::set< index_t > &indices_subset)
Get a set of randomly selected indices.
Definition sac.h:290
pcl::SampleConsensus::computeModel
virtual bool computeModel(int debug_verbosity_level=0)=0
Compute the actual model.
pcl::SampleConsensus::model_
Indices model_
The model found after the last computeModel () as point cloud indices.
Definition sac.h:323
pcl::SampleConsensus::rnd
double rnd()
Boost-based random number generator.
Definition sac.h:354
pcl::SampleConsensus::setNumberOfThreads
void setNumberOfThreads(const int nr_threads=-1)
Set the number of threads to use or turn off parallelization.
Definition sac.h:171
pcl::SampleConsensus::getModel
void getModel(Indices &model) const
Return the best model found so far.
Definition sac.h:304
pcl::SampleConsensus::getModelCoefficients
void getModelCoefficients(Eigen::VectorXf &model_coefficients) const
Return the model coefficients of the best model found so far.
Definition sac.h:316
pcl::SampleConsensus::model_coefficients_
Eigen::VectorXf model_coefficients_
The coefficients of our model computed directly from the model found.
Definition sac.h:329
pcl::SampleConsensus::threshold_
double threshold_
Distance to model threshold.
Definition sac.h:338
pcl::SampleConsensus::sac_model_
SampleConsensusModelPtr sac_model_
The underlying data model used (i.e.
Definition sac.h:320
pcl::SampleConsensus::threads_
int threads_
The number of threads the scheduler should use, or a negative number if no parallelization is wanted.
Definition sac.h:344
pcl::SampleConsensus::SampleConsensus
SampleConsensus(const SampleConsensusModelPtr &model, double threshold, bool random=false)
Constructor for base SAC.
Definition sac.h:98
pcl::SampleConsensus::getProbability
double getProbability() const
Obtain the probability of choosing at least one sample free from outliers, as set by the user.
Definition sac.h:164
pcl::SampleConsensus::max_iterations_
int max_iterations_
Maximum number of iterations before giving up.
Definition sac.h:341
pcl::SampleConsensus::setSampleConsensusModel
void setSampleConsensusModel(const SampleConsensusModelPtr &model)
Set the Sample Consensus model to use.
Definition sac.h:120
pcl::SampleConsensus::setProbability
void setProbability(double probability)
Set the desired probability of choosing at least one sample free from outliers.
Definition sac.h:160
pcl::SampleConsensus::~SampleConsensus
virtual ~SampleConsensus()=default
Destructor for base SAC.
pcl::SampleConsensus::setDistanceThreshold
void setDistanceThreshold(double threshold)
Set the distance to model threshold.
Definition sac.h:139
pcl::SampleConsensus::SampleConsensus
SampleConsensus(const SampleConsensusModelPtr &model, bool random=false)
Constructor for base SAC.
Definition sac.h:77
pcl::SampleConsensus::ConstPtr
shared_ptr< const SampleConsensus< T > > ConstPtr
Definition sac.h:70
pcl::SampleConsensus::refineModel
virtual bool refineModel(const double sigma=3.0, const unsigned int max_iterations=1000)
Refine the model found.
Definition sac.h:189
pcl::SampleConsensus::setMaxIterations
void setMaxIterations(int max_iterations)
Set the maximum number of iterations.
Definition sac.h:149
pcl::SampleConsensusModel::Ptr
shared_ptr< SampleConsensusModel< PointT > > Ptr
Definition sac_model.h:78
boost
Definition boost_graph.h:48
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::IndicesPtr
shared_ptr< Indices > IndicesPtr
Definition pcl_base.h:58