Added segmentation evaluation

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);
  

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_;
 

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) 

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;