hole_detection_alg.cpp

Go to the documentation of this file.

#include "hole_detection_alg.h"
using namespace std;

HoleDetectionAlgorithm::HoleDetectionAlgorithm(void)
{
}

HoleDetectionAlgorithm::~HoleDetectionAlgorithm(void)
{
}

void HoleDetectionAlgorithm::config_update(Config& new_cfg, uint32_t level)
{
  this->lock();

  // save the current configuration
  this->config_=new_cfg;
  
  this->unlock();
}

// HoleDetectionAlgorithm Public API

void HoleDetectionAlgorithm::cloud_all(int hole_min_p, float box_y_end, float box_z_ini, 
               float box_x, float box_y_ini, float box_z_end, 
               float Xl, float Xc, float Xr, float Y, 
               pcl::PointCloud<pcl::PointXYZRGB>& cloud_in, 
               pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_for)
{
int l=0;
int c=0;
int r=0;


for (size_t rowIndex=0, pointIndex=0; rowIndex<cloud_in.height; ++rowIndex)
{
  for (size_t colIndex=0; colIndex<cloud_in.width; ++colIndex, ++pointIndex)
  {
    if (box_y_ini < cloud_in.points[pointIndex].y && 
      cloud_in.points[pointIndex].y < box_y_end && 
      box_z_ini < cloud_in.points[pointIndex].z && 
      cloud_in.points[pointIndex].z < box_z_end)
    {
      if (-(box_x/2) < cloud_in.points[pointIndex].x && 
        cloud_in.points[pointIndex].x < ((box_x/3)-(box_x/2)))
      {
        cloud_in.points[pointIndex].r=0;
        cloud_in.points[pointIndex].g=0;
        cloud_in.points[pointIndex].b=255;
        l=l+1;
      }
      if (((box_x/3)-(box_x/2)) < cloud_in.points[pointIndex].x && 
        cloud_in.points[pointIndex].x < ((box_x/2)-(box_x/3)))
      {
        cloud_in.points[pointIndex].r=0;
        cloud_in.points[pointIndex].g=255;
        cloud_in.points[pointIndex].b=255;
        c=c+1;
      }
      if (((box_x/2)-(box_x/3)) < cloud_in.points[pointIndex].x && 
        cloud_in.points[pointIndex].x < (box_x/2))
      {
        cloud_in.points[pointIndex].r=0;
        cloud_in.points[pointIndex].g=127;
        cloud_in.points[pointIndex].b=255;
        r=r+1;
      }
    }
  }
}

// Hole Point Cloud


Y=-0.1;

for (size_t rowIndex=0, pointIndex=0; rowIndex<5; ++rowIndex)
{
  Xl= -(box_x/2);
  Xc= ((box_x/3)-(box_x/2));
  Xr= ((box_x/2)-(box_x/3));
  for (size_t colIndex=0; colIndex<5; ++colIndex, ++pointIndex)
        {
    if (l<hole_min_p)
    {
      cloud_for->points.push_back (pcl::PointXYZ(Xl+0.05, Y, box_z_ini));
      Xl=Xl+0.05;
    }
    if (c<hole_min_p)
    {
      cloud_for->points.push_back (pcl::PointXYZ(Xc+0.05, Y, box_z_ini));
      Xc=Xc+0.05;
    }
    if (r<hole_min_p)
    {
      cloud_for->points.push_back (pcl::PointXYZ(Xr+0.05, Y, box_z_ini));
      Xr=Xr+0.05;
    }
  }
  Y=Y-0.05;
}

}