obstacle_detection_normals_alg.cpp

Go to the documentation of this file.

#include "obstacle_detection_normals_alg.h"
using namespace std;

ObstacleDetectionNormalsAlgorithm::ObstacleDetectionNormalsAlgorithm(void)
{
}

ObstacleDetectionNormalsAlgorithm::~ObstacleDetectionNormalsAlgorithm(void)
{
}

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

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

// ObstacleDetectionNormalsAlgorithm Public API

void ObstacleDetectionNormalsAlgorithm::cloud_all(int KSearch, float normal_z, 
     float normal_y, float normal_x, float min_dist, 
     const pcl::PointCloud<pcl::PointXYZ>& cloud, 
     pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr& cloud_out, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr& cloud_obs)
{

//pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
  // Output datasets
  pcl::PointCloud<pcl::Normal>::Ptr output (new pcl::PointCloud<pcl::Normal>);

// Normal estimation using integral Images /////////////////////////////////////////////
// NOTE: Point cloud should contain width and height structure

// pcl::IntegralImageNormalEstimation<pcl::PointXYZ, pcl::Normal> n;

// n.setNormalEstimationMethod(n.AVERAGE_3D_GRADIENT);
// n.setMaxDepthChangeFactor(0.02f);
// n.setNormalSmoothingSize(10.0f);
// n.setInputCloud(cloud);
// n.compute(*output);

// ROS Fuerte version //////////////////////////////////////////////////////////////

  // Create the normal estimation class, and pass the input dataset to it
  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
  // [Multi-threat](NOTE: Point cloud should contain width and height structure):
  // pcl::NormaleEstimationOMP<pcl::PointXYZ, pcl::Normal> n;
  // n.setNumberOfThreads(8); 

  // To work with Pointers ::Ptr ///////////////////////////////////////////////////////////
  // // Set input cloud
  // n.setInputCloud (cloud);
            
  // // Create an empty kdtree representation, and pass it to the normal estimation object.
  // // Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).
  // pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
  // n.setSearchMethod (tree);

  // // Use all neighbors in a sphere of radius 3cm
  // n.setRadiusSearch (KSearch);
  // // [Multi-threat](NOTE: Point cloud should contain width and height structure) :
  // // n.setKSearch (KSearch);

  // double secs = ros::Time::now().toSec();

  // // Compute the features
  // n.compute (*output);

  // pcl::concatenateFields (*cloud, *output, *cloud_out);

  // double secs2 = ros::Time::now().toSec();
  // double diff = secs2-secs;
  // ROS_INFO("computation time: %2.2f",diff);


  std::vector<int> indices;
  indices.resize (cloud.points.size());
  for (size_t i = 0; i < indices.size (); ++i) { indices[i] = i; }

  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());


  boost::shared_ptr< vector<int> > indicesptr(new vector<int> (indices));
  // set normal estimation parameters
  n.setIndices(indicesptr);
  n.setSearchMethod(tree);
  n.setKSearch(KSearch); // Use 10 nearest neighbors to estimate the normals

  //estimate
  n.setInputCloud(cloud.makeShared());
  n.compute(*output);

  pcl::concatenateFields (cloud, *output, *cloud_out);



  float e=0.0;

  *cloud_obs=*cloud_out; 

  for (size_t rowIndex=0, pointIndex=0; rowIndex<cloud_out->height; ++rowIndex)
  {
    for (size_t colIndex=0; colIndex<cloud_out->width; ++colIndex, ++pointIndex)
    {
      //ROS_ERROR_STREAM(cloud_out->points[pointIndex].normal_z);
      if (cloud_out->points[pointIndex].z > min_dist)
      {
        if (cloud_out->points[pointIndex].normal_z < normal_z && 
            cloud_out->points[pointIndex].normal_y < normal_y && 
            cloud_out->points[pointIndex].normal_x < normal_x)
        {
          cloud_out->points[pointIndex].r=0;
          cloud_out->points[pointIndex].g=255;
          cloud_out->points[pointIndex].b=0;
          cloud_obs->points[pointIndex].x=std::numeric_limits<float>::quiet_NaN();;
          cloud_obs->points[pointIndex].y=std::numeric_limits<float>::quiet_NaN();;
          cloud_obs->points[pointIndex].z=std::numeric_limits<float>::quiet_NaN();;
        }
        else 
        {
          cloud_out->points[pointIndex].r=255;
          cloud_out->points[pointIndex].g=0;
          cloud_out->points[pointIndex].b=0;
          //error counter looking a plane without obstacles
          e=e+1.0;
        }
      }
      else
      {
        cloud_out->points[pointIndex].r=0;
        cloud_out->points[pointIndex].g=0;
        cloud_out->points[pointIndex].b=255;
      }
    }
  }
  e=(e/float(cloud_out->width*cloud_out->height))*100; 
  //printf("error %f",e); 
}