Merge pull request #1060 from YoshiRi/feat/add_tracking_object_merger…

…_v11_1 chore(tracking_object_merger): add tracking object merger from awf/main
tier4 · Dec 7, 2023 · f408ae8 · f408ae8
2 parents 52f1ab1 + f3d1cbf
commit f408ae8
Show file tree

Hide file tree

Showing 24 changed files with 3,598 additions and 0 deletions.
diff --git a/perception/tracking_object_merger/CMakeLists.txt b/perception/tracking_object_merger/CMakeLists.txt
@@ -0,0 +1,44 @@
+cmake_minimum_required(VERSION 3.8)
+project(tracking_object_merger VERSION 1.0.0)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic -Werror -Wconversion)
+endif()
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+
+# 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(decorative_tracker_merger_node SHARED
+  src/data_association/data_association.cpp
+  src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp
+  src/decorative_tracker_merger.cpp
+  src/utils/utils.cpp
+  src/utils/tracker_state.cpp
+)
+
+target_link_libraries(decorative_tracker_merger_node
+  Eigen3::Eigen
+)
+
+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.
diff --git a/perception/tracking_object_merger/config/data_association_matrix.param.yaml b/perception/tracking_object_merger/config/data_association_matrix.param.yaml
@@ -0,0 +1,168 @@
+/**:
+  ros__parameters:
+    lidar-lidar:
+      can_assign_matrix:
+        #UNKNOWN, CAR, TRUCK, BUS,  TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN
+        [0,       0,   0,     0,    0,       0,         0,      0,         #UNKNOWN
+        0,       1,   1,     1,    1,       0,         0,      0,         #CAR
+        0,       1,   1,     1,    1,       0,         0,      0,         #TRUCK
+        0,       1,   1,     1,    1,       0,         0,      0,         #BUS
+        0,       1,   1,     1,    1,       0,         0,      0,         #TRAILER
+        0,       0,   0,     0,    0,       1,         1,      1,         #MOTORBIKE
+        0,       0,   0,     0,    0,       1,         1,      1,         #BICYCLE
+        0,       0,   0,     0,    0,       1,         1,      1]         #PEDESTRIAN
+
+      max_dist_matrix:
+        #UNKNOWN, CAR, TRUCK, BUS,  TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN
+        [4.0,     4.0, 5.0,   5.0,  5.0,     2.0,       2.0,     2.0,       #UNKNOWN
+        4.0,     2.0, 5.0,   5.0,  5.0,     1.0,       1.0,     1.0,       #CAR
+        5.0,     5.0, 5.0,   5.0,  5.0,     1.0,       1.0,     1.0,       #TRUCK
+        5.0,     5.0, 5.0,   5.0,  5.0,     1.0,       1.0,     1.0,       #BUS
+        5.0,     5.0, 5.0,   5.0,  5.0,     1.0,       1.0,     1.0,       #TRAILER
+        2.0,     1.0, 1.0,   1.0,  1.0,     3.0,       3.0,     3.0,       #MOTORBIKE
+        2.0,     1.0, 1.0,   1.0,  1.0,     3.0,       3.0,     3.0,       #BICYCLE
+        2.0,     1.0, 1.0,   1.0,  1.0,     3.0,       3.0,     2.0]       #PEDESTRIAN
+
+      max_rad_matrix: # If value is greater than pi, it will be ignored.
+        #UNKNOWN, CAR,   TRUCK, BUS,    TRAILER MOTORBIKE, BICYCLE, PEDESTRIAN
+        [3.150,   3.150, 3.150, 3.150,  3.150,  3.150,     3.150,   3.150,      #UNKNOWN
+        3.150,   1.047, 1.047, 1.047,  1.047,  3.150,     3.150,   3.150,      #CAR
+        3.150,   1.047, 1.047, 1.047,  1.047,  3.150,     3.150,   3.150,      #TRUCK
+        3.150,   1.047, 1.047, 1.047,  1.047,  3.150,     3.150,   3.150,      #BUS
+        3.150,   1.047, 1.047, 1.047,  1.047,  3.150,     3.150,   3.150,      #TRAILER
+        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,      #BICYCLE
+        3.150,   3.150, 3.150, 3.150,  3.150,  3.150,     3.150,   3.150]      #PEDESTRIAN
+
+      max_velocity_diff_matrix: # Ignored when value is larger than 100.0
+        #UNKNOWN, CAR, TRUCK, BUS,  TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN
+        [100.0,   100.0,100.0,100.0,100.0,   100.0,     100.0,   100.0,     #UNKNOWN
+        100.0,   8.0,    8.0,  8.0,  8.0,   100.0,     100.0,   100.0,     #CAR
+        100.0,   8.0,    8.0,  8.0,  8.0,   100.0,     100.0,   100.0,     #TRUCK
+        100.0,   8.0,    8.0,  8.0,  8.0,   100.0,     100.0,   100.0,     #BUS
+        100.0,   8.0,    8.0,  8.0,  8.0,   100.0,     100.0,   100.0,     #TRAILER
+        100.0,   100.0,100.0,100.0,100.0,   100.0,     100.0,   100.0,     #MOTORBIKE
+        100.0,   100.0,100.0,100.0,100.0,   100.0,     100.0,   100.0,     #BICYCLE
+        100.0,   100.0,100.0,100.0,100.0,   100.0,     100.0,   100.0]     #PEDESTRIAN
+
+      min_iou_matrix: # If value is negative, it will be ignored.
+        #UNKNOWN, CAR,   TRUCK, BUS,    TRAILER,  MOTORBIKE, BICYCLE, PEDESTRIAN
+        [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.2,      0.1,       0.1,     0.1,       #CAR
+        0.1,     0.2,   0.3,   0.3,    0.3,      0.1,       0.1,     0.1,       #TRUCK
+        0.1,     0.2,   0.3,   0.3,    0.3,      0.1,       0.1,     0.1,       #BUS
+        0.1,     0.2,   0.3,   0.3,    0.3,      0.1,       0.1,     0.1,       #TRAILER
+        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,       #BICYCLE
+        0.1,     0.1,   0.1,   0.1,    0.1,      0.1,       0.1,     0.1]       #PEDESTRIAN
+
+    lidar-radar:
+      can_assign_matrix:
+        #UNKNOWN, CAR, TRUCK, BUS,  TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN
+        [0,       0,   0,     0,    0,       0,         0,      0,         #UNKNOWN
+        0,       1,   1,     1,    1,       0,         0,      0,         #CAR
+        0,       1,   1,     1,    1,       0,         0,      0,         #TRUCK
+        0,       1,   1,     1,    1,       0,         0,      0,         #BUS
+        0,       1,   1,     1,    1,       0,         0,      0,         #TRAILER
+        0,       0,   0,     0,    0,       1,         1,      1,         #MOTORBIKE
+        0,       0,   0,     0,    0,       1,         1,      1,         #BICYCLE
+        0,       0,   0,     0,    0,       1,         1,      1]         #PEDESTRIAN
+
+      max_dist_matrix:
+        #UNKNOWN, CAR, TRUCK, BUS,  TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN
+        [4.0,     4.0, 5.0,   5.0,  5.0,     3.5,       3.5,     3.5,       #UNKNOWN
+        4.0,     5.5, 6.0,   6.0,  6.0,     1.0,       1.0,     1.0,       #CAR
+        5.0,     6.0, 6.5,   6.5,  6.5,     1.0,       1.0,     1.0,       #TRUCK
+        5.0,     6.0, 6.5,   6.5,  6.5,     1.0,       1.0,     1.0,       #BUS
+        5.0,     6.0, 6.5,   6.5,  6.5,     1.0,       1.0,     1.0,       #TRAILER
+        3.5,     1.0, 1.0,   1.0,  1.0,     3.0,       3.0,     3.0,       #MOTORBIKE
+        3.5,     1.0, 1.0,   1.0,  1.0,     3.0,       3.0,     3.0,       #BICYCLE
+        3.5,     1.0, 1.0,   1.0,  1.0,     3.0,       3.0,     3.5]       #PEDESTRIAN
+
+      max_rad_matrix: # If value is greater than pi, it will be ignored.
+        #UNKNOWN, CAR,   TRUCK, BUS,    TRAILER MOTORBIKE, BICYCLE, PEDESTRIAN
+        [3.150,   3.150, 3.150, 3.150,  3.150,  3.150,     3.150,   3.150,      #UNKNOWN
+        3.150,   1.047, 1.047, 1.047,  1.047,  3.150,     3.150,   3.150,      #CAR
+        3.150,   1.047, 1.047, 1.047,  1.047,  3.150,     3.150,   3.150,      #TRUCK
+        3.150,   1.047, 1.047, 1.047,  1.047,  3.150,     3.150,   3.150,      #BUS
+        3.150,   1.047, 1.047, 1.047,  1.047,  3.150,     3.150,   3.150,      #TRAILER
+        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,      #BICYCLE
+        3.150,   3.150, 3.150, 3.150,  3.150,  3.150,     3.150,   3.150]      #PEDESTRIAN
+
+      max_velocity_diff_matrix: # Ignored when value is larger than 100.0
+        #UNKNOWN, CAR, TRUCK, BUS,  TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN
+        [100.0,   100.0,100.0,100.0,100.0,   100.0,     100.0,   100.0,     #UNKNOWN
+        100.0,   8.0,    8.0,  8.0,  8.0,   100.0,     100.0,   100.0,     #CAR
+        100.0,   8.0,    8.0,  8.0,  8.0,   100.0,     100.0,   100.0,     #TRUCK
+        100.0,   8.0,    8.0,  8.0,  8.0,   100.0,     100.0,   100.0,     #BUS
+        100.0,   8.0,    8.0,  8.0,  8.0,   100.0,     100.0,   100.0,     #TRAILER
+        100.0,   100.0,100.0,100.0,100.0,   100.0,     100.0,   100.0,     #MOTORBIKE
+        100.0,   100.0,100.0,100.0,100.0,   100.0,     100.0,   100.0,     #BICYCLE
+        100.0,   100.0,100.0,100.0,100.0,   100.0,     100.0,   100.0]     #PEDESTRIAN
+
+      min_iou_matrix: # set all value to 0.0 to disable this constraint
+        #UNKNOWN, CAR,   TRUCK, BUS,    TRAILER,  MOTORBIKE, BICYCLE, PEDESTRIAN
+        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #UNKNOWN
+         0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #CAR
+         0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #TRUCK
+         0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #BUS
+         0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #TRAILER
+         0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #MOTORBIKE
+         0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #BICYCLE
+         0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] #PEDESTRIAN
+
+    radar-radar:
+      can_assign_matrix:
+        #UNKNOWN, CAR, TRUCK, BUS,  TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN
+        [0,       0,   0,     0,    0,       0,         0,      0,         #UNKNOWN
+        0,       1,   1,     1,    1,       0,         0,      0,         #CAR
+        0,       1,   1,     1,    1,       0,         0,      0,         #TRUCK
+        0,       1,   1,     1,    1,       0,         0,      0,         #BUS
+        0,       1,   1,     1,    1,       0,         0,      0,         #TRAILER
+        0,       0,   0,     0,    0,       1,         1,      1,         #MOTORBIKE
+        0,       0,   0,     0,    0,       1,         1,      1,         #BICYCLE
+        0,       0,   0,     0,    0,       1,         1,      1]         #PEDESTRIAN
+
+      max_dist_matrix:
+        #UNKNOWN, CAR, TRUCK, BUS,  TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN
+        [4.0,     4.0, 5.0,   5.0,  5.0,     3.5,       3.5,     3.5,       #UNKNOWN
+        4.0,     7.0, 7.5,   7.5,  7.5,     1.0,       1.0,     1.0,       #CAR
+        5.0,     7.5, 7.5,   7.5,  7.5,     1.0,       1.0,     1.0,       #TRUCK
+        5.0,     7.5, 7.5,   7.5,  7.5,     1.0,       1.0,     1.0,       #BUS
+        5.0,     7.5, 7.5,   7.5,  7.5,     1.0,       1.0,     1.0,       #TRAILER
+        3.5,     1.0, 1.0,   1.0,  1.0,     3.0,       3.0,     3.0,       #MOTORBIKE
+        3.5,     1.0, 1.0,   1.0,  1.0,     3.0,       3.0,     3.0,       #BICYCLE
+        3.5,     1.0, 1.0,   1.0,  1.0,     3.0,       3.0,     3.5]       #PEDESTRIAN
+      max_rad_matrix: # If value is greater than pi, it will be ignored.
+        #UNKNOWN, CAR,   TRUCK, BUS,    TRAILER MOTORBIKE, BICYCLE, PEDESTRIAN
+        [3.150,   3.150, 3.150, 3.150,  3.150,  3.150,     3.150,   3.150,      #UNKNOWN
+        3.150,   1.047, 1.047, 1.047,  1.047,  3.150,     3.150,   3.150,      #CAR
+        3.150,   1.047, 1.047, 1.047,  1.047,  3.150,     3.150,   3.150,      #TRUCK
+        3.150,   1.047, 1.047, 1.047,  1.047,  3.150,     3.150,   3.150,      #BUS
+        3.150,   1.047, 1.047, 1.047,  1.047,  3.150,     3.150,   3.150,      #TRAILER
+        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,      #BICYCLE
+        3.150,   3.150, 3.150, 3.150,  3.150,  3.150,     3.150,   3.150]      #PEDESTRIAN
+
+      max_velocity_diff_matrix: # Ignored when value is larger than 100.0
+        #UNKNOWN, CAR, TRUCK, BUS,  TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN
+        [100.0,   100.0,100.0,100.0,100.0,   100.0,     100.0,   100.0,     #UNKNOWN
+        100.0,   8.0,    8.0,  8.0,  8.0,   100.0,     100.0,   100.0,     #CAR
+        100.0,   8.0,    8.0,  8.0,  8.0,   100.0,     100.0,   100.0,     #TRUCK
+        100.0,   8.0,    8.0,  8.0,  8.0,   100.0,     100.0,   100.0,     #BUS
+        100.0,   8.0,    8.0,  8.0,  8.0,   100.0,     100.0,   100.0,     #TRAILER
+        100.0,   100.0,100.0,100.0,100.0,   100.0,     100.0,   100.0,     #MOTORBIKE
+        100.0,   100.0,100.0,100.0,100.0,   100.0,     100.0,   100.0,     #BICYCLE
+        100.0,   100.0,100.0,100.0,100.0,   100.0,     100.0,   100.0]     #PEDESTRIAN
+
+      min_iou_matrix: # set all value to 0.0 to disable this constraint
+        #UNKNOWN, CAR,   TRUCK, BUS,    TRAILER,  MOTORBIKE, BICYCLE, PEDESTRIAN
+        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #UNKNOWN
+         0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #CAR
+         0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #TRUCK
+         0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #BUS
+         0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #TRAILER
+         0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #MOTORBIKE
+         0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #BICYCLE
+         0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] #PEDESTRIAN