add tracking object merger for long range radar sensor (#627)

* add tracking object merger paramters Signed-off-by: yoshiri <[email protected]> * fix typo Signed-off-by: yoshiri <[email protected]> --------- Signed-off-by: yoshiri <[email protected]>
tier4 · Oct 16, 2023 · 307dc94 · 307dc94
1 parent d95d929
commit 307dc94
Show file tree

Hide file tree

Showing 6 changed files with 281 additions and 0 deletions.
diff --git a/...rception/object_recognition/tracking/radar_object_tracker/radar_object_tracker.param.yaml b/...rception/object_recognition/tracking/radar_object_tracker/radar_object_tracker.param.yaml
@@ -0,0 +1,27 @@
+# general parameters for radar_object_tracker node
+/**:
+  ros__parameters:
+    # basic settings
+    world_frame_id: "map"
+    tracker_lifetime: 1.0  # [sec]
+    # if empty, use default config declared in this package
+    tracking_config_directory: ""
+
+    # delay compensate parameters
+    publish_rate: 10.0
+    enable_delay_compensation: false
+
+    # logging
+    enable_logging: false
+    logging_file_path: "/tmp/association_log.json"
+
+    # filtering
+    ## 1. distance based filtering: remove closer objects than this threshold
+    use_distance_based_noise_filtering: true
+    minimum_range_threshold: 70.0 # [m]
+
+    ## 2. lanelet map based filtering
+    use_map_based_noise_filtering: true
+    max_distance_from_lane: 5.0 # [m]
+    max_angle_diff_from_lane: 0.785398 # [rad] (45 deg)
+    max_lateral_velocity: 5.0 # [m/s]
diff --git a/...tion/object_recognition/tracking/radar_object_tracker/tracking/linear_motion_tracker.yaml b/...tion/object_recognition/tracking/radar_object_tracker/tracking/linear_motion_tracker.yaml
@@ -0,0 +1,32 @@
+default:
+  # This file defines the parameters for the linear motion tracker.
+  # All this parameter coordinate is assumed to be in the vehicle coordinate system.
+  # So, the x axis is pointing to the front of the vehicle, y axis is pointing to the left of the vehicle.
+  ekf_params:
+    # random walk noise is used to model the acceleration noise
+    process_noise_std: # [m/s^2]
+      ax: 0.98 # assume 0.1G acceleration noise
+      ay: 0.98
+      vx: 0.1 # assume 0.1m/s velocity noise
+      vy: 0.1
+      x: 1.0 # assume 1m position noise
+      y: 1.0
+    measurement_noise_std:
+      x: 0.6 # [m]
+      y: 0.9 # [m]
+      vx: 0.4 # [m/s]
+      vy: 1 # [m/s]
+    initial_covariance_std:
+      x: 3.0 # [m]
+      y: 6.0 # [m]
+      vx: 1.0 # [m/s]
+      vy: 5.0 # [m/s]
+      ax: 0.5 # [m/s^2]
+      ay: 1.0 # [m/s^2]
+  # output limitation
+  limit:
+    max_speed: 80.0 # [m/s]
+  # low pass filter is used to smooth the yaw and shape estimation
+  low_pass_filter:
+    time_constant: 1.0 # [s]
+    sampling_time: 0.1 # [s]
diff --git a/...ion/object_recognition/tracking/tracking_object_merger/data_association_matrix.param.yaml b/...ion/object_recognition/tracking/tracking_object_merger/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
diff --git a/...n/object_recognition/tracking/tracking_object_merger/decorative_tracker_merger.param.yaml b/...n/object_recognition/tracking/tracking_object_merger/decorative_tracker_merger.param.yaml
@@ -0,0 +1,26 @@
+# Node parameters
+/**:
+  ros__parameters:
+    base_link_frame_id: "base_link"
+    time_sync_threshold: 0.999
+    sub_object_timeout_sec: 0.8
+    publish_interpolated_sub_objects: true #for debug
+
+    # choose the input sensor type for each object type
+    # "lidar", "radar", "camera" are available
+    main_sensor_type: "lidar"
+    sub_sensor_type: "radar"
+
+    # tracker settings
+    tracker_state_parameter:
+      remove_probability_threshold: 0.3
+      publish_probability_threshold: 0.5
+      default_lidar_existence_probability: 0.7
+      default_radar_existence_probability: 0.6
+      default_camera_existence_probability: 0.5
+      decay_rate: 0.1
+      max_dt: 1.0
+
+    # logging
+    enable_logging: false
+    log_file_path: "/tmp/decorative_tracker_merger.log"
diff --git a/...t_recognition/tracking/tracking_object_merger/decorative_tracker_merger_policy.param.yaml b/...t_recognition/tracking/tracking_object_merger/decorative_tracker_merger_policy.param.yaml
@@ -0,0 +1,16 @@
+# Merger policy for decorative tracker merger node
+# decorative tracker merger works by merging the sub-object trackers into a main object tracker result
+# There are 3 merger policy:
+# 1. "skip": skip the sub-object tracker result
+# 2. "overwrite": overwrite the main object tracker result with sub-object tracker result
+# 3. "fusion": merge the main object tracker result with sub-object tracker result by using covariance based fusion
+/**:
+  ros__parameters:
+    kinematics_to_be_merged: "velocity" # currently only support "velocity"
+
+    # choose the merger policy for each object type
+    #  : "skip", "overwrite", "fusion"
+    kinematics_merge_policy: "overwrite"
+    classification_merge_policy: "skip"
+    existence_prob_merge_policy: "skip"
+    shape_merge_policy: "skip"
diff --git a/autoware_launch/launch/components/tier4_perception_component.launch.xml b/autoware_launch/launch/components/tier4_perception_component.launch.xml
@@ -81,6 +81,18 @@
       name="object_recognition_tracking_radar_object_tracker_tracking_setting_param_path"
       value="$(find-pkg-share autoware_launch)/config/perception/object_recognition/tracking/radar_object_tracker/default_tracker.param.yaml"
     />
+    <arg
+      name="object_recognition_tracking_radar_object_tracker_node_param_path"
+      value="$(find-pkg-share autoware_launch)/config/perception/object_recognition/tracking/radar_object_tracker/radar_object_tracker.param.yaml"
+    />
+    <arg
+      name="object_recognition_tracking_object_merger_data_association_matrix_param_path"
+      value="$(find-pkg-share autoware_launch)/config/perception/object_recognition/tracking/tracking_object_merger/data_association_matrix.param.yaml"
+    />
+    <arg
+      name="object_recognition_tracking_object_merger_node_param_path"
+      value="$(find-pkg-share autoware_launch)/config/perception/object_recognition/tracking/tracking_object_merger/decorative_tracker_merger.param.yaml"
+    />
 
     <!-- obstacle segmentation -->
     <arg