41 #ifndef PCL_SAMPLE_CONSENSUS_H_ 42 #define PCL_SAMPLE_CONSENSUS_H_ 44 #include <pcl/sample_consensus/boost.h> 45 #include <pcl/sample_consensus/sac_model.h> 65 typedef boost::shared_ptr<SampleConsensus>
Ptr;
66 typedef boost::shared_ptr<const SampleConsensus>
ConstPtr;
86 rng_->base ().seed (static_cast<unsigned> (std::time (0)));
88 rng_->base ().seed (12345u);
112 rng_->base ().seed (static_cast<unsigned> (std::time (0)));
114 rng_->base ().seed (12345u);
127 SampleConsensusModelPtr
179 refineModel (
const double sigma = 3.0,
const unsigned int max_iterations = 1000)
183 PCL_ERROR (
"[pcl::SampleConsensus::refineModel] Critical error: NULL model!\n");
189 double sigma_sqr = sigma * sigma;
190 unsigned int refine_iterations = 0;
191 bool inlier_changed =
false, oscillating =
false;
192 std::vector<int> new_inliers, prev_inliers =
inliers_;
193 std::vector<size_t> inliers_sizes;
198 sac_model_->optimizeModelCoefficients (prev_inliers, new_model_coefficients, new_model_coefficients);
199 inliers_sizes.push_back (prev_inliers.size ());
202 sac_model_->selectWithinDistance (new_model_coefficients, error_threshold, new_inliers);
203 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);
205 if (new_inliers.empty ())
208 if (refine_iterations >= max_iterations)
215 double variance =
sac_model_->computeVariance ();
216 error_threshold = sqrt (std::min (inlier_distance_threshold_sqr, sigma_sqr * variance));
218 PCL_DEBUG (
"[pcl::SampleConsensus::refineModel] New estimated error threshold: %g on iteration %d out of %d.\n", error_threshold, refine_iterations, max_iterations);
219 inlier_changed =
false;
220 std::swap (prev_inliers, new_inliers);
222 if (new_inliers.size () != prev_inliers.size ())
225 if (inliers_sizes.size () >= 4)
227 if (inliers_sizes[inliers_sizes.size () - 1] == inliers_sizes[inliers_sizes.size () - 3] &&
228 inliers_sizes[inliers_sizes.size () - 2] == inliers_sizes[inliers_sizes.size () - 4])
234 inlier_changed =
true;
239 for (
size_t i = 0; i < prev_inliers.size (); ++i)
242 if (prev_inliers[i] != new_inliers[i])
244 inlier_changed =
true;
249 while (inlier_changed && ++refine_iterations < max_iterations);
252 if (new_inliers.empty ())
254 PCL_ERROR (
"[pcl::SampleConsensus::refineModel] Refinement failed: got an empty set of inliers!\n");
260 PCL_DEBUG (
"[pcl::SampleConsensus::refineModel] Detected oscillations in the model refinement.\n");
282 std::set<int> &indices_subset)
284 indices_subset.clear ();
285 while (indices_subset.size () < nr_samples)
287 indices_subset.insert ((*indices)[static_cast<int> (static_cast<double>(indices->size ()) *
rnd ())]);
337 boost::shared_ptr<boost::uniform_01<boost::mt19937> >
rng_;
348 #endif //#ifndef PCL_SAMPLE_CONSENSUS_H_
int getMaxIterations()
Get the maximum number of iterations, as set by the user.
This file defines compatibility wrappers for low level I/O functions.
double threshold_
Distance to model threshold.
double rnd()
Boost-based random number generator.
void setDistanceThreshold(double threshold)
Set the distance to model threshold.
boost::shared_ptr< boost::uniform_01< boost::mt19937 > > rng_
Boost-based random number generator distribution.
double getDistanceThreshold()
Get the distance to model threshold, as set by the user.
void getModel(std::vector< int > &model)
Return the best model found so far.
boost::shared_ptr< SampleConsensus > Ptr
std::vector< int > inliers_
The indices of the points that were chosen as inliers after the last computeModel () call.
void getInliers(std::vector< int > &inliers)
Return the best set of inliers found so far for this model.
SampleConsensusModel represents the base model class.
boost::shared_ptr< const SampleConsensus > ConstPtr
double getProbability()
Obtain the probability of choosing at least one sample free from outliers, as set by the user.
void getRandomSamples(const boost::shared_ptr< std::vector< int > > &indices, size_t nr_samples, std::set< int > &indices_subset)
Get a set of randomly selected indices.
SampleConsensusModelPtr sac_model_
The underlying data model used (i.e.
void getModelCoefficients(Eigen::VectorXf &model_coefficients)
Return the model coefficients of the best model found so far.
std::vector< int > model_
The model found after the last computeModel () as point cloud indices.
virtual bool computeModel(int debug_verbosity_level=0)=0
Compute the actual model.
SampleConsensusModelPtr getSampleConsensusModel() const
Get the Sample Consensus model used.
virtual ~SampleConsensus()
Destructor for base SAC.
void setMaxIterations(int max_iterations)
Set the maximum number of iterations.
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.
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.
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.