360 lines
14 KiB
C
360 lines
14 KiB
C
|
/*
|
||
|
|
* 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 () {};
|
||
|
|
|
||
|
|
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 () {};
|
||
|
|
|
||
|
|
/** \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_) ());
|
||
|
|
}
|
||
|
|
};
|
||
|
|
}
|