diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp index dd6756a17f194..20397448bf21a 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp @@ -19,6 +19,7 @@ #include "perception_online_evaluator/metrics/metric.hpp" #include "perception_online_evaluator/parameters.hpp" #include "perception_online_evaluator/stat.hpp" +#include "perception_online_evaluator/utils/objects_filtering.hpp" #include @@ -118,9 +119,9 @@ class MetricsCalculator 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; + 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 calcPredictedPathDeviationMetrics( const PredictedObjects & objects, const double time_horizon) const; diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/objects_filtering.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/objects_filtering.hpp index 5d4238f45dec9..8a118a14d7368 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/objects_filtering.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/objects_filtering.hpp @@ -37,6 +37,8 @@ using autoware_auto_perception_msgs::msg::ObjectClassification; using autoware_auto_perception_msgs::msg::PredictedObject; using autoware_auto_perception_msgs::msg::PredictedObjects; +using ClassObjectsMap = std::unordered_map; + std::uint8_t getHighestProbLabel(const std::vector & classification); /** @@ -105,6 +107,14 @@ void filterObjects(PredictedObjects & objects, Func filter) filterObjects(objects, removed_objects, filter); } +/** + * @brief Separates the provided objects based on their classification. + * + * @param objects The predicted objects to be separated. + * @return A map of objects separated by their classification. + */ +ClassObjectsMap separateObjectsByClass(const PredictedObjects & objects); + /** * @brief Filters the provided objects based on their classification. * diff --git a/evaluator/perception_online_evaluator/package.xml b/evaluator/perception_online_evaluator/package.xml index bc0f7ef82049b..54281358219d1 100644 --- a/evaluator/perception_online_evaluator/package.xml +++ b/evaluator/perception_online_evaluator/package.xml @@ -24,6 +24,7 @@ libgoogle-glog-dev motion_utils nav_msgs + object_recognition_utils pluginlib rclcpp rclcpp_components diff --git a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp index bec6d84fcd7dd..3afcdc0cd3185 100644 --- a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp +++ b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp @@ -15,13 +15,15 @@ #include "perception_online_evaluator/metrics_calculator.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "perception_online_evaluator/utils/objects_filtering.hpp" +#include "object_recognition_utils/object_recognition_utils.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include namespace perception_diagnostics { +using object_recognition_utils::convertLabelToString; + std::optional MetricsCalculator::calculate(const Metric & metric) const { if (object_map_.empty()) { @@ -35,14 +37,14 @@ std::optional MetricsCalculator::calculate(const Metric & metric) return {}; } const auto target_objects = getObjectsByStamp(target_stamp); - + const ClassObjectsMap class_objects_map = separateObjectsByClass(target_objects); switch (metric) { case Metric::lateral_deviation: - return calcLateralDeviationMetrics(target_objects); + return calcLateralDeviationMetrics(class_objects_map); case Metric::yaw_deviation: - return calcYawDeviationMetrics(target_objects); + return calcYawDeviationMetrics(class_objects_map); case Metric::predicted_path_deviation: - return calcPredictedPathDeviationMetrics(target_objects); + return calcPredictedPathDeviationMetrics(class_objects_map); default: return {}; } @@ -155,65 +157,70 @@ PredictedObjects MetricsCalculator::getObjectsByStamp(const rclcpp::Time stamp) return objects; } -MetricStatMap MetricsCalculator::calcLateralDeviationMetrics(const PredictedObjects & objects) const +MetricStatMap MetricsCalculator::calcLateralDeviationMetrics( + const ClassObjectsMap & class_objects_map) const { - Stat stat{}; - const auto stamp = rclcpp::Time(objects.header.stamp); - - for (const auto & object : objects.objects) { - const auto uuid = tier4_autoware_utils::toHexString(object.object_id); - if (!hasPassedTime(uuid, stamp)) { - continue; - } - const auto object_pose = object.kinematics.initial_pose_with_covariance.pose; - const auto history_path = history_path_map_.at(uuid).second; - if (history_path.empty()) { - continue; + MetricStatMap metric_stat_map{}; + for (const auto & [label, objects] : class_objects_map) { + Stat stat{}; + const auto stamp = rclcpp::Time(objects.header.stamp); + for (const auto & object : objects.objects) { + const auto uuid = tier4_autoware_utils::toHexString(object.object_id); + if (!hasPassedTime(uuid, stamp)) { + continue; + } + const auto object_pose = object.kinematics.initial_pose_with_covariance.pose; + const auto history_path = history_path_map_.at(uuid).second; + if (history_path.empty()) { + continue; + } + stat.add(metrics::calcLateralDeviation(history_path, object_pose)); } - stat.add(metrics::calcLateralDeviation(history_path, object_pose)); + metric_stat_map["lateral_deviation_" + convertLabelToString(label)] = stat; } - - MetricStatMap metric_stat_map; - metric_stat_map["lateral_deviation"] = stat; return metric_stat_map; } -MetricStatMap MetricsCalculator::calcYawDeviationMetrics(const PredictedObjects & objects) const +MetricStatMap MetricsCalculator::calcYawDeviationMetrics( + const ClassObjectsMap & class_objects_map) const { - Stat stat{}; - const auto stamp = rclcpp::Time(objects.header.stamp); - for (const auto & object : objects.objects) { - const auto uuid = tier4_autoware_utils::toHexString(object.object_id); - if (!hasPassedTime(uuid, stamp)) { - continue; - } - const auto object_pose = object.kinematics.initial_pose_with_covariance.pose; - const auto history_path = history_path_map_.at(uuid).second; - if (history_path.empty()) { - continue; + MetricStatMap metric_stat_map{}; + for (const auto & [label, objects] : class_objects_map) { + Stat stat{}; + const auto stamp = rclcpp::Time(objects.header.stamp); + for (const auto & object : objects.objects) { + const auto uuid = tier4_autoware_utils::toHexString(object.object_id); + if (!hasPassedTime(uuid, stamp)) { + continue; + } + const auto object_pose = object.kinematics.initial_pose_with_covariance.pose; + const auto history_path = history_path_map_.at(uuid).second; + if (history_path.empty()) { + continue; + } + stat.add(metrics::calcYawDeviation(history_path, object_pose)); } - stat.add(metrics::calcYawDeviation(history_path, object_pose)); + metric_stat_map["yaw_deviation_" + convertLabelToString(label)] = stat; } - - MetricStatMap metric_stat_map; - metric_stat_map["yaw_deviation"] = stat; return metric_stat_map; } MetricStatMap MetricsCalculator::calcPredictedPathDeviationMetrics( - const PredictedObjects & objects) const + const ClassObjectsMap & class_objects_map) const { const auto time_horizons = parameters_->prediction_time_horizons; - MetricStatMap metric_stat_map; - for (const double time_horizon : time_horizons) { - const auto stat = 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_" + time_horizon_str] = stat; + 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); + 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; + } } - return metric_stat_map; } diff --git a/evaluator/perception_online_evaluator/src/utils/objects_filtering.cpp b/evaluator/perception_online_evaluator/src/utils/objects_filtering.cpp index 97daee36175f3..e39ba348ee42b 100644 --- a/evaluator/perception_online_evaluator/src/utils/objects_filtering.cpp +++ b/evaluator/perception_online_evaluator/src/utils/objects_filtering.cpp @@ -83,6 +83,17 @@ bool isTargetObjectType( (t == ObjectClassification::PEDESTRIAN && target_object_types.check_pedestrian)); } +ClassObjectsMap separateObjectsByClass(const PredictedObjects & objects) +{ + ClassObjectsMap separated_objects; + for (const auto & object : objects.objects) { + const auto label = getHighestProbLabel(object.classification); + separated_objects[label].objects.push_back(object); + separated_objects[label].header = objects.header; + } + return separated_objects; +} + void filterObjectsByClass( PredictedObjects & objects, const ObjectTypesToCheck & target_object_types) { 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 index 095fb130426f0..1bfd27828c98e 100644 --- a/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp +++ b/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp @@ -105,21 +105,20 @@ class EvalTest : public ::testing::Test [=](const DiagnosticArray::ConstSharedPtr msg) { const auto it = std::find_if(msg->status.begin(), msg->status.end(), is_target_metric); if (it != msg->status.end()) { - std::cerr << it->values[0].key << " " << it->values[0].value << " " << it->values[1].key - << " " << it->values[1].value << " " << it->values[2].key << " " - << it->values[2].value << std::endl; metric_value_ = boost::lexical_cast(it->values[2].value); metric_updated_ = true; } }); } - PredictedObject makePredictedObject(const std::vector> & predicted_path) + PredictedObject makePredictedObject( + const std::vector> & predicted_path, + const uint8_t label = ObjectClassification::CAR) { PredictedObject object; object.object_id = uuid_; ObjectClassification classification; - classification.label = ObjectClassification::CAR; + classification.label = label; classification.probability = 1.0; object.classification = {classification}; @@ -153,32 +152,35 @@ class EvalTest : public ::testing::Test } PredictedObjects makePredictedObjects( - const std::vector> & predicted_path) + const std::vector> & predicted_path, + const uint8_t label = ObjectClassification::CAR) { PredictedObjects objects; - objects.objects.push_back(makePredictedObject(predicted_path)); + objects.objects.push_back(makePredictedObject(predicted_path, label)); objects.header.stamp = rclcpp::Time(0); return objects; } - PredictedObjects makeStraightPredictedObjects(const double time) + PredictedObjects makeStraightPredictedObjects( + const double time, const uint8_t label = ObjectClassification::CAR) { std::vector> predicted_path; for (size_t i = 0; i <= time_horizon_ / time_step_; i++) { predicted_path.push_back({velocity_ * (time + i * time_step_), 0.0}); } - auto objects = makePredictedObjects(predicted_path); + auto objects = makePredictedObjects(predicted_path, label); objects.header.stamp = rclcpp::Time(0) + rclcpp::Duration::from_seconds(time); return objects; } - PredictedObjects makeDeviatedStraightPredictedObjects(const double time, const double deviation) + PredictedObjects makeDeviatedStraightPredictedObjects( + const double time, const double deviation, const uint8_t label = ObjectClassification::CAR) { std::vector> predicted_path; for (size_t i = 0; i <= time_horizon_ / time_step_; i++) { predicted_path.push_back({velocity_ * (time + i * time_step_), deviation}); } - auto objects = makePredictedObjects(predicted_path); + auto objects = makePredictedObjects(predicted_path, label); objects.header.stamp = rclcpp::Time(0) + rclcpp::Duration::from_seconds(time); return objects; } @@ -255,7 +257,7 @@ class EvalTest : public ::testing::Test TEST_F(EvalTest, testLateralDeviation_deviation0) { waitForDummyNode(); - setTargetMetric("lateral_deviation"); + setTargetMetric("lateral_deviation_CAR"); const double deviation = 0.0; for (double time = 0; time < time_delay_; time += time_step_) { @@ -270,7 +272,7 @@ TEST_F(EvalTest, testLateralDeviation_deviation0) TEST_F(EvalTest, testLateralDeviation_deviation1) { waitForDummyNode(); - setTargetMetric("lateral_deviation"); + setTargetMetric("lateral_deviation_CAR"); const double deviation = 1.0; for (double time = 0; time < time_delay_; time += time_step_) { @@ -285,7 +287,7 @@ TEST_F(EvalTest, testLateralDeviation_deviation1) TEST_F(EvalTest, testLateralDeviation_oscillation) { waitForDummyNode(); - setTargetMetric("lateral_deviation"); + setTargetMetric("lateral_deviation_CAR"); const double deviation = 1.0; double sign = 1.0; @@ -307,7 +309,7 @@ TEST_F(EvalTest, testLateralDeviation_oscillation) TEST_F(EvalTest, testLateralDeviation_distortion) { waitForDummyNode(); - setTargetMetric("lateral_deviation"); + setTargetMetric("lateral_deviation_CAR"); const double deviation = 1.0; for (double time = 0; time < time_delay_ * 2; time += time_step_) { @@ -325,6 +327,23 @@ TEST_F(EvalTest, testLateralDeviation_distortion) const auto last_objects = makeDeviatedStraightPredictedObjects(time_delay_ * 2, deviation); EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), deviation, epsilon); } + +TEST_F(EvalTest, testLateralDeviation_deviation0_PEDESTRIAN) +{ + waitForDummyNode(); + setTargetMetric("lateral_deviation_PEDESTRIAN"); + + const double deviation = 0.0; + for (double time = 0; time < time_delay_; time += time_step_) { + const auto objects = + makeDeviatedStraightPredictedObjects(time, deviation, ObjectClassification::PEDESTRIAN); + publishObjects(objects); + } + + const auto last_objects = + makeDeviatedStraightPredictedObjects(time_delay_, deviation, ObjectClassification::PEDESTRIAN); + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), 0.0, epsilon); +} // ========================================================================================== // ========================================================================================== @@ -332,7 +351,7 @@ TEST_F(EvalTest, testLateralDeviation_distortion) TEST_F(EvalTest, testYawDeviation_deviation0) { waitForDummyNode(); - setTargetMetric("yaw_deviation"); + setTargetMetric("yaw_deviation_CAR"); const double deviation = 0.0; for (double time = 0; time < time_delay_; time += time_step_) { @@ -347,7 +366,7 @@ TEST_F(EvalTest, testYawDeviation_deviation0) TEST_F(EvalTest, testYawDeviation_deviation1) { waitForDummyNode(); - setTargetMetric("yaw_deviation"); + setTargetMetric("yaw_deviation_CAR"); const double deviation = 1.0; for (double time = 0; time < time_delay_; time += time_step_) { @@ -362,7 +381,7 @@ TEST_F(EvalTest, testYawDeviation_deviation1) TEST_F(EvalTest, testYawDeviation_oscillation) { waitForDummyNode(); - setTargetMetric("yaw_deviation"); + setTargetMetric("yaw_deviation_CAR"); const double deviation = 1.0; double sign = 1.0; @@ -384,7 +403,7 @@ TEST_F(EvalTest, testYawDeviation_oscillation) TEST_F(EvalTest, testYawDeviation_distortion) { waitForDummyNode(); - setTargetMetric("yaw_deviation"); + setTargetMetric("yaw_deviation_CAR"); const double deviation = 1.0; for (double time = 0; time < time_delay_ * 2; time += time_step_) { @@ -406,7 +425,7 @@ TEST_F(EvalTest, testYawDeviation_distortion) TEST_F(EvalTest, testYawDeviation_oscillation_rotate) { waitForDummyNode(); - setTargetMetric("yaw_deviation"); + setTargetMetric("yaw_deviation_CAR"); const double deviation = 1.0; const double yaw = M_PI / 4; @@ -431,7 +450,7 @@ TEST_F(EvalTest, testYawDeviation_oscillation_rotate) TEST_F(EvalTest, testYawDeviation_distortion_rotate) { waitForDummyNode(); - setTargetMetric("yaw_deviation"); + setTargetMetric("yaw_deviation_CAR"); const double deviation = 1.0; const double yaw = M_PI / 4; @@ -454,13 +473,30 @@ TEST_F(EvalTest, testYawDeviation_distortion_rotate) EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), yaw, epsilon); } +TEST_F(EvalTest, testYawDeviation_deviation0_PEDESTRIAN) +{ + waitForDummyNode(); + setTargetMetric("yaw_deviation_PEDESTRIAN"); + + const double deviation = 0.0; + for (double time = 0; time < time_delay_; time += time_step_) { + const auto objects = + makeDeviatedStraightPredictedObjects(time, deviation, ObjectClassification::PEDESTRIAN); + publishObjects(objects); + } + + const auto last_objects = + makeDeviatedStraightPredictedObjects(time_delay_, deviation, ObjectClassification::PEDESTRIAN); + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), 0.0, epsilon); +} + // ========================================================================================== // predicted path deviation{ TEST_F(EvalTest, testPredictedPathDeviation_deviation0) { waitForDummyNode(); - setTargetMetric("predicted_path_deviation_5.00"); + setTargetMetric("predicted_path_deviation_CAR_5.00"); const auto init_objects = makeStraightPredictedObjects(0); publishObjects(init_objects); @@ -481,7 +517,7 @@ TEST_F(EvalTest, testPredictedPathDeviation_deviation1) { waitForDummyNode(); - setTargetMetric("predicted_path_deviation_5.00"); + setTargetMetric("predicted_path_deviation_CAR_5.00"); const auto init_objects = makeStraightPredictedObjects(0); publishObjects(init_objects); @@ -502,7 +538,7 @@ TEST_F(EvalTest, testPredictedPathDeviation_deviation2) { waitForDummyNode(); - setTargetMetric("predicted_path_deviation_5.00"); + setTargetMetric("predicted_path_deviation_CAR_5.00"); const auto init_objects = makeStraightPredictedObjects(0); publishObjects(init_objects); @@ -518,4 +554,27 @@ TEST_F(EvalTest, testPredictedPathDeviation_deviation2) const double mean_deviation = deviation * (num_points - 1) / num_points; EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), mean_deviation, epsilon); } + +TEST_F(EvalTest, testPredictedPathDeviation_deviation0_PEDESTRIAN) +{ + waitForDummyNode(); + + setTargetMetric("predicted_path_deviation_PEDESTRIAN_5.00"); + + const auto init_objects = makeStraightPredictedObjects(0, ObjectClassification::PEDESTRIAN); + 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, ObjectClassification::PEDESTRIAN); + publishObjects(objects); + } + const auto last_objects = + makeDeviatedStraightPredictedObjects(time_delay_, deviation, ObjectClassification::PEDESTRIAN); + + const double num_points = time_delay_ / time_step_ + 1; + const double mean_deviation = deviation * (num_points - 1) / num_points; + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), mean_deviation, epsilon); +} // ==========================================================================================