diff --git a/include/structs_definitions.h b/include/structs_definitions.h
index b8571d45501a800b716a8976cd38198bb75252d3..0d59f13904e759e7ea336563390f6636feaa88c0 100644
--- a/include/structs_definitions.h
+++ b/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;
diff --git a/src/kf_based_terrain_analysis.cpp b/src/kf_based_terrain_analysis.cpp
index 59bb8ea0f3acb24fcb2f012738e41c089858386d..a9f266a8b806469cd653c7affd4fd26d4d62bff6 100644
--- a/src/kf_based_terrain_analysis.cpp
+++ b/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