15
15
#include " perception_online_evaluator/metrics_calculator.hpp"
16
16
17
17
#include " motion_utils/trajectory/trajectory.hpp"
18
- #include " perception_online_evaluator/utils/objects_filtering .hpp"
18
+ #include " object_recognition_utils/object_recognition_utils .hpp"
19
19
#include " tier4_autoware_utils/geometry/geometry.hpp"
20
20
21
21
#include < tier4_autoware_utils/ros/uuid_helper.hpp>
22
22
23
23
namespace perception_diagnostics
24
24
{
25
+ using object_recognition_utils::convertLabelToString;
26
+
25
27
std::optional<MetricStatMap> MetricsCalculator::calculate (const Metric & metric) const
26
28
{
27
29
if (object_map_.empty ()) {
@@ -35,14 +37,14 @@ std::optional<MetricStatMap> MetricsCalculator::calculate(const Metric & metric)
35
37
return {};
36
38
}
37
39
const auto target_objects = getObjectsByStamp (target_stamp);
38
-
40
+ const ClassObjectsMap class_objects_map = separateObjectsByClass (target_objects);
39
41
switch (metric) {
40
42
case Metric::lateral_deviation:
41
- return calcLateralDeviationMetrics (target_objects );
43
+ return calcLateralDeviationMetrics (class_objects_map );
42
44
case Metric::yaw_deviation:
43
- return calcYawDeviationMetrics (target_objects );
45
+ return calcYawDeviationMetrics (class_objects_map );
44
46
case Metric::predicted_path_deviation:
45
- return calcPredictedPathDeviationMetrics (target_objects );
47
+ return calcPredictedPathDeviationMetrics (class_objects_map );
46
48
default :
47
49
return {};
48
50
}
@@ -155,65 +157,70 @@ PredictedObjects MetricsCalculator::getObjectsByStamp(const rclcpp::Time stamp)
155
157
return objects;
156
158
}
157
159
158
- MetricStatMap MetricsCalculator::calcLateralDeviationMetrics (const PredictedObjects & objects) const
160
+ MetricStatMap MetricsCalculator::calcLateralDeviationMetrics (
161
+ const ClassObjectsMap & class_objects_map) const
159
162
{
160
- Stat<double > stat{};
161
- const auto stamp = rclcpp::Time (objects.header .stamp );
162
-
163
- for (const auto & object : objects.objects ) {
164
- const auto uuid = tier4_autoware_utils::toHexString (object.object_id );
165
- if (!hasPassedTime (uuid, stamp)) {
166
- continue ;
167
- }
168
- const auto object_pose = object.kinematics .initial_pose_with_covariance .pose ;
169
- const auto history_path = history_path_map_.at (uuid).second ;
170
- if (history_path.empty ()) {
171
- continue ;
163
+ MetricStatMap metric_stat_map{};
164
+ for (const auto & [label, objects] : class_objects_map) {
165
+ Stat<double > stat{};
166
+ const auto stamp = rclcpp::Time (objects.header .stamp );
167
+ for (const auto & object : objects.objects ) {
168
+ const auto uuid = tier4_autoware_utils::toHexString (object.object_id );
169
+ if (!hasPassedTime (uuid, stamp)) {
170
+ continue ;
171
+ }
172
+ const auto object_pose = object.kinematics .initial_pose_with_covariance .pose ;
173
+ const auto history_path = history_path_map_.at (uuid).second ;
174
+ if (history_path.empty ()) {
175
+ continue ;
176
+ }
177
+ stat.add (metrics::calcLateralDeviation (history_path, object_pose));
172
178
}
173
- stat. add ( metrics::calcLateralDeviation (history_path, object_pose)) ;
179
+ metric_stat_map[ " lateral_deviation_ " + convertLabelToString (label)] = stat ;
174
180
}
175
-
176
- MetricStatMap metric_stat_map;
177
- metric_stat_map[" lateral_deviation" ] = stat;
178
181
return metric_stat_map;
179
182
}
180
183
181
- MetricStatMap MetricsCalculator::calcYawDeviationMetrics (const PredictedObjects & objects) const
184
+ MetricStatMap MetricsCalculator::calcYawDeviationMetrics (
185
+ const ClassObjectsMap & class_objects_map) const
182
186
{
183
- Stat<double > stat{};
184
- const auto stamp = rclcpp::Time (objects.header .stamp );
185
- for (const auto & object : objects.objects ) {
186
- const auto uuid = tier4_autoware_utils::toHexString (object.object_id );
187
- if (!hasPassedTime (uuid, stamp)) {
188
- continue ;
189
- }
190
- const auto object_pose = object.kinematics .initial_pose_with_covariance .pose ;
191
- const auto history_path = history_path_map_.at (uuid).second ;
192
- if (history_path.empty ()) {
193
- continue ;
187
+ MetricStatMap metric_stat_map{};
188
+ for (const auto & [label, objects] : class_objects_map) {
189
+ Stat<double > stat{};
190
+ const auto stamp = rclcpp::Time (objects.header .stamp );
191
+ for (const auto & object : objects.objects ) {
192
+ const auto uuid = tier4_autoware_utils::toHexString (object.object_id );
193
+ if (!hasPassedTime (uuid, stamp)) {
194
+ continue ;
195
+ }
196
+ const auto object_pose = object.kinematics .initial_pose_with_covariance .pose ;
197
+ const auto history_path = history_path_map_.at (uuid).second ;
198
+ if (history_path.empty ()) {
199
+ continue ;
200
+ }
201
+ stat.add (metrics::calcYawDeviation (history_path, object_pose));
194
202
}
195
- stat. add ( metrics::calcYawDeviation (history_path, object_pose)) ;
203
+ metric_stat_map[ " yaw_deviation_ " + convertLabelToString (label)] = stat ;
196
204
}
197
-
198
- MetricStatMap metric_stat_map;
199
- metric_stat_map[" yaw_deviation" ] = stat;
200
205
return metric_stat_map;
201
206
}
202
207
203
208
MetricStatMap MetricsCalculator::calcPredictedPathDeviationMetrics (
204
- const PredictedObjects & objects ) const
209
+ const ClassObjectsMap & class_objects_map ) const
205
210
{
206
211
const auto time_horizons = parameters_->prediction_time_horizons ;
207
212
208
- MetricStatMap metric_stat_map;
209
- for (const double time_horizon : time_horizons) {
210
- const auto stat = calcPredictedPathDeviationMetrics (objects, time_horizon);
211
- std::ostringstream stream;
212
- stream << std::fixed << std::setprecision (2 ) << time_horizon;
213
- std::string time_horizon_str = stream.str ();
214
- metric_stat_map[" predicted_path_deviation_" + time_horizon_str] = stat;
213
+ MetricStatMap metric_stat_map{};
214
+ for (const auto & [label, objects] : class_objects_map) {
215
+ for (const double time_horizon : time_horizons) {
216
+ const auto stat = calcPredictedPathDeviationMetrics (objects, time_horizon);
217
+ std::ostringstream stream;
218
+ stream << std::fixed << std::setprecision (2 ) << time_horizon;
219
+ std::string time_horizon_str = stream.str ();
220
+ metric_stat_map
221
+ [" predicted_path_deviation_" + convertLabelToString (label) + " _" + time_horizon_str] = stat;
222
+ }
215
223
}
216
-
217
224
return metric_stat_map;
218
225
}
219
226
0 commit comments