From 307dc94a121f6f22ed7fc810665e1ba3adf9bdfa Mon Sep 17 00:00:00 2001 From: Yoshi Ri Date: Mon, 16 Oct 2023 13:29:39 +0900 Subject: [PATCH] add tracking object merger for long range radar sensor (#627) * add tracking object merger paramters Signed-off-by: yoshiri * fix typo Signed-off-by: yoshiri --------- Signed-off-by: yoshiri --- .../radar_object_tracker.param.yaml | 27 +++ .../tracking/linear_motion_tracker.yaml | 32 ++++ .../data_association_matrix.param.yaml | 168 ++++++++++++++++++ .../decorative_tracker_merger.param.yaml | 26 +++ ...ecorative_tracker_merger_policy.param.yaml | 16 ++ .../tier4_perception_component.launch.xml | 12 ++ 6 files changed, 281 insertions(+) create mode 100644 autoware_launch/config/perception/object_recognition/tracking/radar_object_tracker/radar_object_tracker.param.yaml create mode 100644 autoware_launch/config/perception/object_recognition/tracking/radar_object_tracker/tracking/linear_motion_tracker.yaml create mode 100644 autoware_launch/config/perception/object_recognition/tracking/tracking_object_merger/data_association_matrix.param.yaml create mode 100644 autoware_launch/config/perception/object_recognition/tracking/tracking_object_merger/decorative_tracker_merger.param.yaml create mode 100644 autoware_launch/config/perception/object_recognition/tracking/tracking_object_merger/decorative_tracker_merger_policy.param.yaml diff --git a/autoware_launch/config/perception/object_recognition/tracking/radar_object_tracker/radar_object_tracker.param.yaml b/autoware_launch/config/perception/object_recognition/tracking/radar_object_tracker/radar_object_tracker.param.yaml new file mode 100644 index 0000000000..f80adffb41 --- /dev/null +++ b/autoware_launch/config/perception/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/autoware_launch/config/perception/object_recognition/tracking/radar_object_tracker/tracking/linear_motion_tracker.yaml b/autoware_launch/config/perception/object_recognition/tracking/radar_object_tracker/tracking/linear_motion_tracker.yaml new file mode 100644 index 0000000000..71367f4575 --- /dev/null +++ b/autoware_launch/config/perception/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/autoware_launch/config/perception/object_recognition/tracking/tracking_object_merger/data_association_matrix.param.yaml b/autoware_launch/config/perception/object_recognition/tracking/tracking_object_merger/data_association_matrix.param.yaml new file mode 100644 index 0000000000..702809b3ce --- /dev/null +++ b/autoware_launch/config/perception/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/autoware_launch/config/perception/object_recognition/tracking/tracking_object_merger/decorative_tracker_merger.param.yaml b/autoware_launch/config/perception/object_recognition/tracking/tracking_object_merger/decorative_tracker_merger.param.yaml new file mode 100644 index 0000000000..13369d5422 --- /dev/null +++ b/autoware_launch/config/perception/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/autoware_launch/config/perception/object_recognition/tracking/tracking_object_merger/decorative_tracker_merger_policy.param.yaml b/autoware_launch/config/perception/object_recognition/tracking/tracking_object_merger/decorative_tracker_merger_policy.param.yaml new file mode 100644 index 0000000000..0b98e1b202 --- /dev/null +++ b/autoware_launch/config/perception/object_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 index 1ec8f2c22f..e3df873492 100644 --- 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" /> + + +