From b61db78c8c9d8658bd43789c72cd7dddbc2b76d8 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 373e343ad64fa..b3280fee47364 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
@@ -13,6 +13,7 @@
   <arg name="use_validator" description="use obstacle_pointcloud based validator"/>
   <arg name="objects_validation_method" description="options: `obstacle_pointcloud` or `occupancy_grid`"/>
   <arg name="use_low_intensity_cluster_filter"/>
+  <arg name="use_image_segmentation_based_filter"/>
   <arg name="use_multi_channel_tracker_merger"/>
 
   <!-- External interfaces -->
@@ -163,6 +164,7 @@
       <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_low_intensity_cluster_filter" value="$(var use_low_intensity_cluster_filter)"/>
+      <arg name="use_image_segmentation_based_filter" value="$(var use_image_segmentation_based_filter)"/>
     </include>
   </group>
   <group if="$(var switch/detector/lidar_dnn)">
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 4c288aa632182..fba0144e6cee1 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
@@ -4,6 +4,7 @@
   <arg name="lidar_detection_model" description="options: `centerpoint`, `apollo`, `pointpainting`, `clustering`"/>
   <arg name="use_roi_based_cluster"/>
   <arg name="use_low_intensity_cluster_filter"/>
+  <arg name="use_image_segmentation_based_filter"/>
 
   <!-- External interfaces -->
   <arg name="number_of_cameras"/>
@@ -95,14 +96,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="$(var input/pointcloud_map/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="$(var input/pointcloud_map/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 5d8d7563e7a7c..6c428a5adea0b 100644
--- a/launch/tier4_perception_launch/launch/perception.launch.xml
+++ b/launch/tier4_perception_launch/launch/perception.launch.xml
@@ -80,6 +80,7 @@
   <arg name="objects_filter_method" default="lanelet_filter" description="options: `lanelet_filter` or `position_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="false" description="use image_segmentation_based_filter in clustering"/>
   <arg
     name="use_empty_dynamic_object_publisher"
     default="false"
@@ -240,6 +241,7 @@
           <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_low_intensity_cluster_filter" value="$(var use_low_intensity_cluster_filter)"/>
+          <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="objects_filter_method" value="$(var objects_filter_method)"/>
           <arg name="use_roi_based_cluster" value="$(var use_roi_based_cluster)"/>