Point Cloud Library (PCL)  1.7.1
sac_model.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_MODEL_H_
42 #define PCL_SAMPLE_CONSENSUS_MODEL_H_
43 
44 #include <cfloat>
45 #include <ctime>
46 #include <limits.h>
47 #include <set>
48 
49 #include <pcl/console/print.h>
50 #include <pcl/point_cloud.h>
51 #include <pcl/sample_consensus/boost.h>
52 #include <pcl/sample_consensus/model_types.h>
53 
54 #include <pcl/search/search.h>
55 
56 namespace pcl
57 {
58  template<class T> class ProgressiveSampleConsensus;
59 
60  /** \brief @b SampleConsensusModel represents the base model class. All sample consensus models must inherit
61  * from this class.
62  * \author Radu B. Rusu
63  * \ingroup sample_consensus
64  */
65  template <typename PointT>
67  {
68  public:
73 
74  typedef boost::shared_ptr<SampleConsensusModel> Ptr;
75  typedef boost::shared_ptr<const SampleConsensusModel> ConstPtr;
76 
77  protected:
78  /** \brief Empty constructor for base SampleConsensusModel.
79  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
80  */
81  SampleConsensusModel (bool random = false)
82  : input_ ()
83  , indices_ ()
84  , radius_min_ (-std::numeric_limits<double>::max ())
85  , radius_max_ (std::numeric_limits<double>::max ())
86  , samples_radius_ (0.)
89  , rng_alg_ ()
90  , rng_dist_ (new boost::uniform_int<> (0, std::numeric_limits<int>::max ()))
91  , rng_gen_ ()
92  , error_sqr_dists_ ()
93  {
94  // Create a random number generator object
95  if (random)
96  rng_alg_.seed (static_cast<unsigned> (std::time(0)));
97  else
98  rng_alg_.seed (12345u);
99 
100  rng_gen_.reset (new boost::variate_generator<boost::mt19937&, boost::uniform_int<> > (rng_alg_, *rng_dist_));
101  }
102 
103  public:
104  /** \brief Constructor for base SampleConsensusModel.
105  * \param[in] cloud the input point cloud dataset
106  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
107  */
108  SampleConsensusModel (const PointCloudConstPtr &cloud, bool random = false)
109  : input_ ()
110  , indices_ ()
111  , radius_min_ (-std::numeric_limits<double>::max ())
112  , radius_max_ (std::numeric_limits<double>::max ())
113  , samples_radius_ (0.)
115  , shuffled_indices_ ()
116  , rng_alg_ ()
117  , rng_dist_ (new boost::uniform_int<> (0, std::numeric_limits<int>::max ()))
118  , rng_gen_ ()
119  , error_sqr_dists_ ()
120  {
121  if (random)
122  rng_alg_.seed (static_cast<unsigned> (std::time (0)));
123  else
124  rng_alg_.seed (12345u);
125 
126  // Sets the input cloud and creates a vector of "fake" indices
127  setInputCloud (cloud);
128 
129  // Create a random number generator object
130  rng_gen_.reset (new boost::variate_generator<boost::mt19937&, boost::uniform_int<> > (rng_alg_, *rng_dist_));
131  }
132 
133  /** \brief Constructor for base SampleConsensusModel.
134  * \param[in] cloud the input point cloud dataset
135  * \param[in] indices a vector of point indices to be used from \a cloud
136  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
137  */
139  const std::vector<int> &indices,
140  bool random = false)
141  : input_ (cloud)
142  , indices_ (new std::vector<int> (indices))
143  , radius_min_ (-std::numeric_limits<double>::max ())
144  , radius_max_ (std::numeric_limits<double>::max ())
145  , samples_radius_ (0.)
147  , shuffled_indices_ ()
148  , rng_alg_ ()
149  , rng_dist_ (new boost::uniform_int<> (0, std::numeric_limits<int>::max ()))
150  , rng_gen_ ()
151  , error_sqr_dists_ ()
152  {
153  if (random)
154  rng_alg_.seed (static_cast<unsigned> (std::time(0)));
155  else
156  rng_alg_.seed (12345u);
157 
158  if (indices_->size () > input_->points.size ())
159  {
160  PCL_ERROR ("[pcl::SampleConsensusModel] Invalid index vector given with size %zu while the input PointCloud has size %zu!\n", indices_->size (), input_->points.size ());
161  indices_->clear ();
162  }
164 
165  // Create a random number generator object
166  rng_gen_.reset (new boost::variate_generator<boost::mt19937&, boost::uniform_int<> > (rng_alg_, *rng_dist_));
167  };
168 
169  /** \brief Destructor for base SampleConsensusModel. */
170  virtual ~SampleConsensusModel () {};
171 
172  /** \brief Get a set of random data samples and return them as point
173  * indices.
174  * \param[out] iterations the internal number of iterations used by SAC methods
175  * \param[out] samples the resultant model samples
176  */
177  virtual void
178  getSamples (int &iterations, std::vector<int> &samples)
179  {
180  // We're assuming that indices_ have already been set in the constructor
181  if (indices_->size () < getSampleSize ())
182  {
183  PCL_ERROR ("[pcl::SampleConsensusModel::getSamples] Can not select %zu unique points out of %zu!\n",
184  samples.size (), indices_->size ());
185  // one of these will make it stop :)
186  samples.clear ();
187  iterations = INT_MAX - 1;
188  return;
189  }
190 
191  // Get a second point which is different than the first
192  samples.resize (getSampleSize ());
193  for (unsigned int iter = 0; iter < max_sample_checks_; ++iter)
194  {
195  // Choose the random indices
196  if (samples_radius_ < std::numeric_limits<double>::epsilon ())
198  else
200 
201  // If it's a good sample, stop here
202  if (isSampleGood (samples))
203  {
204  PCL_DEBUG ("[pcl::SampleConsensusModel::getSamples] Selected %zu samples.\n", samples.size ());
205  return;
206  }
207  }
208  PCL_DEBUG ("[pcl::SampleConsensusModel::getSamples] WARNING: Could not select %d sample points in %d iterations!\n", getSampleSize (), max_sample_checks_);
209  samples.clear ();
210  }
211 
212  /** \brief Check whether the given index samples can form a valid model,
213  * compute the model coefficients from these samples and store them
214  * in model_coefficients. Pure virtual.
215  * \param[in] samples the point indices found as possible good candidates
216  * for creating a valid model
217  * \param[out] model_coefficients the computed model coefficients
218  */
219  virtual bool
220  computeModelCoefficients (const std::vector<int> &samples,
221  Eigen::VectorXf &model_coefficients) = 0;
222 
223  /** \brief Recompute the model coefficients using the given inlier set
224  * and return them to the user. Pure virtual.
225  *
226  * @note: these are the coefficients of the model after refinement
227  * (e.g., after a least-squares optimization)
228  *
229  * \param[in] inliers the data inliers supporting the model
230  * \param[in] model_coefficients the initial guess for the model coefficients
231  * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization
232  */
233  virtual void
234  optimizeModelCoefficients (const std::vector<int> &inliers,
235  const Eigen::VectorXf &model_coefficients,
236  Eigen::VectorXf &optimized_coefficients) = 0;
237 
238  /** \brief Compute all distances from the cloud data to a given model. Pure virtual.
239  *
240  * \param[in] model_coefficients the coefficients of a model that we need to compute distances to
241  * \param[out] distances the resultant estimated distances
242  */
243  virtual void
244  getDistancesToModel (const Eigen::VectorXf &model_coefficients,
245  std::vector<double> &distances) = 0;
246 
247  /** \brief Select all the points which respect the given model
248  * coefficients as inliers. Pure virtual.
249  *
250  * \param[in] model_coefficients the coefficients of a model that we need to compute distances to
251  * \param[in] threshold a maximum admissible distance threshold for determining the inliers from
252  * the outliers
253  * \param[out] inliers the resultant model inliers
254  */
255  virtual void
256  selectWithinDistance (const Eigen::VectorXf &model_coefficients,
257  const double threshold,
258  std::vector<int> &inliers) = 0;
259 
260  /** \brief Count all the points which respect the given model
261  * coefficients as inliers. Pure virtual.
262  *
263  * \param[in] model_coefficients the coefficients of a model that we need to
264  * compute distances to
265  * \param[in] threshold a maximum admissible distance threshold for
266  * determining the inliers from the outliers
267  * \return the resultant number of inliers
268  */
269  virtual int
270  countWithinDistance (const Eigen::VectorXf &model_coefficients,
271  const double threshold) = 0;
272 
273  /** \brief Create a new point cloud with inliers projected onto the model. Pure virtual.
274  * \param[in] inliers the data inliers that we want to project on the model
275  * \param[in] model_coefficients the coefficients of a model
276  * \param[out] projected_points the resultant projected points
277  * \param[in] copy_data_fields set to true (default) if we want the \a
278  * projected_points cloud to be an exact copy of the input dataset minus
279  * the point projections on the plane model
280  */
281  virtual void
282  projectPoints (const std::vector<int> &inliers,
283  const Eigen::VectorXf &model_coefficients,
284  PointCloud &projected_points,
285  bool copy_data_fields = true) = 0;
286 
287  /** \brief Verify whether a subset of indices verifies a given set of
288  * model coefficients. Pure virtual.
289  *
290  * \param[in] indices the data indices that need to be tested against the model
291  * \param[in] model_coefficients the set of model coefficients
292  * \param[in] threshold a maximum admissible distance threshold for
293  * determining the inliers from the outliers
294  */
295  virtual bool
296  doSamplesVerifyModel (const std::set<int> &indices,
297  const Eigen::VectorXf &model_coefficients,
298  const double threshold) = 0;
299 
300  /** \brief Provide a pointer to the input dataset
301  * \param[in] cloud the const boost shared pointer to a PointCloud message
302  */
303  inline virtual void
305  {
306  input_ = cloud;
307  if (!indices_)
308  indices_.reset (new std::vector<int> ());
309  if (indices_->empty ())
310  {
311  // Prepare a set of indices to be used (entire cloud)
312  indices_->resize (cloud->points.size ());
313  for (size_t i = 0; i < cloud->points.size (); ++i)
314  (*indices_)[i] = static_cast<int> (i);
315  }
317  }
318 
319  /** \brief Get a pointer to the input point cloud dataset. */
320  inline PointCloudConstPtr
321  getInputCloud () const { return (input_); }
322 
323  /** \brief Provide a pointer to the vector of indices that represents the input data.
324  * \param[in] indices a pointer to the vector of indices that represents the input data.
325  */
326  inline void
327  setIndices (const boost::shared_ptr <std::vector<int> > &indices)
328  {
329  indices_ = indices;
331  }
332 
333  /** \brief Provide the vector of indices that represents the input data.
334  * \param[out] indices the vector of indices that represents the input data.
335  */
336  inline void
337  setIndices (const std::vector<int> &indices)
338  {
339  indices_.reset (new std::vector<int> (indices));
340  shuffled_indices_ = indices;
341  }
342 
343  /** \brief Get a pointer to the vector of indices used. */
344  inline boost::shared_ptr <std::vector<int> >
345  getIndices () const { return (indices_); }
346 
347  /** \brief Return an unique id for each type of model employed. */
348  virtual SacModel
349  getModelType () const = 0;
350 
351  /** \brief Return the size of a sample from which a model is computed */
352  inline unsigned int
353  getSampleSize () const
354  {
355  std::map<pcl::SacModel, unsigned int>::const_iterator it = SAC_SAMPLE_SIZE.find (getModelType ());
356  if (it == SAC_SAMPLE_SIZE.end ())
357  throw InvalidSACModelTypeException ("No sample size defined for given model type!\n");
358  return (it->second);
359  }
360 
361  /** \brief Set the minimum and maximum allowable radius limits for the
362  * model (applicable to models that estimate a radius)
363  * \param[in] min_radius the minimum radius model
364  * \param[in] max_radius the maximum radius model
365  * \todo change this to set limits on the entire model
366  */
367  inline void
368  setRadiusLimits (const double &min_radius, const double &max_radius)
369  {
370  radius_min_ = min_radius;
371  radius_max_ = max_radius;
372  }
373 
374  /** \brief Get the minimum and maximum allowable radius limits for the
375  * model as set by the user.
376  *
377  * \param[out] min_radius the resultant minimum radius model
378  * \param[out] max_radius the resultant maximum radius model
379  */
380  inline void
381  getRadiusLimits (double &min_radius, double &max_radius)
382  {
383  min_radius = radius_min_;
384  max_radius = radius_max_;
385  }
386 
387  /** \brief Set the maximum distance allowed when drawing random samples
388  * \param[in] radius the maximum distance (L2 norm)
389  */
390  inline void
391  setSamplesMaxDist (const double &radius, SearchPtr search)
392  {
393  samples_radius_ = radius;
394  samples_radius_search_ = search;
395  }
396 
397  /** \brief Get maximum distance allowed when drawing random samples
398  *
399  * \param[out] radius the maximum distance (L2 norm)
400  */
401  inline void
402  getSamplesMaxDist (double &radius)
403  {
404  radius = samples_radius_;
405  }
406 
408 
409  /** \brief Compute the variance of the errors to the model.
410  * \param[in] error_sqr_dists a vector holding the distances
411  */
412  inline double
413  computeVariance (const std::vector<double> &error_sqr_dists)
414  {
415  std::vector<double> dists (error_sqr_dists);
416  std::sort (dists.begin (), dists.end ());
417  double median_error_sqr = dists[dists.size () >> 1];
418  return (2.1981 * median_error_sqr);
419  }
420 
421  /** \brief Compute the variance of the errors to the model from the internally
422  * estimated vector of distances. The model must be computed first (or at least
423  * selectWithinDistance must be called).
424  */
425  inline double
427  {
428  if (error_sqr_dists_.empty ())
429  {
430  PCL_ERROR ("[pcl::SampleConsensusModel::computeVariance] The variance of the Sample Consensus model distances cannot be estimated, as the model has not been computed yet. Please compute the model first or at least run selectWithinDistance before continuing. Returning NAN!\n");
431  return (std::numeric_limits<double>::quiet_NaN ());
432  }
434  }
435 
436  protected:
437  /** \brief Fills a sample array with random samples from the indices_ vector
438  * \param[out] sample the set of indices of target_ to analyze
439  */
440  inline void
441  drawIndexSample (std::vector<int> &sample)
442  {
443  size_t sample_size = sample.size ();
444  size_t index_size = shuffled_indices_.size ();
445  for (unsigned int i = 0; i < sample_size; ++i)
446  // The 1/(RAND_MAX+1.0) trick is when the random numbers are not uniformly distributed and for small modulo
447  // elements, that does not matter (and nowadays, random number generators are good)
448  //std::swap (shuffled_indices_[i], shuffled_indices_[i + (rand () % (index_size - i))]);
449  std::swap (shuffled_indices_[i], shuffled_indices_[i + (rnd () % (index_size - i))]);
450  std::copy (shuffled_indices_.begin (), shuffled_indices_.begin () + sample_size, sample.begin ());
451  }
452 
453  /** \brief Fills a sample array with one random sample from the indices_ vector
454  * and other random samples that are closer than samples_radius_
455  * \param[out] sample the set of indices of target_ to analyze
456  */
457  inline void
458  drawIndexSampleRadius (std::vector<int> &sample)
459  {
460  size_t sample_size = sample.size ();
461  size_t index_size = shuffled_indices_.size ();
462 
463  std::swap (shuffled_indices_[0], shuffled_indices_[0 + (rnd () % (index_size - 0))]);
464  //const PointT& pt0 = (*input_)[shuffled_indices_[0]];
465 
466  std::vector<int> indices;
467  std::vector<float> sqr_dists;
468 
469  // If indices have been set when the search object was constructed,
470  // radiusSearch() expects an index into the indices vector as its
471  // first parameter. This can't be determined efficiently, so we use
472  // the point instead of the index.
473  // Returned indices are converted automatically.
474  samples_radius_search_->radiusSearch (input_->at(shuffled_indices_[0]),
475  samples_radius_, indices, sqr_dists );
476 
477  if (indices.size () < sample_size - 1)
478  {
479  // radius search failed, make an invalid model
480  for(unsigned int i = 1; i < sample_size; ++i)
482  }
483  else
484  {
485  for (unsigned int i = 0; i < sample_size-1; ++i)
486  std::swap (indices[i], indices[i + (rnd () % (indices.size () - i))]);
487  for (unsigned int i = 1; i < sample_size; ++i)
488  shuffled_indices_[i] = indices[i-1];
489  }
490 
491  std::copy (shuffled_indices_.begin (), shuffled_indices_.begin () + sample_size, sample.begin ());
492  }
493 
494  /** \brief Check whether a model is valid given the user constraints.
495  * \param[in] model_coefficients the set of model coefficients
496  */
497  virtual inline bool
498  isModelValid (const Eigen::VectorXf &model_coefficients) = 0;
499 
500  /** \brief Check if a sample of indices results in a good sample of points
501  * indices. Pure virtual.
502  * \param[in] samples the resultant index samples
503  */
504  virtual bool
505  isSampleGood (const std::vector<int> &samples) const = 0;
506 
507  /** \brief A boost shared pointer to the point cloud data array. */
509 
510  /** \brief A pointer to the vector of point indices to use. */
511  boost::shared_ptr <std::vector<int> > indices_;
512 
513  /** The maximum number of samples to try until we get a good one */
514  static const unsigned int max_sample_checks_ = 1000;
515 
516  /** \brief The minimum and maximum radius limits for the model.
517  * Applicable to all models that estimate a radius.
518  */
520 
521  /** \brief The maximum distance of subsequent samples from the first (radius search) */
523 
524  /** \brief The search object for picking subsequent samples using radius search */
526 
527  /** Data containing a shuffled version of the indices. This is used and modified when drawing samples. */
528  std::vector<int> shuffled_indices_;
529 
530  /** \brief Boost-based random number generator algorithm. */
531  boost::mt19937 rng_alg_;
532 
533  /** \brief Boost-based random number generator distribution. */
534  boost::shared_ptr<boost::uniform_int<> > rng_dist_;
535 
536  /** \brief Boost-based random number generator. */
537  boost::shared_ptr<boost::variate_generator< boost::mt19937&, boost::uniform_int<> > > rng_gen_;
538 
539  /** \brief A vector holding the distances to the computed model. Used internally. */
540  std::vector<double> error_sqr_dists_;
541 
542  /** \brief Boost-based random number generator. */
543  inline int
544  rnd ()
545  {
546  return ((*rng_gen_) ());
547  }
548  public:
549  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
550  };
551 
552  /** \brief @b SampleConsensusModelFromNormals represents the base model class
553  * for models that require the use of surface normals for estimation.
554  */
555  template <typename PointT, typename PointNT>
556  class SampleConsensusModelFromNormals //: public SampleConsensusModel<PointT>
557  {
558  public:
561 
562  typedef boost::shared_ptr<SampleConsensusModelFromNormals> Ptr;
563  typedef boost::shared_ptr<const SampleConsensusModelFromNormals> ConstPtr;
564 
565  /** \brief Empty constructor for base SampleConsensusModelFromNormals. */
567 
568  /** \brief Destructor. */
570 
571  /** \brief Set the normal angular distance weight.
572  * \param[in] w the relative weight (between 0 and 1) to give to the angular
573  * distance (0 to pi/2) between point normals and the plane normal.
574  * (The Euclidean distance will have weight 1-w.)
575  */
576  inline void
577  setNormalDistanceWeight (const double w)
578  {
580  }
581 
582  /** \brief Get the normal angular distance weight. */
583  inline double
585 
586  /** \brief Provide a pointer to the input dataset that contains the point
587  * normals of the XYZ dataset.
588  *
589  * \param[in] normals the const boost shared pointer to a PointCloud message
590  */
591  inline void
593  {
594  normals_ = normals;
595  }
596 
597  /** \brief Get a pointer to the normals of the input XYZ point cloud dataset. */
598  inline PointCloudNConstPtr
599  getInputNormals () { return (normals_); }
600 
601  protected:
602  /** \brief The relative weight (between 0 and 1) to give to the angular
603  * distance (0 to pi/2) between point normals and the plane normal.
604  */
606 
607  /** \brief A pointer to the input dataset that contains the point normals
608  * of the XYZ dataset.
609  */
611  };
612 
613  /** Base functor all the models that need non linear optimization must
614  * define their own one and implement operator() (const Eigen::VectorXd& x, Eigen::VectorXd& fvec)
615  * or operator() (const Eigen::VectorXf& x, Eigen::VectorXf& fvec) dependening on the choosen _Scalar
616  */
617  template<typename _Scalar, int NX=Eigen::Dynamic, int NY=Eigen::Dynamic>
618  struct Functor
619  {
620  typedef _Scalar Scalar;
621  enum
622  {
625  };
626 
627  typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,1> ValueType;
628  typedef Eigen::Matrix<Scalar,InputsAtCompileTime,1> InputType;
629  typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType;
630 
631  /** \brief Empty Construtor. */
632  Functor () : m_data_points_ (ValuesAtCompileTime) {}
633 
634  /** \brief Constructor
635  * \param[in] m_data_points number of data points to evaluate.
636  */
637  Functor (int m_data_points) : m_data_points_ (m_data_points) {}
638 
639  virtual ~Functor () {}
640 
641  /** \brief Get the number of values. */
642  int
643  values () const { return (m_data_points_); }
644 
645  private:
646  const int m_data_points_;
647  };
648 }
649 
650 #endif //#ifndef PCL_SAMPLE_CONSENSUS_MODEL_H_