Point Cloud Library (PCL)
1.10.1
|
43 #include <pcl/sample_consensus/boost.h>
44 #include <pcl/sample_consensus/sac_model.h>
45 #include <pcl/pcl_base.h>
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;
200 std::vector<int> 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<int> &indices_subset)
292 indices_subset.clear ();
293 while (indices_subset.size () < nr_samples)
295 indices_subset.insert ((*indices)[
static_cast<int> (
static_cast<double>(indices->size ()) *
rnd ())]);
348 std::shared_ptr<boost::uniform_01<boost::mt19937> >
rng_;
void getRandomSamples(const IndicesPtr &indices, std::size_t nr_samples, std::set< int > &indices_subset)
Get a set of randomly selected indices.
double rnd()
Boost-based random number generator.
int getNumberOfThreads() const
Get the number of threads, as set by the user.
This file defines compatibility wrappers for low level I/O functions.
shared_ptr< Indices > IndicesPtr
shared_ptr< SampleConsensus< WeightSACPointType > > Ptr
SampleConsensusModelPtr getSampleConsensusModel() const
Get the Sample Consensus model used.
SampleConsensus(const SampleConsensusModelPtr &model, double threshold, bool random=false)
Constructor for base SAC.
std::shared_ptr< boost::uniform_01< boost::mt19937 > > rng_
Boost-based random number generator distribution.
virtual ~SampleConsensus()
Destructor for base SAC.
double getProbability() const
Obtain the probability of choosing at least one sample free from outliers, as set by the user.
boost::mt19937 rng_alg_
Boost-based random number generator algorithm.
Eigen::VectorXf model_coefficients_
The coefficients of our model computed directly from the model found.
double probability_
Desired probability of choosing at least one sample free from outliers.
void setProbability(double probability)
Set the desired probability of choosing at least one sample free from outliers.
std::vector< int > model_
The model found after the last computeModel () as point cloud indices.
void getModel(std::vector< int > &model) const
Return the best model found so far.
std::vector< int > inliers_
The indices of the points that were chosen as inliers after the last computeModel () call.
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.
virtual bool computeModel(int debug_verbosity_level=0)=0
Compute the actual model.
SampleConsensus(const SampleConsensusModelPtr &model, bool random=false)
Constructor for base SAC.
double getDistanceThreshold() const
Get the distance to model threshold, as set by the user.
double threshold_
Distance to model threshold.
int iterations_
Total number of internal loop iterations that we've done so far.
int threads_
The number of threads the scheduler should use, or a negative number if no parallelization is wanted.
SampleConsensusModelPtr sac_model_
The underlying data model used (i.e.
void setDistanceThreshold(double threshold)
Set the distance to model threshold.
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 getMaxIterations() const
Get the maximum number of iterations, as set by the user.
shared_ptr< const SampleConsensus< WeightSACPointType > > ConstPtr
void setSampleConsensusModel(const SampleConsensusModelPtr &model)
Set the Sample Consensus model to use.
void getInliers(std::vector< int > &inliers) const
Return the best set of inliers found so far for this model.
SampleConsensusModel represents the base model class.
SampleConsensus represents the base class.
int max_iterations_
Maximum number of iterations before giving up.
boost::shared_ptr< T > shared_ptr
Alias for boost::shared_ptr.