43 #include <pcl/sample_consensus/boost.h>
44 #include <pcl/sample_consensus/sac_model.h>
45 #include <pcl/pcl_base.h>
67 using Ptr = shared_ptr<SampleConsensus<T> >;
68 using ConstPtr = shared_ptr<const SampleConsensus<T> >;
79 ,
threshold_ (std::numeric_limits<double>::max ())
86 rng_->base ().seed (
static_cast<unsigned> (std::time (
nullptr)));
88 rng_->base ().seed (12345u);
109 rng_->base ().seed (
static_cast<unsigned> (std::time (
nullptr)));
111 rng_->base ().seed (12345u);
124 SampleConsensusModelPtr
187 refineModel (
const double sigma = 3.0,
const unsigned int max_iterations = 1000)
191 PCL_ERROR (
"[pcl::SampleConsensus::refineModel] Critical error: NULL model!\n");
197 double sigma_sqr = sigma * sigma;
198 unsigned int refine_iterations = 0;
199 bool inlier_changed =
false, oscillating =
false;
201 std::vector<std::size_t> inliers_sizes;
206 sac_model_->optimizeModelCoefficients (prev_inliers, new_model_coefficients, new_model_coefficients);
207 inliers_sizes.push_back (prev_inliers.size ());
210 sac_model_->selectWithinDistance (new_model_coefficients, error_threshold, new_inliers);
211 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);
213 if (new_inliers.empty ())
216 if (refine_iterations >= max_iterations)
223 double variance =
sac_model_->computeVariance ();
224 error_threshold = sqrt (std::min (inlier_distance_threshold_sqr, sigma_sqr * variance));
226 PCL_DEBUG (
"[pcl::SampleConsensus::refineModel] New estimated error threshold: %g on iteration %d out of %d.\n", error_threshold, refine_iterations, max_iterations);
227 inlier_changed =
false;
228 std::swap (prev_inliers, new_inliers);
230 if (new_inliers.size () != prev_inliers.size ())
233 if (inliers_sizes.size () >= 4)
235 if (inliers_sizes[inliers_sizes.size () - 1] == inliers_sizes[inliers_sizes.size () - 3] &&
236 inliers_sizes[inliers_sizes.size () - 2] == inliers_sizes[inliers_sizes.size () - 4])
242 inlier_changed =
true;
247 for (std::size_t i = 0; i < prev_inliers.size (); ++i)
250 if (prev_inliers[i] != new_inliers[i])
252 inlier_changed =
true;
257 while (inlier_changed && ++refine_iterations < max_iterations);
260 if (new_inliers.empty ())
262 PCL_ERROR (
"[pcl::SampleConsensus::refineModel] Refinement failed: got an empty set of inliers!\n");
268 PCL_DEBUG (
"[pcl::SampleConsensus::refineModel] Detected oscillations in the model refinement.\n");
289 std::size_t nr_samples,
290 std::set<index_t> &indices_subset)
292 indices_subset.clear ();
293 while (indices_subset.size () < nr_samples)
295 indices_subset.insert ((*indices)[
static_cast<index_t> (
static_cast<double>(indices->size ()) *
rnd ())]);
348 std::shared_ptr<boost::uniform_01<boost::mt19937> >
rng_;
SampleConsensus represents the base class.
double probability_
Desired probability of choosing at least one sample free from outliers.
std::shared_ptr< boost::uniform_01< boost::mt19937 > > rng_
Boost-based random number generator distribution.
virtual ~SampleConsensus()
Destructor for base SAC.
SampleConsensusModelPtr getSampleConsensusModel() const
Get the Sample Consensus model used.
shared_ptr< SampleConsensus< T > > Ptr
double getDistanceThreshold() const
Get the distance to model threshold, as set by the user.
boost::mt19937 rng_alg_
Boost-based random number generator algorithm.
int getNumberOfThreads() const
Get the number of threads, as set by the user.
Indices inliers_
The indices of the points that were chosen as inliers after the last computeModel () call.
int getMaxIterations() const
Get the maximum number of iterations, as set by the user.
void getInliers(Indices &inliers) const
Return the best set of inliers found so far for this model.
int iterations_
Total number of internal loop iterations that we've done so far.
void getRandomSamples(const IndicesPtr &indices, std::size_t nr_samples, std::set< index_t > &indices_subset)
Get a set of randomly selected indices.
virtual bool computeModel(int debug_verbosity_level=0)=0
Compute the actual model.
Indices model_
The model found after the last computeModel () as point cloud indices.
double rnd()
Boost-based random number generator.
void setNumberOfThreads(const int nr_threads=-1)
Set the number of threads to use or turn off parallelization.
void getModel(Indices &model) const
Return the best model found so far.
void getModelCoefficients(Eigen::VectorXf &model_coefficients) const
Return the model coefficients of the best model found so far.
Eigen::VectorXf model_coefficients_
The coefficients of our model computed directly from the model found.
double threshold_
Distance to model threshold.
SampleConsensusModelPtr sac_model_
The underlying data model used (i.e.
int threads_
The number of threads the scheduler should use, or a negative number if no parallelization is wanted.
SampleConsensus(const SampleConsensusModelPtr &model, double threshold, bool random=false)
Constructor for base SAC.
double getProbability() const
Obtain the probability of choosing at least one sample free from outliers, as set by the user.
int max_iterations_
Maximum number of iterations before giving up.
void setSampleConsensusModel(const SampleConsensusModelPtr &model)
Set the Sample Consensus model to use.
void setProbability(double probability)
Set the desired probability of choosing at least one sample free from outliers.
void setDistanceThreshold(double threshold)
Set the distance to model threshold.
SampleConsensus(const SampleConsensusModelPtr &model, bool random=false)
Constructor for base SAC.
shared_ptr< const SampleConsensus< T > > ConstPtr
virtual bool refineModel(const double sigma=3.0, const unsigned int max_iterations=1000)
Refine the model found.
void setMaxIterations(int max_iterations)
Set the maximum number of iterations.
SampleConsensusModel represents the base model class.
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
shared_ptr< Indices > IndicesPtr
std::vector< index_t > Indices
Type used for indices in PCL.