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): %zu/%zu, 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_
double rnd()
Boost-based random number generator.
std::vector< int > inliers_
The indices of the points that were chosen as inliers after the last computeModel () call...
boost::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.
SampleConsensus(const SampleConsensusModelPtr &model, bool random=false)
Constructor for base SAC.
int iterations_
Total number of internal loop iterations that we've done so far.
void setMaxIterations(int max_iterations)
Set the maximum number of iterations.
std::vector< int > model_
The model found after the last computeModel () as point cloud indices.
double threshold_
Distance to model threshold.
void getModel(std::vector< int > &model)
Return the best model found so far.
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.
SampleConsensus(const SampleConsensusModelPtr &model, double threshold, bool random=false)
Constructor for base SAC.
void setSampleConsensusModel(const SampleConsensusModelPtr &model)
Set the Sample Consensus model to use.
void setDistanceThreshold(double threshold)
Set the distance to model threshold.
double probability_
Desired probability of choosing at least one sample free from outliers.
boost::shared_ptr< SampleConsensus > Ptr
void getInliers(std::vector< int > &inliers)
Return the best set of inliers found so far for this model.
SampleConsensusModelPtr getSampleConsensusModel() const
Get the Sample Consensus model used.
int getMaxIterations()
Get the maximum number of iterations, as set by the user.
double getDistanceThreshold()
Get the distance to model threshold, as set by the user.
boost::mt19937 rng_alg_
Boost-based random number generator algorithm.
SampleConsensusModel represents the base model class.
double getProbability()
Obtain the probability of choosing at least one sample free from outliers, as set by the user...
SampleConsensus represents the base class.
virtual ~SampleConsensus()
Destructor for base SAC.
void getModelCoefficients(Eigen::VectorXf &model_coefficients)
Return the model coefficients of the best model found so far.
void setProbability(double probability)
Set the desired probability of choosing at least one sample free from outliers.
boost::shared_ptr< const SampleConsensus > ConstPtr
SampleConsensusModelPtr sac_model_
The underlying data model used (i.e.
Eigen::VectorXf model_coefficients_
The coefficients of our model computed directly from the model found.
int max_iterations_
Maximum number of iterations before giving up.
virtual bool computeModel(int debug_verbosity_level=0)=0
Compute the actual model.