From a6c69122541b141cbbf3c37c03151a82009d50fa Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 25 Apr 2024 00:15:54 +0900 Subject: [PATCH] feat(perception_online_evaluator): imporve yaw rate metrics considering flip (#6881) * feat(perception_online_evaluator): imporve yaw rate metrics considering flip Signed-off-by: kosuke55 * fix test Signed-off-by: kosuke55 --------- Signed-off-by: kosuke55 --- .../src/metrics_calculator.cpp | 9 ++++----- .../test/test_perception_online_evaluator_node.cpp | 8 ++++++-- 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp index e6adbafe75985..9c1e0667e4fef 100644 --- a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp +++ b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp @@ -442,13 +442,12 @@ MetricStatMap MetricsCalculator::calcYawRateMetrics(const ClassObjectsMap & clas tf2::getYaw(object.kinematics.initial_pose_with_covariance.pose.orientation); const double previous_yaw = tf2::getYaw(previous_object.kinematics.initial_pose_with_covariance.pose.orientation); + // Calculate the absolute difference between current_yaw and previous_yaw const double yaw_diff = std::abs(tier4_autoware_utils::normalizeRadian(current_yaw - previous_yaw)); - // if yaw_diff is close to PI, reversal of orientation is likely occurring, so ignore it - if (std::abs(M_PI - yaw_diff) < 0.1) { - continue; - } - const auto yaw_rate = yaw_diff / time_diff; + // The yaw difference is flipped if the angle is larger than 90 degrees + const double yaw_diff_flip_fixed = std::min(yaw_diff, M_PI - yaw_diff); + const double yaw_rate = yaw_diff_flip_fixed / time_diff; stat.add(yaw_rate); } metric_stat_map["yaw_rate_" + convertLabelToString(label)] = stat; diff --git a/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp b/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp index 3c829dbed9899..95d6f07cca5d9 100644 --- a/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp +++ b/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp @@ -924,6 +924,8 @@ TEST_F(EvalTest, testYawRate_5) setTargetMetric("yaw_rate_CAR"); const double yaw_rate = 5.0; + const double yaw_rate_flip_fixed = + std::min(yaw_rate, (M_PI - yaw_rate * time_step_) / time_step_); for (double time = 0; time <= time_delay_ + 0.01; time += time_step_) { const auto objects = rotateObjects( @@ -936,7 +938,7 @@ TEST_F(EvalTest, testYawRate_5) const auto objects = rotateObjects( makeStraightPredictedObjects(time, ObjectClassification::CAR, 0.0), yaw_rate * time); publishEgoTF(time); - EXPECT_NEAR(publishObjectsAndGetMetric(objects), yaw_rate, epsilon); + EXPECT_NEAR(publishObjectsAndGetMetric(objects), yaw_rate_flip_fixed, epsilon); } } @@ -946,6 +948,8 @@ TEST_F(EvalTest, testYawRate_minus_5) setTargetMetric("yaw_rate_CAR"); const double yaw_rate = 5.0; + const double yaw_rate_flip_fixed = + std::min(yaw_rate, (M_PI - yaw_rate * time_step_) / time_step_); for (double time = 0; time <= time_delay_ + 0.01; time += time_step_) { const auto objects = rotateObjects( @@ -958,7 +962,7 @@ TEST_F(EvalTest, testYawRate_minus_5) const auto objects = rotateObjects( makeStraightPredictedObjects(time, ObjectClassification::CAR, 0.0), -yaw_rate * time); publishEgoTF(time); - EXPECT_NEAR(publishObjectsAndGetMetric(objects), yaw_rate, epsilon); + EXPECT_NEAR(publishObjectsAndGetMetric(objects), yaw_rate_flip_fixed, epsilon); } } // ==========================================================================================