43 #include <pcl/sample_consensus/sac_model.h>
44 #include <pcl/pcl_base.h>
46 #include <boost/random/mersenne_twister.hpp>
47 #include <boost/random/uniform_01.hpp>
69 using Ptr = shared_ptr<SampleConsensus<T> >;
70 using ConstPtr = shared_ptr<const SampleConsensus<T> >;
81 ,
threshold_ (std::numeric_limits<double>::max ())
88 rng_->base ().seed (static_cast<unsigned> (std::time (
nullptr)));
90 rng_->base ().seed (12345u);
111 rng_->base ().seed (static_cast<unsigned> (std::time (
nullptr)));
113 rng_->base ().seed (12345u);
126 SampleConsensusModelPtr
189 refineModel (
const double sigma = 3.0,
const unsigned int max_iterations = 1000)
193 PCL_ERROR (
"[pcl::SampleConsensus::refineModel] Critical error: NULL model!\n");
199 double sigma_sqr = sigma * sigma;
200 unsigned int refine_iterations = 0;
201 bool inlier_changed =
false, oscillating =
false;
203 std::vector<std::size_t> inliers_sizes;
208 sac_model_->optimizeModelCoefficients (prev_inliers, new_model_coefficients, new_model_coefficients);
209 inliers_sizes.push_back (prev_inliers.size ());
212 sac_model_->selectWithinDistance (new_model_coefficients, error_threshold, new_inliers);
213 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);
215 if (new_inliers.empty ())
218 if (refine_iterations >= max_iterations)
225 double variance =
sac_model_->computeVariance ();
226 error_threshold = sqrt (std::min (inlier_distance_threshold_sqr, sigma_sqr * variance));
228 PCL_DEBUG (
"[pcl::SampleConsensus::refineModel] New estimated error threshold: %g on iteration %d out of %d.\n", error_threshold, refine_iterations, max_iterations);
229 inlier_changed =
false;
230 std::swap (prev_inliers, new_inliers);
232 if (new_inliers.size () != prev_inliers.size ())
235 if (inliers_sizes.size () >= 4)
237 if (inliers_sizes[inliers_sizes.size () - 1] == inliers_sizes[inliers_sizes.size () - 3] &&
238 inliers_sizes[inliers_sizes.size () - 2] == inliers_sizes[inliers_sizes.size () - 4])
244 inlier_changed =
true;
249 for (std::size_t i = 0; i < prev_inliers.size (); ++i)
252 if (prev_inliers[i] != new_inliers[i])
254 inlier_changed =
true;
259 while (inlier_changed && ++refine_iterations < max_iterations);
262 if (new_inliers.empty ())
264 PCL_ERROR (
"[pcl::SampleConsensus::refineModel] Refinement failed: got an empty set of inliers!\n");
270 PCL_DEBUG (
"[pcl::SampleConsensus::refineModel] Detected oscillations in the model refinement.\n");
291 std::size_t nr_samples,
292 std::set<index_t> &indices_subset)
294 indices_subset.clear ();
295 while (indices_subset.size () < nr_samples)
297 indices_subset.insert ((*indices)[static_cast<index_t> (static_cast<double>(indices->size ()) *
rnd ())]);
350 std::shared_ptr<boost::uniform_01<boost::mt19937> >
rng_;
SampleConsensusModelPtr getSampleConsensusModel() const
Get the Sample Consensus model used.
double threshold_
Distance to model threshold.
double rnd()
Boost-based random number generator.
Indices inliers_
The indices of the points that were chosen as inliers after the last computeModel () call...
int threads_
The number of threads the scheduler should use, or a negative number if no parallelization is wanted...
shared_ptr< Indices > IndicesPtr
void setDistanceThreshold(double threshold)
Set the distance to model threshold.
void getInliers(Indices &inliers) const
Return the best set of inliers found so far for this model.
int getMaxIterations() const
Get the maximum number of iterations, as set by the user.
double getProbability() const
Obtain the probability of choosing at least one sample free from outliers, as set by the user...
void getRandomSamples(const IndicesPtr &indices, std::size_t nr_samples, std::set< index_t > &indices_subset)
Get a set of randomly selected indices.
SampleConsensusModel represents the base model class.
shared_ptr< SampleConsensus< WeightSACPointType > > Ptr
shared_ptr< const SampleConsensus< WeightSACPointType > > ConstPtr
SampleConsensusModelPtr sac_model_
The underlying data model used (i.e.
virtual ~SampleConsensus()=default
Destructor for base SAC.
void getModel(Indices &model) const
Return the best model found so far.
double getDistanceThreshold() const
Get the distance to model threshold, as set by the user.
virtual bool computeModel(int debug_verbosity_level=0)=0
Compute the actual model.
IndicesAllocator<> Indices
Type used for indices in PCL.
void setMaxIterations(int max_iterations)
Set the maximum number of iterations.
int getNumberOfThreads() const
Get the number of threads, as set by the user.
std::shared_ptr< boost::uniform_01< boost::mt19937 > > rng_
Boost-based random number generator distribution.
virtual bool refineModel(const double sigma=3.0, const unsigned int max_iterations=1000)
Refine the model found.
int iterations_
Total number of internal loop iterations that we've done so far.
double probability_
Desired probability of choosing at least one sample free from outliers.
SampleConsensus(const SampleConsensusModelPtr &model, bool random=false)
Constructor for base SAC.
Eigen::VectorXf model_coefficients_
The coefficients of our model computed directly from the model found.
Indices model_
The model found after the last computeModel () as point cloud indices.
void setProbability(double probability)
Set the desired probability of choosing at least one sample free from outliers.
void setSampleConsensusModel(const SampleConsensusModelPtr &model)
Set the Sample Consensus model to use.
void setNumberOfThreads(const int nr_threads=-1)
Set the number of threads to use or turn off parallelization.
void getModelCoefficients(Eigen::VectorXf &model_coefficients) const
Return the model coefficients of the best model found so far.
int max_iterations_
Maximum number of iterations before giving up.
boost::mt19937 rng_alg_
Boost-based random number generator algorithm.
SampleConsensus(const SampleConsensusModelPtr &model, double threshold, bool random=false)
Constructor for base SAC.
SampleConsensus represents the base class.