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