fast_edge_estimation_3d.hpp

Go to the documentation of this file.

#ifndef __IMPL_FAST_EDGE_ESTIMATION_3D_H__
#define __IMPL_FAST_EDGE_ESTIMATION_3D_H__

#include "cob_3d_features/fast_edge_estimation_3d.h"

template <typename PointInT, typename PointNT, typename PointOutT> void
cob_3d_features::FastEdgeEstimation3D<PointInT, PointNT, PointOutT>::isEdgePoint (
    const pcl::PointCloud<PointInT> &cloud,
    const PointInT &q_point,
    const std::vector<int> &indices,
    const Eigen::Vector3f &n,
    float &strength)
{
  if (indices.size () < 3)
  {
    strength = 0.0;
    return;
  }
  float nd_dot;
  Eigen::Vector3f delta;
  Eigen::Vector3f q = q_point.getVector3fMap();
  // Compute the angles between each neighboring point and the query point itself
  //int nn_ctr=0;//, nan_ctr=0;
  int b_ctr=0;
  float dist_threshold = 0.0015544 * pixel_search_radius_ * q.norm();

  for (std::vector<int>::const_iterator it = indices.begin(); it != indices.end(); ++it)
  {
    delta = cloud.points[*it].getVector3fMap() - q;
    nd_dot = n.dot (delta);
    if(fabs(nd_dot) > dist_threshold) //is border point
      b_ctr++;
  }
  //std::cout << dist_threshold << "\t" << indices.size() << "\t" << b_ctr << std::endl;
  strength = (float)b_ctr/indices.size();
  return;
}

template <typename PointInT, typename PointNT, typename PointOutT> void
cob_3d_features::FastEdgeEstimation3D<PointInT, PointNT, PointOutT>::computeFeature (
  PointCloudOut &output)
{
  std::vector<int> nn_indices;
  Eigen::Vector3f normal;

  // Iterating over the entire index vector
  for (std::vector<int>::iterator it = indices_->begin(); it != indices_->end(); ++it)
  {
    if (isnan(normals_->points[*it].normal[0]))
      output.points[*it].strength=2;
    else
    {
      this->searchForNeighbors (*it, nn_indices);

      normal[0] = normals_->points[*it].normal_x;
      normal[1] = normals_->points[*it].normal_y;
      normal[2] = normals_->points[*it].normal_z;
      // Estimate whether the point is lying on a boundary surface or not
      isEdgePoint (*surface_, input_->points[*it], nn_indices,
                   normal, output.points[*it].strength);
    }
  }
}

#define PCL_INSTANTIATE_FastEdgeEstimation3D(PointInT,PointNT,PointOutT) template class PCL_EXPORTS cob_3d_features::FastEdgeEstimation3D<PointInT, PointNT, PointOutT>;

#endif    // __IMPL_FAST_EDGE_ESTIMATION_3D_H__