classifying points in elevation cloud as obstacles in case there exist any not...

classifying points in elevation cloud as obstacles in case there exist any not overhanging obstacle point in its cell

src/kf_based_terrain_analysis.cpp

@@ -934,6 +934,9 @@ void CKf_Based_Terrain_Analysis::labelPointcloudUsingGroundModel(
         i->b = B_CLASS_GROUND;
 
         // If the lowest point is ground we need to check one by one the rest of points in the cell
+        // In case we find not-overhanging obstacles in its cell, we will classify the lowest point in cell also as obstacle
+        bool flag_not_overhanging_obstacle_found = false;
+
         // 1) We point to the vector of indices related to the elevation_cloud point under evaluation
         std::vector<std::vector<int>>::const_iterator index_iterator = correspondence_indexes.begin()
             + point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX];
@@ -998,6 +1001,8 @@ void CKf_Based_Terrain_Analysis::labelPointcloudUsingGroundModel(
               pcl_cloud_ptr->points[*point_iterator].b = B_CLASS_OBSTACLE;
 
               pcl_cloud_ptr->points[*point_iterator].data_c[DATA_C_1_ID_CLASS] = (float)CLASS_OBSTACLE + 0.99;
+
+              flag_not_overhanging_obstacle_found = true;
             }
             else
             {
@@ -1016,6 +1021,12 @@ void CKf_Based_Terrain_Analysis::labelPointcloudUsingGroundModel(
             }
           }
         }
+        if (flag_not_overhanging_obstacle_found)
+        {
+          i->r = R_CLASS_OBSTACLE;
+          i->g = G_CLASS_OBSTACLE;
+          i->b = B_CLASS_OBSTACLE;
+        }
       }
       else // If the lowest point in cell is already an obstacle, the remaining points are also obstacles
       {