Original code using Z instead of X as main axis

src/obstacle_detection_normals_alg.cpp

@@ -15,20 +15,15 @@ void ObstacleDetectionNormalsAlgorithm::config_update(Config& new_cfg, uint32_t
 
   // save the current configuration
   this->config_=new_cfg;
-
+  
   this->unlock();
 }
 
 // ObstacleDetectionNormalsAlgorithm Public API
-/*
-It calculates the normal vector to each point in the pointcloud. For that, it uses
-the KSearch nearest neighbors. After that, it checks that each normal is inside
-some thresholds to determine if it is a passable point or an obstacle. If some point
-is nearer than a minimum distance, it is marked in a diferent color.
-*/
-void ObstacleDetectionNormalsAlgorithm::cloud_all(int KSearch, float normal_z,
-     float normal_y, float normal_x, float min_dist,
-     const pcl::PointCloud<pcl::PointXYZ>& cloud,
+
+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)
 {
 
@@ -53,12 +48,12 @@ void ObstacleDetectionNormalsAlgorithm::cloud_all(int KSearch, float normal_z,
   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);
+  // 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> ());
@@ -95,7 +90,6 @@ void ObstacleDetectionNormalsAlgorithm::cloud_all(int KSearch, float normal_z,
   n.setSearchMethod(tree);
   n.setKSearch(KSearch); // Use 10 nearest neighbors to estimate the normals
 
-  //n.setViewPoint (0.26374, 0.0, -0.0485);
   //estimate
   n.setInputCloud(cloud.makeShared());
   n.compute(*output);
@@ -106,27 +100,21 @@ void ObstacleDetectionNormalsAlgorithm::cloud_all(int KSearch, float normal_z,
 
   float e=0.0;
 
-  *cloud_obs=*cloud_out;
+  *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].x > min_dist)
+      if (cloud_out->points[pointIndex].z > min_dist)
       {
 				//ROS_INFO_STREAM("z_normal: " << cloud_out->points[pointIndex].normal_z);
-
-        if (abs(cloud_out->points[pointIndex].normal_z) > normal_z &&
-            cloud_out->points[pointIndex].normal_y < normal_y &&
+				
+        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 )
         {
-          /*if (pointIndex % 200 == 0)
-          {
-            ROS_INFO_STREAM("x_normal: " << cloud_out->points[pointIndex].normal_x);
-            ROS_INFO_STREAM("y_normal: " << cloud_out->points[pointIndex].normal_y);
-            ROS_INFO_STREAM("z_normal: " << cloud_out->points[pointIndex].normal_z);
-          }*/
           cloud_out->points[pointIndex].r=0;
           cloud_out->points[pointIndex].g=255;
           cloud_out->points[pointIndex].b=0;
@@ -134,15 +122,9 @@ void ObstacleDetectionNormalsAlgorithm::cloud_all(int KSearch, float normal_z,
           cloud_obs->points[pointIndex].y=std::numeric_limits<float>::quiet_NaN();;
           cloud_obs->points[pointIndex].z=std::numeric_limits<float>::quiet_NaN();;
         }
-        else
+        else 
         {
-          //ROS_ERROR_STREAM("z_normal: " << cloud_out->points[pointIndex].normal_z);
-          /*if (pointIndex % 200 == 0)
-          {
-            ROS_ERROR_STREAM("x_normal: " << cloud_out->points[pointIndex].normal_x);
-            ROS_ERROR_STREAM("y_normal: " << cloud_out->points[pointIndex].normal_y);
-            ROS_ERROR_STREAM("z_normal: " << cloud_out->points[pointIndex].normal_z);
-          }*/
+					//ROS_ERROR_STREAM("z_normal: " << cloud_out->points[pointIndex].normal_z);
           cloud_out->points[pointIndex].r=255;
           cloud_out->points[pointIndex].g=0;
           cloud_out->points[pointIndex].b=0;
@@ -158,6 +140,6 @@ void ObstacleDetectionNormalsAlgorithm::cloud_all(int KSearch, float normal_z,
       }
     }
   }
-  e=(e/float(cloud_out->width*cloud_out->height))*100;
-  //printf("error %f",e);
+  e=(e/float(cloud_out->width*cloud_out->height))*100; 
+  //printf("error %f",e); 
 }