autowarefoundation · YoshiRi · Jul 14, 2023 · Jul 14, 2023 · Jul 18, 2023 · Jul 18, 2023
@@ -2,11 +2,21 @@
 <launch>
   <arg name="use_vector_map" default="false" description="use vector map in prediction"/>
   <arg name="param_path" default="$(find-pkg-share map_based_prediction)/config/map_based_prediction.param.yaml"/>
+  <arg name="output_topic" default="/perception/object_recognition/objects"/>
+
+  <!-- "use_tracking_merger" controls wheather to join radar far tracking objects to lidar tracking objects.
+      Prediction input will changed to merged tracking objects if "use_tracking_merger" is true. -->
+  <arg name="use_tracking_merger" default="false" description="flag to use tracking merger in prediction"/>
+  <arg name="default_tracking_objects" default="/perception/object_recognition/tracking/objects"/>
+  <arg name="merged_tracking_objects" default="/perception/object_recognition/tracking/merged/objects"/>
+  <let name="input_topic_to_prediction" value="$(var merged_tracking_objects)" if="$(var use_tracking_merger)"/>
+  <let name="input_topic_to_prediction" value="$(var default_tracking_objects)" unless="$(var use_tracking_merger)"/>
 
   <group if="$(var use_vector_map)">
     <set_remap from="objects" to="/perception/object_recognition/objects"/>
     <include file="$(find-pkg-share map_based_prediction)/launch/map_based_prediction.launch.xml">
-      <arg name="output_topic" value="/perception/object_recognition/objects"/>
+      <arg name="output_topic" value="$(var output_topic)"/>
+      <arg name="input_topic" value="$(var input_topic_to_prediction)"/>
       <arg name="param_path" value="$(var object_recognition_prediction_map_based_prediction_param_path)"/>
     </include>
   </group>

@@ -1,11 +1,19 @@
 <?xml version="1.0"?>
 <launch>
+  <!-- Arg with default value -->
   <arg name="mode" default="lidar" description="options: `camera_lidar_radar_fusion`, `camera_lidar_fusion`, `lidar_radar_fusion`, `lidar` or `radar`"/>
   <arg name="publish_rate" default="10.0"/>
   <arg name="enable_delay_compensation" default="true"/>
   <arg name="radar_tracker_input" default="/perception/object_recognition/detection/radar/far_objects"/>
   <arg name="radar_tracker_output" default="/perception/object_recognition/tracking/radar/far_objects"/>
 
+  <!-- Arg without default value -->
+  <arg name="object_recognition_tracking_radar_object_tracker_data_association_matrix_param_path" description="association param file for radar far object tracking"/>
+  <arg name="object_recognition_tracking_radar_object_tracker_tracking_setting_param_path" description="tracking setting param file for radar far object tracking"/>
+  <arg name="object_recognition_tracking_radar_object_tracker_node_param_path" description="node param file for radar far object tracking"/>
+  <arg name="object_recognition_tracking_object_merger_data_association_matrix_param_path" description="association param file for radar and lidar object merger"/>
+  <arg name="object_recognition_tracking_object_merger_node_param_path" description="node param file for radar and lidar object merger"/>
+
   <group>
     <!--multi object tracking-->
     <include file="$(find-pkg-share multi_object_tracker)/launch/multi_object_tracker.launch.xml">
@@ -24,6 +32,16 @@
         <arg name="output" value="$(var radar_tracker_output)"/>
         <arg name="data_association_matrix_path" value="$(var object_recognition_tracking_radar_object_tracker_data_association_matrix_param_path)"/>
         <arg name="tracker_setting_path" value="$(var object_recognition_tracking_radar_object_tracker_tracking_setting_param_path)"/>
+        <arg name="radar_object_tracker_param_path" value="$(var object_recognition_tracking_radar_object_tracker_node_param_path)"/>
+      </include>
+
+      <!--radar long range dynamic object tracking merger-->
+      <include file="$(find-pkg-share tracking_object_merger)/launch/decorative_tracker_merger.launch.xml">
+        <arg name="main_object" value="/perception/object_recognition/tracking/objects"/>
+        <arg name="sub_object" value="$(var radar_tracker_output)"/>
+        <arg name="output" value="/perception/object_recognition/tracking/merged/objects"/>
+        <arg name="data_association_matrix_path" value="$(var object_recognition_tracking_radar_object_tracker_data_association_matrix_param_path)"/>
+        <arg name="node_param_file_path" value="$(find-pkg-share tracking_object_merger)/config/decorative_tracker_merger.param.yaml"/>
       </include>
     </group>
     -->

@@ -15,6 +15,9 @@
   <arg name="object_recognition_tracking_multi_object_tracker_data_association_matrix_param_path"/>
   <arg name="object_recognition_tracking_radar_object_tracker_data_association_matrix_param_path"/>
   <arg name="object_recognition_tracking_radar_object_tracker_tracking_setting_param_path"/>
+  <arg name="object_recognition_tracking_radar_object_tracker_node_param_path"/>
+  <arg name="object_recognition_tracking_object_merger_data_association_matrix_param_path"/>
+  <arg name="object_recognition_tracking_object_merger_node_param_path"/>
   <arg name="obstacle_segmentation_ground_segmentation_param_path"/>
   <arg name="obstacle_segmentation_ground_segmentation_elevation_map_param_path"/>
   <arg name="object_recognition_detection_obstacle_pointcloud_based_validator_param_path"/>
@@ -189,6 +192,7 @@
         <push-ros-namespace namespace="prediction"/>
         <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/prediction/prediction.launch.xml">
           <arg name="use_vector_map" value="$(var use_vector_map)"/>
+          <arg name="use_tracking_merger" value="true"/>
         </include>
       </group>
     </group>

diff --git a/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp b/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp
@@ -214,59 +214,56 @@ RadarObjectTrackerNode::RadarObjectTrackerNode(const rclcpp::NodeOptions & node_
   logging_.path = declare_parameter<std::string>("logging_file_path");
 
   // noise filter
-  use_distance_based_noise_filtering_ =
-    declare_parameter<bool>("use_distance_based_noise_filtering");
+  declare_parameter<bool>("use_distance_based_noise_filtering");
   use_map_based_noise_filtering_ = declare_parameter<bool>("use_map_based_noise_filtering");
   minimum_range_threshold_ = declare_parameter<double>("minimum_range_threshold");
   max_distance_from_lane_ = declare_parameter<double>("max_distance_from_lane");
   max_angle_diff_from_lane_ = declare_parameter<double>("max_angle_diff_from_lane");
   max_lateral_velocity_ = declare_parameter<double>("max_lateral_velocity");
 
-  // Load tracking config file
-  if (tracker_config_directory_.empty()) {
-    tracker_config_directory_ =
-      ament_index_cpp::get_package_share_directory("radar_object_tracker") + "/config/tracking/";
-  }
+  tracker_config_directory_ =
+    ament_index_cpp::get_package_share_directory("radar_object_tracker") + "/config/tracking/";
+}
 
-  auto cti = std::make_shared<tf2_ros::CreateTimerROS>(
-    this->get_node_base_interface(), this->get_node_timers_interface());
-  tf_buffer_.setCreateTimerInterface(cti);
+auto cti = std::make_shared<tf2_ros::CreateTimerROS>(
+  this->get_node_base_interface(), this->get_node_timers_interface());
+tf_buffer_.setCreateTimerInterface(cti);
 
-  // Create ROS time based timer
-  if (enable_delay_compensation) {
-    const auto period_ns = rclcpp::Rate(publish_rate).period();
-    publish_timer_ = rclcpp::create_timer(
-      this, get_clock(), period_ns, std::bind(&RadarObjectTrackerNode::onTimer, this));
-  }
+// Create ROS time based timer
+if (enable_delay_compensation) {
+  const auto period_ns = rclcpp::Rate(publish_rate).period();
+  publish_timer_ = rclcpp::create_timer(
+    this, get_clock(), period_ns, std::bind(&RadarObjectTrackerNode::onTimer, this));
+}
 
-  const auto tmp = this->declare_parameter<std::vector<int64_t>>("can_assign_matrix");
-  const std::vector<int> can_assign_matrix(tmp.begin(), tmp.end());
-
-  // import association matrices
-  const auto max_dist_matrix = this->declare_parameter<std::vector<double>>("max_dist_matrix");
-  const auto max_area_matrix = this->declare_parameter<std::vector<double>>("max_area_matrix");
-  const auto min_area_matrix = this->declare_parameter<std::vector<double>>("min_area_matrix");
-  const auto max_rad_matrix = this->declare_parameter<std::vector<double>>("max_rad_matrix");
-  const auto min_iou_matrix = this->declare_parameter<std::vector<double>>("min_iou_matrix");
-  data_association_ = std::make_unique<DataAssociation>(
-    can_assign_matrix, max_dist_matrix, max_area_matrix, min_area_matrix, max_rad_matrix,
-    min_iou_matrix);
-
-  // tracker map
-  tracker_map_.insert(
-    std::make_pair(Label::CAR, this->declare_parameter<std::string>("car_tracker")));
-  tracker_map_.insert(
-    std::make_pair(Label::TRUCK, this->declare_parameter<std::string>("truck_tracker")));
-  tracker_map_.insert(
-    std::make_pair(Label::BUS, this->declare_parameter<std::string>("bus_tracker")));
-  tracker_map_.insert(
-    std::make_pair(Label::TRAILER, this->declare_parameter<std::string>("trailer_tracker")));
-  tracker_map_.insert(
-    std::make_pair(Label::PEDESTRIAN, this->declare_parameter<std::string>("pedestrian_tracker")));
-  tracker_map_.insert(
-    std::make_pair(Label::BICYCLE, this->declare_parameter<std::string>("bicycle_tracker")));
-  tracker_map_.insert(
-    std::make_pair(Label::MOTORCYCLE, this->declare_parameter<std::string>("motorcycle_tracker")));
+const auto tmp = this -> declare_parameter<std::vector<int64_t>>("can_assign_matrix");
+const std::vector<int> can_assign_matrix(tmp.begin(), tmp.end());
+
+// import association matrices
+const auto max_dist_matrix = this -> declare_parameter<std::vector<double>>("max_dist_matrix");
+const auto max_area_matrix = this -> declare_parameter<std::vector<double>>("max_area_matrix");
+const auto min_area_matrix = this -> declare_parameter<std::vector<double>>("min_area_matrix");
+const auto max_rad_matrix = this -> declare_parameter<std::vector<double>>("max_rad_matrix");
+const auto min_iou_matrix = this -> declare_parameter<std::vector<double>>("min_iou_matrix");
+data_association_ = std::make_unique<DataAssociation>(
+  can_assign_matrix, max_dist_matrix, max_area_matrix, min_area_matrix, max_rad_matrix,
+  min_iou_matrix);
+
+// tracker map
+tracker_map_.insert(
+  std::make_pair(Label::CAR, this->declare_parameter<std::string>("car_tracker")));
+tracker_map_.insert(
+  std::make_pair(Label::TRUCK, this->declare_parameter<std::string>("truck_tracker")));
+tracker_map_.insert(
+  std::make_pair(Label::BUS, this->declare_parameter<std::string>("bus_tracker")));
+tracker_map_.insert(
+  std::make_pair(Label::TRAILER, this->declare_parameter<std::string>("trailer_tracker")));
+tracker_map_.insert(
+  std::make_pair(Label::PEDESTRIAN, this->declare_parameter<std::string>("pedestrian_tracker")));
+tracker_map_.insert(
+  std::make_pair(Label::BICYCLE, this->declare_parameter<std::string>("bicycle_tracker")));
+tracker_map_.insert(
+  std::make_pair(Label::MOTORCYCLE, this->declare_parameter<std::string>("motorcycle_tracker")));
 }
 
 // load map information to node parameter

diff --git a/perception/tracking_object_merger/CMakeLists.txt b/perception/tracking_object_merger/CMakeLists.txt
@@ -0,0 +1,50 @@
+cmake_minimum_required(VERSION 3.8)
+project(tracking_object_merger)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+find_package(nlohmann_json REQUIRED) # for debug
+
+
+# find dependencies
+find_package(eigen3_cmake_module REQUIRED)
+find_package(Eigen3 REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+include_directories(
+  SYSTEM
+    ${EIGEN3_INCLUDE_DIR}
+)
+
+ament_auto_add_library(mu_successive_shortest_path SHARED
+  src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp
+)
+
+ament_auto_add_library(decorative_tracker_merger_node SHARED
+  src/data_association/data_association.cpp
+  src/decorative_tracker_merger.cpp
+  src/utils/utils.cpp
+  src/utils/tracker_state.cpp
+)
+
+target_link_libraries(decorative_tracker_merger_node
+  mu_successive_shortest_path
+  Eigen3::Eigen
+  nlohmann_json::nlohmann_json # for debug
+)
+
+rclcpp_components_register_node(decorative_tracker_merger_node
+  PLUGIN "tracking_object_merger::DecorativeTrackerMergerNode"
+  EXECUTABLE decorative_tracker_merger
+)
+
+ament_auto_package(INSTALL_TO_SHARE
+  launch
+  config
+)
diff --git a/perception/tracking_object_merger/README.md b/perception/tracking_object_merger/README.md
@@ -0,0 +1,114 @@
+# Tracking Object Merger
+
+## Purpose
+
+This package try to merge two tracking objects from different sensor.
+
+## Inner-workings / Algorithms
+
+Merging tracking objects from different sensor is a combination of data association and state fusion algorithms.
+
+Detailed process depends on the merger policy.
+
+### decorative_tracker_merger
+
+In decorative_tracker_merger, we assume there are dominant tracking objects and sub tracking objects.
+The name `decorative` means that sub tracking objects are used to complement the main objects.
+
+Usually the dominant tracking objects are from LiDAR and sub tracking objects are from Radar or Camera.
+
+Here show the processing pipeline.
+
+![decorative_tracker_merger](./image/decorative_tracker_merger.drawio.svg)
+
+#### time sync
+
+Sub object(Radar or Camera) often has higher frequency than dominant object(LiDAR). So we need to sync the time of sub object to dominant object.
+
+![time sync](image/time_sync.drawio.svg)
+
+#### data association
+
+In the data association, we use the following rules to determine whether two tracking objects are the same object.
+
+- gating
+  - `distance gate`: distance between two tracking objects
+  - `angle gate`: angle between two tracking objects
+  - `mahalanobis_distance_gate`: Mahalanobis distance between two tracking objects
+  - `min_iou_gate`: minimum IoU between two tracking objects
+  - `max_velocity_gate`: maximum velocity difference between two tracking objects
+- score
+  - score used in matching is equivalent to the distance between two tracking objects
+
+#### tracklet update
+
+Sub tracking objects are merged into dominant tracking objects.
+
+Depends on the tracklet input sensor state, we update the tracklet state with different rules.
+
+| state\priority             | 1st    | 2nd   | 3rd    |
+| -------------------------- | ------ | ----- | ------ |
+| Kinematics except velocity | LiDAR  | Radar | Camera |
+| Forward velocity           | Radar  | LiDAR | Camera |
+| Object classification      | Camera | LiDAR | Radar  |
+
+#### tracklet management
+
+We use the `existence_probability` to manage tracklet.
+
+- When we create a new tracklet, we set the `existence_probability` to $p_{sensor}$ value.
+- In each update with specific sensor, we set the `existence_probability` to $p_{sensor}$ value.
+- When tracklet does not have update with specific sensor, we reduce the `existence_probability` by `decay_rate`
+- Object can be published if `existence_probability` is larger than `publish_probability_threshold`
+- Object will be removed if `existence_probability` is smaller than `remove_probability_threshold`
+
+![tracklet_management](./image/tracklet_management.drawio.svg)
+
+These parameter can be set in `config/decorative_tracker_merger.param.yaml`.
+
+```yaml
+tracker_state_parameter:
+  remove_probability_threshold: 0.3
+  publish_probability_threshold: 0.6
+  default_lidar_existence_probability: 0.7
+  default_radar_existence_probability: 0.6
+  default_camera_existence_probability: 0.6
+  decay_rate: 0.1
+  max_dt: 1.0
+```
+
+#### input/parameters
+
+| topic name                      | message type                                    | description                                                                           |
+| ------------------------------- | ----------------------------------------------- | ------------------------------------------------------------------------------------- |
+| `~input/main_object`            | `autoware_auto_perception_msgs::TrackedObjects` | Dominant tracking objects. Output will be published with this dominant object stamps. |
+| `~input/sub_object`             | `autoware_auto_perception_msgs::TrackedObjects` | Sub tracking objects.                                                                 |
+| `output/object`                 | `autoware_auto_perception_msgs::TrackedObjects` | Merged tracking objects.                                                              |
+| `debug/interpolated_sub_object` | `autoware_auto_perception_msgs::TrackedObjects` | Interpolated sub tracking objects.                                                    |
+
+Default parameters are set in [config/decorative_tracker_merger.param.yaml](./config/decorative_tracker_merger.param.yaml).
+
+| parameter name            | description                                                                                                                                                      | default value |
+| ------------------------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- |
+| `base_link_frame_id`      | base link frame id. This is used to transform the tracking object.                                                                                               | "base_link"   |
+| `time_sync_threshold`     | time sync threshold. If the time difference between two tracking objects is smaller than this value, we consider these two tracking objects are the same object. | 0.05          |
+| `sub_object_timeout_sec`  | sub object timeout. If the sub object is not updated for this time, we consider this object is not exist.                                                        | 0.5           |
+| `main_sensor_type`        | main sensor type. This is used to determine the dominant tracking object.                                                                                        | "lidar"       |
+| `sub_sensor_type`         | sub sensor type. This is used to determine the sub tracking object.                                                                                              | "radar"       |
+| `tracker_state_parameter` | tracker state parameter. This is used to manage the tracklet.                                                                                                    |               |
+
+- the detail of `tracker_state_parameter` is described in [tracklet management](#tracklet-management)
+
+#### tuning
+
+As explained in [tracklet management](#tracklet-management), this tracker merger tend to maintain the both input tracking objects.
+
+If there are many false positive tracking objects,
+
+- decrease `default_<sensor>_existence_probability` of that sensor
+- increase `decay_rate`
+- increase `publish_probability_threshold` to publish only reliable tracking objects
+
+### equivalent_tracker_merger
+
+This is future work.