feat(radar_fusion_to_detected_object): add yaw gate

perception/radar_fusion_to_detected_object/README.md

@@ -13,10 +13,11 @@ The document of core algorithm is [here](docs/algorithm.md)
 
 ### Parameters for sensor fusion
 
-| Name                     | Type   | Description                                                                                                                                                                | Default value |
-| :----------------------- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
-| bounding_box_margin      | double | The distance to extend the 2D bird's-eye view Bounding Box on each side. This distance is used as a threshold to find radar centroids falling inside the extended box. [m] | 2.0           |
-| split_threshold_velocity | double | The object's velocity threshold to decide to split for two objects from radar information (currently not implemented) [m/s]                                                | 5.0           |
+| Name                     | Type   | Description                                                                                                                                                                                                                                                                      | Default value |
+| :----------------------- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
+| bounding_box_margin      | double | The distance to extend the 2D bird's-eye view Bounding Box on each side. This distance is used as a threshold to find radar centroids falling inside the extended box. [m]                                                                                                       | 2.0           |
+| split_threshold_velocity | double | The object's velocity threshold to decide to split for two objects from radar information (currently not implemented) [m/s]                                                                                                                                                      | 5.0           |
+| threshold_yaw_diff       | double | The yaw orientation threshold. If $ \vert \theta _{ob} - \theta_ {ra} \vert < threshold*yaw_diff $ attached to radar information include estimated velocity, where $ \theta*{ob} $ is yaw angle from 3d detected object, $ \theta\_ {ra} $ is yaw angle from radar object. [rad] | 0.35          |
 
 ### Weight parameters for velocity estimation
 

perception/radar_fusion_to_detected_object/config/radar_object_fusion_to_detected_object.param.yaml

@@ -6,6 +6,7 @@
     core_params:
       bounding_box_margin: 2.0
       split_threshold_velocity: 5.0
+      threshold_yaw_diff: 0.35
       velocity_weight_average: 0.0
       velocity_weight_median: 0.0
       velocity_weight_min_distance: 1.0

perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object.hpp

@@ -48,6 +48,7 @@ class RadarFusionToDetectedObject
     // Radar fusion param
     double bounding_box_margin{};
     double split_threshold_velocity{};
+    double threshold_yaw_diff{};
 
     // Weight param for velocity estimation
     double velocity_weight_average{};
@@ -97,6 +98,7 @@ class RadarFusionToDetectedObject
     const DetectedObject & object, std::shared_ptr<std::vector<RadarInput>> & radars);
   TwistWithCovariance convertDopplerToTwist(
     const DetectedObject & object, const TwistWithCovariance & twist_with_covariance);
+  bool isYawCorrect(const double & yaw, const double & yaw_threshold);
   Twist addTwist(const Twist & twist_1, const Twist & twist_2);
   Twist scaleTwist(const Twist & twist, const double scale);
   double getTwistNorm(const Twist & twist);

perception/radar_fusion_to_detected_object/launch/radar_object_fusion_to_detected_object.launch.xml

@@ -12,6 +12,6 @@
     <remap from="~/input/objects" to="$(var input/objects)"/>
     <remap from="~/input/radars" to="$(var input/radars)"/>
     <remap from="~/output/objects" to="$(var output/objects)"/>
-    <!--<param from="$(var config_file)" /> -->
+    <param from="$(var config_file)"/>
   </node>
 </launch>

perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp

@@ -40,6 +40,7 @@ void RadarFusionToDetectedObject::setParam(const Param & param)
   // Radar fusion param
   param_.bounding_box_margin = param.bounding_box_margin;
   param_.split_threshold_velocity = param.split_threshold_velocity;
+  param_.threshold_yaw_diff = param.split_threshold_velocity;
 
   // normalize weight param
   double sum_weight = param.velocity_weight_median + param.velocity_weight_min_distance +
@@ -97,12 +98,19 @@ RadarFusionToDetectedObject::Output RadarFusionToDetectedObject::update(
         // If object is split, then filter radar again
         radars_within_split_object = filterRadarWithinObject(object, radars_within_object);
       }
-
       // Estimate twist of object
       if (!radars_within_split_object || !(*radars_within_split_object).empty()) {
-        split_object.kinematics.has_twist = true;
-        split_object.kinematics.twist_with_covariance =
+        TwistWithCovariance twist_with_covariance =
           estimateTwist(split_object, radars_within_split_object);
+        const double twist_yaw = tier4_autoware_utils::normalizeRadian(
+          std::atan2(twist_with_covariance.twist.linear.y, twist_with_covariance.twist.linear.x));
+        const double object_yaw = tier4_autoware_utils::normalizeRadian(
+          tf2::getYaw(split_object.kinematics.pose_with_covariance.pose.orientation));
+        const double diff_yaw = tier4_autoware_utils::normalizeRadian(twist_yaw - object_yaw);
+        if (isYawCorrect(diff_yaw, param_.threshold_yaw_diff)) {
+          split_object.kinematics.twist_with_covariance = twist_with_covariance;
+          split_object.kinematics.has_twist = true;
+        }
       }
 
       // Delete objects with low probability
@@ -116,6 +124,20 @@ RadarFusionToDetectedObject::Output RadarFusionToDetectedObject::update(
   return output;
 }
 
+// Judge whether object's yaw is same direction with twist's yaw.
+// This function improve multi object tracking with observed speed.
+bool RadarFusionToDetectedObject::isYawCorrect(const double & yaw, const double & yaw_threshold)
+{
+  double normalized_yaw = tier4_autoware_utils::normalizeRadian(yaw);
+  if (std::abs(normalized_yaw) < yaw_threshold) {
+    return true;
+  } else if (M_PI - yaw_threshold < std::abs(normalized_yaw)) {
+    return true;
+  } else {
+    return false;
+  }
+}
+
 // Choose radar pointcloud/objects within 3D bounding box from lidar-base detection with margin
 // space from bird's-eye view.
 std::shared_ptr<std::vector<RadarFusionToDetectedObject::RadarInput>>
@@ -249,7 +271,7 @@ TwistWithCovariance RadarFusionToDetectedObject::estimateTwist(
   return twist_with_covariance;
 }
 
-// Jugde wether low confidence objects that do not have some radar points/objects or not.
+// Judge whether low confidence objects that do not have some radar points/objects or not.
 bool RadarFusionToDetectedObject::isQualified(
   const DetectedObject & object, std::shared_ptr<std::vector<RadarInput>> & radars)
 {

perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp

@@ -69,6 +69,8 @@ RadarObjectFusionToDetectedObjectNode::RadarObjectFusionToDetectedObjectNode(
     declare_parameter<double>("core_params.bounding_box_margin", 0.5);
   core_param_.split_threshold_velocity =
     declare_parameter<double>("core_params.split_threshold_velocity", 0.0);
+  core_param_.threshold_yaw_diff =
+    declare_parameter<double>("core_params.threshold_yaw_diff", 0.35);
   core_param_.velocity_weight_average =
     declare_parameter<double>("core_params.velocity_weight_average", 0.0);
   core_param_.velocity_weight_median =
@@ -136,6 +138,7 @@ rcl_interfaces::msg::SetParametersResult RadarObjectFusionToDetectedObjectNode::
       // Update params
       update_param(params, "core_params.bounding_box_margin", p.bounding_box_margin);
       update_param(params, "core_params.split_threshold_velocity", p.split_threshold_velocity);
+      update_param(params, "core_params.threshold_yaw_diff", p.threshold_yaw_diff);
       update_param(params, "core_params.velocity_weight_average", p.velocity_weight_average);
       update_param(params, "core_params.velocity_weight_median", p.velocity_weight_median);
       update_param(