From 4f498d4f2be5b823d6a1cb5b637e164afeb5d12b Mon Sep 17 00:00:00 2001
From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com>
Date: Fri, 5 Jul 2024 16:48:06 +0900
Subject: [PATCH] feat(tier4_perception_launch): add image segmentation based
 pointcloud filter (#7225)

* feat(tier4_perception_launch): add image segmentation based pointcloud filter

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

* chore: typo

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

* fix: detection launch

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

* chore: add maintainer

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

* Revert "chore: add maintainer"

This reverts commit 5adfef6e9ca8196d3ba88ad574b2ba35489a5e49.

---------

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
---
 .../detection/detection.launch.xml                  |  2 ++
 .../detector/camera_lidar_detector.launch.xml       | 13 ++++++++++++-
 .../launch/perception.launch.xml                    |  2 ++
 3 files changed, 16 insertions(+), 1 deletion(-)

diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml
index 754d07c6d99bf..df7733b3102ba 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml
@@ -3,6 +3,7 @@
   <!-- Interface parameters -->
   <arg name="mode" default="lidar" description="options: `camera_lidar_radar_fusion`, `camera_lidar_fusion`, `lidar_radar_fusion`, `lidar` or `radar`"/>
   <arg name="use_detection_by_tracker"/>
+  <arg name="use_image_segmentation_based_filter"/>
 
   <!-- Lidar parameters -->
   <arg name="input/pointcloud"/>
@@ -60,6 +61,7 @@
       <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
       <arg name="use_roi_based_cluster" value="$(var use_roi_based_cluster)"/>
       <arg name="detection_by_tracker_param_path" value="$(var detection_by_tracker_param_path)"/>
+      <arg name="use_image_segmentation_based_filter" value="$(var use_image_segmentation_based_filter)"/>
     </include>
     <!-- Lidar dnn-based detectors-->
     <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml">
diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml
index fe0ba0e614e60..8390c7031b732 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml
@@ -9,6 +9,7 @@
   <arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`, `pointpainting`, `clustering`"/>
   <arg name="use_roi_based_cluster" default="false"/>
   <arg name="use_low_intensity_cluster_filter" default="false"/>
+  <arg name="use_image_segmentation_based_filter" default="false"/>
 
   <!-- Camera parameters -->
   <arg name="image_raw0" default="/image_raw" description="image raw topic name"/>
@@ -104,14 +105,24 @@
     </include>
   </group>
 
+  <!-- Image_segmentation based filter, apply for camera0 only-->
+  <group>
+    <include file="$(find-pkg-share image_projection_based_fusion)/launch/segmentation_pointcloud_fusion.launch.xml" if="$(var use_image_segmentation_based_filter)">
+      <arg name="input/pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud"/>
+      <arg name="output/pointcloud" value="/perception/object_recognition/detection/segmentation_based_filtered/pointcloud"/>
+    </include>
+  </group>
+
   <!-- Clustering -->
   <group>
     <push-ros-namespace namespace="clustering"/>
     <group>
+      <let name="euclidean_cluster_input" value="$(var input/pointcloud_map/pointcloud)" unless="$(var use_image_segmentation_based_filter)"/>
+      <let name="euclidean_cluster_input" value="/perception/object_recognition/detection/segmentation_based_filtered/pointcloud" if="$(var use_image_segmentation_based_filter)"/>
       <let name="euclidean_cluster_output" value="euclidean_cluster/clusters" if="$(var use_roi_based_cluster)"/>
       <let name="euclidean_cluster_output" value="clusters" unless="$(var use_roi_based_cluster)"/>
       <include file="$(find-pkg-share euclidean_cluster)/launch/voxel_grid_based_euclidean_cluster.launch.xml">
-        <arg name="input_pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud"/>
+        <arg name="input_pointcloud" value="$(var euclidean_cluster_input)"/>
         <arg name="output_clusters" value="$(var euclidean_cluster_output)"/>
         <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
       </include>
diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml
index dd80ab5b2cde7..40ab96043e983 100644
--- a/launch/tier4_perception_launch/launch/perception.launch.xml
+++ b/launch/tier4_perception_launch/launch/perception.launch.xml
@@ -79,6 +79,7 @@
   <arg name="use_object_filter" default="true" description="use object filter"/>
   <arg name="use_roi_based_cluster" default="true" description="use roi_based_cluster in clustering"/>
   <arg name="use_low_intensity_cluster_filter" default="true" description="use low_intensity_cluster_filter in clustering"/>
+  <arg name="use_image_segmentation_based_filter" default="true" description="use image_segmentation_based_filter in clustering"/>
   <arg
     name="use_empty_dynamic_object_publisher"
     default="false"
@@ -221,6 +222,7 @@
           <arg name="use_radar_tracking_fusion" value="$(var use_radar_tracking_fusion)"/>
           <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
           <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
+          <arg name="use_image_segmentation_based_filter" value="$(var use_image_segmentation_based_filter)"/>
           <arg name="use_object_filter" value="$(var use_object_filter)"/>
           <arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
           <arg name="use_roi_based_cluster" value="$(var use_roi_based_cluster)"/>