1515#include " perception_online_evaluator/metrics_calculator.hpp"
1616
1717#include " motion_utils/trajectory/trajectory.hpp"
18- #include " perception_online_evaluator/utils/objects_filtering .hpp"
18+ #include " object_recognition_utils/object_recognition_utils .hpp"
1919#include " tier4_autoware_utils/geometry/geometry.hpp"
2020
2121#include < tier4_autoware_utils/ros/uuid_helper.hpp>
2222
2323namespace perception_diagnostics
2424{
25+ using object_recognition_utils::convertLabelToString;
26+
2527std::optional<MetricStatMap> MetricsCalculator::calculate (const Metric & metric) const
2628{
2729 if (object_map_.empty ()) {
@@ -35,14 +37,14 @@ std::optional<MetricStatMap> MetricsCalculator::calculate(const Metric & metric)
3537 return {};
3638 }
3739 const auto target_objects = getObjectsByStamp (target_stamp);
38-
40+ const ClassObjectsMap class_objects_map = separateObjectsByClass (target_objects);
3941 switch (metric) {
4042 case Metric::lateral_deviation:
41- return calcLateralDeviationMetrics (target_objects );
43+ return calcLateralDeviationMetrics (class_objects_map );
4244 case Metric::yaw_deviation:
43- return calcYawDeviationMetrics (target_objects );
45+ return calcYawDeviationMetrics (class_objects_map );
4446 case Metric::predicted_path_deviation:
45- return calcPredictedPathDeviationMetrics (target_objects );
47+ return calcPredictedPathDeviationMetrics (class_objects_map );
4648 default :
4749 return {};
4850 }
@@ -155,65 +157,70 @@ PredictedObjects MetricsCalculator::getObjectsByStamp(const rclcpp::Time stamp)
155157 return objects;
156158}
157159
158- MetricStatMap MetricsCalculator::calcLateralDeviationMetrics (const PredictedObjects & objects) const
160+ MetricStatMap MetricsCalculator::calcLateralDeviationMetrics (
161+ const ClassObjectsMap & class_objects_map) const
159162{
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));
172178 }
173- stat. add ( metrics::calcLateralDeviation (history_path, object_pose)) ;
179+ metric_stat_map[ " lateral_deviation_ " + convertLabelToString (label)] = stat ;
174180 }
175-
176- MetricStatMap metric_stat_map;
177- metric_stat_map[" lateral_deviation"
178181 return metric_stat_map;
179182}
180183
181- MetricStatMap MetricsCalculator::calcYawDeviationMetrics (const PredictedObjects & objects) const
184+ MetricStatMap MetricsCalculator::calcYawDeviationMetrics (
185+ const ClassObjectsMap & class_objects_map) const
182186{
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));
194202 }
195- stat. add ( metrics::calcYawDeviation (history_path, object_pose)) ;
203+ metric_stat_map[ " yaw_deviation_ " + convertLabelToString (label)] = stat ;
196204 }
197-
198- MetricStatMap metric_stat_map;
199- metric_stat_map[" yaw_deviation"
200205 return metric_stat_map;
201206}
202207
203208MetricStatMap MetricsCalculator::calcPredictedPathDeviationMetrics (
204- const PredictedObjects & objects ) const
209+ const ClassObjectsMap & class_objects_map ) const
205210{
206211 const auto time_horizons = parameters_->prediction_time_horizons ;
207212
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_"
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) + " _"
222+ }
215223 }
216-
217224 return metric_stat_map;
218225}
219226
0 commit comments