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> >;
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;
200 Indices new_inliers, prev_inliers =
inliers_;
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_;
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.
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.
virtual ~SampleConsensus()
Destructor for base SAC.
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.