feat(perception_online_evaluator): imporve yaw rate metrics consideri…

…ng flip (autowarefoundation#6881) * feat(perception_online_evaluator): imporve yaw rate metrics considering flip Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * fix test Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> --------- Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
lexavtanke · Apr 24, 2024 · a6c6912 · a6c6912
1 parent 41bee43
commit a6c6912
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 7 deletions.
diff --git 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
@@ -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);
  }
 }
 // ==========================================================================================