sac_model.h

Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010-2011, Willow Garage, Inc.
00006  *  
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of Willow Garage, Inc. nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * $Id: sac_model.h 3027 2011-11-01 04:04:27Z rusu $
00037  *
00038  */
00039 
00040 #ifndef PCL_SAMPLE_CONSENSUS_MODEL_H_
00041 #define PCL_SAMPLE_CONSENSUS_MODEL_H_
00042 
00043 #include <cfloat>
00044 #include <limits.h>
00045 #include <set>
00046 
00047 #include <pcl/console/print.h>
00048 #include <pcl/point_cloud.h>
00049 #include "pcl/sample_consensus/model_types.h"
00050 
00051 namespace pcl
00052 {
00053   // Forward declaration
00054 #ifndef __ANDROID__
00055   template<class T> class ProgressiveSampleConsensus;
00056 #endif
00057 
00063   template <typename PointT>
00064   class SampleConsensusModel
00065   {
00066     public:
00067       typedef typename pcl::PointCloud<PointT> PointCloud;
00068       typedef typename pcl::PointCloud<PointT>::ConstPtr PointCloudConstPtr;
00069       typedef typename pcl::PointCloud<PointT>::Ptr PointCloudPtr;
00070 
00071       typedef boost::shared_ptr<SampleConsensusModel> Ptr;
00072       typedef boost::shared_ptr<const SampleConsensusModel> ConstPtr;
00073 
00074     private:
00076       SampleConsensusModel () : radius_min_ (-DBL_MAX), radius_max_ (DBL_MAX) {};
00077 
00078     public:
00082       SampleConsensusModel (const PointCloudConstPtr &cloud) : 
00083         radius_min_ (-DBL_MAX), radius_max_ (DBL_MAX)
00084       {
00085         // Sets the input cloud and creates a vector of "fake" indices
00086         setInputCloud (cloud);
00087       }
00088 
00093       SampleConsensusModel (const PointCloudConstPtr &cloud, const std::vector<int> &indices) :
00094                             input_ (cloud),
00095                             radius_min_ (-DBL_MAX), radius_max_ (DBL_MAX) 
00096     
00097       {
00098         indices_.reset (new std::vector<int> (indices));
00099         if (indices_->size () > input_->points.size ())
00100         {
00101           PCL_ERROR ("[pcl::SampleConsensusModel] Invalid index vector given with size %lu while the input PointCloud has size %lu!\n", (unsigned long)indices_->size (), (unsigned long)input_->points.size ());
00102           indices_->clear ();
00103         }
00104         shuffled_indices_ = *indices_;
00105       };
00106 
00108       virtual ~SampleConsensusModel () {};
00109 
00115       void 
00116       getSamples (int &iterations, std::vector<int> &samples)
00117       {
00118         // We're assuming that indices_ have already been set in the constructor
00119         if (indices_->size () < getSampleSize ())
00120         {
00121           PCL_ERROR ("[pcl::SampleConsensusModel::getSamples] Can not select %lu unique points out of %lu!\n",
00122                      (unsigned long)samples.size (), (unsigned long)indices_->size ());
00123           // one of these will make it stop :)
00124           samples.clear ();
00125           iterations = INT_MAX - 1;
00126           return;
00127         }
00128 
00129         // Get a second point which is different than the first
00130         samples.resize (getSampleSize());
00131         for (unsigned int iter = 0; iter < max_sample_checks_; ++iter)
00132         {
00133           // Choose the random indices
00134           SampleConsensusModel<PointT>::drawIndexSample (samples);
00135 
00136           // If it's a good sample, stop here
00137           if (isSampleGood (samples))
00138             return;
00139         }
00140         PCL_DEBUG ("[pcl::SampleConsensusModel::getSamples] WARNING: Could not select %d sample points in %d iterations!\n", getSampleSize (), max_sample_checks_);
00141         samples.clear ();
00142       }
00143 
00151       virtual bool 
00152       computeModelCoefficients (const std::vector<int> &samples, 
00153                                 Eigen::VectorXf &model_coefficients) = 0;
00154 
00165       virtual void 
00166       optimizeModelCoefficients (const std::vector<int> &inliers, 
00167                                  const Eigen::VectorXf &model_coefficients,
00168                                  Eigen::VectorXf &optimized_coefficients) = 0;
00169 
00175       virtual void 
00176       getDistancesToModel (const Eigen::VectorXf &model_coefficients, 
00177                            std::vector<double> &distances) = 0;
00178 
00187       virtual void 
00188       selectWithinDistance (const Eigen::VectorXf &model_coefficients, 
00189                             const double threshold,
00190                             std::vector<int> &inliers) = 0;
00191 
00201       virtual int
00202       countWithinDistance (const Eigen::VectorXf &model_coefficients, 
00203                            const double threshold) = 0;
00204 
00213       virtual void 
00214       projectPoints (const std::vector<int> &inliers, 
00215                      const Eigen::VectorXf &model_coefficients,
00216                      PointCloud &projected_points, 
00217                      bool copy_data_fields = true) = 0;
00218 
00227       virtual bool 
00228       doSamplesVerifyModel (const std::set<int> &indices, 
00229                             const Eigen::VectorXf &model_coefficients, 
00230                             const double threshold) = 0;
00231 
00235       inline virtual void
00236       setInputCloud (const PointCloudConstPtr &cloud)
00237       {
00238         input_ = cloud;
00239         if (!indices_)
00240           indices_.reset (new std::vector<int> ());
00241         if (indices_->empty ())
00242         {
00243           // Prepare a set of indices to be used (entire cloud)
00244           indices_->resize (cloud->points.size ());
00245           for (size_t i = 0; i < cloud->points.size (); ++i) 
00246             (*indices_)[i] = (int) i;
00247         }
00248         shuffled_indices_ = *indices_;
00249       }
00250 
00252       inline PointCloudConstPtr 
00253       getInputCloud () const { return (input_); }
00254 
00258       inline void 
00259       setIndices (const boost::shared_ptr <std::vector<int> > &indices) 
00260       { 
00261         indices_ = indices; 
00262         shuffled_indices_ = *indices_;
00263       }
00264 
00268       inline void 
00269       setIndices (const std::vector<int> &indices) 
00270       { 
00271         indices_.reset (new std::vector<int> (indices));
00272         shuffled_indices_ = indices;
00273       }
00274 
00276       inline boost::shared_ptr <std::vector<int> > 
00277       getIndices () const { return (indices_); }
00278 
00280       virtual SacModel 
00281       getModelType () const = 0;
00282 
00284       inline unsigned int 
00285       getSampleSize () const 
00286       { 
00287         std::map<pcl::SacModel, unsigned int>::const_iterator it = SAC_SAMPLE_SIZE.find (getModelType ());
00288         if (it == SAC_SAMPLE_SIZE.end ())
00289           throw InvalidSACModelTypeException ("No sample size defined for given model type!\n");
00290         return (it->second);
00291       }
00292 
00299       inline void
00300       setRadiusLimits (const double &min_radius, const double &max_radius)
00301       {
00302         radius_min_ = min_radius;
00303         radius_max_ = max_radius;
00304       }
00305 
00312       inline void
00313       getRadiusLimits (double &min_radius, double &max_radius)
00314       {
00315         min_radius = radius_min_;
00316         max_radius = radius_max_;
00317       }
00318 #ifndef __ANDROID__
00319       friend class ProgressiveSampleConsensus<PointT>;
00320 #endif
00321     protected:
00325       inline void
00326       drawIndexSample (std::vector<int> &sample)
00327       {
00328         size_t sample_size = sample.size ();
00329         size_t index_size = shuffled_indices_.size ();
00330         for (unsigned int i = 0; i < sample_size; ++i)
00331           // The 1/(RAND_MAX+1.0) trick is when the random numbers are not uniformly distributed and for small modulo
00332           // elements, that does not matter (and nowadays, random number generators are good)
00333           std::swap (shuffled_indices_[i], shuffled_indices_[i + (rand () % (index_size - i))]);
00334         std::copy (shuffled_indices_.begin (), shuffled_indices_.begin () + sample_size, sample.begin ());
00335       }
00336 
00340       virtual inline bool
00341       isModelValid (const Eigen::VectorXf &model_coefficients) = 0;
00342 
00347       virtual bool
00348       isSampleGood (const std::vector<int> &samples) const = 0;
00349 
00351       PointCloudConstPtr input_;
00352 
00354       boost::shared_ptr <std::vector<int> > indices_;
00355 
00357       static const unsigned int max_sample_checks_ = 1000;
00358 
00362       double radius_min_, radius_max_;
00363 
00365       std::vector<int> shuffled_indices_;
00366   };
00367 
00371   template <typename PointT, typename PointNT>
00372   class SampleConsensusModelFromNormals
00373   {
00374     public:
00375       typedef typename pcl::PointCloud<PointNT>::ConstPtr PointCloudNConstPtr;
00376       typedef typename pcl::PointCloud<PointNT>::Ptr PointCloudNPtr;
00377 
00378       typedef boost::shared_ptr<SampleConsensusModelFromNormals> Ptr;
00379       typedef boost::shared_ptr<const SampleConsensusModelFromNormals> ConstPtr;
00380 
00382       SampleConsensusModelFromNormals () : normal_distance_weight_ (0.0) {};
00383 
00389       inline void 
00390       setNormalDistanceWeight (const double w) 
00391       { 
00392         normal_distance_weight_ = w; 
00393       }
00394 
00396       inline double 
00397       getNormalDistanceWeight () { return (normal_distance_weight_); }
00398 
00404       inline void 
00405       setInputNormals (const PointCloudNConstPtr &normals) 
00406       { 
00407         normals_ = normals; 
00408       }
00409 
00411       inline PointCloudNConstPtr 
00412       getInputNormals () { return (normals_); }
00413 
00414     protected:
00418       double normal_distance_weight_;
00419 
00423       PointCloudNConstPtr normals_;
00424   };
00425 
00430   template<typename _Scalar, int NX=Eigen::Dynamic, int NY=Eigen::Dynamic>
00431   struct Functor
00432   {
00433     typedef _Scalar Scalar;
00434     enum {
00435       InputsAtCompileTime = NX,
00436       ValuesAtCompileTime = NY
00437     };
00438     typedef Eigen::Matrix<Scalar,InputsAtCompileTime,1> InputType;
00439     typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,1> ValueType;
00440     typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType;
00441     
00442     const int m_inputs, m_values;
00443     
00444     Functor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {}
00445     Functor(int inputs, int values) : m_inputs(inputs), m_values(values) {}
00446   
00447     int inputs() const { return m_inputs; }
00448     int values() const { return m_values; }
00449   };
00450 }
00451 
00452 #endif  //#ifndef PCL_SAMPLE_CONSENSUS_MODEL_H_