From 2e9ca11f032dad44b2281226549442b85255d08d Mon Sep 17 00:00:00 2001 From: Yoshi Ri Date: Tue, 4 Jul 2023 23:06:13 +0900 Subject: [PATCH 1/3] feat(multi_object_tracker): suppress yaw rotation oscillation problem in bicycle tracking model (#4146) * enable only tracking and prediction Signed-off-by: yoshiri * set bicycle model parameter to be understeered Signed-off-by: yoshiri * Revert "enable only tracking and prediction" This reverts commit d97db405c1083fa8aefc4027da35f19308cbd7e3. --------- Signed-off-by: yoshiri --- .../multi_object_tracker/src/tracker/model/bicycle_tracker.cpp | 2 +- .../src/tracker/model/big_vehicle_tracker.cpp | 2 +- .../src/tracker/model/normal_vehicle_tracker.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index 5de81eb2d4538..75667f0b60fbb 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -135,7 +135,7 @@ BicycleTracker::BicycleTracker( ekf_.init(X, P); // Set lf, lr - double point_ratio = 0.5; // comes to front if larger + double point_ratio = 0.2; // under steered if smaller than 0.5 lf_ = bounding_box_.length * point_ratio; lr_ = bounding_box_.length * (1.0 - point_ratio); } diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 458509cf06e86..3fcbdc2f794aa 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -150,7 +150,7 @@ BigVehicleTracker::BigVehicleTracker( setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step // Set lf, lr - double point_ratio = 0.5; // comes to front if larger + double point_ratio = 0.1; // under steered if smaller than 0.5 lf_ = bounding_box_.length * point_ratio; lr_ = bounding_box_.length * (1.0 - point_ratio); } diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index 000f728892d11..713bd58360c32 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -150,7 +150,7 @@ NormalVehicleTracker::NormalVehicleTracker( setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step // Set lf, lr - double point_ratio = 0.5; // comes to front if larger + double point_ratio = 0.2; // under steered if smaller than 0.5 lf_ = bounding_box_.length * point_ratio; lr_ = bounding_box_.length * (1.0 - point_ratio); } From 224c74cfb19cb9f126f6f4ef2e58918c2d6d9742 Mon Sep 17 00:00:00 2001 From: Yoshi Ri Date: Wed, 23 Aug 2023 10:26:40 +0900 Subject: [PATCH 2/3] feat(multi_object_tracker): tune system noise to suppress yaw oscillation (#4679) * tune system noise to surpress yaw ossilation Signed-off-by: yoshiri * style(pre-commit): autofix --------- Signed-off-by: yoshiri Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../src/tracker/model/bicycle_tracker.cpp | 2 +- .../src/tracker/model/big_vehicle_tracker.cpp | 4 ++-- .../src/tracker/model/normal_vehicle_tracker.cpp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index 75667f0b60fbb..587b8f608976b 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -55,7 +55,7 @@ BicycleTracker::BicycleTracker( float q_stddev_y = 0.6; // [m/s] float q_stddev_yaw = tier4_autoware_utils::deg2rad(10); // [rad/s] float q_stddev_vx = tier4_autoware_utils::kmph2mps(10); // [m/(s*s)] - float q_stddev_slip = tier4_autoware_utils::deg2rad(25); // [rad/(s*s)] + float q_stddev_slip = tier4_autoware_utils::deg2rad(15); // [rad/(s*s)] float r_stddev_x = 0.6; // [m] float r_stddev_y = 0.4; // [m] float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 3fcbdc2f794aa..7ab0031f66e5d 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -54,7 +54,7 @@ BigVehicleTracker::BigVehicleTracker( float q_stddev_y = 1.5; // [m/s] float q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad/s] float q_stddev_vx = tier4_autoware_utils::kmph2mps(10); // [m/(s*s)] - float q_stddev_slip = tier4_autoware_utils::deg2rad(20); // [rad/(s*s)] + float q_stddev_slip = tier4_autoware_utils::deg2rad(5); // [rad/(s*s)] float r_stddev_x = 1.5; // [m] float r_stddev_y = 0.5; // [m] float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] @@ -150,7 +150,7 @@ BigVehicleTracker::BigVehicleTracker( setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step // Set lf, lr - double point_ratio = 0.1; // under steered if smaller than 0.5 + double point_ratio = 0.2; // under steered if smaller than 0.5 lf_ = bounding_box_.length * point_ratio; lr_ = bounding_box_.length * (1.0 - point_ratio); } diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index 713bd58360c32..30461b1b5acd7 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -54,7 +54,7 @@ NormalVehicleTracker::NormalVehicleTracker( float q_stddev_y = 1.0; // object coordinate [m/s] float q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // map coordinate[rad/s] float q_stddev_vx = tier4_autoware_utils::kmph2mps(10); // object coordinate [m/(s*s)] - float q_stddev_slip = tier4_autoware_utils::deg2rad(20); // object coordinate [rad/(s*s)] + float q_stddev_slip = tier4_autoware_utils::deg2rad(5); // object coordinate [rad/(s*s)] float r_stddev_x = 1.0; // object coordinate [m] float r_stddev_y = 0.3; // object coordinate [m] float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // map coordinate [rad] From bc064576d4738c7eb5d99d3af838edbbb5f8bde0 Mon Sep 17 00:00:00 2001 From: Yoshi Ri Date: Tue, 22 Aug 2023 11:20:55 +0900 Subject: [PATCH 3/3] feat(tier4_perception_launch): lower the detection by tracker priority to suppress yaw oscillation (#4690) lower the detection by tracker priority to suppress yaw oscillation Signed-off-by: yoshiri --- .../detection/camera_lidar_fusion_based_detection.launch.xml | 1 + .../detection/lidar_based_detection.launch.xml | 1 + 2 files changed, 2 insertions(+) diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index 725c4fad19c48..7d7d298de1015 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -241,6 +241,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml index f4f5b4139080a..a3475bcd22e8e 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml @@ -129,6 +129,7 @@ +