tf_est_multi_cors.h

Go to the documentation of this file.

#ifndef TF_EST_MULTI_CORS_H_
#define TF_EST_MULTI_CORS_H_

#include <pcl/registration/registration.h>
#ifdef PCL_VERSION_COMPARE
  #include <pcl/registration/correspondence_estimation.h>
  #include <pcl/registration/correspondence_types.h>
#endif

template <typename Point>
class TransformationEstimationMultipleCorrespondences : public pcl::Registration<Point, Point>
{

  struct COR_S {
    int ind_o, ind_n;
    float dis;
  };
  
  struct SORT_S2 {
    float dis;
    int ind;

    bool operator() (const SORT_S2 &i, const SORT_S2 &j) const { return (i.dis<j.dis);}
  };

  typedef typename pcl::Registration<Point, Point>::PointCloudSource PointCloudSource;
  typedef typename PointCloudSource::Ptr PointCloudSourcePtr;
  typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr;

public:
  TransformationEstimationMultipleCorrespondences () :
    tmax_(0.1f), rmax_(0.1f)
  {
    reg_name_ = "TransformationEstimationMultipleCorrespondences";
  };

  void setMaxAngularDistance(const float f) {rmax_=f;}
  void setMaxTranslationDistance(const float f) {tmax_=f;}

  Eigen::Matrix4f compute(const pcl::PointCloud<Point> &pc_old, const pcl::PointCloud<Point> &pc_new);

protected:
  virtual void
  computeTransformation (PointCloudSource &output);

  virtual void
  computeTransformation (PointCloudSource &output,const Eigen::Matrix4f& guess){};


  Eigen::Matrix4f findTF_fast
  (const pcl::PointCloud<Point> &pc_old, const pcl::PointCloud<Point> &pc_new,
                         const float rmax=0.1, const float tmax=0.1, Eigen::Matrix4f tf=Eigen::Matrix4f::Identity());
  int search_sorted_vector(const std::vector<SORT_S2> &tv, const float val);

  float tmax_,rmax_;

  using pcl::Registration<Point, Point>::converged_;
  using pcl::Registration<Point, Point>::reg_name_;
  using pcl::Registration<Point, Point>::final_transformation_;
  using pcl::Registration<Point, Point>::target_;

};

#include "impl/tf_est_multi_cors.hpp"

#endif /* TF_EST_MULTI_CORS_H_ */