Point Cloud Library (PCL)
1.11.1-dev
|
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_;
double rnd()
Boost-based random number generator.
void getInliers(Indices &inliers) const
Return the best set of inliers found so far for this model.
int getNumberOfThreads() const
Get the number of threads, as set by the user.
Indices model_
The model found after the last computeModel () as point cloud indices.
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.
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
void setProbability(double probability)
Set the desired probability of choosing at least one sample free from outliers.
Indices 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.
IndicesAllocator<> Indices
Type used for indices in PCL.
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 getRandomSamples(const IndicesPtr &indices, std::size_t nr_samples, std::set< index_t > &indices_subset)
Get a set of randomly selected indices.
void setSampleConsensusModel(const SampleConsensusModelPtr &model)
Set the Sample Consensus model to use.
void getModel(Indices &model) const
Return the best model found so far.
SampleConsensusModel represents the base model class.
SampleConsensus represents the base class.
int max_iterations_
Maximum number of iterations before giving up.