Point Cloud Library (PCL)  1.10.0-dev
sac.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  * $Id$
38  *
39  */
40 
41 #pragma once
42 
43 #include <pcl/sample_consensus/boost.h>
44 #include <pcl/sample_consensus/sac_model.h>
45 #include <pcl/pcl_base.h>
46 
47 #include <ctime>
48 #include <memory>
49 #include <set>
50 
51 namespace pcl
52 {
53  /** \brief SampleConsensus represents the base class. All sample consensus methods must inherit from this class.
54  * \author Radu Bogdan Rusu
55  * \ingroup sample_consensus
56  */
57  template <typename T>
59  {
60  using SampleConsensusModelPtr = typename SampleConsensusModel<T>::Ptr;
61 
62  private:
63  /** \brief Constructor for base SAC. */
64  SampleConsensus () {};
65 
66  public:
69 
70 
71  /** \brief Constructor for base SAC.
72  * \param[in] model a Sample Consensus model
73  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
74  */
75  SampleConsensus (const SampleConsensusModelPtr &model, bool random = false)
76  : sac_model_ (model)
77  , probability_ (0.99)
78  , iterations_ (0)
79  , threshold_ (std::numeric_limits<double>::max ())
80  , max_iterations_ (1000)
81  , threads_ (-1)
82  , rng_ (new boost::uniform_01<boost::mt19937> (rng_alg_))
83  {
84  // Create a random number generator object
85  if (random)
86  rng_->base ().seed (static_cast<unsigned> (std::time (nullptr)));
87  else
88  rng_->base ().seed (12345u);
89  };
90 
91  /** \brief Constructor for base SAC.
92  * \param[in] model a Sample Consensus model
93  * \param[in] threshold distance to model threshold
94  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
95  */
96  SampleConsensus (const SampleConsensusModelPtr &model,
97  double threshold,
98  bool random = false)
99  : sac_model_ (model)
100  , probability_ (0.99)
101  , iterations_ (0)
102  , threshold_ (threshold)
103  , max_iterations_ (1000)
104  , threads_ (-1)
105  , rng_ (new boost::uniform_01<boost::mt19937> (rng_alg_))
106  {
107  // Create a random number generator object
108  if (random)
109  rng_->base ().seed (static_cast<unsigned> (std::time (nullptr)));
110  else
111  rng_->base ().seed (12345u);
112  };
113 
114  /** \brief Set the Sample Consensus model to use.
115  * \param[in] model a Sample Consensus model
116  */
117  void
118  setSampleConsensusModel (const SampleConsensusModelPtr &model)
119  {
120  sac_model_ = model;
121  }
122 
123  /** \brief Get the Sample Consensus model used. */
124  SampleConsensusModelPtr
126  {
127  return (sac_model_);
128  }
129 
130  /** \brief Destructor for base SAC. */
131  virtual ~SampleConsensus () {};
132 
133  /** \brief Set the distance to model threshold.
134  * \param[in] threshold distance to model threshold
135  */
136  inline void
137  setDistanceThreshold (double threshold) { threshold_ = threshold; }
138 
139  /** \brief Get the distance to model threshold, as set by the user. */
140  inline double
141  getDistanceThreshold () const { return (threshold_); }
142 
143  /** \brief Set the maximum number of iterations.
144  * \param[in] max_iterations maximum number of iterations
145  */
146  inline void
147  setMaxIterations (int max_iterations) { max_iterations_ = max_iterations; }
148 
149  /** \brief Get the maximum number of iterations, as set by the user. */
150  inline int
151  getMaxIterations () const { return (max_iterations_); }
152 
153  /** \brief Set the desired probability of choosing at least one sample free from outliers.
154  * \param[in] probability the desired probability of choosing at least one sample free from outliers
155  * \note internally, the probability is set to 99% (0.99) by default.
156  */
157  inline void
158  setProbability (double probability) { probability_ = probability; }
159 
160  /** \brief Obtain the probability of choosing at least one sample free from outliers, as set by the user. */
161  inline double
162  getProbability () const { return (probability_); }
163 
164  /** \brief Set the number of threads to use or turn off parallelization.
165  * \param[in] nr_threads the number of hardware threads to use (0 sets the value automatically, a negative number turns parallelization off)
166  * \note Not all SAC methods have a parallel implementation. Some will ignore this setting.
167  */
168  inline void
169  setNumberOfThreads (const int nr_threads = -1) { threads_ = nr_threads; }
170 
171  /** \brief Get the number of threads, as set by the user. */
172  inline int
173  getNumberOfThreads () const { return (threads_); }
174 
175  /** \brief Compute the actual model. Pure virtual. */
176  virtual bool
177  computeModel (int debug_verbosity_level = 0) = 0;
178 
179  /** \brief Refine the model found.
180  * This loops over the model coefficients and optimizes them together
181  * with the set of inliers, until the change in the set of inliers is
182  * minimal.
183  * \param[in] sigma standard deviation multiplier for considering a sample as inlier (Mahalanobis distance)
184  * \param[in] max_iterations the maxim number of iterations to try to refine in case the inliers keep on changing
185  */
186  virtual bool
187  refineModel (const double sigma = 3.0, const unsigned int max_iterations = 1000)
188  {
189  if (!sac_model_)
190  {
191  PCL_ERROR ("[pcl::SampleConsensus::refineModel] Critical error: NULL model!\n");
192  return (false);
193  }
194 
195  double inlier_distance_threshold_sqr = threshold_ * threshold_,
196  error_threshold = threshold_;
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;
202  Eigen::VectorXf new_model_coefficients = model_coefficients_;
203  do
204  {
205  // Optimize the model coefficients
206  sac_model_->optimizeModelCoefficients (prev_inliers, new_model_coefficients, new_model_coefficients);
207  inliers_sizes.push_back (prev_inliers.size ());
208 
209  // Select the new inliers based on the optimized coefficients and new threshold
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);
212 
213  if (new_inliers.empty ())
214  {
215  refine_iterations++;
216  if (refine_iterations >= max_iterations)
217  break;
218  continue;
219  //return (false);
220  }
221 
222  // Estimate the variance and the new threshold
223  double variance = sac_model_->computeVariance ();
224  error_threshold = sqrt (std::min (inlier_distance_threshold_sqr, sigma_sqr * variance));
225 
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);
229  // If the number of inliers changed, then we are still optimizing
230  if (new_inliers.size () != prev_inliers.size ())
231  {
232  // Check if the number of inliers is oscillating in between two values
233  if (inliers_sizes.size () >= 4)
234  {
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])
237  {
238  oscillating = true;
239  break;
240  }
241  }
242  inlier_changed = true;
243  continue;
244  }
245 
246  // Check the values of the inlier set
247  for (std::size_t i = 0; i < prev_inliers.size (); ++i)
248  {
249  // If the value of the inliers changed, then we are still optimizing
250  if (prev_inliers[i] != new_inliers[i])
251  {
252  inlier_changed = true;
253  break;
254  }
255  }
256  }
257  while (inlier_changed && ++refine_iterations < max_iterations);
258 
259  // If the new set of inliers is empty, we didn't do a good job refining
260  if (new_inliers.empty ())
261  {
262  PCL_ERROR ("[pcl::SampleConsensus::refineModel] Refinement failed: got an empty set of inliers!\n");
263  return (false);
264  }
265 
266  if (oscillating)
267  {
268  PCL_DEBUG ("[pcl::SampleConsensus::refineModel] Detected oscillations in the model refinement.\n");
269  return (true);
270  }
271 
272  // If no inliers have been changed anymore, then the refinement was successful
273  if (!inlier_changed)
274  {
275  std::swap (inliers_, new_inliers);
276  model_coefficients_ = new_model_coefficients;
277  return (true);
278  }
279  return (false);
280  }
281 
282  /** \brief Get a set of randomly selected indices.
283  * \param[in] indices the input indices vector
284  * \param[in] nr_samples the desired number of point indices to randomly select
285  * \param[out] indices_subset the resultant output set of randomly selected indices
286  */
287  inline void
288  getRandomSamples (const IndicesPtr &indices,
289  std::size_t nr_samples,
290  std::set<int> &indices_subset)
291  {
292  indices_subset.clear ();
293  while (indices_subset.size () < nr_samples)
294  //indices_subset.insert ((*indices)[(int) (indices->size () * (rand () / (RAND_MAX + 1.0)))]);
295  indices_subset.insert ((*indices)[static_cast<int> (static_cast<double>(indices->size ()) * rnd ())]);
296  }
297 
298  /** \brief Return the best model found so far.
299  * \param[out] model the resultant model
300  */
301  inline void
302  getModel (std::vector<int> &model) const { model = model_; }
303 
304  /** \brief Return the best set of inliers found so far for this model.
305  * \param[out] inliers the resultant set of inliers
306  */
307  inline void
308  getInliers (std::vector<int> &inliers) const { inliers = inliers_; }
309 
310  /** \brief Return the model coefficients of the best model found so far.
311  * \param[out] model_coefficients the resultant model coefficients, as documented in \ref sample_consensus
312  */
313  inline void
314  getModelCoefficients (Eigen::VectorXf &model_coefficients) const { model_coefficients = model_coefficients_; }
315 
316  protected:
317  /** \brief The underlying data model used (i.e. what is it that we attempt to search for). */
318  SampleConsensusModelPtr sac_model_;
319 
320  /** \brief The model found after the last computeModel () as point cloud indices. */
321  std::vector<int> model_;
322 
323  /** \brief The indices of the points that were chosen as inliers after the last computeModel () call. */
324  std::vector<int> inliers_;
325 
326  /** \brief The coefficients of our model computed directly from the model found. */
327  Eigen::VectorXf model_coefficients_;
328 
329  /** \brief Desired probability of choosing at least one sample free from outliers. */
330  double probability_;
331 
332  /** \brief Total number of internal loop iterations that we've done so far. */
334 
335  /** \brief Distance to model threshold. */
336  double threshold_;
337 
338  /** \brief Maximum number of iterations before giving up. */
340 
341  /** \brief The number of threads the scheduler should use, or a negative number if no parallelization is wanted. */
342  int threads_;
343 
344  /** \brief Boost-based random number generator algorithm. */
345  boost::mt19937 rng_alg_;
346 
347  /** \brief Boost-based random number generator distribution. */
348  std::shared_ptr<boost::uniform_01<boost::mt19937> > rng_;
349 
350  /** \brief Boost-based random number generator. */
351  inline double
352  rnd ()
353  {
354  return ((*rng_) ());
355  }
356  };
357 }
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
double threshold_
Distance to model threshold.
Definition: sac.h:336
double rnd()
Boost-based random number generator.
Definition: sac.h:352
int threads_
The number of threads the scheduler should use, or a negative number if no parallelization is wanted...
Definition: sac.h:342
shared_ptr< Indices > IndicesPtr
Definition: pcl_base.h:61
void setDistanceThreshold(double threshold)
Set the distance to model threshold.
Definition: sac.h:137
double getProbability() const
Obtain the probability of choosing at least one sample free from outliers, as set by the user...
Definition: sac.h:162
void getRandomSamples(const IndicesPtr &indices, std::size_t nr_samples, std::set< int > &indices_subset)
Get a set of randomly selected indices.
Definition: sac.h:288
void getInliers(std::vector< int > &inliers) const
Return the best set of inliers found so far for this model.
Definition: sac.h:308
std::vector< int > inliers_
The indices of the points that were chosen as inliers after the last computeModel () call...
Definition: sac.h:324
SampleConsensusModel represents the base model class.
Definition: sac_model.h:68
shared_ptr< SampleConsensus< WeightSACPointType > > Ptr
Definition: sac.h:67
shared_ptr< const SampleConsensus< WeightSACPointType > > ConstPtr
Definition: sac.h:68
SampleConsensusModelPtr sac_model_
The underlying data model used (i.e.
Definition: sac.h:318
int getMaxIterations() const
Get the maximum number of iterations, as set by the user.
Definition: sac.h:151
double getDistanceThreshold() const
Get the distance to model threshold, as set by the user.
Definition: sac.h:141
std::vector< int > model_
The model found after the last computeModel () as point cloud indices.
Definition: sac.h:321
virtual bool computeModel(int debug_verbosity_level=0)=0
Compute the actual model.
SampleConsensusModelPtr getSampleConsensusModel() const
Get the Sample Consensus model used.
Definition: sac.h:125
virtual ~SampleConsensus()
Destructor for base SAC.
Definition: sac.h:131
void setMaxIterations(int max_iterations)
Set the maximum number of iterations.
Definition: sac.h:147
std::shared_ptr< boost::uniform_01< boost::mt19937 > > rng_
Boost-based random number generator distribution.
Definition: sac.h:348
virtual bool refineModel(const double sigma=3.0, const unsigned int max_iterations=1000)
Refine the model found.
Definition: sac.h:187
int iterations_
Total number of internal loop iterations that we&#39;ve done so far.
Definition: sac.h:333
double probability_
Desired probability of choosing at least one sample free from outliers.
Definition: sac.h:330
SampleConsensus(const SampleConsensusModelPtr &model, bool random=false)
Constructor for base SAC.
Definition: sac.h:75
Eigen::VectorXf model_coefficients_
The coefficients of our model computed directly from the model found.
Definition: sac.h:327
void setProbability(double probability)
Set the desired probability of choosing at least one sample free from outliers.
Definition: sac.h:158
void setSampleConsensusModel(const SampleConsensusModelPtr &model)
Set the Sample Consensus model to use.
Definition: sac.h:118
void setNumberOfThreads(const int nr_threads=-1)
Set the number of threads to use or turn off parallelization.
Definition: sac.h:169
void getModel(std::vector< int > &model) const
Return the best model found so far.
Definition: sac.h:302
boost::shared_ptr< T > shared_ptr
Alias for boost::shared_ptr.
Definition: pcl_macros.h:90
int max_iterations_
Maximum number of iterations before giving up.
Definition: sac.h:339
boost::mt19937 rng_alg_
Boost-based random number generator algorithm.
Definition: sac.h:345
SampleConsensus(const SampleConsensusModelPtr &model, double threshold, bool random=false)
Constructor for base SAC.
Definition: sac.h:96
SampleConsensus represents the base class.
Definition: sac.h:58
int getNumberOfThreads() const
Get the number of threads, as set by the user.
Definition: sac.h:173
void getModelCoefficients(Eigen::VectorXf &model_coefficients) const
Return the model coefficients of the best model found so far.
Definition: sac.h:314