From 8c34a7c8998731d559107c9dc4460674ac89b11e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Iv=C3=A1n=20del=20Pino?= <idelpino@iri.upc.edu>
Date: Tue, 12 Jul 2022 11:24:26 +0200
Subject: [PATCH] added heuristic to label points outside the local area
analysis as ground or obstacles
---
include/structs_definitions.h | 1 +
src/kf_based_terrain_analysis.cpp | 10 ++++++++++
2 files changed, 11 insertions(+)
diff --git a/include/structs_definitions.h b/include/structs_definitions.h
index b8571d4..0d59f13 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 59bb8ea..a9f266a 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
--
GitLab