modified_icp.hpp

Go to the documentation of this file.

#pragma once

#include <pcl/registration/icp.h>
#include <pcl/kdtree/kdtree.h>
#include "../feature_container.h"
#ifdef PCL_VERSION_COMPARE
  #include <pcl/registration/registration.h>
  #include <pcl/features/feature.h>
  #include <pcl/search/kdtree.h>
  #include <pcl/search/search.h>
#endif
#ifdef GICP_ENABLE
#include <pcl/registration/gicp.h>
#include <pcl/registration/transformation_estimation_lm.h>
#endif

class ModifiedICP_G {
public:
  virtual void setLM()=0;
  virtual void setSearchFeatures(FeatureContainerInterface* features)=0;
};

template <typename ParentClass, typename PointT>
class ModifiedICP_T : public ParentClass, public ModifiedICP_G {
  const FeatureContainerInterface* features_;

  template <typename PointT2>
  #ifdef PCL_VERSION_COMPARE
    #if PCL_MINOR_VERSION > 6 // not tested with pcl 1.4 yet
      class FeatureSearch: public pcl::search::KdTree<PointT2>
    #else
      class FeatureSearch: public pcl::KdTreeFLANN<PointT2>
    #endif
  #else
    class FeatureSearch : public pcl::KdTree<PointT2>
  #endif
  {
    FeatureContainerInterface* features_;

    void error() const {ROS_ERROR("unsupported");}

  public:
    FeatureSearch(FeatureContainerInterface* features):
      features_(features)
      { }

    virtual int
    nearestKSearch (const pcl::PointCloud<PointT2> &cloud, int index, int k, std::vector<int> &k_indices, std::vector<float> &k_sqr_distances)
    {
      features_->findFeatureCorrespondences (index, k_indices, k_sqr_distances);
      return k_indices.size();
    }

    virtual int
    nearestKSearch (const PointT2 &p_q, int k, std::vector<int> &k_indices, std::vector<float> &k_sqr_distances) {error();return 0;}

    virtual int
    nearestKSearch (int index, int k, std::vector<int> &k_indices, std::vector<float> &k_sqr_distances) {error();return 0;}

    virtual int
    radiusSearch (const pcl::PointCloud<PointT2> &cloud, int index, double radius, std::vector<int> &k_indices,
                  std::vector<float> &k_sqr_distances, int max_nn = INT_MAX) const {error();return 0;}

    virtual int
    radiusSearch (const PointT2 &p_q, double radius, std::vector<int> &k_indices,
                  std::vector<float> &k_sqr_distances, int max_nn = INT_MAX) const {error();return 0;}

    virtual int
    radiusSearch (int index, double radius, std::vector<int> &k_indices,
                  std::vector<float> &k_sqr_distances, int max_nn = INT_MAX) const {error();return 0;}

    #if PCL_MINOR_VERSION > 6
    virtual const std::string& getName() const {return "ICP Feature";}
    #else
    virtual std::string getName() const {return "ICP Feature";}
    #endif

  };

  public:

  int getNeededIterations() {return this->nr_iterations_;}

  void setLM() {
#ifdef GICP_ENABLE
    this->min_number_correspondences_ = 4;
    this->reg_name_ = "IterativeClosestPointNonLinear";

    this->transformation_estimation_.reset (new pcl::registration::TransformationEstimationLM<PointT, PointT>);
#endif
  }

  void setSearchFeatures(FeatureContainerInterface* features) {
    this->features_ = features;
    //this->tree_.reset( static_cast<typename FeatureSearch<PointT>::BaseSearch*>(new FeatureSearch<PointT>(features)) );
    this->tree_.reset( new FeatureSearch<PointT>(features) );
  }

};

template <class PointT>
class ModifiedICP : public ModifiedICP_T<pcl::IterativeClosestPoint<PointT,PointT>, PointT>
{
};


#ifdef GICP_ENABLE

template <class PointT>
class ModifiedGICP : public pcl::GeneralizedIterativeClosestPoint<PointT,PointT>, public ModifiedICP_G
{
  public:
  int getNeededIterations() {return 1;} //dummy

  void setLM() {
    std::cerr<<"ERROR: not supported\n";
  }

  void setSearchFeatures(FeatureContainerInterface* features) {
    std::cerr<<"ERROR: not supported\n";
  }
};
#endif