edges.hpp

Go to the documentation of this file.

template<typename Point>
void Feature_Edges<Point>::extractFeatures(const pcl::PointCloud<Point>& point_cloud, pcl::PointCloud<Point> &edges) {
  pcl::PointCloud<Normal>::Ptr n(new pcl::PointCloud<Normal>);
  pcl::PointCloud<InterestPoint>::Ptr ip(new pcl::PointCloud<InterestPoint>);

  ROS_INFO("normals...");

  pcl::IntegralImageNormalEstimation<Point, pcl::Normal> ne;

  ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
  ne.setMaxDepthChangeFactor(0.2f);
  ne.setRectSize(10,10);
  ne.setNormalSmoothingSize(10.0f);
  ne.setDepthDependentSmoothing(true);
  ne.setInputCloud(point_cloud.makeShared());

  ne.compute(*n);

  ROS_INFO("edges...");
#ifdef GICP_ENABLE
  boost::shared_ptr<pcl::search::OrganizedNeighbor<Point> > oTree (new pcl::search::OrganizedNeighbor<Point> );
#else
  #ifdef PCL_VERSION_COMPARE
    boost::shared_ptr<search::OrganizedNeighbor<Point> > oTree (new search::OrganizedNeighbor<Point> );
  #else
    boost::shared_ptr<pcl::OrganizedDataIndex<Point> > oTree (new pcl::OrganizedDataIndex<Point> );
  #endif

#endif
  cob_3d_features::FastEdgeEstimation3DOMP<Point, Normal, InterestPoint> ee;
  /*ee.setRadiusSearch(radius_);
  ee.dist_threshold_ = dist_threshold_;
  ee.setSearchMethod(oTree);*/
  ee.setInputCloud(point_cloud.makeShared());
  ee.setInputNormals(n);
  ee.compute(*ip);

  edges.points.clear();
  for (size_t i = 0; i < ip->points.size(); i++)
  {
    if(ip->points[i].strength>thr_ && ip->points[i].strength<=1.f) {
      edges.points.push_back(point_cloud.points[i]);
    }
  }

  edges.width=edges.size();
  edges.height=1;
}