diff --git a/cfg/GroundSegmentation.cfg b/cfg/GroundSegmentation.cfg
index 4673e08c21ec6dd3c524f235eeb0a30a288ebdb0..95565c7c31496208bc18ac484f46e60929fe5737 100644
--- a/cfg/GroundSegmentation.cfg
+++ b/cfg/GroundSegmentation.cfg
@@ -86,6 +86,7 @@ gen.add("max_navigation_slope", double_t, 0, "slopes greater than this threshold
# visualization and debug parameters
gen.add("evaluate_segmentation", bool_t, 0, "generates quantitative evaluation metrics", False);
+gen.add("only_road_is_traversable", bool_t, 0, "If set to false terrain and vegetation will be considered non-traversable, but sidewalks, parkings, etc will be considered to be traversable", False);
gen.add("measure_performance", bool_t, 0, "to measure times of inner functions", False);
gen.add("show_dense_reconstruction", bool_t, 0, "creates dense pointcloud with ground predictions", False);
diff --git a/include/ground_segmentation_alg_node.h b/include/ground_segmentation_alg_node.h
index d4e25e1a685150821724d5604cbea4817520595c..3ac96b2071a65d11263c92870f5d6c631f8f31a7 100644
--- a/include/ground_segmentation_alg_node.h
+++ b/include/ground_segmentation_alg_node.h
@@ -45,6 +45,12 @@ struct metrics
unsigned long int tn = 0;
unsigned long int fn = 0;
+ unsigned long int FN_due_to_unknown_points = 0;
+ unsigned long int TN_due_to_unknown_points = 0;
+
+ unsigned long int key_obstacle_true_positives = 0;
+ unsigned long int key_obstacle_false_negatives = 0;
+
float prec = 0.0;
float rec = 0.0;
float f1 = 0.0;
@@ -85,6 +91,7 @@ private:
bool flag_new_ground_truth_lidar_data_;
bool flag_use_low_obstacle_detection_;
bool flag_evaluate_segmentation_;
+ bool flag_only_road_is_traversable_;
metrics evaluation_metrics_;
diff --git a/params/iri_ground_segmentation.yaml b/params/iri_ground_segmentation.yaml
index 4eba5ee5d1dec71707e812b1ef06af6c89ce73b3..9626478273bbf474c7b5e80ac89cb35201be669b 100644
--- a/params/iri_ground_segmentation.yaml
+++ b/params/iri_ground_segmentation.yaml
@@ -46,19 +46,20 @@ iri_ground_segmentation: {
# labeling parameters
max_pred_std_dev_for_labelling: 0.333, ## ONLY IN USE TO GIVE COLOUR TO DENSE RECONSTRUCTION
- score_threshold: 0.0, ## for assigning ground class label: one means maha. dist. equal to zero, zero means mahalanobis dist equal to maha. thres
- classify_not_labeled_points_as_obstacles: true, ## when a point has no reference satisfying the max_pred_std_dev_for_labelling threshold we can leave as unknown or label it as obstacle
+ score_threshold: 0.475, #0.0 when using neural net ## for assigning ground class label: one means maha. dist. equal to zero, zero means mahalanobis dist equal to maha. thres
+ classify_not_labeled_points_as_obstacles: false, ## when a point has no reference satisfying the max_pred_std_dev_for_labelling threshold we can leave as unknown or label it as obstacle
ground_threshold_in_not_analyzed_areas: 0.1, ## when it is not possible to make a local analysis of the data, we will use the lowest point (the one in elevation_cloud) as ground height, and
## all the points above it and below this threshold will be classified as ground
# low obstacle detection parameters
- use_low_obstacle_detection: false,
+ use_low_obstacle_detection: true,
low_obstacle_min_height: 0.05,
sensor_best_resolution_deg: 0.2, # Best resolution, either horizontal or vertical
max_navigation_slope: 0.5,
# visualization and debug parameters
evaluate_segmentation: true,
+ only_road_is_traversable: true, # If set to false terrain and vegetation will be considered non-traversable, but sidewalks, parkings, etc will be considered to be traversable
measure_performance: false, ## (feature still not debugged) Flag to measure number of execution and execution times of the different functions of the algorithm
show_dense_reconstruction: false, ## To show a dense ground surface reconstruction using the predictions of the ground mode (colored using the std_dev of z coordinate)
## or alternatively the "elevation point cloud" (useful for parameter tunning)
diff --git a/src/ground_segmentation_alg_node.cpp b/src/ground_segmentation_alg_node.cpp
index 61e56d8e676a76c977880c7b9eb5f01c790327fe..2750fc1154028e6069c84554fe0039a84846f286 100644
--- a/src/ground_segmentation_alg_node.cpp
+++ b/src/ground_segmentation_alg_node.cpp
@@ -264,6 +264,13 @@ GroundSegmentationAlgNode::GroundSegmentationAlgNode(void) :
else
this->flag_evaluate_segmentation_ = config_.evaluate_segmentation;
+ if (!this->private_node_handle_.getParam("only_road_is_traversable", this->config_.only_road_is_traversable))
+ {
+ ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'only_road_is_traversable' not found");
+ }
+ else
+ this->flag_only_road_is_traversable_ = config_.only_road_is_traversable;
+
// [init subscribers]
this->lidar_subscriber_ = this->private_node_handle_.subscribe("lidar_points", 1,
&GroundSegmentationAlgNode::cb_lidar, this);
@@ -547,8 +554,13 @@ void GroundSegmentationAlgNode::convertInputsToSuitableFormats(void)
lidar_pcl_cloud_.points.push_back(extended_point);
- if(flag_evaluate_segmentation_)
+ if (flag_evaluate_segmentation_)
+ {
+ extended_point.data_c[DATA_C_1_ID_CLASS] = regular_point->label; // We recover the SemanticKitti ground truth
+ // because we want to have info about key obstacles for
+ // evaluation
evaluated_segmentation_pointcloud_pcl_cloud_.points.push_back(extended_point);
+ }
}
flag_new_ground_truth_lidar_data_ = false;
@@ -596,6 +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(ground_dense_reconstruction_pcl_cloud_, ground_dense_reconstruction_ros_cloud_);
this->nodesPCLPointCloudToVerticesArray(roadmap_vertices_pcl_cloud_, roadmap_vertices_array_);
@@ -615,6 +629,7 @@ void GroundSegmentationAlgNode::publishAll(void)
this->lidar_points_ground_segmented_publisher_.publish(this->lidar_points_ground_segmented_ros_cloud_);
this->obstacle_points_publisher_.publish(this->obstacle_points_ros_cloud_);
this->ground_points_publisher_.publish(this->ground_points_ros_cloud_);
+ this->evaluated_segmentation_pointcloud_publisher_.publish(this->evaluated_segmentation_pointcloud_ros_cloud_);
// Roadmap topics
this->roadmap_edges_publisher_.publish(this->roadmap_edges_array_);
@@ -657,9 +672,44 @@ void GroundSegmentationAlgNode::showPerformanceStatistics(void)
std::cout << "--------------------- ALGORITHM: ---------------------------------" << std::endl;
this->alg_.showPerformanceStatistics();
std::cout << "-------------------------------------------------------------------------------" << std::endl;
-// std::cout << "--------------------- PROCESSOR: ---------------------------------" << std::endl;
-// this->alg_.processor_.showPerformanceStatistics();
-// std::cout << "-------------------------------------------------------------------------------" << std::endl;
+
+ // std::cout << "TN_due_to_unknown_points = " << TN_due_to_unknown_points << std::endl;
+ // std::cout << "FN_due_to_unknown_points = " << FN_due_to_unknown_points << std::endl;
+ //
+ // std::cout << "number_of_evaluated_points = " << number_of_evaluated_points << std::endl;
+
+ evaluation_metrics_.prec = (float)evaluation_metrics_.tp / (float)(evaluation_metrics_.tp + evaluation_metrics_.fp);
+
+ 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_.iou = (float)evaluation_metrics_.tp
+ / (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);
+
+ evaluation_metrics_.kor = (float)evaluation_metrics_.key_obstacle_true_positives
+ / (float)(evaluation_metrics_.key_obstacle_true_positives + evaluation_metrics_.key_obstacle_false_negatives);
+
+ std::cout << "Statistics: " << std::endl;
+ std::cout << "TP = " << evaluation_metrics_.tp << std::endl;
+ std::cout << "FP = " << evaluation_metrics_.fp << std::endl;
+
+ std::cout << "TN = " << evaluation_metrics_.tn << std::endl;
+ 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 << "F1 = " << evaluation_metrics_.f1 << std::endl;
+ std::cout << "Acc = " << evaluation_metrics_.acc << std::endl;
+ std::cout << "IoU = " << evaluation_metrics_.iou << std::endl;
+
+ std::cout << "KOR = " << evaluation_metrics_.kor << std::endl;
}
struct Cell
@@ -764,10 +814,122 @@ void GroundSegmentationAlgNode::evaluateSegmentation(void)
pcl::PointXYZRGBNormal classified_point = *i;
pcl::PointXYZRGBNormal gt_point = *gt_i;
-// if ((int)std::floor(data_c[DATA_C_1_ID_CLASS]) == CLASS_GROUND)
-// {
-//
-// }
+ int gt_label = gt_point.data_c[DATA_C_1_ID_CLASS];
+
+ // Condensing the labels into bigger semantic meanings
+ bool gt_exclude_from_evaluation = false;
+ if (gt_label == OUTLIER || gt_label == UNLABELED || gt_label == OTHER_STRUCTURE || gt_label == OTHER_OBJECT)
+ gt_exclude_from_evaluation = true;
+
+ bool gt_ground_point = false;
+ bool gt_ordinary_obstacle_point = false;
+
+ // Extracting info from our segmentation algorithm
+ //std::cout << "Detected class + score as comes in the message = " << (float)it->data_c[DATA_C_1_ID_CLASS] << std::endl;
+ int detected_class = (int)std::floor(i->data_c[DATA_C_1_ID_CLASS]);
+ //std::cout << "detected_class = " << detected_class << std::endl;
+ float detection_score = i->data_c[DATA_C_1_ID_CLASS] - (float)detected_class; //we package the score_thres as decimal part of the label
+ //std::cout << "detection_score = " << detection_score << std::endl;
+
+ bool flag_not_analyzed_point = false; // Just to keep track of which TN and FN are due to the unknown points
+ if (detected_class == CLASS_UNKNOWN)
+ flag_not_analyzed_point = true;
+
+ if (flag_only_road_is_traversable_)
+ {
+ if (detected_class == ROAD || detected_class == CLASS_GROUND)
+ detected_class = CLASS_GROUND;
+ else
+ detected_class = CLASS_OBSTACLE;
+
+ if (gt_label == ROAD)
+ gt_ground_point = true;
+
+ if (gt_label == BUILDING || gt_label == FENCE || gt_label == TRUNK || gt_label == POLE || gt_label == TRAFFIC_SIGN
+ || gt_label == VEGETATION || gt_label == OTHER_GROUND || gt_label == LANE_MARKING || gt_label == TERRAIN
+ || gt_label == SIDEWALK || gt_label == PARKING)
+ gt_ordinary_obstacle_point = true;
+ }
+ else
+ {
+ if (detected_class == VEGETATION || detected_class == CLASS_UNKNOWN || detected_class == CLASS_OBSTACLE
+ || detected_class == CLASS_OVERHANGING_OBSTACLE || detected_class == TERRAIN)
+ detected_class = CLASS_OBSTACLE;
+ else
+ detected_class = CLASS_GROUND;
+
+ if (gt_label == ROAD || gt_label == PARKING || gt_label == SIDEWALK || gt_label == OTHER_GROUND
+ || gt_label == LANE_MARKING)
+ gt_ground_point = true;
+
+ if (gt_label == BUILDING || gt_label == FENCE || gt_label == TRUNK || gt_label == POLE || gt_label == TRAFFIC_SIGN
+ || gt_label == VEGETATION || gt_label == TERRAIN)
+ gt_ordinary_obstacle_point = true;
+ }
+
+ bool gt_key_obstacle = false;
+ if (!gt_exclude_from_evaluation && !gt_ground_point && !gt_ordinary_obstacle_point)
+ gt_key_obstacle = true;
+
+ if (!gt_exclude_from_evaluation)
+ {
+ bool gt_transversable = false;
+ if (gt_ground_point)
+ gt_transversable = true;
+
+ bool transversable = false;
+ if (detected_class == CLASS_GROUND)
+ transversable = true;
+
+ if (gt_transversable && transversable)
+ {
+ evaluation_metrics_.tp++;
+ gt_i->r = 0;
+ gt_i->g = 0;
+ gt_i->b = 255;
+ }
+
+ if (gt_transversable && !transversable)
+ {
+ evaluation_metrics_.fn++;
+ gt_i->r = 0;
+ gt_i->g = 255;
+ gt_i->b = 255;
+ if (flag_not_analyzed_point)
+ evaluation_metrics_.FN_due_to_unknown_points++;
+ }
+
+ if (!gt_transversable && transversable)
+ {
+ evaluation_metrics_.fp++;
+ gt_i->r = 255;
+ gt_i->g = 0;
+ gt_i->b = 0;
+ }
+
+ if (!gt_transversable && !transversable)
+ {
+ evaluation_metrics_.tn++;
+ gt_i->r = 0;
+ gt_i->g = 255;
+ gt_i->b = 0;
+ if (flag_not_analyzed_point)
+ evaluation_metrics_.TN_due_to_unknown_points++;
+ }
+
+ // evaluating KEY OBSTACLES /////////////////////////////////////////
+ if (gt_key_obstacle && !transversable)
+ evaluation_metrics_.key_obstacle_true_positives++;
+
+ if (gt_key_obstacle && transversable)
+ evaluation_metrics_.key_obstacle_false_negatives++;
+ }
+ else
+ {
+ gt_i->r = 127;
+ gt_i->g = 127;
+ gt_i->b = 127;
+ }
++gt_i;
}
return;
@@ -815,7 +977,7 @@ void GroundSegmentationAlgNode::cb_lidar(const sensor_msgs::PointCloud2::ConstPt
{
this->mutex_enter();
lidar_ros_cloud_ = *msg;
- //std::cout << "GroundSegmentationAlgNode::cb_lidar --> lidar msg received!" << std::endl;
+//std::cout << "GroundSegmentationAlgNode::cb_lidar --> lidar msg received!" << std::endl;
assert(msg != NULL && "Null pointer!!! in function cb_lidar!");
flag_new_lidar_data_ = true;
@@ -827,7 +989,7 @@ void GroundSegmentationAlgNode::cb_ground_truth_lidar(const sensor_msgs::PointCl
{
this->mutex_enter();
lidar_ros_cloud_ = *msg;
- //std::cout << "GroundSegmentationAlgNode::cb_lidar --> lidar msg received!" << std::endl;
+//std::cout << "GroundSegmentationAlgNode::cb_lidar --> lidar msg received!" << std::endl;
assert(msg != NULL && "Null pointer!!! in function cb_lidar!");
flag_new_ground_truth_lidar_data_ = true;
@@ -859,45 +1021,45 @@ void GroundSegmentationAlgNode::node_config_update(Config &config, uint32_t leve
if (config_.rate != this->getRate())
this->setRate(config_.rate);
- // Robot related geometric parameters
+// Robot related geometric parameters
this->alg_.lidar_configuration_.sensor_height = config.sensor_height;
this->alg_.filtering_configuration_.robot_height = config.robot_height;
- // Parameters affecting the exploration of the pointcloud
+// Parameters affecting the exploration of the pointcloud
this->alg_.filtering_configuration_.ROI_delta_x_and_y = config.ROI_delta_x_and_y;
this->alg_.filtering_configuration_.ROI_shadow_area = config.ROI_shadow_area;
this->alg_.filtering_configuration_.ground_reference_search_resolution = config.ground_reference_search_resolution_deg
* M_PI / 180.0;
this->alg_.filtering_configuration_.elevation_grid_resolution = config.elevation_grid_resolution;
- // Kalman filter noise parameters
- // Initial uncertainties
+// Kalman filter noise parameters
+// Initial uncertainties
this->alg_.filtering_configuration_.z_initial_std_dev = config.z_initial_std_dev;
this->alg_.filtering_configuration_.roll_initial_std_dev = tan(config.initial_angular_std_dev_deg * M_PI / 180.0);
this->alg_.filtering_configuration_.pitch_initial_std_dev = tan(config.initial_angular_std_dev_deg * M_PI / 180.0);
- // Propagation additive noises
+// Propagation additive noises
this->alg_.filtering_configuration_.propagation_z_additive_noise = config.propagation_z_additive_noise_per_meter;
this->alg_.filtering_configuration_.propagation_additive_noise = tan(
config.propagation_additive_noise_deg_per_meter * M_PI / 180.0);
- // Measurement noise
+// Measurement noise
this->alg_.filtering_configuration_.z_observation_std_dev = config.z_observation_std_dev;
- // threshold for outlier rejection and classification
+// threshold for outlier rejection and classification
this->alg_.filtering_configuration_.mahalanobis_threshold = config.mahalanobis_threshold;
- // labeling parameters
+// labeling parameters
this->alg_.filtering_configuration_.score_threshold = config_.score_threshold;
this->alg_.filtering_configuration_.max_pred_std_dev_for_labelling = config.max_pred_std_dev_for_labelling;
this->alg_.filtering_configuration_.classify_not_labeled_points_as_obstacles =
config_.classify_not_labeled_points_as_obstacles;
- // Low obstacles detection parameters
+// Low obstacles detection parameters
this->flag_use_low_obstacle_detection_ = config.use_low_obstacle_detection;
this->alg_.filtering_configuration_.low_obstacle_min_height = config_.low_obstacle_min_height;
this->alg_.filtering_configuration_.sensor_best_resolution_deg = config_.sensor_best_resolution_deg;
this->alg_.filtering_configuration_.max_navigation_slope = config_.max_navigation_slope;
- // visualization and debug parameters
+// visualization and debug parameters
this->alg_.filtering_configuration_.measure_performance = config.measure_performance;
this->alg_.filtering_configuration_.show_dense_reconstruction = config.show_dense_reconstruction;