curvature_classifier.hpp

Go to the documentation of this file.

#ifndef __IMPL_CURVATURE_CLASSIFIER_H__
#define __IMPL_CURVATURE_CLASSIFIER_H__

#include "cob_3d_features/curvature_classifier.h"


template <typename PointInT, typename PointOutT> void
cob_3d_features::CurvatureClassifier<PointInT,PointOutT>::classify(PointCloudOut &output)
{
  //std::cout << "da" << std::endl;
  if(!initCompute())
  {
    output.width = output.height = 0;
    output.points.clear ();
    std::cout << "error" << std::endl;
    return;
  }
  if (output.points.size() != input_->size())
  {
    output.points.resize(input_->size());
    output.height = input_->height;
    output.width = input_->width;
  }
  for (size_t i=0; i < indices_->size(); ++i)
  {
    float pc1 = std::fabs(input_->points[(*indices_)[i]].pc1);
    float pc2 = std::fabs(input_->points[(*indices_)[i]].pc2);
    if (output.points[i].label == I_BORDER) {continue;}
    if (pc1 < c_lower_)
    {
      output.points[i].label = I_PLANE;
    }
    else if (pc1 > c_upper_)
    {
      output.points[i].label = I_EDGE;
      /* // Uncomment for corner support:
      if (input_->points[(*indices_)[i]].pc1 < c_ratio_edge_corner_ * input_->points[(*indices_)[i]].pc2)
        output.points[i].label = I_CORNER;
      else
        output.points[i].label = I_EDGE;
      */
    }
    else if (pc1 < c_ratio_cylinder_sphere_ * pc2)
    {
      output.points[i].label = I_SPHERE;
    }
    else
    {
      output.points[i].label = I_CYL;
    }
  }
}

template <typename PointInT, typename PointOutT> void
cob_3d_features::CurvatureClassifier<PointInT,PointOutT>::classifyForSegmentation(PointCloudOut &output)
{
  //std::cout << "da" << std::endl;
  if(!initCompute())
  {
    output.width = output.height = 0;
    output.points.clear ();
    std::cout << "error" << std::endl;
    return;
  }
  if (output.points.size() != input_->size())
  {
    output.points.resize(input_->size());
    output.height = input_->height;
    output.width = input_->width;
  }
  for (size_t i=0; i < indices_->size(); ++i)
  {
    if (output.points[i].label == I_NAN) {continue;}
    if (output.points[i].label == I_BORDER) {output.points[i].label = I_EDGE; continue;}
    if (input_->points[(*indices_)[i]].pc1 > c_upper_)
    {
      output.points[i].label = I_EDGE;
    }
    else
    {
      output.points[i].label = I_UNDEF;
    }
  }
}

#define PCL_INSTANTIATE_CurvatureClassifier(T,OutT) template class PCL_EXPORTS cob_3d_features::CurvatureClassifier<T,OutT>;

#endif