feat(radar_object_fusion_to_detected_object): enable confidence compe…

…nsation in radar fusion (tier4#1755) * update parameter Signed-off-by: scepter914 <scepter914@gmail.com> * feature(radar_fusion_to_detected_object): add debug topic Signed-off-by: scepter914 <scepter914@gmail.com> * feat(tier4_perception_launch): enable confidence compensation in radar fusion Signed-off-by: scepter914 <scepter914@gmail.com> * add compensate probability paramter Signed-off-by: scepter914 <scepter914@gmail.com> * fix parameter Signed-off-by: scepter914 <scepter914@gmail.com> * update paramter Signed-off-by: scepter914 <scepter914@gmail.com> * update paramter Signed-off-by: scepter914 <scepter914@gmail.com> * fix parameter Signed-off-by: scepter914 <scepter914@gmail.com> * ci(pre-commit): autofix Signed-off-by: scepter914 <scepter914@gmail.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
boyali · Oct 3, 2022 · 512bebd · 512bebd
1 parent 8283421
commit 512bebd
Show file tree

Hide file tree

Showing 10 changed files with 34 additions and 7 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
@@ -27,6 +27,7 @@
   <arg name="use_pointcloud_container" default="false" description="use pointcloud container for detection preprocessor"/>
   <arg name="container_name" default="pointcloud_container"/>
   <arg name="use_validator" default="true" description="use obstacle_pointcloud based validator"/>
+  <arg name="score_threshold" default="0.45"/>
 
   <!-- Jetson AGX -->
   <!-- <include file="$(find-pkg-share tensorrt_yolo)/launch/yolo.launch.xml">
@@ -154,6 +155,7 @@
     <include file="$(find-pkg-share lidar_centerpoint)/launch/lidar_centerpoint.launch.xml">
       <arg name="input/pointcloud" value="$(var input/pointcloud)"/>
       <arg name="output/objects" value="objects"/>
+      <arg name="score_threshold" value="$(var score_threshold)"/>
     </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
@@ -7,6 +7,7 @@
   <arg name="use_object_filter" default="true" description="use object filter"/>
   <arg name="use_pointcloud_container" default="false" description="use pointcloud container for detection preprocessor"/>
   <arg name="container_name" default="pointcloud_container"/>
+  <arg name="score_threshold" default="0.20"/>
 
   <!-- camera param -->
   <arg name="image_raw0" default="/image_raw" description="image raw topic name"/>
@@ -54,6 +55,7 @@
       <arg name="use_object_filter" value="$(var use_object_filter)"/>
       <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
       <arg name="container_name" value="$(var container_name)"/>
+      <arg name="score_threshold" value="$(var score_threshold)"/>
     </include>
   </group>
 

diff --git a/...r4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/...r4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml
@@ -10,6 +10,7 @@
   <arg name="use_pointcloud_container" default="false" description="use pointcloud container for detection preprocessor"/>
   <arg name="container_name" default="pointcloud_container"/>
   <arg name="use_validator" default="true" description="use obstacle_pointcloud based validator"/>
+  <arg name="score_threshold" default="0.45"/>
 
   <!-- Pointcloud map filter -->
   <group>
@@ -65,6 +66,8 @@
     <group>
       <include file="$(find-pkg-share lidar_centerpoint)/launch/lidar_centerpoint.launch.xml">
         <arg name="input/pointcloud" value="$(var input/pointcloud)"/>
+        <arg name="output/objects" value="objects"/>
+        <arg name="score_threshold" value="$(var score_threshold)"/>
       </include>
     </group>
   </group>

diff --git a/...ception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml b/...ception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml
@@ -7,6 +7,7 @@
   <arg name="use_object_filter" default="true" description="use object filter"/>
   <arg name="use_pointcloud_container" default="false" description="use pointcloud container for detection preprocessor"/>
   <arg name="container_name" default="pointcloud_container"/>
+  <arg name="score_threshold" default="0.20"/>
 
   <!-- lidar based detection-->
   <group>
@@ -18,6 +19,7 @@
       <arg name="use_object_filter" value="$(var use_object_filter)"/>
       <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
       <arg name="container_name" value="$(var container_name)"/>
+      <arg name="score_threshold" value="$(var score_threshold)"/>
     </include>
   </group>
 

diff --git a/perception/radar_fusion_to_detected_object/README.md b/perception/radar_fusion_to_detected_object/README.md
@@ -38,6 +38,7 @@ To tune these weight parameters, please see [document](docs/algorithm.md) in det
 | :----------------------- | :---- | :----------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
 | convert_doppler_to_twist | bool  | Convert doppler velocity to twist using the yaw information of a detected object.                                                                      | false         |
 | threshold_probability    | float | If the probability of an output object is lower than this parameter, and the output object does not have radar points/objects, then delete the object. | 0.4           |
+| compensate_probability   | bool  | If this parameter is true, compensate probability of objects to threshold probability.                                                                 | false         |
 
 ## radar_object_fusion_to_detected_object
 
@@ -62,9 +63,10 @@ ros2 launch radar_fusion_to_detected_object radar_object_to_detected_object.laun
 
 ### Output
 
-| Name               | Type                                                  | Description                    |
-| ------------------ | ----------------------------------------------------- | ------------------------------ |
-| `~/output/objects` | autoware_auto_perception_msgs/msg/DetectedObjects.msg | 3D detected object with twist. |
+| Name                             | Type                                                  | Description                                                                            |
+| -------------------------------- | ----------------------------------------------------- | -------------------------------------------------------------------------------------- |
+| `~/output/objects`               | autoware_auto_perception_msgs/msg/DetectedObjects.msg | 3D detected object with twist.                                                         |
+| `~/debug/low_confidence_objects` | autoware_auto_perception_msgs/msg/DetectedObjects.msg | 3D detected object that doesn't output as `~/output/objects` because of low confidence |
 
 ### Parameters
 

diff --git a/.../radar_fusion_to_detected_object/config/radar_object_fusion_to_detected_object.param.yaml b/.../radar_fusion_to_detected_object/config/radar_object_fusion_to_detected_object.param.yaml
@@ -13,4 +13,5 @@
       velocity_weight_target_value_average: 0.0
       velocity_weight_target_value_top: 0.0
       convert_doppler_to_twist: false
-      threshold_probability: 0.4
+      threshold_probability: 0.35
+      compensate_probability: false
diff --git a/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object.hpp b/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object.hpp
@@ -64,6 +64,7 @@ class RadarFusionToDetectedObject
     // Parameters for fixed object information
     bool convert_doppler_to_twist{};
     float threshold_probability{};
+    bool compensate_probability{};
   };
 
   struct RadarInput
@@ -83,6 +84,7 @@ class RadarFusionToDetectedObject
   struct Output
   {
     DetectedObjects objects{};
+    DetectedObjects debug_low_confidence_objects{};
   };
 
   void setParam(const Param & param);

diff --git a/...de/radar_object_fusion_to_detected_object/radar_object_fusion_to_detected_object_node.hpp b/...de/radar_object_fusion_to_detected_object/radar_object_fusion_to_detected_object_node.hpp
@@ -69,6 +69,7 @@ class RadarObjectFusionToDetectedObjectNode : public rclcpp::Node
 
   // Publisher
   rclcpp::Publisher<DetectedObjects>::SharedPtr pub_objects_{};
+  rclcpp::Publisher<DetectedObjects>::SharedPtr pub_debug_low_confidence_objects_{};
 
   // Parameter Server
   OnSetParametersCallbackHandle::SharedPtr set_param_res_;

diff --git a/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp b/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp
@@ -65,6 +65,7 @@ void RadarFusionToDetectedObject::setParam(const Param & param)
   // Parameters for fixing object information
   param_.threshold_probability = param.threshold_probability;
   param_.convert_doppler_to_twist = param.convert_doppler_to_twist;
+  param_.compensate_probability = param.compensate_probability;
 }
 
 RadarFusionToDetectedObject::Output RadarFusionToDetectedObject::update(
@@ -112,9 +113,13 @@ RadarFusionToDetectedObject::Output RadarFusionToDetectedObject::update(
 
       // Delete objects with low probability
       if (isQualified(split_object, radars_within_split_object)) {
-        split_object.classification.at(0).probability =
-          std::max(split_object.classification.at(0).probability, param_.threshold_probability);
+        if (param_.compensate_probability) {
+          split_object.existence_probability =
+            std::max(split_object.existence_probability, param_.threshold_probability);
+        }
         output.objects.objects.emplace_back(split_object);
+      } else {
+        output.debug_low_confidence_objects.objects.emplace_back(split_object);
       }
     }
   }

diff --git a/...dar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp b/...dar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp
@@ -85,6 +85,8 @@ RadarObjectFusionToDetectedObjectNode::RadarObjectFusionToDetectedObjectNode(
     declare_parameter<bool>("core_params.convert_doppler_to_twist", false);
   core_param_.threshold_probability =
     declare_parameter<float>("core_params.threshold_probability", 0.0);
+  core_param_.compensate_probability =
+    declare_parameter<bool>("core_params.compensate_probability", false);
 
   // Core
   radar_fusion_to_detected_object_ = std::make_unique<RadarFusionToDetectedObject>(get_logger());
@@ -102,6 +104,8 @@ RadarObjectFusionToDetectedObjectNode::RadarObjectFusionToDetectedObjectNode(
 
   // Publisher
   pub_objects_ = create_publisher<DetectedObjects>("~/output/objects", 1);
+  pub_debug_low_confidence_objects_ =
+    create_publisher<DetectedObjects>("~/debug/low_confidence_objects", 1);
 }
 
 rcl_interfaces::msg::SetParametersResult RadarObjectFusionToDetectedObjectNode::onSetParam(
@@ -134,6 +138,9 @@ rcl_interfaces::msg::SetParametersResult RadarObjectFusionToDetectedObjectNode::
         p.velocity_weight_target_value_average);
       update_param(
         params, "core_params.velocity_weight_target_value_top", p.velocity_weight_target_value_top);
+      update_param(params, "core_params.convert_doppler_to_twist", p.convert_doppler_to_twist);
+      update_param(params, "core_params.threshold_probability", p.threshold_probability);
+      update_param(params, "core_params.compensate_probability", p.compensate_probability);
 
       // Set parameter to instance
       if (radar_fusion_to_detected_object_) {
@@ -173,7 +180,6 @@ bool RadarObjectFusionToDetectedObjectNode::isDataReady()
   return true;
 }
 
-// void RadarObjectFusionToDetectedObjectNode::onTimer()
 void RadarObjectFusionToDetectedObjectNode::onData(
   const DetectedObjects::ConstSharedPtr object_msg, const TrackedObjects::ConstSharedPtr radar_msg)
 {
@@ -201,6 +207,7 @@ void RadarObjectFusionToDetectedObjectNode::onData(
   // Update
   output_ = radar_fusion_to_detected_object_->update(input);
   pub_objects_->publish(output_.objects);
+  pub_debug_low_confidence_objects_->publish(output_.debug_low_confidence_objects);
 }
 
 RadarFusionToDetectedObject::RadarInput RadarObjectFusionToDetectedObjectNode::setRadarInput(