diff --git a/src/kf_based_terrain_analysis.cpp b/src/kf_based_terrain_analysis.cpp
index d8ed96ff96c20ecc88cfc979a618584ce8750da1..8e01a98c9ac2bc98597c74884d14d2a2b1399913 100644
--- a/src/kf_based_terrain_analysis.cpp
+++ b/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
       {