feat(perception_online_evaluator): publish metrics of each object cla…

…ss (autowarefoundation#6556)

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

evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp

@@ -19,6 +19,7 @@
 #include "perception_online_evaluator/metrics/metric.hpp"
 #include "perception_online_evaluator/parameters.hpp"
 #include "perception_online_evaluator/stat.hpp"
+#include "perception_online_evaluator/utils/objects_filtering.hpp"
 
 #include <rclcpp/time.hpp>
 
@@ -118,9 +119,9 @@ class MetricsCalculator
   void deleteOldObjects(const rclcpp::Time stamp);
 
   // Calculate metrics
-  MetricStatMap calcLateralDeviationMetrics(const PredictedObjects & objects) const;
-  MetricStatMap calcYawDeviationMetrics(const PredictedObjects & objects) const;
-  MetricStatMap calcPredictedPathDeviationMetrics(const PredictedObjects & objects) const;
+  MetricStatMap calcLateralDeviationMetrics(const ClassObjectsMap & class_objects_map) const;
+  MetricStatMap calcYawDeviationMetrics(const ClassObjectsMap & class_objects_map) const;
+  MetricStatMap calcPredictedPathDeviationMetrics(const ClassObjectsMap & class_objects_map) const;
   Stat<double> calcPredictedPathDeviationMetrics(
     const PredictedObjects & objects, const double time_horizon) const;
 

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

@@ -37,6 +37,8 @@ using autoware_auto_perception_msgs::msg::ObjectClassification;
 using autoware_auto_perception_msgs::msg::PredictedObject;
 using autoware_auto_perception_msgs::msg::PredictedObjects;
 
+using ClassObjectsMap = std::unordered_map<uint8_t, PredictedObjects>;
+
 std::uint8_t getHighestProbLabel(const std::vector<ObjectClassification> & classification);
 
 /**
@@ -105,6 +107,14 @@ void filterObjects(PredictedObjects & objects, Func filter)
   filterObjects(objects, removed_objects, filter);
 }
 
+/**
+ * @brief Separates the provided objects based on their classification.
+ *
+ * @param objects The predicted objects to be separated.
+ * @return A map of objects separated by their classification.
+ */
+ClassObjectsMap separateObjectsByClass(const PredictedObjects & objects);
+
 /**
  * @brief Filters the provided objects based on their classification.
  *

evaluator/perception_online_evaluator/package.xml

@@ -24,6 +24,7 @@
   <depend>libgoogle-glog-dev</depend>
   <depend>motion_utils</depend>
   <depend>nav_msgs</depend>
+  <depend>object_recognition_utils</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>

evaluator/perception_online_evaluator/src/metrics_calculator.cpp

@@ -15,13 +15,15 @@
 #include "perception_online_evaluator/metrics_calculator.hpp"
 
 #include "motion_utils/trajectory/trajectory.hpp"
-#include "perception_online_evaluator/utils/objects_filtering.hpp"
+#include "object_recognition_utils/object_recognition_utils.hpp"
 #include "tier4_autoware_utils/geometry/geometry.hpp"
 
 #include <tier4_autoware_utils/ros/uuid_helper.hpp>
 
 namespace perception_diagnostics
 {
+using object_recognition_utils::convertLabelToString;
+
 std::optional<MetricStatMap> MetricsCalculator::calculate(const Metric & metric) const
 {
   if (object_map_.empty()) {
@@ -35,14 +37,14 @@ std::optional<MetricStatMap> MetricsCalculator::calculate(const Metric & metric)
     return {};
   }
   const auto target_objects = getObjectsByStamp(target_stamp);
-
+  const ClassObjectsMap class_objects_map = separateObjectsByClass(target_objects);
   switch (metric) {
     case Metric::lateral_deviation:
-      return calcLateralDeviationMetrics(target_objects);
+      return calcLateralDeviationMetrics(class_objects_map);
     case Metric::yaw_deviation:
-      return calcYawDeviationMetrics(target_objects);
+      return calcYawDeviationMetrics(class_objects_map);
     case Metric::predicted_path_deviation:
-      return calcPredictedPathDeviationMetrics(target_objects);
+      return calcPredictedPathDeviationMetrics(class_objects_map);
     default:
       return {};
   }
@@ -155,65 +157,70 @@ PredictedObjects MetricsCalculator::getObjectsByStamp(const rclcpp::Time stamp)
   return objects;
 }
 
-MetricStatMap MetricsCalculator::calcLateralDeviationMetrics(const PredictedObjects & objects) const
+MetricStatMap MetricsCalculator::calcLateralDeviationMetrics(
+  const ClassObjectsMap & class_objects_map) const
 {
-  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 object_pose = object.kinematics.initial_pose_with_covariance.pose;
-    const auto history_path = history_path_map_.at(uuid).second;
-    if (history_path.empty()) {
-      continue;
+  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 object_pose = object.kinematics.initial_pose_with_covariance.pose;
+      const auto history_path = history_path_map_.at(uuid).second;
+      if (history_path.empty()) {
+        continue;
+      }
+      stat.add(metrics::calcLateralDeviation(history_path, object_pose));
     }
-    stat.add(metrics::calcLateralDeviation(history_path, object_pose));
+    metric_stat_map["lateral_deviation_" + convertLabelToString(label)] = stat;
   }
-
-  MetricStatMap metric_stat_map;
-  metric_stat_map["lateral_deviation"] = stat;
   return metric_stat_map;
 }
 
-MetricStatMap MetricsCalculator::calcYawDeviationMetrics(const PredictedObjects & objects) const
+MetricStatMap MetricsCalculator::calcYawDeviationMetrics(
+  const ClassObjectsMap & class_objects_map) const
 {
-  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 object_pose = object.kinematics.initial_pose_with_covariance.pose;
-    const auto history_path = history_path_map_.at(uuid).second;
-    if (history_path.empty()) {
-      continue;
+  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 object_pose = object.kinematics.initial_pose_with_covariance.pose;
+      const auto history_path = history_path_map_.at(uuid).second;
+      if (history_path.empty()) {
+        continue;
+      }
+      stat.add(metrics::calcYawDeviation(history_path, object_pose));
     }
-    stat.add(metrics::calcYawDeviation(history_path, object_pose));
+    metric_stat_map["yaw_deviation_" + convertLabelToString(label)] = stat;
   }
-
-  MetricStatMap metric_stat_map;
-  metric_stat_map["yaw_deviation"] = stat;
   return metric_stat_map;
 }
 
 MetricStatMap MetricsCalculator::calcPredictedPathDeviationMetrics(
-  const PredictedObjects & objects) const
+  const ClassObjectsMap & class_objects_map) const
 {
   const auto time_horizons = parameters_->prediction_time_horizons;
 
-  MetricStatMap metric_stat_map;
-  for (const double time_horizon : time_horizons) {
-    const auto stat = calcPredictedPathDeviationMetrics(objects, time_horizon);
-    std::ostringstream stream;
-    stream << std::fixed << std::setprecision(2) << time_horizon;
-    std::string time_horizon_str = stream.str();
-    metric_stat_map["predicted_path_deviation_" + time_horizon_str] = stat;
+  MetricStatMap metric_stat_map{};
+  for (const auto & [label, objects] : class_objects_map) {
+    for (const double time_horizon : time_horizons) {
+      const auto stat = calcPredictedPathDeviationMetrics(objects, time_horizon);
+      std::ostringstream stream;
+      stream << std::fixed << std::setprecision(2) << time_horizon;
+      std::string time_horizon_str = stream.str();
+      metric_stat_map
+        ["predicted_path_deviation_" + convertLabelToString(label) + "_" + time_horizon_str] = stat;
+    }
   }
-
   return metric_stat_map;
 }
 

evaluator/perception_online_evaluator/src/utils/objects_filtering.cpp

@@ -83,6 +83,17 @@ bool isTargetObjectType(
     (t == ObjectClassification::PEDESTRIAN && target_object_types.check_pedestrian));
 }
 
+ClassObjectsMap separateObjectsByClass(const PredictedObjects & objects)
+{
+  ClassObjectsMap separated_objects;
+  for (const auto & object : objects.objects) {
+    const auto label = getHighestProbLabel(object.classification);
+    separated_objects[label].objects.push_back(object);
+    separated_objects[label].header = objects.header;
+  }
+  return separated_objects;
+}
+
 void filterObjectsByClass(
   PredictedObjects & objects, const ObjectTypesToCheck & target_object_types)
 {