sac_model.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2010-2011, Willow Garage, Inc.
 *  Copyright (c) 2012-, Open Perception, Inc.
 *  
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the copyright holder(s) nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * $Id$
 *
 */

#ifndef PCL_SAMPLE_CONSENSUS_MODEL_H_
#define PCL_SAMPLE_CONSENSUS_MODEL_H_

#include <cfloat>
#include <ctime>
#include <limits.h>
#include <set>

#include <pcl/console/print.h>
#include <pcl/point_cloud.h>
#include <pcl/sample_consensus/boost.h>
#include <pcl/sample_consensus/model_types.h>

#include <pcl/search/search.h>

namespace pcl
{
  template<class T> class ProgressiveSampleConsensus;

  template <typename PointT>
  class SampleConsensusModel
  {
    public:
      typedef typename pcl::PointCloud<PointT> PointCloud;
      typedef typename pcl::PointCloud<PointT>::ConstPtr PointCloudConstPtr;
      typedef typename pcl::PointCloud<PointT>::Ptr PointCloudPtr;
      typedef typename pcl::search::Search<PointT>::Ptr SearchPtr;

      typedef boost::shared_ptr<SampleConsensusModel> Ptr;
      typedef boost::shared_ptr<const SampleConsensusModel> ConstPtr;

    protected:
      SampleConsensusModel (bool random = false) 
        : input_ ()
        , indices_ ()
        , radius_min_ (-std::numeric_limits<double>::max ())
        , radius_max_ (std::numeric_limits<double>::max ())
        , samples_radius_ (0.)
        , samples_radius_search_ ()
        , shuffled_indices_ ()
        , rng_alg_ ()
        , rng_dist_ (new boost::uniform_int<> (0, std::numeric_limits<int>::max ()))
        , rng_gen_ ()
        , error_sqr_dists_ ()
      {
        // Create a random number generator object
        if (random)
          rng_alg_.seed (static_cast<unsigned> (std::time(0)));
        else
          rng_alg_.seed (12345u);

        rng_gen_.reset (new boost::variate_generator<boost::mt19937&, boost::uniform_int<> > (rng_alg_, *rng_dist_)); 
       }

    public:
      SampleConsensusModel (const PointCloudConstPtr &cloud, bool random = false) 
        : input_ ()
        , indices_ ()
        , radius_min_ (-std::numeric_limits<double>::max ())
        , radius_max_ (std::numeric_limits<double>::max ())
        , samples_radius_ (0.)
        , samples_radius_search_ ()
        , shuffled_indices_ ()
        , rng_alg_ ()
        , rng_dist_ (new boost::uniform_int<> (0, std::numeric_limits<int>::max ()))
        , rng_gen_ ()
        , error_sqr_dists_ ()
      {
        if (random)
          rng_alg_.seed (static_cast<unsigned> (std::time (0)));
        else
          rng_alg_.seed (12345u);

        // Sets the input cloud and creates a vector of "fake" indices
        setInputCloud (cloud);

        // Create a random number generator object
        rng_gen_.reset (new boost::variate_generator<boost::mt19937&, boost::uniform_int<> > (rng_alg_, *rng_dist_)); 
      }

      SampleConsensusModel (const PointCloudConstPtr &cloud, 
                            const std::vector<int> &indices, 
                            bool random = false) 
        : input_ (cloud)
        , indices_ (new std::vector<int> (indices))
        , radius_min_ (-std::numeric_limits<double>::max ())
        , radius_max_ (std::numeric_limits<double>::max ())
        , samples_radius_ (0.)
        , samples_radius_search_ ()
        , shuffled_indices_ ()
        , rng_alg_ ()
        , rng_dist_ (new boost::uniform_int<> (0, std::numeric_limits<int>::max ()))
        , rng_gen_ ()
        , error_sqr_dists_ ()
      {
        if (random)
          rng_alg_.seed (static_cast<unsigned> (std::time(0)));
        else
          rng_alg_.seed (12345u);

        if (indices_->size () > input_->points.size ())
        {
          PCL_ERROR ("[pcl::SampleConsensusModel] Invalid index vector given with size %zu while the input PointCloud has size %zu!\n", indices_->size (), input_->points.size ());
          indices_->clear ();
        }
        shuffled_indices_ = *indices_;

        // Create a random number generator object
        rng_gen_.reset (new boost::variate_generator<boost::mt19937&, boost::uniform_int<> > (rng_alg_, *rng_dist_)); 
       };

      virtual ~SampleConsensusModel () {};

      virtual void 
      getSamples (int &iterations, std::vector<int> &samples)
      {
        // We're assuming that indices_ have already been set in the constructor
        if (indices_->size () < getSampleSize ())
        {
          PCL_ERROR ("[pcl::SampleConsensusModel::getSamples] Can not select %zu unique points out of %zu!\n",
                     samples.size (), indices_->size ());
          // one of these will make it stop :)
          samples.clear ();
          iterations = INT_MAX - 1;
          return;
        }

        // Get a second point which is different than the first
        samples.resize (getSampleSize ());
        for (unsigned int iter = 0; iter < max_sample_checks_; ++iter)
        {
          // Choose the random indices
          if (samples_radius_ < std::numeric_limits<double>::epsilon ())
                  SampleConsensusModel<PointT>::drawIndexSample (samples);
          else
                  SampleConsensusModel<PointT>::drawIndexSampleRadius (samples);

          // If it's a good sample, stop here
          if (isSampleGood (samples))
          {
            PCL_DEBUG ("[pcl::SampleConsensusModel::getSamples] Selected %zu samples.\n", samples.size ());
            return;
          }
        }
        PCL_DEBUG ("[pcl::SampleConsensusModel::getSamples] WARNING: Could not select %d sample points in %d iterations!\n", getSampleSize (), max_sample_checks_);
        samples.clear ();
      }

      virtual bool 
      computeModelCoefficients (const std::vector<int> &samples, 
                                Eigen::VectorXf &model_coefficients) = 0;

      virtual void 
      optimizeModelCoefficients (const std::vector<int> &inliers, 
                                 const Eigen::VectorXf &model_coefficients,
                                 Eigen::VectorXf &optimized_coefficients) = 0;

      virtual void 
      getDistancesToModel (const Eigen::VectorXf &model_coefficients, 
                           std::vector<double> &distances) = 0;

      virtual void 
      selectWithinDistance (const Eigen::VectorXf &model_coefficients, 
                            const double threshold,
                            std::vector<int> &inliers) = 0;

      virtual int
      countWithinDistance (const Eigen::VectorXf &model_coefficients, 
                           const double threshold) = 0;

      virtual void 
      projectPoints (const std::vector<int> &inliers, 
                     const Eigen::VectorXf &model_coefficients,
                     PointCloud &projected_points, 
                     bool copy_data_fields = true) = 0;

      virtual bool 
      doSamplesVerifyModel (const std::set<int> &indices, 
                            const Eigen::VectorXf &model_coefficients, 
                            const double threshold) = 0;

      inline virtual void
      setInputCloud (const PointCloudConstPtr &cloud)
      {
        input_ = cloud;
        if (!indices_)
          indices_.reset (new std::vector<int> ());
        if (indices_->empty ())
        {
          // Prepare a set of indices to be used (entire cloud)
          indices_->resize (cloud->points.size ());
          for (size_t i = 0; i < cloud->points.size (); ++i) 
            (*indices_)[i] = static_cast<int> (i);
        }
        shuffled_indices_ = *indices_;
       }

      inline PointCloudConstPtr 
      getInputCloud () const { return (input_); }

      inline void 
      setIndices (const boost::shared_ptr <std::vector<int> > &indices) 
      { 
        indices_ = indices; 
        shuffled_indices_ = *indices_;
       }

      inline void 
      setIndices (const std::vector<int> &indices) 
      { 
        indices_.reset (new std::vector<int> (indices));
        shuffled_indices_ = indices;
       }

      inline boost::shared_ptr <std::vector<int> > 
      getIndices () const { return (indices_); }

      virtual SacModel 
      getModelType () const = 0;

      inline unsigned int 
      getSampleSize () const 
      { 
        std::map<pcl::SacModel, unsigned int>::const_iterator it = SAC_SAMPLE_SIZE.find (getModelType ());
        if (it == SAC_SAMPLE_SIZE.end ())
          throw InvalidSACModelTypeException ("No sample size defined for given model type!\n");
        return (it->second);
      }

      inline void
      setRadiusLimits (const double &min_radius, const double &max_radius)
      {
        radius_min_ = min_radius;
        radius_max_ = max_radius;
      }

      inline void
      getRadiusLimits (double &min_radius, double &max_radius)
      {
        min_radius = radius_min_;
        max_radius = radius_max_;
      }
      
      inline void
      setSamplesMaxDist (const double &radius, SearchPtr search)
      {
        samples_radius_ = radius;
        samples_radius_search_ = search;
      }

      inline void
      getSamplesMaxDist (double &radius)
      {
        radius = samples_radius_;
      }

      friend class ProgressiveSampleConsensus<PointT>;

      inline double
      computeVariance (const std::vector<double> &error_sqr_dists)
      {
        std::vector<double> dists (error_sqr_dists);
        std::sort (dists.begin (), dists.end ());
        double median_error_sqr = dists[dists.size () >> 1];
        return (2.1981 * median_error_sqr);
      }

      inline double
      computeVariance ()
      {
        if (error_sqr_dists_.empty ())
        {
          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");
          return (std::numeric_limits<double>::quiet_NaN ());
        }
        return (computeVariance (error_sqr_dists_));
      }

                protected:
      inline void
      drawIndexSample (std::vector<int> &sample)
      {
        size_t sample_size = sample.size ();
        size_t index_size = shuffled_indices_.size ();
        for (unsigned int i = 0; i < sample_size; ++i)
          // The 1/(RAND_MAX+1.0) trick is when the random numbers are not uniformly distributed and for small modulo
          // elements, that does not matter (and nowadays, random number generators are good)
          //std::swap (shuffled_indices_[i], shuffled_indices_[i + (rand () % (index_size - i))]);
          std::swap (shuffled_indices_[i], shuffled_indices_[i + (rnd () % (index_size - i))]);
        std::copy (shuffled_indices_.begin (), shuffled_indices_.begin () + sample_size, sample.begin ());
      }

      inline void
      drawIndexSampleRadius (std::vector<int> &sample)
      {
        size_t sample_size = sample.size ();
        size_t index_size = shuffled_indices_.size ();

        std::swap (shuffled_indices_[0], shuffled_indices_[0 + (rnd () % (index_size - 0))]);
        //const PointT& pt0 = (*input_)[shuffled_indices_[0]];

        std::vector<int> indices;
        std::vector<float> sqr_dists;

        // If indices have been set when the search object was constructed,
        // radiusSearch() expects an index into the indices vector as its
        // first parameter. This can't be determined efficiently, so we use
        // the point instead of the index.
        // Returned indices are converted automatically.
        samples_radius_search_->radiusSearch (input_->at(shuffled_indices_[0]),
                                              samples_radius_, indices, sqr_dists );

        if (indices.size () < sample_size - 1)
        {
          // radius search failed, make an invalid model
          for(unsigned int i = 1; i < sample_size; ++i)
            shuffled_indices_[i] = shuffled_indices_[0];
        }
        else
        {
          for (unsigned int i = 0; i < sample_size-1; ++i)
            std::swap (indices[i], indices[i + (rnd () % (indices.size () - i))]);
          for (unsigned int i = 1; i < sample_size; ++i)
            shuffled_indices_[i] = indices[i-1];
        }

        std::copy (shuffled_indices_.begin (), shuffled_indices_.begin () + sample_size, sample.begin ());
      }

      virtual inline bool
      isModelValid (const Eigen::VectorXf &model_coefficients) = 0;

      virtual bool
      isSampleGood (const std::vector<int> &samples) const = 0;

      PointCloudConstPtr input_;

      boost::shared_ptr <std::vector<int> > indices_;

      static const unsigned int max_sample_checks_ = 1000;

      double radius_min_, radius_max_;

      double samples_radius_;

      SearchPtr samples_radius_search_;

      std::vector<int> shuffled_indices_;

      boost::mt19937 rng_alg_;

      boost::shared_ptr<boost::uniform_int<> > rng_dist_;

      boost::shared_ptr<boost::variate_generator< boost::mt19937&, boost::uniform_int<> > > rng_gen_;

      std::vector<double> error_sqr_dists_;

      inline int
      rnd ()
      {
        return ((*rng_gen_) ());
      }
    public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };

  template <typename PointT, typename PointNT>
  class SampleConsensusModelFromNormals //: public SampleConsensusModel<PointT>
  {
    public:
      typedef typename pcl::PointCloud<PointNT>::ConstPtr PointCloudNConstPtr;
      typedef typename pcl::PointCloud<PointNT>::Ptr PointCloudNPtr;

      typedef boost::shared_ptr<SampleConsensusModelFromNormals> Ptr;
      typedef boost::shared_ptr<const SampleConsensusModelFromNormals> ConstPtr;

      SampleConsensusModelFromNormals () : normal_distance_weight_ (0.0), normals_ () {};

      virtual ~SampleConsensusModelFromNormals () {}

      inline void 
      setNormalDistanceWeight (const double w) 
      { 
        normal_distance_weight_ = w; 
      }

      inline double 
      getNormalDistanceWeight () { return (normal_distance_weight_); }

      inline void 
      setInputNormals (const PointCloudNConstPtr &normals) 
      { 
        normals_ = normals; 
      }

      inline PointCloudNConstPtr 
      getInputNormals () { return (normals_); }

    protected:
      double normal_distance_weight_;

      PointCloudNConstPtr normals_;
  };

  template<typename _Scalar, int NX=Eigen::Dynamic, int NY=Eigen::Dynamic>
  struct Functor
  {
    typedef _Scalar Scalar;
    enum 
    {
      InputsAtCompileTime = NX,
      ValuesAtCompileTime = NY
    };

    typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,1> ValueType;
    typedef Eigen::Matrix<Scalar,InputsAtCompileTime,1> InputType;
    typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType;

    Functor () : m_data_points_ (ValuesAtCompileTime) {}

    Functor (int m_data_points) : m_data_points_ (m_data_points) {}
  
    virtual ~Functor () {}

    int
    values () const { return (m_data_points_); }

    private:
      const int m_data_points_;
  };
}

#endif  //#ifndef PCL_SAMPLE_CONSENSUS_MODEL_H_