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 {