From 5fd4b9598ccbf408dc6cb6ceba5e986121d6903b Mon Sep 17 00:00:00 2001
From: Yukihiro Saito <yukky.saito@gmail.com>
Date: Thu, 10 Mar 2022 11:30:50 +0900
Subject: [PATCH 1/3] fix(perception_launch): integration miss related to
 camera lidar fusion (#234)

* fix(perception_launch): integration miss related to camera lidar fusion

Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com>

* add detection by tracker

Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com>

* Apply suggestions from code review

Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com>

Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com>
---
 ...ra_lidar_fusion_based_detection.launch.xml | 19 ++++++++++++++++---
 .../lidar_based_detection.launch.xml          |  2 +-
 2 files changed, 17 insertions(+), 4 deletions(-)

diff --git a/perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
index 98bc1019d..ba7933ce6 100644
--- a/perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
+++ b/perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
@@ -37,6 +37,7 @@
     <push-ros-namespace namespace="euclidean"/>
     <include file="$(find-pkg-share euclidean_cluster)/launch/voxel_grid_based_euclidean_cluster.launch.xml">
       <arg name="output_clusters" value="lidar/clusters"/>
+      <arg name="use_pointcloud_map" value="false"/>
     </include>
     <include file="$(find-pkg-share roi_cluster_fusion)/launch/roi_cluster_fusion.launch.xml">
       <arg name="input/camera_info0" value="$(var camera_info0)"/>
@@ -68,10 +69,8 @@
       <arg name="output" value="camera_lidar_fusion/clusters"/>
     </include>
     <include file="$(find-pkg-share shape_estimation)/launch/shape_estimation.launch.xml">
-      <arg name="use_map_current" value="$(var use_vector_map)"/>
       <arg name="input/objects" value="camera_lidar_fusion/clusters" />
       <arg name="output/objects" value="camera_lidar_fusion/objects_with_feature" />
-      <arg name="orientation_reliable" value="false"/>
     </include>
     <!-- convert DynamicObjectsWithFeatureArray to DynamicObjects -->
     <include file="$(find-pkg-share detected_object_feature_remover)/launch/detected_object_feature_remover.launch.xml">
@@ -101,7 +100,7 @@
       <include file="$(find-pkg-share lidar_apollo_instance_segmentation)/launch/lidar_apollo_instance_segmentation.launch.xml" />
       <include file="$(find-pkg-share shape_estimation)/launch/shape_estimation.launch.xml">
         <arg name="output/objects" value="objects_with_feature"/>
-        <arg name="use_map_current" value="$(var use_vector_map)"/>
+        <arg name="use_vehicle_reference_yaw" value="true"/>
       </include>
       <!-- convert DynamicObjectsWithFeatureArray to DynamicObjects -->
       <include file="$(find-pkg-share detected_object_feature_remover)/launch/detected_object_feature_remover.launch.xml">
@@ -111,9 +110,23 @@
     </group>
   </group>
 
+  <!-- DetectionByTracker -->
+  <group>
+    <push-ros-namespace namespace="detection_by_tracker"/>
+    <include file="$(find-pkg-share detection_by_tracker)/launch/detection_by_tracker.launch.xml">
+    </include>
+  </group>
+
+  <!-- Merger -->
   <include file="$(find-pkg-share object_merger)/launch/object_association_merger.launch.xml">
     <arg name="input/object0" value="$(var lidar_detection_model)/objects"/>
     <arg name="input/object1" value="euclidean/camera_lidar_fusion/short_range_objects"/>
+    <arg name="output/object" value="camera_lidar_fusion/objects"/>
+  </include>
+  <include file="$(find-pkg-share object_merger)/launch/object_association_merger.launch.xml">
+    <arg name="input/object0" value="detection_by_tracker/objects"/>
+    <arg name="input/object1" value="camera_lidar_fusion/objects"/>
     <arg name="output/object" value="objects"/>
   </include>
+
 </launch>
diff --git a/perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml
index a836c4198..692697d6d 100644
--- a/perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml
+++ b/perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml
@@ -33,7 +33,7 @@
       <include file="$(find-pkg-share lidar_apollo_instance_segmentation)/launch/lidar_apollo_instance_segmentation.launch.xml" />
       <include file="$(find-pkg-share shape_estimation)/launch/shape_estimation.launch.xml">
         <arg name="output/objects" value="objects_with_feature"/>
-        <arg name="use_map_current" value="$(var use_vector_map)"/>
+        <arg name="use_vehicle_reference_yaw" value="true"/>
       </include>
       <!-- convert DynamicObjectsWithFeatureArray to DynamicObjects -->
       <include file="$(find-pkg-share detected_object_feature_remover)/launch/detected_object_feature_remover.launch.xml">

From e0d419848c0bf7029222867a4d2e374ed1f9c175 Mon Sep 17 00:00:00 2001
From: Yukihiro Saito <yukky.saito@gmail.com>
Date: Fri, 11 Mar 2022 00:22:35 +0900
Subject: [PATCH 2/3] feat: add iou gate (#237)

Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com>
---
 .../data_association_matrix.param.yaml                 | 10 ++++++++++
 1 file changed, 10 insertions(+)

diff --git a/perception_launch/config/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml b/perception_launch/config/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml
index 393bb949d..385eb335c 100644
--- a/perception_launch/config/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml
+++ b/perception_launch/config/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml
@@ -50,3 +50,13 @@
        3.150,   3.150, 3.150, 3.150,  3.150,   3.150,     3.150,      3.150,  #MOTORBIKE
        3.150,   3.150, 3.150, 3.150,  3.150,   3.150,     3.150,      3.150,  #PEDESTRIAN
        3.150,   3.150, 3.150, 3.150,  3.150,   3.150,     3.150,      3.150]  #ANIMAL
+    min_iou_matrix:
+      #UNKNOWN, CAR,   TRUCK, BUS,    BICYCLE, MOTORBIKE, PEDESTRIAN, ANIMAL
+      [0.1,     0.1,   0.1,   0.1,    0.1,     0.1,       0.1,        0.1,  #UNKNOWN
+       0.1,     0.3,   0.2,   0.2,    0.1,     0.1,       0.1,        0.1,  #CAR
+       0.1,     0.2,   0.3,   0.3,    0.1,     0.1,       0.1,        0.1,  #TRUCK
+       0.1,     0.2,   0.3,   0.3,    0.1,     0.1,       0.1,        0.1,  #BUS
+       0.1,     0.1,   0.1,   0.1,    0.1,     0.1,       0.1,        0.1,  #BICYCLE
+       0.1,     0.1,   0.1,   0.1,    0.1,     0.1,       0.1,        0.1,  #MOTORBIKE
+       0.1,     0.1,   0.1,   0.1,    0.1,     0.1,       0.1,        0.1,  #PEDESTRIAN
+       0.1,     0.1,   0.1,   0.1,    0.1,     0.1,       0.1,        0.1]  #ANIMAL

From 769fe69010d9ebfe616708278ba0acbd9a3a9f68 Mon Sep 17 00:00:00 2001
From: Yukihiro Saito <yukky.saito@gmail.com>
Date: Fri, 11 Mar 2022 11:14:59 +0900
Subject: [PATCH 3/3] fix(detection by tracker): integration miss (#241)

* fix bug

Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com>

* fix bug

Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com>
---
 .../camera_lidar_fusion_based_detection.launch.xml        | 8 ++++----
 .../detection/lidar_based_detection.launch.xml            | 1 +
 2 files changed, 5 insertions(+), 4 deletions(-)

diff --git a/perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
index ba7933ce6..16fa7234a 100644
--- a/perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
+++ b/perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
@@ -34,9 +34,9 @@
   </include> -->
 
   <group>
-    <push-ros-namespace namespace="euclidean"/>
+    <push-ros-namespace namespace="clustering"/>
     <include file="$(find-pkg-share euclidean_cluster)/launch/voxel_grid_based_euclidean_cluster.launch.xml">
-      <arg name="output_clusters" value="lidar/clusters"/>
+      <arg name="output_clusters" value="clusters"/>
       <arg name="use_pointcloud_map" value="false"/>
     </include>
     <include file="$(find-pkg-share roi_cluster_fusion)/launch/roi_cluster_fusion.launch.xml">
@@ -57,7 +57,7 @@
       <arg name="input/camera_info7" value="$(var camera_info7)"/>
       <arg name="input/rois7" value="/perception/object_recognition/detection/rois7"/>
       <arg name="input/rois_number" value="$(var image_number)"/>
-      <arg name="input/clusters" value="clustering/lidar/clusters"/>
+      <arg name="input/clusters" value="clusters"/>
       <arg name="input/image0" value="$(var image_raw0)"/>
       <arg name="input/image1" value="$(var image_raw1)"/>
       <arg name="input/image2" value="$(var image_raw2)"/>
@@ -120,7 +120,7 @@
   <!-- Merger -->
   <include file="$(find-pkg-share object_merger)/launch/object_association_merger.launch.xml">
     <arg name="input/object0" value="$(var lidar_detection_model)/objects"/>
-    <arg name="input/object1" value="euclidean/camera_lidar_fusion/short_range_objects"/>
+    <arg name="input/object1" value="clustering/camera_lidar_fusion/short_range_objects"/>
     <arg name="output/object" value="camera_lidar_fusion/objects"/>
   </include>
   <include file="$(find-pkg-share object_merger)/launch/object_association_merger.launch.xml">
diff --git a/perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml
index 692697d6d..579e38e8f 100644
--- a/perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml
+++ b/perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml
@@ -5,6 +5,7 @@
 
   <!-- Clustering -->
   <group>
+    <push-ros-namespace namespace="clustering"/>
     <include file="$(find-pkg-share euclidean_cluster)/launch/voxel_grid_based_euclidean_cluster.launch.xml">
       <arg name="output_clusters" value="clusters"/>
       <arg name="use_pointcloud_map" value="false"/>