feat(perception_online_evaluator): add predicted path variance (#6793)

* feat(perception_online_evaluator): add predicted path variance Signed-off-by: kosuke55 <[email protected]> * add unit test Signed-off-by: kosuke55 <[email protected]> * update readme Signed-off-by: kosuke55 <[email protected]> * pre commit Signed-off-by: kosuke55 <[email protected]> --------- Signed-off-by: kosuke55 <[email protected]>
autowarefoundation · Apr 16, 2024 · 5ef5758 · 5ef5758
1 parent cf5a2ed
commit 5ef5758
Show file tree

Hide file tree

Showing 9 changed files with 1,995 additions and 32 deletions.
diff --git a/evaluator/perception_online_evaluator/CMakeLists.txt b/evaluator/perception_online_evaluator/CMakeLists.txt
@@ -31,6 +31,7 @@ target_link_libraries(${PROJECT_NAME}_node glog::glog)
 if(BUILD_TESTING)
   ament_add_ros_isolated_gtest(test_${PROJECT_NAME}
     test/test_perception_online_evaluator_node.cpp
+    TIMEOUT 180
   )
   target_link_libraries(test_${PROJECT_NAME}
     ${PROJECT_NAME}_node

diff --git a/evaluator/perception_online_evaluator/README.md b/evaluator/perception_online_evaluator/README.md
@@ -13,6 +13,57 @@ This module allows for the evaluation of how accurately perception results are g
 - Calculates yaw deviation between the smoothed traveled trajectory and the perceived position to evaluate the stability of yaw recognition.
 - Calculates yaw rate based on the yaw of the object received in the previous cycle to evaluate the stability of the yaw rate recognition.
 
+### Predicted Path Deviation / Predicted Path Deviation Variance
+
+Compare the predicted path of past objects with their actual traveled path to determine the deviation. For each object, calculate the mean distance between the predicted path points and the corresponding points on the actual path, up to the specified time step. In other words, this calculates the Average Displacement Error (ADE). The target object to be evaluated is the object from $T_N$ seconds ago, where $T_N$ is the maximum value of the prediction time horizon $[T_1, T_2, ..., T_N]$.
+
+![path_deviation_each_object](./images/path_deviation_each_object.drawio.svg)
+
+$$
+\begin{align}
+n_{points} = T / dt \\
+ADE = \Sigma_{i=1}^{n_{points}} d_i / n_{points}　\\
+Var = \Sigma_{i=1}^{n_{points}} (d_i - ADE)^2 / n_{points}
+\end{align}
+$$
+
+- $n_{points}$ : Number of points in the predicted path
+- $T$ : Time horizon for prediction evaluation.
+- $dt$ : Time interval of the predicted path
+- $d_i$ : Distance between the predicted path and the actual traveled path at path point $i$
+- $ADE$ : Mean deviation of the predicted path for the target object.
+- $Var$ : Variance of the predicted path deviation for the target object.
+
+The final predicted path deviation metrics are calculated by averaging the mean deviation of the predicted path for all objects of the same class, and then calculating the mean, maximum, and minimum values of the mean deviation.
+
+![path_deviation](./images/path_deviation.drawio.svg)
+
+$$
+\begin{align}
+ADE_{mean} = \Sigma_{j=1}^{n_{objects}} ADE_j / n_{objects} \\
+ADE_{max} = max(ADE_j) \\
+ADE_{min} = min(ADE_j)
+\end{align}
+$$
+
+$$
+\begin{align}
+Var_{mean} = \Sigma_{j=1}^{n_{objects}} Var_j / n_{objects} \\
+Var_{max} = max(Var_j) \\
+Var_{min} = min(Var_j)
+\end{align}
+$$
+
+- $n_{objects}$ : Number of objects
+- $ADE_{mean}$ : Mean deviation of the predicted path through all objects
+- $ADE_{max}$ : Maximum deviation of the predicted path through all objects
+- $ADE_{min}$ : Minimum deviation of the predicted path through all objects
+- $Var_{mean}$ : Mean variance of the predicted path deviation through all objects
+- $Var_{max}$ : Maximum variance of the predicted path deviation through all objects
+- $Var_{min}$ : Minimum variance of the predicted path deviation through all objects
+
+The actual metric name is determined by the object class and time horizon. For example, `predicted_path_deviation_variance_CAR_5.00`
+
 ## Inputs / Outputs
 
 | Name              | Type                                                   | Description                                       |

diff --git a/evaluator/perception_online_evaluator/images/path_deviation.drawio.svg b/evaluator/perception_online_evaluator/images/path_deviation.drawio.svg
diff --git a/evaluator/perception_online_evaluator/images/path_deviation_each_object.drawio.svg b/evaluator/perception_online_evaluator/images/path_deviation_each_object.drawio.svg
diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp
@@ -37,6 +37,12 @@ enum class Metric {
 
 using MetricStatMap = std::unordered_map<std::string, Stat<double>>;
 
+struct PredictedPathDeviationMetrics
+{
+  Stat<double> mean;
+  Stat<double> variance;
+};
+
 static const std::unordered_map<std::string, Metric> str_to_metric = {
   {"lateral_deviation", Metric::lateral_deviation},
   {"yaw_deviation", Metric::yaw_deviation},

diff --git a/...or/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp b/...or/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp
@@ -126,7 +126,7 @@ class MetricsCalculator
   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(
+  PredictedPathDeviationMetrics calcPredictedPathDeviationMetrics(
     const PredictedObjects & objects, const double time_horizon) const;
   MetricStatMap calcYawRateMetrics(const ClassObjectsMap & class_objects_map) const;
 

diff --git a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp
@@ -152,11 +152,17 @@ std::optional<StampObjectMapIterator> MetricsCalculator::getClosestObjectIterato
 std::optional<PredictedObject> MetricsCalculator::getObjectByStamp(
   const std::string uuid, const rclcpp::Time stamp) const
 {
+  constexpr double eps = 0.01;
+  constexpr double close_time_threshold = 0.1;
+
   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;
+    if (std::abs((it->first - getClosestStamp(stamp)).seconds()) < eps) {
+      const double time_diff = std::abs((it->first - stamp).seconds());
+      if (time_diff < close_time_threshold) {
+        return it->second;
+      }
     }
   }
   return std::nullopt;
@@ -256,31 +262,34 @@ MetricStatMap MetricsCalculator::calcPredictedPathDeviationMetrics(
   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);
+      const auto metrics = 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;
+        ["predicted_path_deviation_" + convertLabelToString(label) + "_" + time_horizon_str] =
+          metrics.mean;
+      metric_stat_map
+        ["predicted_path_deviation_variance_" + convertLabelToString(label) + "_" +
+         time_horizon_str] = metrics.variance;
     }
   }
   return metric_stat_map;
 }
 
-Stat<double> MetricsCalculator::calcPredictedPathDeviationMetrics(
+PredictedPathDeviationMetrics MetricsCalculator::calcPredictedPathDeviationMetrics(
   const PredictedObjects & objects, const double time_horizon) const
 {
-  // For each object, select the predicted path that is closest to the history path and store the
-  // distance to the history path
+  // Step 1: For each object and its predicted paths, calculate the deviation between each predicted
+  // path pose and the corresponding historical path pose. Store the deviations in
+  // deviation_map_for_paths.
   std::unordered_map<std::string, std::unordered_map<size_t, std::vector<double>>>
     deviation_map_for_paths;
-  // For debugging. Save the pairs of predicted path pose and history path pose.
-  // Visualize the correspondence in rviz from the node.
+
+  // For debugging: Save the pairs of predicted path pose and history path pose.
   std::unordered_map<std::string, std::vector<std::pair<Pose, Pose>>>
     debug_predicted_path_pairs_map;
 
-  // Find the corresponding pose in the history path for each pose of the predicted path of each
-  // object, and record the distances
   const auto stamp = objects.header.stamp;
   for (const auto & object : objects.objects) {
     const auto uuid = tier4_autoware_utils::toHexString(object.object_id);
@@ -309,30 +318,36 @@ Stat<double> MetricsCalculator::calcPredictedPathDeviationMetrics(
         const double distance =
           tier4_autoware_utils::calcDistance2d(p.position, history_pose.position);
         deviation_map_for_paths[uuid][i].push_back(distance);
-        // debug
+
+        // Save debug information
         debug_predicted_path_pairs_map[path_id].push_back(std::make_pair(p, history_pose));
       }
     }
   }
 
-  // Select the predicted path with the smallest deviation for each object
+  // Step 2: For each object, select the predicted path with the smallest mean deviation.
+  // Store the selected path's deviations in deviation_map_for_objects.
   std::unordered_map<std::string, std::vector<double>> deviation_map_for_objects;
   for (const auto & [uuid, deviation_map] : deviation_map_for_paths) {
-    size_t min_deviation_index = 0;
-    double min_sum_deviation = std::numeric_limits<double>::max();
+    std::optional<std::pair<size_t, double>> min_deviation;
     for (const auto & [i, deviations] : deviation_map) {
       if (deviations.empty()) {
         continue;
       }
       const double sum = std::accumulate(deviations.begin(), deviations.end(), 0.0);
-      if (sum < min_sum_deviation) {
-        min_sum_deviation = sum;
-        min_deviation_index = i;
+      const double mean = sum / deviations.size();
+      if (!min_deviation.has_value() || mean < min_deviation.value().second) {
+        min_deviation = std::make_pair(i, mean);
       }
     }
-    deviation_map_for_objects[uuid] = deviation_map.at(min_deviation_index);
 
-    // debug: save the delayed target object and the corresponding predicted path
+    if (!min_deviation.has_value()) {
+      continue;
+    }
+
+    // Save the delayed target object and the corresponding predicted path for debugging
+    const auto [min_deviation_index, min_mean_deviation] = min_deviation.value();
+    deviation_map_for_objects[uuid] = deviation_map.at(min_deviation_index);
     const auto path_id = uuid + "_" + std::to_string(min_deviation_index);
     const auto target_stamp_object = getObjectByStamp(uuid, stamp);
     if (target_stamp_object.has_value()) {
@@ -343,17 +358,26 @@ Stat<double> MetricsCalculator::calcPredictedPathDeviationMetrics(
     }
   }
 
-  // Store the deviation as a metric
-  Stat<double> stat;
-  for (const auto & [uuid, deviations] : deviation_map_for_objects) {
-    if (deviations.empty()) {
-      continue;
-    }
-    for (const auto & deviation : deviations) {
-      stat.add(deviation);
+  // Step 3: Calculate the mean and variance of the deviations for each object's selected predicted
+  // path. Store the results in the PredictedPathDeviationMetrics structure.
+  PredictedPathDeviationMetrics metrics;
+  for (const auto & [object_id, object_path_deviations] : deviation_map_for_objects) {
+    if (!object_path_deviations.empty()) {
+      const double sum_of_deviations =
+        std::accumulate(object_path_deviations.begin(), object_path_deviations.end(), 0.0);
+      const double mean_deviation = sum_of_deviations / object_path_deviations.size();
+      metrics.mean.add(mean_deviation);
+      double sum_of_squared_deviations = 0.0;
+      for (const auto path_point_deviation : object_path_deviations) {
+        sum_of_squared_deviations += std::pow(path_point_deviation - mean_deviation, 2);
+      }
+      const double variance_deviation = sum_of_squared_deviations / object_path_deviations.size();
+
+      metrics.variance.add(variance_deviation);
     }
   }
-  return stat;
+
+  return metrics;
 }
 
 MetricStatMap MetricsCalculator::calcYawRateMetrics(const ClassObjectsMap & class_objects_map) const

diff --git a/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp b/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp
@@ -119,7 +119,7 @@ MarkerArray createDeviationLines(
   for (size_t i = 0; i < max_idx; ++i) {
     auto marker = createDefaultMarker(
       "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, first_id + i, Marker::LINE_STRIP,
-      createMarkerScale(0.005, 0.0, 0.0), createMarkerColor(r, g, b, 0.5));
+      createMarkerScale(0.01, 0.0, 0.0), createMarkerColor(r, g, b, 0.5));
     marker.points.push_back(poses1.at(i).position);
     marker.points.push_back(poses2.at(i).position);
     msg.markers.push_back(marker);

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
@@ -497,7 +497,7 @@ TEST_F(EvalTest, testYawDeviation_deviation0_PEDESTRIAN)
 }
 
 // ==========================================================================================
-// predicted path deviation{
+// predicted path deviation
 TEST_F(EvalTest, testPredictedPathDeviation_deviation0)
 {
   waitForDummyNode();
@@ -585,6 +585,145 @@ TEST_F(EvalTest, testPredictedPathDeviation_deviation0_PEDESTRIAN)
 }
 // ==========================================================================================
 
+// ==========================================================================================
+// predicted path deviation variance
+TEST_F(EvalTest, testPredictedPathDeviationVariance_deviation0)
+{
+  waitForDummyNode();
+
+  setTargetMetric("predicted_path_deviation_variance_CAR_5.00");
+
+  const auto init_objects = makeStraightPredictedObjects(0);
+  publishObjects(init_objects);
+
+  const double deviation = 0.0;
+  for (double time = time_step_; time < time_delay_; time += time_step_) {
+    const auto objects = makeDeviatedStraightPredictedObjects(time, deviation);
+    publishObjects(objects);
+  }
+  const auto last_objects = makeDeviatedStraightPredictedObjects(time_delay_, deviation);
+
+  EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), 0.0, epsilon);
+}
+
+TEST_F(EvalTest, testPredictedPathDeviationVariance_deviation1)
+{
+  waitForDummyNode();
+
+  setTargetMetric("predicted_path_deviation_variance_CAR_5.00");
+
+  const auto init_objects = makeStraightPredictedObjects(0);
+  publishObjects(init_objects);
+
+  const double deviation = 1.0;
+  for (double time = time_step_; time < time_delay_; time += time_step_) {
+    const auto objects = makeDeviatedStraightPredictedObjects(time, deviation);
+    publishObjects(objects);
+  }
+  const auto last_objects = makeDeviatedStraightPredictedObjects(time_delay_, deviation);
+
+  const double num_points = time_delay_ / time_step_ + 1;
+  // deviations
+  //   All    - 11 points (num_points)
+  //   0.0[m] -  1 points
+  //   1.0[m] - 10 points
+  const double mean_deviation = deviation * (num_points - 1) / num_points;
+  const double variance =
+    (pow(0.0 - mean_deviation, 2) + 10 * pow(1.0 - mean_deviation, 2)) / num_points;
+
+  EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), variance, epsilon);
+}
+
+TEST_F(EvalTest, testPredictedPathDeviationVariance_deviationIncreasing)
+{
+  waitForDummyNode();
+
+  setTargetMetric("predicted_path_deviation_variance_CAR_5.00");
+
+  const auto init_objects = makeStraightPredictedObjects(0);
+  publishObjects(init_objects);
+
+  const double deviation_step = 0.1;
+  double deviation = deviation_step;
+  for (double time = time_step_; time < time_delay_; time += time_step_) {
+    const auto objects = makeDeviatedStraightPredictedObjects(time, deviation);
+    publishObjects(objects);
+    deviation += deviation_step;
+  }
+  const auto last_objects = makeDeviatedStraightPredictedObjects(time_delay_, deviation);
+
+  const double num_points = time_delay_ / time_step_ + 1;
+  // deviations
+  //   All          - 11 points (num_points)
+  //   0.0[m]       -  1 points
+  //   0.1[m]       -  1 points
+  //   0.2[m]       -  1 points
+  //      :
+  //   0.9[m]       -  1 points
+  //   1.0[m]       -  1 points
+  const double mean_deviation = std::invoke([&]() {
+    double sum = 0.0;
+    for (size_t i = 0; i < num_points; ++i) {
+      sum += static_cast<double>(i) * deviation_step;
+    }
+    return sum / num_points;
+  });
+  double sum_squared_deviations = 0.0;
+  for (size_t i = 0; i < num_points; ++i) {
+    const double deviation = static_cast<double>(i) * deviation_step;
+    sum_squared_deviations += pow(deviation - mean_deviation, 2);
+  }
+  const double variance = sum_squared_deviations / num_points;
+
+  EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), variance, epsilon);
+}
+
+TEST_F(EvalTest, testPredictedPathDeviationVariance_deviationOscillating)
+{
+  waitForDummyNode();
+
+  setTargetMetric("predicted_path_deviation_variance_CAR_5.00");
+
+  const auto init_objects = makeStraightPredictedObjects(0);
+  publishObjects(init_objects);
+
+  const std::vector<double> deviations = {-0.1, -0.2, -0.1, 0.0, 0.1, 0.2, 0.1, 0.0, -0.1, -0.2};
+  for (size_t i = 0; i < deviations.size() - 1; ++i) {
+    const double time = static_cast<double>(i + 1) * time_step_;
+    const auto objects = makeDeviatedStraightPredictedObjects(time, deviations[i]);
+    publishObjects(objects);
+  }
+
+  const double num_points = deviations.size() + 1;
+  // deviations
+  //   All          - 11 points (num_points)
+  //   0.0[m]       -  1 points
+  //  -0.1[m]       -  1 points
+  //  -0.2[m]       -  1 points
+  //  -0.1[m]       -  1 points
+  //  -0.0[m]       -  1 points
+  //   0.1[m]       -  1 points
+  //   0.2[m]       -  1 points
+  //   0.1[m]       -  1 points
+  //   0.0[m]       -  1 points
+  //  -0.1[m]       -  1 points
+  //  -0.2[m]       -  1 points
+  const double mean_deviation =
+    std::accumulate(
+      deviations.begin(), deviations.end(), 0.0,
+      [](double sum, double deviation) { return sum + std::abs(deviation); }) /
+    num_points;
+
+  double sum_squared_deviations = pow(0 - mean_deviation, 2);
+  for (const auto deviation : deviations) {
+    sum_squared_deviations += pow(std::abs(deviation) - mean_deviation, 2);
+  }
+  const double variance = sum_squared_deviations / num_points;
+  const auto last_objects = makeDeviatedStraightPredictedObjects(time_delay_, deviations.back());
+  EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), variance, epsilon);
+}
+// ==========================================================================================
+
 // ==========================================================================================
 // yaw rate
 TEST_F(EvalTest, testYawRate_0)