feat(perception_online_evaluator): add yaw rate metrics for stopped o…

…bject (#6667)

* feat(perception_online_evaluator): add yaw rate metrics for stopped object

Signed-off-by: kosuke55 <[email protected]>

add

Signed-off-by: kosuke55 <[email protected]>

add test

* feat: add stopped vel parameter

Signed-off-by: kosuke55 <[email protected]>

---------

Signed-off-by: kosuke55 <[email protected]>

evaluator/perception_online_evaluator/README.md

@@ -27,6 +27,7 @@ This module allows for the evaluation of how accurately perception results are g
 | `selected_metrics`                | List         | Metrics to be evaluated, such as lateral deviation, yaw deviation, and predicted path deviation. |
 | `smoothing_window_size`           | Integer      | Determines the window size for smoothing path, should be an odd number.                          |
 | `prediction_time_horizons`        | list[double] | Time horizons for prediction evaluation in seconds.                                              |
+| `stopped_velocity_threshold`      | double       | threshold velocity to check if vehicle is stopped                                                |
 | `target_object.*.check_deviation` | bool         | Whether to check deviation for specific object types (car, truck, etc.).                         |
 | `debug_marker.*`                  | bool         | Debugging parameters for marker visualization (history path, predicted path, etc.).              |
 

evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp

@@ -31,6 +31,7 @@ enum class Metric {
   lateral_deviation,
   yaw_deviation,
   predicted_path_deviation,
+  yaw_rate,
   SIZE,
 };
 
@@ -39,18 +40,21 @@ using MetricStatMap = std::unordered_map<std::string, Stat<double>>;
 static const std::unordered_map<std::string, Metric> str_to_metric = {
   {"lateral_deviation", Metric::lateral_deviation},
   {"yaw_deviation", Metric::yaw_deviation},
-  {"predicted_path_deviation", Metric::predicted_path_deviation}};
+  {"predicted_path_deviation", Metric::predicted_path_deviation},
+  {"yaw_rate", Metric::yaw_rate}};
 
 static const std::unordered_map<Metric, std::string> metric_to_str = {
   {Metric::lateral_deviation, "lateral_deviation"},
   {Metric::yaw_deviation, "yaw_deviation"},
-  {Metric::predicted_path_deviation, "predicted_path_deviation"}};
+  {Metric::predicted_path_deviation, "predicted_path_deviation"},
+  {Metric::yaw_rate, "yaw_rate"}};
 
 // Metrics descriptions
 static const std::unordered_map<Metric, std::string> metric_descriptions = {
   {Metric::lateral_deviation, "Lateral_deviation[m]"},
   {Metric::yaw_deviation, "Yaw_deviation[rad]"},
-  {Metric::predicted_path_deviation, "Predicted_path_deviation[m]"}};
+  {Metric::predicted_path_deviation, "Predicted_path_deviation[m]"},
+  {Metric::yaw_rate, "Yaw_rate[rad/s]"}};
 
 namespace details
 {

evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp

@@ -72,6 +72,10 @@ using ObjectDataMap = std::unordered_map<std::string, ObjectData>;
 using HistoryPathMap =
   std::unordered_map<std::string, std::pair<std::vector<Pose>, std::vector<Pose>>>;
 
+using StampObjectMap = std::map<rclcpp::Time, PredictedObject>;
+using StampObjectMapIterator = std::map<rclcpp::Time, PredictedObject>::const_iterator;
+using ObjectMap = std::unordered_map<std::string, StampObjectMap>;
+
 class MetricsCalculator
 {
 public:
@@ -98,7 +102,7 @@ class MetricsCalculator
   std::shared_ptr<Parameters> parameters_;
 
   // Store predicted objects information and calculation results
-  std::unordered_map<std::string, std::map<rclcpp::Time, PredictedObject>> object_map_;
+  ObjectMap object_map_;
   HistoryPathMap history_path_map_;
 
   rclcpp::Time current_stamp_;
@@ -124,15 +128,20 @@ class MetricsCalculator
   MetricStatMap calcPredictedPathDeviationMetrics(const ClassObjectsMap & class_objects_map) const;
   Stat<double> calcPredictedPathDeviationMetrics(
     const PredictedObjects & objects, const double time_horizon) const;
+  MetricStatMap calcYawRateMetrics(const ClassObjectsMap & class_objects_map) const;
 
   bool hasPassedTime(const rclcpp::Time stamp) const;
   bool hasPassedTime(const std::string uuid, const rclcpp::Time stamp) const;
   double getTimeDelay() const;
 
   // Extract object
   rclcpp::Time getClosestStamp(const rclcpp::Time stamp) const;
+  std::optional<StampObjectMapIterator> getClosestObjectIterator(
+    const std::string & uuid, const rclcpp::Time & stamp) const;
   std::optional<PredictedObject> getObjectByStamp(
     const std::string uuid, const rclcpp::Time stamp) const;
+  std::optional<std::pair<rclcpp::Time, PredictedObject>> getPreviousObjectByStamp(
+    const std::string uuid, const rclcpp::Time stamp) const;
   PredictedObjects getObjectsByStamp(const rclcpp::Time stamp) const;
 
 };  // class MetricsCalculator

evaluator/perception_online_evaluator/include/perception_online_evaluator/parameters.hpp

@@ -48,6 +48,7 @@ struct Parameters
   std::vector<Metric> metrics;
   size_t smoothing_window_size{0};
   std::vector<double> prediction_time_horizons;
+  double stopped_velocity_threshold{0.0};
   DebugMarkerParameter debug_marker_parameters;
   // parameters depend on object class
   std::unordered_map<uint8_t, ObjectParameter> object_parameters;

evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/objects_filtering.hpp

@@ -21,6 +21,7 @@
 #include <autoware_auto_perception_msgs/msg/predicted_object.hpp>
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
 
+#include <cmath>
 #include <memory>
 #include <unordered_map>
 #include <utility>
@@ -39,6 +40,9 @@ using autoware_auto_perception_msgs::msg::PredictedObjects;
 
 using ClassObjectsMap = std::unordered_map<uint8_t, PredictedObjects>;
 
+bool velocity_filter(
+  const PredictedObject & object, double velocity_threshold, double max_velocity);
+
 std::uint8_t getHighestProbLabel(const std::vector<ObjectClassification> & classification);
 
 /**
@@ -133,6 +137,39 @@ void filterObjectsByClass(
 void filterDeviationCheckObjects(
   PredictedObjects & objects, const std::unordered_map<uint8_t, ObjectParameter> & params);
 
+/**
+ * @brief Filters objects based on their velocity.
+ *
+ * Depending on the remove_above_threshold parameter, this function either removes objects with
+ * velocities above the given threshold or only keeps those objects. It uses the helper function
+ * filterObjectsByVelocity() to do the actual filtering.
+ *
+ * @param objects The objects to be filtered.
+ * @param velocity_threshold The velocity threshold for the filtering.
+ * @param remove_above_threshold If true, objects with velocities above the threshold are removed.
+ *                               If false, only objects with velocities above the threshold are
+ *                               kept.
+ * @return A new collection of objects that have been filtered according to the rules.
+ */
+PredictedObjects filterObjectsByVelocity(
+  const PredictedObjects & objects, const double velocity_threshold,
+  const bool remove_above_threshold = true);
+
+/**
+ * @brief Helper function to filter objects based on their velocity.
+ *
+ * This function iterates over all objects and calculates their velocity norm. If the velocity norm
+ * is within the velocity_threshold and max_velocity range, the object is added to a new collection.
+ * This new collection is then returned.
+ *
+ * @param objects The objects to be filtered.
+ * @param velocity_threshold The minimum velocity for an object to be included in the output.
+ * @param max_velocity The maximum velocity for an object to be included in the output.
+ * @return A new collection of objects that have been filtered according to the rules.
+ */
+PredictedObjects filterObjectsByVelocity(
+  const PredictedObjects & objects, double velocity_threshold, double max_velocity);
+
 }  // namespace perception_diagnostics
 
 #endif  // PERCEPTION_ONLINE_EVALUATOR__UTILS__OBJECTS_FILTERING_HPP_

evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml

@@ -4,12 +4,15 @@
       - lateral_deviation
       - yaw_deviation
       - predicted_path_deviation
+      - yaw_rate
 
     # this should be an odd number, because it includes the target point
     smoothing_window_size: 11
 
     prediction_time_horizons: [1.0, 2.0, 3.0, 5.0]
 
+    stopped_velocity_threshold: 0.3
+
     target_object:
       car:
         check_deviation: true

evaluator/perception_online_evaluator/src/metrics_calculator.cpp

@@ -36,15 +36,23 @@ std::optional<MetricStatMap> MetricsCalculator::calculate(const Metric & metric)
   if (!hasPassedTime(target_stamp)) {
     return {};
   }
-  const auto target_objects = getObjectsByStamp(target_stamp);
-  const ClassObjectsMap class_objects_map = separateObjectsByClass(target_objects);
+  const auto target_stamp_objects = getObjectsByStamp(target_stamp);
+  const auto class_objects_map = separateObjectsByClass(target_stamp_objects);
+
+  // filter stopped objects
+  const auto stopped_objects =
+    filterObjectsByVelocity(target_stamp_objects, parameters_->stopped_velocity_threshold);
+  const auto class_stopped_objects_map = separateObjectsByClass(stopped_objects);
+
   switch (metric) {
     case Metric::lateral_deviation:
       return calcLateralDeviationMetrics(class_objects_map);
     case Metric::yaw_deviation:
       return calcYawDeviationMetrics(class_objects_map);
     case Metric::predicted_path_deviation:
       return calcPredictedPathDeviationMetrics(class_objects_map);
+    case Metric::yaw_rate:
+      return calcYawRateMetrics(class_stopped_objects_map);
     default:
       return {};
   }
@@ -124,16 +132,46 @@ rclcpp::Time MetricsCalculator::getClosestStamp(const rclcpp::Time stamp) const
   return closest_stamp;
 }
 
-std::optional<PredictedObject> MetricsCalculator::getObjectByStamp(
-  const std::string uuid, const rclcpp::Time stamp) const
+std::optional<StampObjectMapIterator> MetricsCalculator::getClosestObjectIterator(
+  const std::string & uuid, const rclcpp::Time & stamp) const
 {
   const auto closest_stamp = getClosestStamp(stamp);
-  auto it = std::lower_bound(
+  const auto it = std::lower_bound(
     object_map_.at(uuid).begin(), object_map_.at(uuid).end(), closest_stamp,
     [](const auto & pair, const rclcpp::Time & val) { return pair.first < val; });
 
-  if (it != object_map_.at(uuid).end() && it->first == closest_stamp) {
-    return it->second;
+  return it != object_map_.at(uuid).end() ? std::optional<StampObjectMapIterator>(it)
+                                          : std::nullopt;
+}
+
+std::optional<PredictedObject> MetricsCalculator::getObjectByStamp(
+  const std::string uuid, const rclcpp::Time stamp) const
+{
+  const auto obj_it_opt = getClosestObjectIterator(uuid, stamp);
+  if (obj_it_opt.has_value()) {
+    const auto it = obj_it_opt.value();
+    if (it->first == getClosestStamp(stamp)) {
+      return it->second;
+    }
+  }
+  return std::nullopt;
+}
+
+std::optional<std::pair<rclcpp::Time, PredictedObject>> MetricsCalculator::getPreviousObjectByStamp(
+  const std::string uuid, const rclcpp::Time stamp) const
+{
+  const auto obj_it_opt = getClosestObjectIterator(uuid, stamp);
+  if (obj_it_opt.has_value()) {
+    auto it = obj_it_opt.value();
+    if (it != object_map_.at(uuid).begin()) {
+      // If it is exactly the closest stamp, move one back to get the previous
+      if (it->first == getClosestStamp(stamp)) {
+        --it;
+      } else {
+        // If it is not the closest stamp, it already points to the previous one due to lower_bound
+      }
+      return std::make_pair(it->first, it->second);
+    }
   }
   return std::nullopt;
 }
@@ -313,9 +351,46 @@ Stat<double> MetricsCalculator::calcPredictedPathDeviationMetrics(
   return stat;
 }
 
+MetricStatMap MetricsCalculator::calcYawRateMetrics(const ClassObjectsMap & class_objects_map) const
+{
+  // calculate yaw rate for each object
+
+  MetricStatMap metric_stat_map{};
+
+  for (const auto & [label, objects] : class_objects_map) {
+    Stat<double> stat{};
+    const auto stamp = rclcpp::Time(objects.header.stamp);
+
+    for (const auto & object : objects.objects) {
+      const auto uuid = tier4_autoware_utils::toHexString(object.object_id);
+      if (!hasPassedTime(uuid, stamp)) {
+        continue;
+      }
+      const auto previous_object_with_stamp_opt = getPreviousObjectByStamp(uuid, stamp);
+      if (!previous_object_with_stamp_opt.has_value()) {
+        continue;
+      }
+      const auto [previous_stamp, previous_object] = previous_object_with_stamp_opt.value();
+
+      const auto time_diff = (stamp - previous_stamp).seconds();
+      if (time_diff < 0.01) {
+        continue;
+      }
+      const auto current_yaw =
+        tf2::getYaw(object.kinematics.initial_pose_with_covariance.pose.orientation);
+      const auto previous_yaw =
+        tf2::getYaw(previous_object.kinematics.initial_pose_with_covariance.pose.orientation);
+      const auto yaw_rate =
+        std::abs(tier4_autoware_utils::normalizeRadian(current_yaw - previous_yaw) / time_diff);
+      stat.add(yaw_rate);
+    }
+    metric_stat_map["yaw_rate_" + convertLabelToString(label)] = stat;
+  }
+  return metric_stat_map;
+}
+
 void MetricsCalculator::setPredictedObjects(const PredictedObjects & objects)
 {
-  // using TimeStamp = builtin_interfaces::msg::Time;
   current_stamp_ = objects.header.stamp;
 
   // store objects to check deviation

evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp

@@ -280,6 +280,8 @@ void PerceptionOnlineEvaluatorNode::initParameter()
   p->smoothing_window_size = getOrDeclareParameter<int>(*this, "smoothing_window_size");
   p->prediction_time_horizons =
     getOrDeclareParameter<std::vector<double>>(*this, "prediction_time_horizons");
+  p->stopped_velocity_threshold =
+    getOrDeclareParameter<double>(*this, "stopped_velocity_threshold");
 
   // set metrics
   const auto selected_metrics =

evaluator/perception_online_evaluator/src/utils/objects_filtering.cpp

@@ -16,6 +16,17 @@
 
 namespace perception_diagnostics
 {
+namespace filter
+{
+bool velocity_filter(const PredictedObject & object, double velocity_threshold, double max_velocity)
+{
+  const auto v_norm = std::hypot(
+    object.kinematics.initial_twist_with_covariance.twist.linear.x,
+    object.kinematics.initial_twist_with_covariance.twist.linear.y);
+  return (velocity_threshold < v_norm && v_norm < max_velocity);
+}
+}  // namespace filter
+
 ObjectTypesToCheck getDeviationCheckObjectTypes(
   const std::unordered_map<uint8_t, ObjectParameter> & params)
 {
@@ -111,4 +122,25 @@ void filterDeviationCheckObjects(
   filterObjectsByClass(objects, object_types);
 }
 
+PredictedObjects filterObjectsByVelocity(
+  const PredictedObjects & objects, const double velocity_threshold,
+  const bool remove_above_threshold)
+{
+  if (remove_above_threshold) {
+    return filterObjectsByVelocity(objects, -velocity_threshold, velocity_threshold);
+  }
+  return filterObjectsByVelocity(objects, velocity_threshold, std::numeric_limits<double>::max());
+}
+
+PredictedObjects filterObjectsByVelocity(
+  const PredictedObjects & objects, double velocity_threshold, double max_velocity)
+{
+  const auto filter = [&](const auto & object) {
+    return filter::velocity_filter(object, velocity_threshold, max_velocity);
+  };
+
+  auto filtered = objects;
+  filterObjects(filtered, filter);
+  return filtered;
+}
 }  // namespace perception_diagnostics