Add interface for rosbags extracted by semantickitti2bag

include/ground_segmentation_alg_node.h

@@ -38,6 +38,25 @@
 
 // [action server client headers]
 
+struct PointXYZIRGBL
+{
+  float x;
+  float y;
+  float z;
+  float intensity;
+  uint32_t rgb;
+  uint16_t label;
+};
+
+POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZIRGBL,
+                                  (float, x, x)
+                                  (float, y, y)
+                                  (float, z, z)
+                                  (float, intensity, intensity)
+                                  (uint32_t, rgb, rgb)
+                                  (uint16_t, label, label)
+                                  )
+
 /**
  * \brief IRI ROS Specific Algorithm Class
  *
@@ -78,6 +97,7 @@ private:
   // Variables for copying the callbacks inputs
   sensor_msgs::PointCloud2 lidar_ros_cloud_;
   pcl::PointCloud<pcl::PointXYZRGBNormal> lidar_pcl_cloud_;
+  pcl::PointCloud<PointXYZIRGBL> ground_truth_pcl_cloud_;
 
   pcl::PointCloud<pcl::PointXYZI> xyzi_lidar_pcl_cloud_;
   ros::Subscriber lidar_subscriber_;
@@ -96,6 +116,9 @@ private:
   void rosPointCloudtoPCLPointCloud(const sensor_msgs::PointCloud2 &ros_point_cloud,
                                     pcl::PointCloud<pcl::PointXYZI> &pcl_point_cloud);
 
+  void rosPointCloudtoPCLPointCloud(const sensor_msgs::PointCloud2 &ros_point_cloud,
+                                    pcl::PointCloud<PointXYZIRGBL> &pcl_point_cloud);
+
   void PCLPointCloudToRosPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> &pcl_point_cloud,
                                     sensor_msgs::PointCloud2 &ros_point_cloud);
 

launch/iri_ground_segmentation.launch

@@ -2,7 +2,8 @@
 <launch>
   <env name="LIBGL_ALWAYS_SOFTWARE" value="1" />
   <rosparam command="load" file="$(find iri_ground_segmentation)/params/iri_ground_segmentation.yaml" />
-  <remap from="/lidar_points_extended_structure" to="/xyzi_to_extended_structure_converter/lidar_points_extended_structure"/> 
+  <remap from="/iri_ground_segmentation/lidar_points" to="/kitti/velo/pointcloud"/> 
+  <remap from="/iri_ground_segmentation/ground_truth_lidar_points" to="/velodyne_points"/> 
   <node pkg="iri_ground_segmentation" type="iri_ground_segmentation" name="iri_ground_segmentation" output="screen" respawn="false">
   </node>
 </launch>

params/iri_ground_segmentation.yaml

@@ -36,7 +36,7 @@ iri_ground_segmentation: {
                                     ## and a big value will increase the number of false ground points)
 
   # Neural Network related parameters
-  use_neural_network: false,
+  use_neural_network: true,
   extract_data_to_external_training_of_the_network: false,
   dataset_filename_with_global_path: '/home/idelpino/Documentos/dataset.csv', # This is the filename with path where the data for neural net training is dumped
   neural_net_filename_with_global_path: '/home/idelpino/iri-lab/ground_segmentation_ws/src/iri_ground_segmentation/shallow_neural_nets/veg_terrain_and_obs_13_features_39_neurons.csv',

src/ground_segmentation_alg_node.cpp

@@ -296,6 +296,14 @@ void GroundSegmentationAlgNode::rosPointCloudtoPCLPointCloud(const sensor_msgs::
   pcl::fromPCLPointCloud2(aux, pcl_point_cloud);
 }
 
+void GroundSegmentationAlgNode::rosPointCloudtoPCLPointCloud(const sensor_msgs::PointCloud2 &ros_point_cloud,
+                                                             pcl::PointCloud<PointXYZIRGBL> &pcl_point_cloud)
+{
+  pcl::PCLPointCloud2 aux;
+  pcl_conversions::toPCL(ros_point_cloud, aux);
+  pcl::fromPCLPointCloud2(aux, pcl_point_cloud);
+}
+
 void GroundSegmentationAlgNode::PCLPointCloudToRosPointCloud(
     const pcl::PointCloud<pcl::PointXYZRGBNormal> &pcl_point_cloud, sensor_msgs::PointCloud2 &ros_point_cloud)
 {
@@ -426,7 +434,35 @@ void GroundSegmentationAlgNode::convertInputsToSuitableFormats(void)
 
   if(flag_new_ground_truth_lidar_data_)
   {
-    this->rosPointCloudtoPCLPointCloud(lidar_ros_cloud_, lidar_pcl_cloud_);
+    this->rosPointCloudtoPCLPointCloud(lidar_ros_cloud_, ground_truth_pcl_cloud_);
+
+    int i = 0;
+    for (pcl::PointCloud<PointXYZIRGBL>::iterator regular_point = ground_truth_pcl_cloud_.begin();
+        regular_point != ground_truth_pcl_cloud_.end(); ++regular_point)
+    {
+      pcl::PointXYZRGBNormal extended_point;
+      extended_point.x = regular_point->x;
+      extended_point.y = regular_point->y;
+      extended_point.z = regular_point->z;
+
+      extended_point.data_n[DATA_N_0_INTENSITY] = regular_point->intensity;
+      extended_point.data_c[DATA_C_2_INDEX_OF_GROUND_REF_THAT_MADE_THE_LABEL] = UNKNOWN_MEAN;
+      extended_point.data_n[DATA_N_1_INTENSITY_VARIANCE] = UNKOWN_VARIANCE;
+      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;
+
+      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);
+    }
+
     flag_new_ground_truth_lidar_data_ = false;
   }