pcl_sac_model_orientation.h

Go to the documentation of this file.

#ifndef PCL_IAS_SAMPLE_CONSENSUS_SACMODELORIENTATION_H_
#define PCL_IAS_SAMPLE_CONSENSUS_SACMODELORIENTATION_H_

#include <ros/assert.h>
// SaC
#include <pcl/sample_consensus/sac_model.h>
#include <pcl/sample_consensus/model_types.h>

#include <pcl/features/normal_3d.h>

#include <boost/make_shared.hpp>
// Kd Tree
//#include <pcl/kdtree/kdtree_ann.h>

// Eigen
//#include <Eigen/Array>
//#include <Eigen/Geometry>

namespace pcl
{
  inline Eigen::Vector3f rotateAroundAxis (Eigen::Vector3f r, Eigen::Vector3f axis, float angle)
  {
    // Rodrigues' rotation formula
    return r * cos(angle) + (axis.cross(r)) * sin(angle) + axis * axis.dot(r) * (1-cos(angle));
  }

  template <typename NormalT>
  class NormalPointRepresentation : public PointRepresentation <NormalT>
  {
    public:

      using PointRepresentation<NormalT>::nr_dimensions_;

      typedef boost::shared_ptr<NormalPointRepresentation<NormalT> > Ptr;
      typedef boost::shared_ptr<const NormalPointRepresentation<NormalT> > ConstPtr;

      NormalPointRepresentation ()
      {
        nr_dimensions_ = 3;
      }

      virtual void
      copyToFloatArray (const NormalT &p, float * out) const
      {
        out[0] = p.normal[0];
        out[1] = p.normal[1];
        out[2] = p.normal[2];
      }
  };

  template <typename NormalT>
  class SACModelOrientation : public SampleConsensusModel<NormalT>
  {
    public:
      using SampleConsensusModel<NormalT>::input_;
      using SampleConsensusModel<NormalT>::indices_;

      typedef typename SampleConsensusModel<NormalT>::PointCloud Normals;
      typedef typename SampleConsensusModel<NormalT>::PointCloudPtr NormalsPtr;
      typedef typename SampleConsensusModel<NormalT>::PointCloudConstPtr NormalsConstPtr;

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


      Eigen::Vector3f axis_;


      SACModelOrientation (const NormalsConstPtr &cloud) : SampleConsensusModel<NormalT> (cloud)
      {
        typename NormalPointRepresentation<NormalT>::ConstPtr repr (new NormalPointRepresentation<NormalT>);
        kdtree_ = boost::make_shared<KdTreeFLANN <NormalT> >();
        kdtree_->setPointRepresentation (repr);
        kdtree_->setInputCloud (cloud);
      }


      SACModelOrientation (const NormalsConstPtr &cloud, const std::vector<int> &indices) : SampleConsensusModel<NormalT> (cloud, indices)
      {
        kdtree_ = boost::make_shared<KdTreeFLANN <NormalT> >();
        kdtree_->setInputCloud (cloud);
      }


      inline void setAxis (Eigen::Vector3f axis) { axis_ = axis; }


      inline Eigen::Vector3f getAxis () const { return (axis_); }


      void getSamples (int &iterations, std::vector<int> &samples);


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


      void refitModel (const std::vector<int> &inliers, std::vector<double> &refit_coefficients);


      void getDistancesToModel (const Eigen::VectorXf &model_coefficients, std::vector<double> &distances) {}


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

      int countWithinDistance(const Eigen::VectorXf& model_coefficients, double threshold)
      {
        std::vector<int> inliers;
        selectWithinDistance (model_coefficients, threshold, inliers);
        return inliers.size ();
      }


      void projectPoints (const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Normals &projected_points, bool copy_data_fields = true){}



      bool doSamplesVerifyModel (const std::set<int> &indices, const Eigen::VectorXf &model_coefficients, double threshold) { return true; }

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


      //@TODO: fix return to return SACMODEL_ORIENTATION
      inline pcl::SacModel getModelType () const { return pcl::SacModel(SACMODEL_PLANE);}

      inline void optimizeModelCoefficients(   const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients,
                                        Eigen::VectorXf &optimized_coefficients){}

      EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    protected:


       inline bool
          isModelValid (const Eigen::VectorXf &model_coefficients)
       {
          // Needs a valid model coefficients
          ROS_ASSERT (model_coefficients.size () == 4);
          return (true);
       }


      //int nx_idx_;

    private:

      typename KdTreeFLANN<NormalT>::Ptr kdtree_;
      std::vector<int> front_indices_;
      std::vector<int>  back_indices_;
      std::vector<int>  left_indices_;
      std::vector<int> right_indices_;
      std::vector<float> points_sqr_distances_;

  };
}

#include "pcl_ias_sample_consensus/pcl_sac_model_orientation.hpp"

#endif