Point Cloud Library (PCL)  1.14.1-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/sac_model.h>
44 #include <pcl/pcl_base.h>
45 
46 #include <boost/random/mersenne_twister.hpp> // for mt19937
47 #include <boost/random/uniform_01.hpp> // for uniform_01
48 
49 #include <ctime>
50 #include <memory>
51 #include <set>
52 
53 namespace pcl
54 {
55  /** \brief SampleConsensus represents the base class. All sample consensus methods must inherit from this class.
56  * \author Radu Bogdan Rusu
57  * \ingroup sample_consensus
58  */
59  template <typename T>
61  {
62  using SampleConsensusModelPtr = typename SampleConsensusModel<T>::Ptr;
63 
64  private:
65  /** \brief Constructor for base SAC. */
66  SampleConsensus () = default;
67 
68  public:
69  using Ptr = shared_ptr<SampleConsensus<T> >;
70  using ConstPtr = shared_ptr<const SampleConsensus<T> >;
71 
72 
73  /** \brief Constructor for base SAC.
74  * \param[in] model a Sample Consensus model
75  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
76  */
77  SampleConsensus (const SampleConsensusModelPtr &model, bool random = false)
78  : sac_model_ (model)
79  , probability_ (0.99)
80  , iterations_ (0)
81  , threshold_ (std::numeric_limits<double>::max ())
82  , max_iterations_ (1000)
83  , threads_ (-1)
84  , rng_ (new boost::uniform_01<boost::mt19937> (rng_alg_))
85  {
86  // Create a random number generator object
87  if (random)
88  rng_->base ().seed (static_cast<unsigned> (std::time (nullptr)));
89  else
90  rng_->base ().seed (12345u);
91  };
92 
93  /** \brief Constructor for base SAC.
94  * \param[in] model a Sample Consensus model
95  * \param[in] threshold distance to model threshold
96  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
97  */
98  SampleConsensus (const SampleConsensusModelPtr &model,
99  double threshold,
100  bool random = false)
101  : sac_model_ (model)
102  , probability_ (0.99)
103  , iterations_ (0)
104  , threshold_ (threshold)
105  , max_iterations_ (1000)
106  , threads_ (-1)
107  , rng_ (new boost::uniform_01<boost::mt19937> (rng_alg_))
108  {
109  // Create a random number generator object
110  if (random)
111  rng_->base ().seed (static_cast<unsigned> (std::time (nullptr)));
112  else
113  rng_->base ().seed (12345u);
114  };
115 
116  /** \brief Set the Sample Consensus model to use.
117  * \param[in] model a Sample Consensus model
118  */
119  void
120  setSampleConsensusModel (const SampleConsensusModelPtr &model)
121  {
122  sac_model_ = model;
123  }
124 
125  /** \brief Get the Sample Consensus model used. */
126  SampleConsensusModelPtr
128  {
129  return (sac_model_);
130  }
131 
132  /** \brief Destructor for base SAC. */
133  virtual ~SampleConsensus () = default;
134 
135  /** \brief Set the distance to model threshold.
136  * \param[in] threshold distance to model threshold
137  */
138  inline void
139  setDistanceThreshold (double threshold) { threshold_ = threshold; }
140 
141  /** \brief Get the distance to model threshold, as set by the user. */
142  inline double
143  getDistanceThreshold () const { return (threshold_); }
144 
145  /** \brief Set the maximum number of iterations.
146  * \param[in] max_iterations maximum number of iterations
147  */
148  inline void
149  setMaxIterations (int max_iterations) { max_iterations_ = max_iterations; }
150 
151  /** \brief Get the maximum number of iterations, as set by the user. */
152  inline int
153  getMaxIterations () const { return (max_iterations_); }
154 
155  /** \brief Set the desired probability of choosing at least one sample free from outliers.
156  * \param[in] probability the desired probability of choosing at least one sample free from outliers
157  * \note internally, the probability is set to 99% (0.99) by default.
158  */
159  inline void
160  setProbability (double probability) { probability_ = probability; }
161 
162  /** \brief Obtain the probability of choosing at least one sample free from outliers, as set by the user. */
163  inline double
164  getProbability () const { return (probability_); }
165 
166  /** \brief Set the number of threads to use or turn off parallelization.
167  * \param[in] nr_threads the number of hardware threads to use (0 sets the value automatically, a negative number turns parallelization off)
168  * \note Not all SAC methods have a parallel implementation. Some will ignore this setting.
169  */
170  inline void
171  setNumberOfThreads (const int nr_threads = -1) { threads_ = nr_threads; }
172 
173  /** \brief Get the number of threads, as set by the user. */
174  inline int
175  getNumberOfThreads () const { return (threads_); }
176 
177  /** \brief Compute the actual model. Pure virtual. */
178  virtual bool
179  computeModel (int debug_verbosity_level = 0) = 0;
180 
181  /** \brief Refine the model found.
182  * This loops over the model coefficients and optimizes them together
183  * with the set of inliers, until the change in the set of inliers is
184  * minimal.
185  * \param[in] sigma standard deviation multiplier for considering a sample as inlier (Mahalanobis distance)
186  * \param[in] max_iterations the maxim number of iterations to try to refine in case the inliers keep on changing
187  */
188  virtual bool
189  refineModel (const double sigma = 3.0, const unsigned int max_iterations = 1000)
190  {
191  if (!sac_model_)
192  {
193  PCL_ERROR ("[pcl::SampleConsensus::refineModel] Critical error: NULL model!\n");
194  return (false);
195  }
196 
197  double inlier_distance_threshold_sqr = threshold_ * threshold_,
198  error_threshold = threshold_;
199  double sigma_sqr = sigma * sigma;
200  unsigned int refine_iterations = 0;
201  bool inlier_changed = false, oscillating = false;
202  Indices new_inliers, prev_inliers = inliers_;
203  std::vector<std::size_t> inliers_sizes;
204  Eigen::VectorXf new_model_coefficients = model_coefficients_;
205  do
206  {
207  // Optimize the model coefficients
208  sac_model_->optimizeModelCoefficients (prev_inliers, new_model_coefficients, new_model_coefficients);
209  inliers_sizes.push_back (prev_inliers.size ());
210 
211  // Select the new inliers based on the optimized coefficients and new threshold
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);
214 
215  if (new_inliers.empty ())
216  {
217  refine_iterations++;
218  if (refine_iterations >= max_iterations)
219  break;
220  continue;
221  //return (false);
222  }
223 
224  // Estimate the variance and the new threshold
225  double variance = sac_model_->computeVariance ();
226  error_threshold = sqrt (std::min (inlier_distance_threshold_sqr, sigma_sqr * variance));
227 
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);
231  // If the number of inliers changed, then we are still optimizing
232  if (new_inliers.size () != prev_inliers.size ())
233  {
234  // Check if the number of inliers is oscillating in between two values
235  if (inliers_sizes.size () >= 4)
236  {
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])
239  {
240  oscillating = true;
241  break;
242  }
243  }
244  inlier_changed = true;
245  continue;
246  }
247 
248  // Check the values of the inlier set
249  for (std::size_t i = 0; i < prev_inliers.size (); ++i)
250  {
251  // If the value of the inliers changed, then we are still optimizing
252  if (prev_inliers[i] != new_inliers[i])
253  {
254  inlier_changed = true;
255  break;
256  }
257  }
258  }
259  while (inlier_changed && ++refine_iterations < max_iterations);
260 
261  // If the new set of inliers is empty, we didn't do a good job refining
262  if (new_inliers.empty ())
263  {
264  PCL_ERROR ("[pcl::SampleConsensus::refineModel] Refinement failed: got an empty set of inliers!\n");
265  return (false);
266  }
267 
268  if (oscillating)
269  {
270  PCL_DEBUG ("[pcl::SampleConsensus::refineModel] Detected oscillations in the model refinement.\n");
271  return (true);
272  }
273 
274  // If no inliers have been changed anymore, then the refinement was successful
275  if (!inlier_changed)
276  {
277  std::swap (inliers_, new_inliers);
278  model_coefficients_ = new_model_coefficients;
279  return (true);
280  }
281  return (false);
282  }
283 
284  /** \brief Get a set of randomly selected indices.
285  * \param[in] indices the input indices vector
286  * \param[in] nr_samples the desired number of point indices to randomly select
287  * \param[out] indices_subset the resultant output set of randomly selected indices
288  */
289  inline void
290  getRandomSamples (const IndicesPtr &indices,
291  std::size_t nr_samples,
292  std::set<index_t> &indices_subset)
293  {
294  indices_subset.clear ();
295  while (indices_subset.size () < nr_samples)
296  //indices_subset.insert ((*indices)[(index_t) (indices->size () * (rand () / (RAND_MAX + 1.0)))]);
297  indices_subset.insert ((*indices)[static_cast<index_t> (static_cast<double>(indices->size ()) * rnd ())]);
298  }
299 
300  /** \brief Return the best model found so far.
301  * \param[out] model the resultant model
302  */
303  inline void
304  getModel (Indices &model) const { model = model_; }
305 
306  /** \brief Return the best set of inliers found so far for this model.
307  * \param[out] inliers the resultant set of inliers
308  */
309  inline void
310  getInliers (Indices &inliers) const { inliers = inliers_; }
311 
312  /** \brief Return the model coefficients of the best model found so far.
313  * \param[out] model_coefficients the resultant model coefficients, as documented in \ref sample_consensus
314  */
315  inline void
316  getModelCoefficients (Eigen::VectorXf &model_coefficients) const { model_coefficients = model_coefficients_; }
317 
318  protected:
319  /** \brief The underlying data model used (i.e. what is it that we attempt to search for). */
320  SampleConsensusModelPtr sac_model_;
321 
322  /** \brief The model found after the last computeModel () as point cloud indices. */
324 
325  /** \brief The indices of the points that were chosen as inliers after the last computeModel () call. */
327 
328  /** \brief The coefficients of our model computed directly from the model found. */
329  Eigen::VectorXf model_coefficients_;
330 
331  /** \brief Desired probability of choosing at least one sample free from outliers. */
332  double probability_;
333 
334  /** \brief Total number of internal loop iterations that we've done so far. */
336 
337  /** \brief Distance to model threshold. */
338  double threshold_;
339 
340  /** \brief Maximum number of iterations before giving up. */
342 
343  /** \brief The number of threads the scheduler should use, or a negative number if no parallelization is wanted. */
344  int threads_;
345 
346  /** \brief Boost-based random number generator algorithm. */
347  boost::mt19937 rng_alg_;
348 
349  /** \brief Boost-based random number generator distribution. */
350  std::shared_ptr<boost::uniform_01<boost::mt19937> > rng_;
351 
352  /** \brief Boost-based random number generator. */
353  inline double
354  rnd ()
355  {
356  return ((*rng_) ());
357  }
358  };
359 }
SampleConsensus represents the base class.
Definition: sac.h:61
double probability_
Desired probability of choosing at least one sample free from outliers.
Definition: sac.h:332
std::shared_ptr< boost::uniform_01< boost::mt19937 > > rng_
Boost-based random number generator distribution.
Definition: sac.h:350
SampleConsensusModelPtr getSampleConsensusModel() const
Get the Sample Consensus model used.
Definition: sac.h:127
shared_ptr< SampleConsensus< T > > Ptr
Definition: sac.h:69
double getDistanceThreshold() const
Get the distance to model threshold, as set by the user.
Definition: sac.h:143
boost::mt19937 rng_alg_
Boost-based random number generator algorithm.
Definition: sac.h:347
int getNumberOfThreads() const
Get the number of threads, as set by the user.
Definition: sac.h:175
Indices inliers_
The indices of the points that were chosen as inliers after the last computeModel () call.
Definition: sac.h:326
int getMaxIterations() const
Get the maximum number of iterations, as set by the user.
Definition: sac.h:153
void getInliers(Indices &inliers) const
Return the best set of inliers found so far for this model.
Definition: sac.h:310
int iterations_
Total number of internal loop iterations that we've done so far.
Definition: sac.h:335
void getRandomSamples(const IndicesPtr &indices, std::size_t nr_samples, std::set< index_t > &indices_subset)
Get a set of randomly selected indices.
Definition: sac.h:290
virtual bool computeModel(int debug_verbosity_level=0)=0
Compute the actual model.
Indices model_
The model found after the last computeModel () as point cloud indices.
Definition: sac.h:323
double rnd()
Boost-based random number generator.
Definition: sac.h:354
void setNumberOfThreads(const int nr_threads=-1)
Set the number of threads to use or turn off parallelization.
Definition: sac.h:171
void getModel(Indices &model) const
Return the best model found so far.
Definition: sac.h:304
void getModelCoefficients(Eigen::VectorXf &model_coefficients) const
Return the model coefficients of the best model found so far.
Definition: sac.h:316
Eigen::VectorXf model_coefficients_
The coefficients of our model computed directly from the model found.
Definition: sac.h:329
double threshold_
Distance to model threshold.
Definition: sac.h:338
SampleConsensusModelPtr sac_model_
The underlying data model used (i.e.
Definition: sac.h:320
int threads_
The number of threads the scheduler should use, or a negative number if no parallelization is wanted.
Definition: sac.h:344
SampleConsensus(const SampleConsensusModelPtr &model, double threshold, bool random=false)
Constructor for base SAC.
Definition: sac.h:98
double getProbability() const
Obtain the probability of choosing at least one sample free from outliers, as set by the user.
Definition: sac.h:164
int max_iterations_
Maximum number of iterations before giving up.
Definition: sac.h:341
void setSampleConsensusModel(const SampleConsensusModelPtr &model)
Set the Sample Consensus model to use.
Definition: sac.h:120
void setProbability(double probability)
Set the desired probability of choosing at least one sample free from outliers.
Definition: sac.h:160
virtual ~SampleConsensus()=default
Destructor for base SAC.
void setDistanceThreshold(double threshold)
Set the distance to model threshold.
Definition: sac.h:139
SampleConsensus(const SampleConsensusModelPtr &model, bool random=false)
Constructor for base SAC.
Definition: sac.h:77
shared_ptr< const SampleConsensus< T > > ConstPtr
Definition: sac.h:70
virtual bool refineModel(const double sigma=3.0, const unsigned int max_iterations=1000)
Refine the model found.
Definition: sac.h:189
void setMaxIterations(int max_iterations)
Set the maximum number of iterations.
Definition: sac.h:149
SampleConsensusModel represents the base model class.
Definition: sac_model.h:71
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
Definition: types.h:112
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
shared_ptr< Indices > IndicesPtr
Definition: pcl_base.h:58