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