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.
 *  
 *  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 Willow Garage, Inc. 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: sac_model.h 6214 2012-07-06 19:31:29Z rusu $
 *
 */

#ifndef PCL_SAMPLE_CONSENSUS_MODEL_H_
#define PCL_SAMPLE_CONSENSUS_MODEL_H_

#include <cfloat>
#include <ctime>
#include <limits.h>
#include <set>
#include <boost/random.hpp>

#include <pcl/console/print.h>
#include <pcl/point_cloud.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.),
        shuffled_indices_ (),
        rng_alg_ (),
        rng_dist_ (new boost::uniform_int<> (0, std::numeric_limits<int>::max ())),
        rng_gen_ ()
      {
        // 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.),
        shuffled_indices_ (),
        rng_alg_ (),
        rng_dist_ (new boost::uniform_int<> (0, std::numeric_limits<int>::max ())),
        rng_gen_ ()
      {
        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.),
                            shuffled_indices_ (),
                            rng_alg_ (),
                            rng_dist_ (new boost::uniform_int<> (0, std::numeric_limits<int>::max ())),
                            rng_gen_ ()
      {
        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 () {};

      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))
            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>;

                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;

        samples_radius_search_->radiusSearch (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_;

      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_