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