Finished semantickitti2bag integration

launch/iri_ground_segmentation.launch

 <?xml version="1.0"?>
 <launch>
-  <env name="LIBGL_ALWAYS_SOFTWARE" value="1" />
+  <env name="LIBGL_ALWAYS_SOFTWARE" value="1" /> <!-- for running the rviz in virtual box, comment this if you do not use a VM-->
   <rosparam command="load" file="$(find iri_ground_segmentation)/params/iri_ground_segmentation.yaml" />
   <remap from="/iri_ground_segmentation/lidar_points" to="/kitti/velo/pointcloud"/> 
   <remap from="/iri_ground_segmentation/ground_truth_lidar_points" to="/velodyne_points"/> 

src/ground_segmentation_alg_node.cpp

@@ -138,8 +138,7 @@ GroundSegmentationAlgNode::GroundSegmentationAlgNode(void) :
         "GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'dataset_filename_with_global_path' not found");
   }
   else
-    this->alg_.filtering_configuration_.dataset_filename_with_global_path =
-        config_.dataset_filename_with_global_path;
+    this->alg_.filtering_configuration_.dataset_filename_with_global_path = config_.dataset_filename_with_global_path;
 
   if (!this->private_node_handle_.getParam("neural_net_filename_with_global_path",
                                            this->config_.neural_net_filename_with_global_path))
@@ -154,12 +153,10 @@ GroundSegmentationAlgNode::GroundSegmentationAlgNode(void) :
   if (!this->private_node_handle_.getParam("neural_net_number_of_features",
                                            this->config_.neural_net_number_of_features))
   {
-    ROS_WARN(
-        "GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'neural_net_number_of_features' not found");
+    ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'neural_net_number_of_features' not found");
   }
   else
-    this->alg_.filtering_configuration_.neural_net_number_of_features =
-        config_.neural_net_number_of_features;
+    this->alg_.filtering_configuration_.neural_net_number_of_features = config_.neural_net_number_of_features;
 
   if (!this->private_node_handle_.getParam("neural_net_number_of_neurons_in_hidden_layer",
                                            this->config_.neural_net_number_of_neurons_in_hidden_layer))
@@ -236,8 +233,8 @@ GroundSegmentationAlgNode::GroundSegmentationAlgNode(void) :
   this->lidar_subscriber_ = this->private_node_handle_.subscribe("lidar_points", 1,
                                                                  &GroundSegmentationAlgNode::cb_lidar, this);
 
-  this->ground_truth_lidar_subscriber_ = this->private_node_handle_.subscribe("ground_truth_lidar_points", 1,
-                                                                 &GroundSegmentationAlgNode::cb_ground_truth_lidar, this);
+  this->ground_truth_lidar_subscriber_ = this->private_node_handle_.subscribe(
+      "ground_truth_lidar_points", 1, &GroundSegmentationAlgNode::cb_ground_truth_lidar, this);
 
   // [init publishers]
 
@@ -382,7 +379,7 @@ void GroundSegmentationAlgNode::storeHeaderForSync(void)
 void GroundSegmentationAlgNode::convertInputsToSuitableFormats(void)
 {
   //std::cout << "GroundSegmentationAlgNode::convertInputsToSuitableFormats..." << std::endl;
-  if(flag_new_lidar_data_)
+  if (flag_new_lidar_data_)
   {
     this->rosPointCloudtoPCLPointCloud(lidar_ros_cloud_, xyzi_lidar_pcl_cloud_);
 
@@ -432,7 +429,7 @@ void GroundSegmentationAlgNode::convertInputsToSuitableFormats(void)
 //  std::cout << "DATA_C_2_MEAN_INTENSITY = " << lidar_pcl_cloud_.points[100].data_c[DATA_C_2_MEAN_INTENSITY] << std::endl;
 //  std::cout << "DATA_C_3_ORIGINAL_INDEX = " << lidar_pcl_cloud_.points[100].data_c[DATA_C_3_ORIGINAL_INDEX] << std::endl;
 
-  if(flag_new_ground_truth_lidar_data_)
+  if (flag_new_ground_truth_lidar_data_)
   {
     this->rosPointCloudtoPCLPointCloud(lidar_ros_cloud_, ground_truth_pcl_cloud_);
 
@@ -451,13 +448,63 @@ void GroundSegmentationAlgNode::convertInputsToSuitableFormats(void)
       extended_point.data_n[DATA_N_2_Z_MEAN] = UNKNOWN_MEAN;
       extended_point.data_n[DATA_N_3_Z_VARIANCE] = UNKOWN_VARIANCE;
 
-      // These three values are stored in data_c[0]
-      //extended_point.r = R_CLASS_UNKNOWN;
-      //extended_point.g = G_CLASS_UNKNOWN;
-      //extended_point.b = B_CLASS_UNKNOWN;
-      extended_point.rgb = regular_point->rgb;
+      int gt_label = regular_point->label;
+
+      // We are only interester in ROAD, VEGETATION, SIDEWALK, TERRAIN and OBSTACLE classes, so we consolide them
+      // here. TODO: We leave this todo mark here, because maybe in the future we will want to work with more classes
+      if (gt_label == ROAD || gt_label == PARKING || gt_label == OTHER_GROUND || gt_label == LANE_MARKING)
+        gt_label = ROAD;
+
+      if (gt_label != ROAD && gt_label != TERRAIN && gt_label != VEGETATION && gt_label != SIDEWALK)
+        gt_label = CLASS_OBSTACLE;
+
+      // Experimental thresholds to avoid reflected points in the Ego-Vehicle
+      if ((extended_point.x < 3.0 && extended_point.x > -2.2) && (extended_point.y < 1.8 && extended_point.y > -1.8))
+      {
+        // These three values are stored in data_c[0]
+        extended_point.r = R_CLASS_OUTLIER;
+        extended_point.g = G_CLASS_OUTLIER;
+        extended_point.b = B_CLASS_OUTLIER;
+
+        extended_point.data_c[DATA_C_1_ID_CLASS] = (float)OUTLIER;
+      }
+      else
+      {
+        switch (gt_label)
+        {
+          case ROAD:
+            extended_point.r = R_CLASS_ROAD;
+            extended_point.g = G_CLASS_ROAD;
+            extended_point.b = B_CLASS_ROAD;
+            break;
+          case TERRAIN:
+            extended_point.r = R_CLASS_TERRAIN;
+            extended_point.g = G_CLASS_TERRAIN;
+            extended_point.b = B_CLASS_TERRAIN;
+            break;
+
+          case VEGETATION:
+            extended_point.r = R_CLASS_VEGETATION;
+            extended_point.g = G_CLASS_VEGETATION;
+            extended_point.b = B_CLASS_VEGETATION;
+            break;
+
+          case SIDEWALK:
+            extended_point.r = R_CLASS_SIDEWALK;
+            extended_point.g = G_CLASS_SIDEWALK;
+            extended_point.b = B_CLASS_SIDEWALK;
+            break;
+
+          default:
+            extended_point.r = R_CLASS_KITTI_OBSTACLE;
+            extended_point.g = G_CLASS_KITTI_OBSTACLE;
+            extended_point.b = B_CLASS_KITTI_OBSTACLE;
+        }
+
+        extended_point.data_c[DATA_C_1_ID_CLASS] = (float)CLASS_UNKNOWN; // We need this to be set to unknown for the algorithm to work
+                                                                         // We use the color information to get the ground truth
+      }
 
-      extended_point.data_c[DATA_C_1_ID_CLASS] = (float)regular_point->label;
       extended_point.data_c[DATA_C_3_ORIGINAL_INDEX] = (float)i++;
 
       lidar_pcl_cloud_.points.push_back(extended_point);
@@ -535,7 +582,6 @@ void GroundSegmentationAlgNode::publishAll(void)
   // Visualization topics
   this->ground_dense_reconstruction_publisher_.publish(this->ground_dense_reconstruction_ros_cloud_);
 
-
   visualization_msgs::Marker markerD;
   markerD.header = common_header_;
   markerD.action = visualization_msgs::Marker::DELETEALL;
@@ -723,11 +769,11 @@ void GroundSegmentationAlgNode::node_config_update(Config &config, uint32_t leve
   std::cout << "mahalanobis_threshold       =                   "
       << this->alg_.filtering_configuration_.mahalanobis_threshold << std::endl;
 
-  std::cout << "score_threshold       =                   "
-      << this->alg_.filtering_configuration_.score_threshold << std::endl;
+  std::cout << "score_threshold       =                   " << this->alg_.filtering_configuration_.score_threshold
+      << std::endl;
 
-  std::cout << "use_neural_network       =                   "
-      << this->alg_.filtering_configuration_.use_neural_network << std::endl;
+  std::cout << "use_neural_network       =                   " << this->alg_.filtering_configuration_.use_neural_network
+      << std::endl;
 
   std::cout << "extract_data_to_external_training_of_the_network       =                   "
       << this->alg_.filtering_configuration_.extract_data_to_external_training_of_the_network << std::endl;