feat(perception_online_evaluator): add perception_online_evaluator

evaluator/perception_online_evaluator/CMakeLists.txt

@@ -0,0 +1,44 @@
+cmake_minimum_required(VERSION 3.14)
+project(perception_online_evaluator)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+find_package(pluginlib REQUIRED)
+
+find_package(glog REQUIRED)
+
+ament_auto_add_library(${PROJECT_NAME}_node SHARED
+  src/metrics_calculator.cpp
+  src/${PROJECT_NAME}_node.cpp
+  src/metrics/deviation_metrics.cpp
+  src/utils/marker_utils.cpp
+  src/utils/objects_filtering.cpp
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}_node
+  PLUGIN "perception_diagnostics::PerceptionOnlineEvaluatorNode"
+  EXECUTABLE ${PROJECT_NAME}
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}_node
+  PLUGIN "perception_diagnostics::MotionEvaluatorNode"
+  EXECUTABLE motion_evaluator
+)
+
+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
+  )
+  target_link_libraries(test_${PROJECT_NAME}
+    ${PROJECT_NAME}_node
+  )
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+    param
+    launch
+)

evaluator/perception_online_evaluator/README.md

@@ -0,0 +1,43 @@
+# Perception Evaluator
+
+A node for evaluating the output of perception systems.
+
+## Purpose
+
+This module allows for the evaluation of how accurately perception results are generated without the need for annotations. It is capable of confirming performance and can evaluate results from a few seconds prior, enabling online execution.
+
+## Inner-workings / Algorithms
+
+- Calculates lateral deviation between the predicted path and the actual traveled trajectory.
+- Calculates lateral deviation between the smoothed traveled trajectory and the perceived position to evaluate the stability of lateral position recognition.
+- Calculates yaw deviation between the smoothed traveled trajectory and the perceived position to evaluate the stability of yaw recognition.
+
+## Inputs / Outputs
+
+| Name              | Type                                                   | Description                                       |
+| ----------------- | ------------------------------------------------------ | ------------------------------------------------- |
+| `~/input/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | The predicted objects to evaluate.                |
+| `~/metrics`       | `diagnostic_msgs::msg::DiagnosticArray`                | Diagnostic information about perception accuracy. |
+| `~/markers`       | `visualization_msgs::msg::MarkerArray`                 | Visual markers for debugging and visualization.   |
+
+## Parameters
+
+| Name                              | Type         | Description                                                                                      |
+| --------------------------------- | ------------ | ------------------------------------------------------------------------------------------------ |
+| `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.                                              |
+| `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.).              |
+
+## Assumptions / Known limits
+
+It is assumed that the current positions of PredictedObjects are reasonably accurate.
+
+## Future extensions / Unimplemented parts
+
+- Increase rate in recognition per class
+- Metrics for objects with strange physical behavior (e.g., going through a fence)
+- Metrics for splitting objects
+- Metrics for problems with objects that are normally stationary but move
+- Disappearing object metrics

evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/deviation_metrics.hpp

@@ -0,0 +1,53 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef PERCEPTION_ONLINE_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_
+#define PERCEPTION_ONLINE_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_
+
+#include "perception_online_evaluator/stat.hpp"
+
+#include <autoware_auto_perception_msgs/msg/predicted_path.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+
+#include <vector>
+
+namespace perception_diagnostics
+{
+namespace metrics
+{
+using autoware_auto_perception_msgs::msg::PredictedPath;
+using geometry_msgs::msg::Pose;
+
+/**
+ * @brief calculate lateral deviation of the given path from the reference path
+ * @param [in] ref_path reference path
+ * @param [in] pred_path predicted path
+ * @return calculated statistics
+ */
+double calcLateralDeviation(const std::vector<Pose> & ref_path, const Pose & target_pose);
+
+/**
+ * @brief calculate yaw deviation of the given path from the reference path
+ * @param [in] ref_path reference path
+ * @param [in] pred_path predicted path
+ * @return calculated statistics
+ */
+double calcYawDeviation(const std::vector<Pose> & ref_path, const Pose & target_pose);
+
+std::vector<double> calcPredictedPathDeviation(
+  const std::vector<Pose> & ref_path, const PredictedPath & pred_path);
+}  // namespace metrics
+}  // namespace perception_diagnostics
+
+#endif  // PERCEPTION_ONLINE_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_

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

@@ -0,0 +1,75 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef PERCEPTION_ONLINE_EVALUATOR__METRICS__METRIC_HPP_
+#define PERCEPTION_ONLINE_EVALUATOR__METRICS__METRIC_HPP_
+
+#include "perception_online_evaluator/stat.hpp"
+
+#include <iostream>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+namespace perception_diagnostics
+{
+/**
+ * @brief Enumeration of trajectory metrics
+ */
+enum class Metric {
+  lateral_deviation,
+  yaw_deviation,
+  predicted_path_deviation,
+  SIZE,
+};
+
+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}};
+
+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"}};
+
+// 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]"}};
+
+namespace details
+{
+static struct CheckCorrectMaps
+{
+  CheckCorrectMaps()
+  {
+    if (
+      str_to_metric.size() != static_cast<size_t>(Metric::SIZE) ||
+      metric_to_str.size() != static_cast<size_t>(Metric::SIZE) ||
+      metric_descriptions.size() != static_cast<size_t>(Metric::SIZE)) {
+      std::cerr << "[metrics/metrics.hpp] Maps are not defined for all metrics: ";
+      std::cerr << str_to_metric.size() << " " << metric_to_str.size() << " "
+                << metric_descriptions.size() << std::endl;
+    }
+  }
+} check;
+
+}  // namespace details
+}  // namespace perception_diagnostics
+
+#endif  // PERCEPTION_ONLINE_EVALUATOR__METRICS__METRIC_HPP_

evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp

@@ -0,0 +1,141 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef PERCEPTION_ONLINE_EVALUATOR__METRICS_CALCULATOR_HPP_
+#define PERCEPTION_ONLINE_EVALUATOR__METRICS_CALCULATOR_HPP_
+
+#include "perception_online_evaluator/metrics/deviation_metrics.hpp"
+#include "perception_online_evaluator/metrics/metric.hpp"
+#include "perception_online_evaluator/parameters.hpp"
+#include "perception_online_evaluator/stat.hpp"
+
+#include <rclcpp/time.hpp>
+
+#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp"
+#include "geometry_msgs/msg/pose.hpp"
+#include <unique_identifier_msgs/msg/uuid.hpp>
+
+#include <algorithm>
+#include <map>
+#include <optional>
+#include <utility>
+#include <vector>
+
+namespace perception_diagnostics
+{
+using autoware_auto_perception_msgs::msg::PredictedObject;
+using autoware_auto_perception_msgs::msg::PredictedObjects;
+using geometry_msgs::msg::Point;
+using geometry_msgs::msg::Pose;
+using unique_identifier_msgs::msg::UUID;
+
+struct ObjectData
+{
+  PredictedObject object;
+  std::vector<std::pair<Pose, Pose>> path_pairs;
+
+  std::vector<Pose> getPredictedPath() const
+  {
+    std::vector<Pose> path;
+    path.resize(path_pairs.size());
+    std::transform(
+      path_pairs.begin(), path_pairs.end(), path.begin(),
+      [](const std::pair<Pose, Pose> & pair) -> Pose { return pair.first; });
+    return path;
+  }
+
+  std::vector<Pose> getHistoryPath() const
+  {
+    std::vector<Pose> path;
+    path.resize(path_pairs.size());
+    std::transform(
+      path_pairs.begin(), path_pairs.end(), path.begin(),
+      [](const std::pair<Pose, Pose> & pair) -> Pose { return pair.second; });
+    return path;
+  }
+};
+using ObjectDataMap = std::unordered_map<std::string, ObjectData>;
+
+// pair of history_path and smoothed_history_path for each object id
+using HistoryPathMap =
+  std::unordered_map<std::string, std::pair<std::vector<Pose>, std::vector<Pose>>>;
+
+class MetricsCalculator
+{
+public:
+  explicit MetricsCalculator(const std::shared_ptr<Parameters> & parameters)
+  : parameters_(parameters){};
+
+  /**
+   * @brief calculate
+   * @param [in] metric Metric enum value
+   * @return map of string describing the requested metric and the calculated value
+   */
+  std::optional<MetricStatMap> calculate(const Metric & metric) const;
+
+  /**
+   * @brief set the dynamic objects used to calculate obstacle metrics
+   * @param [in] objects predicted objects
+   */
+  void setPredictedObjects(const PredictedObjects & objects);
+
+  HistoryPathMap getHistoryPathMap() const { return history_path_map_; }
+  ObjectDataMap getDebugObjectData() const { return debug_target_object_; }
+
+private:
+  std::shared_ptr<Parameters> parameters_;
+
+  // Store predicted objects information and calculation results
+  std::unordered_map<std::string, std::map<rclcpp::Time, PredictedObject>> object_map_;
+  HistoryPathMap history_path_map_;
+
+  rclcpp::Time current_stamp_;
+
+  // debug
+  mutable ObjectDataMap debug_target_object_;
+
+  // Functions to calculate history path
+  void updateHistoryPath();
+  std::vector<Pose> averageFilterPath(
+    const std::vector<Pose> & path, const size_t window_size) const;
+  std::vector<Pose> generateHistoryPathWithPrev(
+    const std::vector<Pose> & prev_history_path, const Pose & new_pose, const size_t window_size);
+
+  // Update object data
+  void updateObjects(
+    const std::string uuid, const rclcpp::Time stamp, const PredictedObject & object);
+  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;
+  Stat<double> calcPredictedPathDeviationMetrics(
+    const PredictedObjects & objects, const double time_horizon) 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<PredictedObject> getObjectByStamp(
+    const std::string uuid, const rclcpp::Time stamp) const;
+  PredictedObjects getObjectsByStamp(const rclcpp::Time stamp) const;
+
+};  // class MetricsCalculator
+
+}  // namespace perception_diagnostics
+
+#endif  // PERCEPTION_ONLINE_EVALUATOR__METRICS_CALCULATOR_HPP_