fix: add param of selectable semantic id

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml

@@ -0,0 +1,30 @@
+/**:
+  ros__parameters:
+    # if the semantic label is applied for pointcloud filtering
+    filter_semantic_label_target:
+      [
+      true, # road
+      true, # sidewalk
+      true, # building
+      true, # wall
+      true, # fence
+      true, # pole
+      true, # traffic_light
+      true, # traffic_sign
+      true, # vegetation
+      true, # terrain
+      true, # sky
+      false, # person
+      false, # ride
+      false, # car
+      false, # truck
+      false, # bus
+      false, # train
+      false, # motorcycle
+      false, # bicycle
+      false, # others
+      ]
+    # the maximum distance of pointcloud to be applied filter,
+    # this is selected based on semantic segmentation model accuracy,
+    # calibration accuracy and unknown reaction distance
+    filter_distance_threshold: 60.0

perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp

@@ -32,6 +32,8 @@ class SegmentPointCloudFusionNode : public FusionNode<PointCloud2, PointCloud2,
 {
 private:
   rclcpp::Publisher<PointCloud2>::SharedPtr pub_pointcloud_ptr_;
+  std::vector<bool> filter_semantic_label_target_;
+  float filter_distance_threshold_;
   /* data */
 public:
   explicit SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options);

perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml

@@ -19,7 +19,7 @@
   <arg name="input/pointcloud" default="/sensing/lidar/top/outlier_filtered/pointcloud"/>
   <arg name="output/pointcloud" default="output/pointcloud"/>
   <arg name="sync_param_path" default="$(find-pkg-share image_projection_based_fusion)/config/roi_sync.param.yaml"/>
-
+  <arg name="semantic_segmentation_based_filter_param_path" default="$(find-pkg-share image_projection_based_fusion)/config/segmentation_pointcloud_fusion.param.yaml"/>
   <!-- debug -->
   <!-- cspell: ignore minx, maxx, miny, maxy, minz, maxz  -->
   <arg name="debug_mode" default="false"/>
@@ -42,6 +42,7 @@
   <group>
     <node pkg="image_projection_based_fusion" exec="segmentation_pointcloud_fusion_node" name="segmentation_pointcloud_fusion" output="screen">
       <param name="rois_number" value="$(var input/camera_number)"/>
+      <param from="$(var semantic_segmentation_based_filter_param_path)"/>
       <param from="$(var sync_param_path)"/>
       <remap from="input" to="$(var input/pointcloud)"/>
       <remap from="output" to="$(var output/pointcloud)"/>

perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp

@@ -30,6 +30,9 @@ namespace image_projection_based_fusion
 SegmentPointCloudFusionNode::SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options)
 : FusionNode<PointCloud2, PointCloud2, Image>("segmentation_pointcloud_fusion", options)
 {
+  filter_distance_threshold_ = declare_parameter<float>("filter_distance_threshold");
+  filter_semantic_label_target_ =
+    declare_parameter<std::vector<bool>>("filter_semantic_label_target");
 }
 
 void SegmentPointCloudFusionNode::preprocess(__attribute__((unused)) PointCloud2 & pointcloud_msg)
@@ -91,10 +94,12 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage(
        iter_orig_z(input_pointcloud_msg, "z");
        iter_x != iter_x.end();
        ++iter_x, ++iter_y, ++iter_z, ++iter_orig_x, ++iter_orig_y, ++iter_orig_z) {
-    if (*iter_z <= 0.0) {
+    // skip filtering pointcloud behind the camera or too far from camera
+    if (*iter_z <= 0.0 || *iter_z > filter_distance_threshold_) {
       output_cloud.push_back(pcl::PointXYZ(*iter_orig_x, *iter_orig_y, *iter_orig_z));
       continue;
     }
+
     Eigen::Vector4d projected_point = projection * Eigen::Vector4d(*iter_x, *iter_y, *iter_z, 1.0);
     Eigen::Vector2d normalized_projected_point = Eigen::Vector2d(
       projected_point.x() / projected_point.z(), projected_point.y() / projected_point.z());
@@ -106,12 +111,16 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage(
       output_cloud.push_back(pcl::PointXYZ(*iter_orig_x, *iter_orig_y, *iter_orig_z));
       continue;
     }
-    // TODO(badai-nguyen): replace single road class to a list of outlier classes
-    bool is_keep_class = mask.at<uint8_t>(
-                           static_cast<uint16_t>(normalized_projected_point.y()),
-                           static_cast<uint16_t>(normalized_projected_point.x())) > 10;
 
-    if (is_keep_class) {
+    // skip filtering pointcloud where semantic id out of the defined list
+    uint8_t semantic_id = mask.at<uint8_t>(
+      static_cast<uint16_t>(normalized_projected_point.y()),
+      static_cast<uint16_t>(normalized_projected_point.x()));
+    if (static_cast<size_t>(semantic_id) >= filter_semantic_label_target_.size()) {
+      output_cloud.push_back(pcl::PointXYZ(*iter_orig_x, *iter_orig_y, *iter_orig_z));
+      continue;
+    }
+    if (!filter_semantic_label_target_.at(semantic_id)) {
       output_cloud.push_back(pcl::PointXYZ(*iter_orig_x, *iter_orig_y, *iter_orig_z));
     }
   }