From a9a524be3dc2758fd479eb5f214fa503b6c966d5 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Iv=C3=A1n=20del=20Pino?= <idelpino@iri.upc.edu>
Date: Wed, 4 May 2022 12:45:17 +0200
Subject: [PATCH] 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 | 11 +++++++++++
1 file changed, 11 insertions(+)
diff --git a/src/kf_based_terrain_analysis.cpp b/src/kf_based_terrain_analysis.cpp
index d8ed96f..8e01a98 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
{
--
GitLab