added heuristic to label points outside the local area analysis as ground or obstacles

include/structs_definitions.h

@@ -126,6 +126,7 @@ namespace kf_based_terrain_analysis_lib
 struct FilteringConfiguration
 {
   float robot_height;
+  float ground_threshold_in_not_analyzed_areas;
 
   float ROI_delta_x_and_y;
   float ROI_shadow_area;

src/kf_based_terrain_analysis.cpp

@@ -1117,6 +1117,16 @@ void CKf_Based_Terrain_Analysis::labelPointcloudUsingGroundModel(
 
           //std::cout << "obstacle z distance from ground = " << pcl_cloud_ptr->points[*point_iterator].z - point_in_sensor_frame.z << std::endl;
           if ((pcl_cloud_ptr->points[*point_iterator].z - point_in_sensor_frame.z)
+              < filtering_configuration.ground_threshold_in_not_analyzed_areas)
+          {
+            pcl_cloud_ptr->points[*point_iterator].r = R_CLASS_GROUND;  // We use instead the r g b channels directly
+            pcl_cloud_ptr->points[*point_iterator].g = G_CLASS_GROUND;
+            pcl_cloud_ptr->points[*point_iterator].b = B_CLASS_GROUND;
+
+            float score = 0.01; // we don't have too much confidence in these points, because they are not analyzed
+            pcl_cloud_ptr->points[*point_iterator].data_c[DATA_C_1_ID_CLASS] = (float)CLASS_GROUND + score;
+          }
+          else if((pcl_cloud_ptr->points[*point_iterator].z - point_in_sensor_frame.z)
               < filtering_configuration.robot_height)
           {
             // data_c[0] is reserved to cast the RGB values into a float (PCL convention) so we do not use it