fix(image_projection_based_fusion): add iou_x use in long range for r…

…oi_cluster_fusion (autowarefoundation#5148) * fix: add iou_x for long range obj Signed-off-by: badai-nguyen <[email protected]> * fix: add launch file param Signed-off-by: badai-nguyen <[email protected]> * chore: fix unexpect calc iou in long range Signed-off-by: badai-nguyen <[email protected]> * fix: multi iou usable Signed-off-by: badai-nguyen <[email protected]> * chore: typo Signed-off-by: badai-nguyen <[email protected]> * docs: update readme Signed-off-by: badai-nguyen <[email protected]> * chore: refactor Signed-off-by: badai-nguyen <[email protected]> --------- Signed-off-by: badai-nguyen <[email protected]>
tier4 · Oct 26, 2023 · 944845c · 944845c
1 parent 81e7a11
commit 944845c
Show file tree

Hide file tree

Showing 8 changed files with 87 additions and 54 deletions.
diff --git a/...launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/...launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
@@ -15,7 +15,8 @@
   <arg name="use_pointcloud_container" default="false" description="use pointcloud container for detection preprocessor"/>
   <arg name="use_validator" default="true" description="use obstacle_pointcloud based validator"/>
   <arg name="remove_unknown" default="true"/>
-  <arg name="trust_distance" default="30.0"/>
+  <arg name="fusion_distance" default="100.0"/>
+  <arg name="trust_object_distance" default="100.0"/>
   <arg name="use_roi_based_cluster" default="false"/>
 
   <!-- Camera parameters -->
@@ -169,7 +170,8 @@
           <arg name="input/image7" value="$(var image_raw7)"/>
           <arg name="output/clusters" value="clusters"/>
           <arg name="remove_unknown" value="$(var remove_unknown)"/>
-          <arg name="trust_distance" value="$(var trust_distance)"/>
+          <arg name="fusion_distance" value="$(var fusion_distance)"/>
+          <arg name="trust_object_distance" value="$(var trust_object_distance)"/>
         </include>
       </group>
 

diff --git a/.../launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/.../launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml
@@ -15,7 +15,8 @@
   <arg name="use_pointcloud_container" default="false" description="use pointcloud container for detection preprocessor"/>
   <arg name="use_validator" default="true" description="use obstacle_pointcloud based validator"/>
   <arg name="remove_unknown" default="true"/>
-  <arg name="trust_distance" default="30.0"/>
+  <arg name="fusion_distance" default="100.0"/>
+  <arg name="trust_object_distance" default="100.0"/>
 
   <!-- Camera parameters -->
   <arg name="image_raw0" default="/image_raw" description="image raw topic name"/>
@@ -194,7 +195,8 @@
           <arg name="input/image7" value="$(var image_raw7)"/>
           <arg name="output/clusters" value="clusters"/>
           <arg name="remove_unknown" value="$(var remove_unknown)"/>
-          <arg name="trust_distance" value="$(var trust_distance)"/>
+          <arg name="fusion_distance" value="$(var fusion_distance)"/>
+          <arg name="trust_object_distance" value="$(var trust_object_distance)"/>
         </include>
       </group>
 

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
@@ -35,7 +35,8 @@
 
   <!-- Camera-LiDAR fusion parameters -->
   <arg name="remove_unknown" default="true"/>
-  <arg name="trust_distance" default="30.0"/>
+  <arg name="fusion_distance" default="100.0"/>
+  <arg name="trust_object_distance" default="100.0"/>
 
   <!-- Camera-LiDAR-Radar fusion based detection -->
   <group if="$(eval '&quot;$(var mode)&quot;==&quot;camera_lidar_radar_fusion&quot;')">
@@ -64,7 +65,8 @@
       <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
       <arg name="container_name" value="$(var container_name)"/>
       <arg name="remove_unknown" value="$(var remove_unknown)"/>
-      <arg name="trust_distance" value="$(var trust_distance)"/>
+      <arg name="fusion_distance" value="$(var fusion_distance)"/>
+      <arg name="trust_object_distance" value="$(var trust_object_distance)"/>
       <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="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>

diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml
@@ -91,7 +91,8 @@
 
   <!-- Camera-Lidar fusion parameters -->
   <arg name="remove_unknown" default="true"/>
-  <arg name="trust_distance" default="30.0"/>
+  <arg name="fusion_distance" default="100.0"/>
+  <arg name="trust_object_distance" default="100.0"/>
 
   <!-- Radar long range integration methods -->
   <arg
@@ -174,7 +175,8 @@
           <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
           <arg name="container_name" value="$(var pointcloud_container_name)"/>
           <arg name="remove_unknown" value="$(var remove_unknown)"/>
-          <arg name="trust_distance" value="$(var trust_distance)"/>
+          <arg name="fusion_distance" value="$(var fusion_distance)"/>
+          <arg name="trust_object_distance" value="$(var trust_object_distance)"/>
           <arg name="use_roi_based_cluster" value="$(var use_roi_based_cluster)"/>
         </include>
       </group>

diff --git a/perception/image_projection_based_fusion/docs/roi-cluster-fusion.md b/perception/image_projection_based_fusion/docs/roi-cluster-fusion.md
@@ -32,18 +32,20 @@ The clusters are projected onto image planes, and then if the ROIs of clusters a
 
 ### Core Parameters
 
-| Name                        | Type  | Description                                                                      |
-| --------------------------- | ----- | -------------------------------------------------------------------------------- |
-| `use_iou_x`                 | bool  | calculate IoU only along x-axis                                                  |
-| `use_iou_y`                 | bool  | calculate IoU only along y-axis                                                  |
-| `use_iou`                   | bool  | calculate IoU both along x-axis and y-axis                                       |
-| `use_cluster_semantic_type` | bool  | if `false`, the labels of clusters are overwritten by `UNKNOWN` before fusion    |
-| `only_allow_inside_cluster` | bool  | if `true`, the only clusters contained inside RoIs by a detector                 |
-| `roi_scale_factor`          | float | the scale factor for offset of detector RoIs if `only_allow_inside_cluster=true` |
-| `iou_threshold`             | float | the IoU threshold to overwrite a label of clusters with a label of roi           |
-| `unknown_iou_threshold`     | float | the IoU threshold to fuse cluster with unknown label of roi                      |
-| `rois_number`               | int   | the number of input rois                                                         |
-| `debug_mode`                | bool  | If `true`, subscribe and publish images for visualization.                       |
+| Name                        | Type   | Description                                                                                                                                                                                                                        |
+| --------------------------- | ------ | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
+| `fusion_distance`           | double | If the detected object's distance to frame_id is less than the threshold, the fusion will be processed                                                                                                                             |
+| `trust_object_distance`     | double | if the detected object's distance is less than the `trust_object_distance`, `trust_object_iou_mode` will be used, otherwise `non_trust_object_iou_mode` will be used                                                               |
+| `trust_object_iou_mode`     | string | select mode from 3 options {`iou`, `iou_x`, `iou_y`} to calculate IoU in range of [`0`, `trust_distance`]. <br> &emsp;`iou`: IoU along x-axis and y-axis <br> &emsp;`iou_x`: IoU along x-axis <br> &emsp;`iou_y`: IoU along y-axis |
+| `non_trust_object_iou_mode` | string | the IOU mode using in range of [`trust_distance`, `fusion_distance`] if `trust_distance` < `fusion_distance`                                                                                                                       |
+| `use_cluster_semantic_type` | bool   | if `false`, the labels of clusters are overwritten by `UNKNOWN` before fusion                                                                                                                                                      |
+| `only_allow_inside_cluster` | bool   | if `true`, the only clusters contained inside RoIs by a detector                                                                                                                                                                   |
+| `roi_scale_factor`          | double | the scale factor for offset of detector RoIs if `only_allow_inside_cluster=true`                                                                                                                                                   |
+| `iou_threshold`             | double | the IoU threshold to overwrite a label of clusters with a label of roi                                                                                                                                                             |
+| `unknown_iou_threshold`     | double | the IoU threshold to fuse cluster with unknown label of roi                                                                                                                                                                        |
+| `remove_unknown`            | bool   | if `true`, remove all `UNKNOWN` labeled objects from output                                                                                                                                                                        |
+| `rois_number`               | int    | the number of input rois                                                                                                                                                                                                           |
+| `debug_mode`                | bool   | If `true`, subscribe and publish images for visualization.                                                                                                                                                                         |
 
 ## Assumptions / Known limits
 

diff --git a/...projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp b/...projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp
@@ -17,10 +17,12 @@
 
 #include "image_projection_based_fusion/fusion_node.hpp"
 
+#include <map>
 #include <memory>
-
+#include <string>
 namespace image_projection_based_fusion
 {
+const std::map<std::string, uint8_t> IOU_MODE_MAP{{"iou", 0}, {"iou_x", 1}, {"iou_y", 2}};
 
 class RoiClusterFusionNode
 : public FusionNode<DetectedObjectsWithFeature, DetectedObjectWithFeature>
@@ -38,9 +40,7 @@ class RoiClusterFusionNode
     const sensor_msgs::msg::CameraInfo & camera_info,
     DetectedObjectsWithFeature & output_cluster_msg) override;
 
-  bool use_iou_x_{false};
-  bool use_iou_y_{false};
-  bool use_iou_{false};
+  std::string trust_object_iou_mode_{"iou"};
   bool use_cluster_semantic_type_{false};
   bool only_allow_inside_cluster_{false};
   double roi_scale_factor_{1.1};
@@ -49,10 +49,14 @@ class RoiClusterFusionNode
   const float min_roi_existence_prob_ =
     0.1;  // keep small value to lessen affect on merger object stage
   bool remove_unknown_;
-  double trust_distance_;
-
-  bool filter_by_distance(const DetectedObjectWithFeature & obj);
+  double fusion_distance_;
+  double trust_object_distance_;
+  std::string non_trust_object_iou_mode_{"iou_x"};
+  bool is_far_enough(const DetectedObjectWithFeature & obj, const double distance_threshold);
   bool out_of_scope(const DetectedObjectWithFeature & obj);
+  double cal_iou_by_mode(
+    const sensor_msgs::msg::RegionOfInterest & roi_1,
+    const sensor_msgs::msg::RegionOfInterest & roi_2, const std::string iou_mode);
   // bool CheckUnknown(const DetectedObjectsWithFeature & obj);
 };
 

diff --git a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml
@@ -1,5 +1,5 @@
 <launch>
-  <arg name="input/rois_number" default="1"/>
+  <arg name="input/rois_number" default="6"/>
   <arg name="input/rois0" default="rois0"/>
   <arg name="input/camera_info0" default="/camera_info0"/>
   <arg name="input/rois1" default="rois1"/>
@@ -20,7 +20,8 @@
   <arg name="output/clusters" default="labeled_clusters"/>
   <arg name="sync_param_path" default="$(find-pkg-share image_projection_based_fusion)/config/roi_sync.param.yaml"/>
   <arg name="remove_unknown" default="true"/>
-  <arg name="trust_distance" default="100.0"/>
+  <arg name="fusion_distance" default="100.0"/>
+  <arg name="trust_object_distance" default="100.0"/>
 
   <!-- for eval variable-->
   <arg name="input_rois_number" default="$(var input/rois_number)"/>
@@ -46,17 +47,17 @@
   <arg name="input/image7" default="/image_raw7"/>
   <group>
     <node pkg="image_projection_based_fusion" exec="roi_cluster_fusion_node" name="roi_cluster_fusion" output="screen">
-      <param name="use_iou" value="true"/>
-      <param name="use_iou_x" value="false"/>
-      <param name="use_iou_y" value="false"/>
+      <param name="trust_object_iou_mode" value="iou"/>
+      <param name="non_trust_object_iou_mode" value="iou_x"/>
       <param name="use_cluster_semantic_type" value="false"/>
       <param name="only_allow_inside_cluster" value="true"/>
       <param name="roi_scale_factor" value="1.1"/>
       <param name="iou_threshold" value="0.65"/>
       <param name="unknown_iou_threshold" value="0.1"/>
       <param name="rois_number" value="$(var input/rois_number)"/>
       <param name="remove_unknown" value="$(var remove_unknown)"/>
-      <param name="trust_distance" value="$(var trust_distance)"/>
+      <param name="fusion_distance" value="$(var fusion_distance)"/>
+      <param name="trust_object_distance" value="$(var trust_object_distance)"/>
       <param from="$(var sync_param_path)"/>
       <remap from="input" to="$(var input/clusters)"/>
       <remap from="output" to="$(var output/clusters)"/>

diff --git a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp
@@ -36,16 +36,16 @@ namespace image_projection_based_fusion
 RoiClusterFusionNode::RoiClusterFusionNode(const rclcpp::NodeOptions & options)
 : FusionNode<DetectedObjectsWithFeature, DetectedObjectWithFeature>("roi_cluster_fusion", options)
 {
-  use_iou_x_ = declare_parameter<bool>("use_iou_x");
-  use_iou_y_ = declare_parameter<bool>("use_iou_y");
-  use_iou_ = declare_parameter<bool>("use_iou");
+  trust_object_iou_mode_ = declare_parameter<std::string>("trust_object_iou_mode");
+  non_trust_object_iou_mode_ = declare_parameter<std::string>("non_trust_object_iou_mode");
   use_cluster_semantic_type_ = declare_parameter<bool>("use_cluster_semantic_type");
   only_allow_inside_cluster_ = declare_parameter<bool>("only_allow_inside_cluster");
   roi_scale_factor_ = declare_parameter<double>("roi_scale_factor");
   iou_threshold_ = declare_parameter<double>("iou_threshold");
   unknown_iou_threshold_ = declare_parameter<double>("unknown_iou_threshold");
   remove_unknown_ = declare_parameter<bool>("remove_unknown");
-  trust_distance_ = declare_parameter<double>("trust_distance");
+  fusion_distance_ = declare_parameter<double>("fusion_distance");
+  trust_object_distance_ = declare_parameter<double>("trust_object_distance");
 }
 
 void RoiClusterFusionNode::preprocess(DetectedObjectsWithFeature & output_cluster_msg)
@@ -111,7 +111,7 @@ void RoiClusterFusionNode::fuseOnSingleImage(
       continue;
     }
 
-    if (filter_by_distance(input_cluster_msg.feature_objects.at(i))) {
+    if (is_far_enough(input_cluster_msg.feature_objects.at(i), fusion_distance_)) {
       continue;
     }
 
@@ -174,25 +174,24 @@ void RoiClusterFusionNode::fuseOnSingleImage(
     bool is_roi_label_known =
       feature_obj.object.classification.front().label != ObjectClassification::UNKNOWN;
     for (const auto & cluster_map : m_cluster_roi) {
-      double iou(0.0), iou_x(0.0), iou_y(0.0);
-      if (use_iou_) {
-        iou = calcIoU(cluster_map.second, feature_obj.feature.roi);
-      }
-      // use for unknown roi to improve small objects like traffic cone detect
-      // TODO(badai-nguyen): add option to disable roi_cluster mode
-      if (use_iou_x_ || !is_roi_label_known) {
-        iou_x = calcIoUX(cluster_map.second, feature_obj.feature.roi);
-      }
-      if (use_iou_y_) {
-        iou_y = calcIoUY(cluster_map.second, feature_obj.feature.roi);
+      double iou(0.0);
+      bool is_use_non_trust_object_iou_mode = is_far_enough(
+        input_cluster_msg.feature_objects.at(cluster_map.first), trust_object_distance_);
+      if (is_use_non_trust_object_iou_mode || is_roi_label_known) {
+        iou =
+          cal_iou_by_mode(cluster_map.second, feature_obj.feature.roi, non_trust_object_iou_mode_);
+      } else {
+        iou = cal_iou_by_mode(cluster_map.second, feature_obj.feature.roi, trust_object_iou_mode_);
       }
+
       const bool passed_inside_cluster_gate =
         only_allow_inside_cluster_
           ? is_inside(feature_obj.feature.roi, cluster_map.second, roi_scale_factor_)
           : true;
-      if (max_iou < iou + iou_x + iou_y && passed_inside_cluster_gate) {
+      if (max_iou < iou && passed_inside_cluster_gate) {
         index = cluster_map.first;
-        max_iou = iou + iou_x + iou_y;
+        max_iou = iou;
+        associated = true;
       }
     }
     if (!output_cluster_msg.feature_objects.empty()) {
@@ -267,11 +266,30 @@ bool RoiClusterFusionNode::out_of_scope(const DetectedObjectWithFeature & obj)
   return is_out;
 }
 
-bool RoiClusterFusionNode::filter_by_distance(const DetectedObjectWithFeature & obj)
+bool RoiClusterFusionNode::is_far_enough(
+  const DetectedObjectWithFeature & obj, const double distance_threshold)
 {
   const auto & position = obj.object.kinematics.pose_with_covariance.pose.position;
-  const auto square_distance = position.x * position.x + position.y + position.y;
-  return square_distance > trust_distance_ * trust_distance_;
+  return position.x * position.x + position.y * position.y >
+         distance_threshold * distance_threshold;
+}
+
+double RoiClusterFusionNode::cal_iou_by_mode(
+  const sensor_msgs::msg::RegionOfInterest & roi_1,
+  const sensor_msgs::msg::RegionOfInterest & roi_2, const std::string iou_mode)
+{
+  switch (IOU_MODE_MAP.at(iou_mode)) {
+    case 0 /* use iou mode */:
+      return calcIoU(roi_1, roi_2);
+
+    case 1 /* use iou_x mode */:
+      return calcIoUX(roi_1, roi_2);
+
+    case 2 /* use iou_y mode */:
+      return calcIoUY(roi_1, roi_2);
+    default:
+      return 0.0;
+  }
 }
 
 }  // namespace image_projection_based_fusion