Skip to content

Commit 433a59a

Browse files
kosuke55HansRobo
kosuke55
authored and
HansRobo
committed
feat(perception_online_evaluator): add perception_online_evaluator (#6493)
* feat(perception_evaluator): add perception_evaluator Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> tmp Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> update Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> add Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> add add update clean up Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> change time horizon Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * fix build werror Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * fix topic name Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * clean up Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * rename to perception_online_evaluator Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * refactor: remove timer Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * feat: add test Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * fix: ci check Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> --------- Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> Signed-off-by: Kotaro Yoshimoto <pythagora.yoshimoto@gmail.com>
1 parent 3d384b9 commit 433a59a

22 files changed

+2599
-0
lines changed

‎evaluator/perception_online_evaluator/CMakeLists.txt

+44
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,44 @@
1+
cmake_minimum_required(VERSION 3.14)
2+
project(perception_online_evaluator)
3+
4+
find_package(autoware_cmake REQUIRED)
5+
autoware_package()
6+
7+
find_package(pluginlib REQUIRED)
8+
9+
find_package(glog REQUIRED)
10+
11+
ament_auto_add_library(${PROJECT_NAME}_node SHARED
12+
src/metrics_calculator.cpp
13+
src/${PROJECT_NAME}_node.cpp
14+
src/metrics/deviation_metrics.cpp
15+
src/utils/marker_utils.cpp
16+
src/utils/objects_filtering.cpp
17+
)
18+
19+
rclcpp_components_register_node(${PROJECT_NAME}_node
20+
PLUGIN "perception_diagnostics::PerceptionOnlineEvaluatorNode"
21+
EXECUTABLE ${PROJECT_NAME}
22+
)
23+
24+
rclcpp_components_register_node(${PROJECT_NAME}_node
25+
PLUGIN "perception_diagnostics::MotionEvaluatorNode"
26+
EXECUTABLE motion_evaluator
27+
)
28+
29+
target_link_libraries(${PROJECT_NAME}_node glog::glog)
30+
31+
if(BUILD_TESTING)
32+
ament_add_ros_isolated_gtest(test_${PROJECT_NAME}
33+
test/test_perception_online_evaluator_node.cpp
34+
)
35+
target_link_libraries(test_${PROJECT_NAME}
36+
${PROJECT_NAME}_node
37+
)
38+
endif()
39+
40+
ament_auto_package(
41+
INSTALL_TO_SHARE
42+
param
43+
launch
44+
)

‎evaluator/perception_online_evaluator/README.md

+43
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,43 @@
1+
# Perception Evaluator
2+
3+
A node for evaluating the output of perception systems.
4+
5+
## Purpose
6+
7+
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.
8+
9+
## Inner-workings / Algorithms
10+
11+
- Calculates lateral deviation between the predicted path and the actual traveled trajectory.
12+
- Calculates lateral deviation between the smoothed traveled trajectory and the perceived position to evaluate the stability of lateral position recognition.
13+
- Calculates yaw deviation between the smoothed traveled trajectory and the perceived position to evaluate the stability of yaw recognition.
14+
15+
## Inputs / Outputs
16+
17+
| Name | Type | Description |
18+
| ----------------- | ------------------------------------------------------ | ------------------------------------------------- |
19+
| `~/input/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | The predicted objects to evaluate. |
20+
| `~/metrics` | `diagnostic_msgs::msg::DiagnosticArray` | Diagnostic information about perception accuracy. |
21+
| `~/markers` | `visualization_msgs::msg::MarkerArray` | Visual markers for debugging and visualization. |
22+
23+
## Parameters
24+
25+
| Name | Type | Description |
26+
| --------------------------------- | ------------ | ------------------------------------------------------------------------------------------------ |
27+
| `selected_metrics` | List | Metrics to be evaluated, such as lateral deviation, yaw deviation, and predicted path deviation. |
28+
| `smoothing_window_size` | Integer | Determines the window size for smoothing path, should be an odd number. |
29+
| `prediction_time_horizons` | list[double] | Time horizons for prediction evaluation in seconds. |
30+
| `target_object.*.check_deviation` | bool | Whether to check deviation for specific object types (car, truck, etc.). |
31+
| `debug_marker.*` | bool | Debugging parameters for marker visualization (history path, predicted path, etc.). |
32+
33+
## Assumptions / Known limits
34+
35+
It is assumed that the current positions of PredictedObjects are reasonably accurate.
36+
37+
## Future extensions / Unimplemented parts
38+
39+
- Increase rate in recognition per class
40+
- Metrics for objects with strange physical behavior (e.g., going through a fence)
41+
- Metrics for splitting objects
42+
- Metrics for problems with objects that are normally stationary but move
43+
- Disappearing object metrics

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

+53
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,53 @@
1+
// Copyright 2024 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef PERCEPTION_ONLINE_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_
16+
#define PERCEPTION_ONLINE_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_
17+
18+
#include "perception_online_evaluator/stat.hpp"
19+
20+
#include <autoware_auto_perception_msgs/msg/predicted_path.hpp>
21+
#include <geometry_msgs/msg/pose.hpp>
22+
23+
#include <vector>
24+
25+
namespace perception_diagnostics
26+
{
27+
namespace metrics
28+
{
29+
using autoware_auto_perception_msgs::msg::PredictedPath;
30+
using geometry_msgs::msg::Pose;
31+
32+
/**
33+
* @brief calculate lateral deviation of the given path from the reference path
34+
* @param [in] ref_path reference path
35+
* @param [in] pred_path predicted path
36+
* @return calculated statistics
37+
*/
38+
double calcLateralDeviation(const std::vector<Pose> & ref_path, const Pose & target_pose);
39+
40+
/**
41+
* @brief calculate yaw deviation of the given path from the reference path
42+
* @param [in] ref_path reference path
43+
* @param [in] pred_path predicted path
44+
* @return calculated statistics
45+
*/
46+
double calcYawDeviation(const std::vector<Pose> & ref_path, const Pose & target_pose);
47+
48+
std::vector<double> calcPredictedPathDeviation(
49+
const std::vector<Pose> & ref_path, const PredictedPath & pred_path);
50+
} // namespace metrics
51+
} // namespace perception_diagnostics
52+
53+
#endif // PERCEPTION_ONLINE_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_

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

+75
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,75 @@
1+
// Copyright 2024 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef PERCEPTION_ONLINE_EVALUATOR__METRICS__METRIC_HPP_
16+
#define PERCEPTION_ONLINE_EVALUATOR__METRICS__METRIC_HPP_
17+
18+
#include "perception_online_evaluator/stat.hpp"
19+
20+
#include <iostream>
21+
#include <string>
22+
#include <unordered_map>
23+
#include <vector>
24+
25+
namespace perception_diagnostics
26+
{
27+
/**
28+
* @brief Enumeration of trajectory metrics
29+
*/
30+
enum class Metric {
31+
lateral_deviation,
32+
yaw_deviation,
33+
predicted_path_deviation,
34+
SIZE,
35+
};
36+
37+
using MetricStatMap = std::unordered_map<std::string, Stat<double>>;
38+
39+
static const std::unordered_map<std::string, Metric> str_to_metric = {
40+
{"lateral_deviation", Metric::lateral_deviation},
41+
{"yaw_deviation", Metric::yaw_deviation},
42+
{"predicted_path_deviation", Metric::predicted_path_deviation}};
43+
44+
static const std::unordered_map<Metric, std::string> metric_to_str = {
45+
{Metric::lateral_deviation, "lateral_deviation"},
46+
{Metric::yaw_deviation, "yaw_deviation"},
47+
{Metric::predicted_path_deviation, "predicted_path_deviation"}};
48+
49+
// Metrics descriptions
50+
static const std::unordered_map<Metric, std::string> metric_descriptions = {
51+
{Metric::lateral_deviation, "Lateral_deviation[m]"},
52+
{Metric::yaw_deviation, "Yaw_deviation[rad]"},
53+
{Metric::predicted_path_deviation, "Predicted_path_deviation[m]"}};
54+
55+
namespace details
56+
{
57+
static struct CheckCorrectMaps
58+
{
59+
CheckCorrectMaps()
60+
{
61+
if (
62+
str_to_metric.size() != static_cast<size_t>(Metric::SIZE) ||
63+
metric_to_str.size() != static_cast<size_t>(Metric::SIZE) ||
64+
metric_descriptions.size() != static_cast<size_t>(Metric::SIZE)) {
65+
std::cerr << "[metrics/metrics.hpp] Maps are not defined for all metrics: ";
66+
std::cerr << str_to_metric.size() << " " << metric_to_str.size() << " "
67+
<< metric_descriptions.size() << std::endl;
68+
}
69+
}
70+
} check;
71+
72+
} // namespace details
73+
} // namespace perception_diagnostics
74+
75+
#endif // PERCEPTION_ONLINE_EVALUATOR__METRICS__METRIC_HPP_

‎evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp

+141
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,141 @@
1+
// Copyright 2024 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef PERCEPTION_ONLINE_EVALUATOR__METRICS_CALCULATOR_HPP_
16+
#define PERCEPTION_ONLINE_EVALUATOR__METRICS_CALCULATOR_HPP_
17+
18+
#include "perception_online_evaluator/metrics/deviation_metrics.hpp"
19+
#include "perception_online_evaluator/metrics/metric.hpp"
20+
#include "perception_online_evaluator/parameters.hpp"
21+
#include "perception_online_evaluator/stat.hpp"
22+
23+
#include <rclcpp/time.hpp>
24+
25+
#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp"
26+
#include "geometry_msgs/msg/pose.hpp"
27+
#include <unique_identifier_msgs/msg/uuid.hpp>
28+
29+
#include <algorithm>
30+
#include <map>
31+
#include <optional>
32+
#include <utility>
33+
#include <vector>
34+
35+
namespace perception_diagnostics
36+
{
37+
using autoware_auto_perception_msgs::msg::PredictedObject;
38+
using autoware_auto_perception_msgs::msg::PredictedObjects;
39+
using geometry_msgs::msg::Point;
40+
using geometry_msgs::msg::Pose;
41+
using unique_identifier_msgs::msg::UUID;
42+
43+
struct ObjectData
44+
{
45+
PredictedObject object;
46+
std::vector<std::pair<Pose, Pose>> path_pairs;
47+
48+
std::vector<Pose> getPredictedPath() const
49+
{
50+
std::vector<Pose> path;
51+
path.resize(path_pairs.size());
52+
std::transform(
53+
path_pairs.begin(), path_pairs.end(), path.begin(),
54+
[](const std::pair<Pose, Pose> & pair) -> Pose { return pair.first; });
55+
return path;
56+
}
57+
58+
std::vector<Pose> getHistoryPath() const
59+
{
60+
std::vector<Pose> path;
61+
path.resize(path_pairs.size());
62+
std::transform(
63+
path_pairs.begin(), path_pairs.end(), path.begin(),
64+
[](const std::pair<Pose, Pose> & pair) -> Pose { return pair.second; });
65+
return path;
66+
}
67+
};
68+
using ObjectDataMap = std::unordered_map<std::string, ObjectData>;
69+
70+
// pair of history_path and smoothed_history_path for each object id
71+
using HistoryPathMap =
72+
std::unordered_map<std::string, std::pair<std::vector<Pose>, std::vector<Pose>>>;
73+
74+
class MetricsCalculator
75+
{
76+
public:
77+
explicit MetricsCalculator(const std::shared_ptr<Parameters> & parameters)
78+
: parameters_(parameters){};
79+
80+
/**
81+
* @brief calculate
82+
* @param [in] metric Metric enum value
83+
* @return map of string describing the requested metric and the calculated value
84+
*/
85+
std::optional<MetricStatMap> calculate(const Metric & metric) const;
86+
87+
/**
88+
* @brief set the dynamic objects used to calculate obstacle metrics
89+
* @param [in] objects predicted objects
90+
*/
91+
void setPredictedObjects(const PredictedObjects & objects);
92+
93+
HistoryPathMap getHistoryPathMap() const { return history_path_map_; }
94+
ObjectDataMap getDebugObjectData() const { return debug_target_object_; }
95+
96+
private:
97+
std::shared_ptr<Parameters> parameters_;
98+
99+
// Store predicted objects information and calculation results
100+
std::unordered_map<std::string, std::map<rclcpp::Time, PredictedObject>> object_map_;
101+
HistoryPathMap history_path_map_;
102+
103+
rclcpp::Time current_stamp_;
104+
105+
// debug
106+
mutable ObjectDataMap debug_target_object_;
107+
108+
// Functions to calculate history path
109+
void updateHistoryPath();
110+
std::vector<Pose> averageFilterPath(
111+
const std::vector<Pose> & path, const size_t window_size) const;
112+
std::vector<Pose> generateHistoryPathWithPrev(
113+
const std::vector<Pose> & prev_history_path, const Pose & new_pose, const size_t window_size);
114+
115+
// Update object data
116+
void updateObjects(
117+
const std::string uuid, const rclcpp::Time stamp, const PredictedObject & object);
118+
void deleteOldObjects(const rclcpp::Time stamp);
119+
120+
// Calculate metrics
121+
MetricStatMap calcLateralDeviationMetrics(const PredictedObjects & objects) const;
122+
MetricStatMap calcYawDeviationMetrics(const PredictedObjects & objects) const;
123+
MetricStatMap calcPredictedPathDeviationMetrics(const PredictedObjects & objects) const;
124+
Stat<double> calcPredictedPathDeviationMetrics(
125+
const PredictedObjects & objects, const double time_horizon) const;
126+
127+
bool hasPassedTime(const rclcpp::Time stamp) const;
128+
bool hasPassedTime(const std::string uuid, const rclcpp::Time stamp) const;
129+
double getTimeDelay() const;
130+
131+
// Extract object
132+
rclcpp::Time getClosestStamp(const rclcpp::Time stamp) const;
133+
std::optional<PredictedObject> getObjectByStamp(
134+
const std::string uuid, const rclcpp::Time stamp) const;
135+
PredictedObjects getObjectsByStamp(const rclcpp::Time stamp) const;
136+
137+
}; // class MetricsCalculator
138+
139+
} // namespace perception_diagnostics
140+
141+
#endif // PERCEPTION_ONLINE_EVALUATOR__METRICS_CALCULATOR_HPP_

0 commit comments

Comments
 (0)