From e2e4bd5fff4a614bea5cada98b0a15fc7de52077 Mon Sep 17 00:00:00 2001
From: idelpino <telecosg@gmail.com>
Date: Wed, 8 May 2024 18:39:10 +0200
Subject: [PATCH] Added mean filter for near range cells in low obstacle
detection algorithm
---
params/ONA2_iri_ground_segmentation.yaml | 6 +--
src/ground_segmentation_alg_node.cpp | 64 +++++++++++++++++++-----
2 files changed, 54 insertions(+), 16 deletions(-)
diff --git a/params/ONA2_iri_ground_segmentation.yaml b/params/ONA2_iri_ground_segmentation.yaml
index 8bb4e7a..d4d5483 100644
--- a/params/ONA2_iri_ground_segmentation.yaml
+++ b/params/ONA2_iri_ground_segmentation.yaml
@@ -2,7 +2,7 @@ iri_ground_segmentation: {
rate: 10.00,
# Robot related geometric parameters
- sensor_height: 0.3, ## Ground is expected to be at z = -1*sensor_height
+ sensor_height: 0.21, ## Ground is expected to be at z = -1*sensor_height
robot_height: 1.5, ## All obstacle points above this value (w.r.t the z ground predicted by the algorithm) are treated as overhanging obstacles
# Parameters affecting the exploration of the pointcloud
@@ -21,7 +21,7 @@ iri_ground_segmentation: {
# Kalman filter noise parameters
## initial uncertainties
- z_initial_std_dev: 0.05, ## Uncertainty in z = -1*sensor_height
+ z_initial_std_dev: 0.02, ## Uncertainty in z = -1*sensor_height
initial_angular_std_dev_deg: 1.5, ## Used to initialize x_slope and y_slope variances
## propagation additive noises
@@ -53,7 +53,7 @@ iri_ground_segmentation: {
# low obstacle detection parameters
use_low_obstacle_detection: true,
- low_obstacle_min_height: 0.07,
+ low_obstacle_min_height: 0.05,
sensor_best_resolution_deg: 0.2, # Best resolution, either horizontal or vertical
max_navigation_slope: 0.3,
diff --git a/src/ground_segmentation_alg_node.cpp b/src/ground_segmentation_alg_node.cpp
index 4c9f07b..d2052ca 100644
--- a/src/ground_segmentation_alg_node.cpp
+++ b/src/ground_segmentation_alg_node.cpp
@@ -608,7 +608,8 @@ void GroundSegmentationAlgNode::convertOutputsToSuitableFormats(void)
this->PCLPointCloudToRosPointCloud(lidar_pcl_cloud_, lidar_points_ground_segmented_ros_cloud_);
- this->PCLPointCloudToRosPointCloud(evaluated_segmentation_pointcloud_pcl_cloud_, evaluated_segmentation_pointcloud_ros_cloud_);
+ this->PCLPointCloudToRosPointCloud(evaluated_segmentation_pointcloud_pcl_cloud_,
+ evaluated_segmentation_pointcloud_ros_cloud_);
this->PCLPointCloudToRosPointCloud(ground_dense_reconstruction_pcl_cloud_, ground_dense_reconstruction_ros_cloud_);
@@ -683,11 +684,10 @@ void GroundSegmentationAlgNode::showPerformanceStatistics(void)
evaluation_metrics_.rec = (float)evaluation_metrics_.tp / (float)(evaluation_metrics_.tp + evaluation_metrics_.fn);
evaluation_metrics_.f1 = (2.0 * evaluation_metrics_.prec * evaluation_metrics_.rec)
- / (evaluation_metrics_.prec + evaluation_metrics_.rec);
+ / (evaluation_metrics_.prec + evaluation_metrics_.rec);
evaluation_metrics_.iou = (float)evaluation_metrics_.tp
- / (float)(evaluation_metrics_.tp + evaluation_metrics_.fp
- + evaluation_metrics_.fn);
+ / (float)(evaluation_metrics_.tp + evaluation_metrics_.fp + evaluation_metrics_.fn);
evaluation_metrics_.acc = (float)(evaluation_metrics_.tp + evaluation_metrics_.tn)
/ (float)(evaluation_metrics_.tp + evaluation_metrics_.tn + evaluation_metrics_.fp + evaluation_metrics_.fn);
@@ -703,7 +703,7 @@ void GroundSegmentationAlgNode::showPerformanceStatistics(void)
std::cout << "FN = " << evaluation_metrics_.fn << std::endl;
std::cout << "Precision = " << evaluation_metrics_.prec << std::endl;
- std::cout << "Recall = " << evaluation_metrics_.rec << std::endl;
+ std::cout << "Recall = " << evaluation_metrics_.rec << std::endl;
std::cout << "F1 = " << evaluation_metrics_.f1 << std::endl;
std::cout << "Acc = " << evaluation_metrics_.acc << std::endl;
@@ -714,12 +714,16 @@ void GroundSegmentationAlgNode::showPerformanceStatistics(void)
struct Cell
{
- float z_min, z_max;
+ bool is_near = false;
+ float z_min, z_max = -1000.0;
+ std::vector<float> z_coordinates;
int label = -1; // -1 uninitialized, 0 traversable, 1 obstacle
};
void GroundSegmentationAlgNode::detectLowObstacles(float h, float res_deg, float alpha)
{
+ float near_range_thres = 1.75; // TODO: Extract this parameter!
+
//std::cout << "Starting detectLowObstacles function!" << std::endl;
float res = res_deg * M_PI / 180.0;
float r_max = h / alpha;
@@ -728,11 +732,11 @@ void GroundSegmentationAlgNode::detectLowObstacles(float h, float res_deg, float
float cell_side = r_max / sqrt(8.0); // This comes from here: sqrt((r_max/2)^2 / 2)
int num_of_cells = (d_max * 2.0 / cell_side);
-// std::cout << "r_max = " << r_max << std::endl;
-// std::cout << "K = " << K << std::endl;
-// std::cout << "d_max = " << d_max << std::endl;
-// std::cout << "cell_side = " << cell_side << std::endl;
-// std::cout << "num_of_cells = " << num_of_cells << std::endl;
+ std::cout << "r_max = " << r_max << std::endl;
+ std::cout << "K = " << K << std::endl;
+ std::cout << "d_max = " << d_max << std::endl;
+ std::cout << "cell_side = " << cell_side << std::endl;
+ std::cout << "num_of_cells = " << num_of_cells << std::endl;
std::vector < std::vector < Cell >> grid(num_of_cells, std::vector < Cell > (num_of_cells));
@@ -744,8 +748,13 @@ void GroundSegmentationAlgNode::detectLowObstacles(float h, float res_deg, float
int x = i->x / cell_side + num_of_cells / 2.0;
int y = i->y / cell_side + num_of_cells / 2.0;
//std::cout << "x cell number = " << x << " y cell number = " << y << std::endl;
+
if (x >= 0 && x < num_of_cells && y >= 0 && y < num_of_cells)
{
+ if (sqrt(i->x * i->x + i->y * i->y) < near_range_thres)
+ grid[x][y].is_near = true;
+
+ grid[x][y].z_coordinates.push_back(i->z);
if (grid[x][y].label == -1)
{
grid[x][y].z_min = grid[x][y].z_max = i->z;
@@ -766,6 +775,13 @@ void GroundSegmentationAlgNode::detectLowObstacles(float h, float res_deg, float
{
if (grid[i][j].label != -1)
{
+ float sum = std::accumulate(grid[i][j].z_coordinates.begin(), grid[i][j].z_coordinates.end(), 0.0);
+ float z_mean = sum / grid[i][j].z_coordinates.size();
+
+// std::cout << "z_max - z_min = " << grid[i][j].z_max - grid[i][j].z_min
+// << " z_mean = " << z_mean << " Total points = "
+// << grid[i][j].z_coordinates.size() << std::endl;
+
for (int dx = -1; dx <= 1; ++dx)
{
for (int dy = -1; dy <= 1; ++dy)
@@ -773,7 +789,28 @@ void GroundSegmentationAlgNode::detectLowObstacles(float h, float res_deg, float
int nx = i + dx, ny = j + dy;
if (nx >= 0 && nx < num_of_cells && ny >= 0 && ny < num_of_cells && grid[nx][ny].label != -1)
{
- if (abs(grid[i][j].z_min - grid[nx][ny].z_max) > h || abs(grid[i][j].z_max - grid[nx][ny].z_min) > h)
+ float sum_neighbour = std::accumulate(grid[nx][ny].z_coordinates.begin(),
+ grid[nx][ny].z_coordinates.end(), 0.0);
+ float z_mean_neighbour = sum_neighbour / grid[nx][ny].z_coordinates.size();
+
+ bool low_obstacle_detected = false;
+ bool use_mean = false;
+ if (grid[i][j].is_near)
+ use_mean = true;
+
+ if (use_mean)
+ {
+ if (abs(z_mean - z_mean_neighbour) > h)
+ low_obstacle_detected = true;
+ }
+ else
+ {
+ if (abs(grid[i][j].z_min - grid[nx][ny].z_max) > h
+ || abs(grid[i][j].z_max - grid[nx][ny].z_min) > h)
+ low_obstacle_detected = true;
+ }
+
+ if (low_obstacle_detected)
{
grid[i][j].label = 1;
dx = dy = 2; // break the loop
@@ -853,7 +890,8 @@ void GroundSegmentationAlgNode::evaluateSegmentation(void)
else
{
if (detected_class == VEGETATION || detected_class == CLASS_UNKNOWN || detected_class == CLASS_OBSTACLE
- || detected_class == CLASS_OVERHANGING_OBSTACLE || detected_class == TERRAIN || detected_class == CLASS_LOW_OBSTACLE)
+ || detected_class == CLASS_OVERHANGING_OBSTACLE || detected_class == TERRAIN
+ || detected_class == CLASS_LOW_OBSTACLE)
detected_class = CLASS_OBSTACLE;
else
detected_class = CLASS_GROUND;
--
GitLab