registration_icp.h

Go to the documentation of this file.

#ifndef REGISTRATION_ICP_H_
#define REGISTRATION_ICP_H_

#include "general_registration.h"
#include "impl/modified_icp.hpp"
#include "feature_container.h"
#include <pcl/point_cloud.h>

#include <Eigen/Core>
#include <pcl/io/io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/registration/ia_ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/io/pcd_io.h>
#include <pcl/registration/icp.h>
#include <pcl/features/fpfh.h>
#include <pcl/features/normal_3d.h>

namespace cob_3d_registration {

template <typename Point>
class Registration_ICP : public GeneralRegistration<Point>
{
public:
  Registration_ICP():
    icp_max_iterations_(50),
    icp_max_corr_dist_(0.05),
    outlier_rejection_threshold_(0.01),
    icp_trf_epsilon_(0.0001),
    non_linear_(false),
    use_only_last_refrence_(false),
    use_gicp_(false)
  {}

  void setNonLinear(bool b) {non_linear_=b;}

  void setUseOnlyLastReference(bool b) {use_only_last_refrence_=b;}

  void setUseGICP(bool b) {
    use_gicp_=b;
#ifndef GICP_ENABLE
    ROS_ERROR("GICP is not supported by this version");
#endif
  }

  virtual boost::shared_ptr<pcl::PointCloud<Point> > getMap() {return register_.makeShared();}

  void setMaxIterations(int v) {icp_max_iterations_=v;}

  void setCorrDist(float v) {icp_max_corr_dist_=v;}

  void setTrfEpsilon(float v) {icp_trf_epsilon_=v;}

  void setOutlierRejectionThreshold(float v) {outlier_rejection_threshold_=v;}

protected:

  virtual bool compute_features();
  virtual bool compute_corrospondences();
  virtual bool compute_transformation();

  virtual void setSettingsForICP(ModifiedICP_G *icp);

  //internal states
  pcl::PointCloud<Point> register_;

  // parameters
  int icp_max_iterations_;
  float icp_max_corr_dist_,
  outlier_rejection_threshold_,
  icp_trf_epsilon_;
  bool non_linear_, use_only_last_refrence_, use_gicp_;
};

template <typename Point>
class Registration_ICP_Features : public Registration_ICP<Point>
{
public:

  void setFeatures(FeatureContainerInterface* features) {features_ = features;}
protected:

  FeatureContainerInterface* features_;

  virtual void setSettingsForICP(ModifiedICP_G *icp);
};

template <typename Point, typename FeatureType>
class Registration_ICP_Features_Extra : public Registration_ICP_Features<Point>
{
public:
  //virtual bool calculateFeature(pcl::PointCloud<Point>::Ptr input, pcl::PointCloud<Point>::Ptr output, pcl::PointCloud<FeatureType>::Ptr features);
  virtual bool calculateFeature(boost::shared_ptr<pcl::PointCloud<Point> > input, boost::shared_ptr<pcl::PointCloud<Point> > output, boost::shared_ptr<pcl::PointCloud<FeatureType> > features);
protected:
  virtual bool compute_features();
};


}

#endif