diff --git a/evaluator/perception_online_evaluator/README.md b/evaluator/perception_online_evaluator/README.md index b801e5f418cef..c0e2a516cf948 100644 --- a/evaluator/perception_online_evaluator/README.md +++ b/evaluator/perception_online_evaluator/README.md @@ -27,6 +27,7 @@ This module allows for the evaluation of how accurately perception results are g | `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. | +| `stopped_velocity_threshold` | double | threshold velocity to check if vehicle is stopped | | `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.). | diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp index 8a2cddca476d4..f5dee74828424 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp @@ -31,6 +31,7 @@ enum class Metric { lateral_deviation, yaw_deviation, predicted_path_deviation, + yaw_rate, SIZE, }; @@ -39,18 +40,21 @@ using MetricStatMap = std::unordered_map>; static const std::unordered_map str_to_metric = { {"lateral_deviation", Metric::lateral_deviation}, {"yaw_deviation", Metric::yaw_deviation}, - {"predicted_path_deviation", Metric::predicted_path_deviation}}; + {"predicted_path_deviation", Metric::predicted_path_deviation}, + {"yaw_rate", Metric::yaw_rate}}; static const std::unordered_map metric_to_str = { {Metric::lateral_deviation, "lateral_deviation"}, {Metric::yaw_deviation, "yaw_deviation"}, - {Metric::predicted_path_deviation, "predicted_path_deviation"}}; + {Metric::predicted_path_deviation, "predicted_path_deviation"}, + {Metric::yaw_rate, "yaw_rate"}}; // Metrics descriptions static const std::unordered_map metric_descriptions = { {Metric::lateral_deviation, "Lateral_deviation[m]"}, {Metric::yaw_deviation, "Yaw_deviation[rad]"}, - {Metric::predicted_path_deviation, "Predicted_path_deviation[m]"}}; + {Metric::predicted_path_deviation, "Predicted_path_deviation[m]"}, + {Metric::yaw_rate, "Yaw_rate[rad/s]"}}; namespace details { 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 20397448bf21a..f6566326ba979 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 @@ -72,6 +72,10 @@ using ObjectDataMap = std::unordered_map; using HistoryPathMap = std::unordered_map, std::vector>>; +using StampObjectMap = std::map; +using StampObjectMapIterator = std::map::const_iterator; +using ObjectMap = std::unordered_map; + class MetricsCalculator { public: @@ -98,7 +102,7 @@ class MetricsCalculator std::shared_ptr parameters_; // Store predicted objects information and calculation results - std::unordered_map> object_map_; + ObjectMap object_map_; HistoryPathMap history_path_map_; rclcpp::Time current_stamp_; @@ -124,6 +128,7 @@ class MetricsCalculator MetricStatMap calcPredictedPathDeviationMetrics(const ClassObjectsMap & class_objects_map) const; Stat calcPredictedPathDeviationMetrics( const PredictedObjects & objects, const double time_horizon) const; + MetricStatMap calcYawRateMetrics(const ClassObjectsMap & class_objects_map) const; bool hasPassedTime(const rclcpp::Time stamp) const; bool hasPassedTime(const std::string uuid, const rclcpp::Time stamp) const; @@ -131,8 +136,12 @@ class MetricsCalculator // Extract object rclcpp::Time getClosestStamp(const rclcpp::Time stamp) const; + std::optional getClosestObjectIterator( + const std::string & uuid, const rclcpp::Time & stamp) const; std::optional getObjectByStamp( const std::string uuid, const rclcpp::Time stamp) const; + std::optional> getPreviousObjectByStamp( + const std::string uuid, const rclcpp::Time stamp) const; PredictedObjects getObjectsByStamp(const rclcpp::Time stamp) const; }; // class MetricsCalculator diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/parameters.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/parameters.hpp index 98cd3c25b71a3..e15ae2f350954 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/parameters.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/parameters.hpp @@ -48,6 +48,7 @@ struct Parameters std::vector metrics; size_t smoothing_window_size{0}; std::vector prediction_time_horizons; + double stopped_velocity_threshold{0.0}; DebugMarkerParameter debug_marker_parameters; // parameters depend on object class std::unordered_map object_parameters; 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 8a118a14d7368..ee348b108d139 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 @@ -21,6 +21,7 @@ #include #include +#include #include #include #include @@ -39,6 +40,9 @@ using autoware_auto_perception_msgs::msg::PredictedObjects; using ClassObjectsMap = std::unordered_map; +bool velocity_filter( + const PredictedObject & object, double velocity_threshold, double max_velocity); + std::uint8_t getHighestProbLabel(const std::vector & classification); /** @@ -133,6 +137,39 @@ void filterObjectsByClass( void filterDeviationCheckObjects( PredictedObjects & objects, const std::unordered_map & params); +/** + * @brief Filters objects based on their velocity. + * + * Depending on the remove_above_threshold parameter, this function either removes objects with + * velocities above the given threshold or only keeps those objects. It uses the helper function + * filterObjectsByVelocity() to do the actual filtering. + * + * @param objects The objects to be filtered. + * @param velocity_threshold The velocity threshold for the filtering. + * @param remove_above_threshold If true, objects with velocities above the threshold are removed. + * If false, only objects with velocities above the threshold are + * kept. + * @return A new collection of objects that have been filtered according to the rules. + */ +PredictedObjects filterObjectsByVelocity( + const PredictedObjects & objects, const double velocity_threshold, + const bool remove_above_threshold = true); + +/** + * @brief Helper function to filter objects based on their velocity. + * + * This function iterates over all objects and calculates their velocity norm. If the velocity norm + * is within the velocity_threshold and max_velocity range, the object is added to a new collection. + * This new collection is then returned. + * + * @param objects The objects to be filtered. + * @param velocity_threshold The minimum velocity for an object to be included in the output. + * @param max_velocity The maximum velocity for an object to be included in the output. + * @return A new collection of objects that have been filtered according to the rules. + */ +PredictedObjects filterObjectsByVelocity( + const PredictedObjects & objects, double velocity_threshold, double max_velocity); + } // namespace perception_diagnostics #endif // PERCEPTION_ONLINE_EVALUATOR__UTILS__OBJECTS_FILTERING_HPP_ diff --git a/evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml b/evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml index 6749bac81aa35..976b10a2c73f9 100644 --- a/evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml +++ b/evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml @@ -4,12 +4,15 @@ - lateral_deviation - yaw_deviation - predicted_path_deviation + - yaw_rate # this should be an odd number, because it includes the target point smoothing_window_size: 11 prediction_time_horizons: [1.0, 2.0, 3.0, 5.0] + stopped_velocity_threshold: 0.3 + target_object: car: check_deviation: true diff --git a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp index 3afcdc0cd3185..60e02c8f24c39 100644 --- a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp +++ b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp @@ -36,8 +36,14 @@ std::optional MetricsCalculator::calculate(const Metric & metric) if (!hasPassedTime(target_stamp)) { return {}; } - const auto target_objects = getObjectsByStamp(target_stamp); - const ClassObjectsMap class_objects_map = separateObjectsByClass(target_objects); + const auto target_stamp_objects = getObjectsByStamp(target_stamp); + const auto class_objects_map = separateObjectsByClass(target_stamp_objects); + + // filter stopped objects + const auto stopped_objects = + filterObjectsByVelocity(target_stamp_objects, parameters_->stopped_velocity_threshold); + const auto class_stopped_objects_map = separateObjectsByClass(stopped_objects); + switch (metric) { case Metric::lateral_deviation: return calcLateralDeviationMetrics(class_objects_map); @@ -45,6 +51,8 @@ std::optional MetricsCalculator::calculate(const Metric & metric) return calcYawDeviationMetrics(class_objects_map); case Metric::predicted_path_deviation: return calcPredictedPathDeviationMetrics(class_objects_map); + case Metric::yaw_rate: + return calcYawRateMetrics(class_stopped_objects_map); default: return {}; } @@ -124,16 +132,46 @@ rclcpp::Time MetricsCalculator::getClosestStamp(const rclcpp::Time stamp) const return closest_stamp; } -std::optional MetricsCalculator::getObjectByStamp( - const std::string uuid, const rclcpp::Time stamp) const +std::optional MetricsCalculator::getClosestObjectIterator( + const std::string & uuid, const rclcpp::Time & stamp) const { const auto closest_stamp = getClosestStamp(stamp); - auto it = std::lower_bound( + const auto it = std::lower_bound( object_map_.at(uuid).begin(), object_map_.at(uuid).end(), closest_stamp, [](const auto & pair, const rclcpp::Time & val) { return pair.first < val; }); - if (it != object_map_.at(uuid).end() && it->first == closest_stamp) { - return it->second; + return it != object_map_.at(uuid).end() ? std::optional(it) + : std::nullopt; +} + +std::optional MetricsCalculator::getObjectByStamp( + const std::string uuid, const rclcpp::Time stamp) const +{ + const auto obj_it_opt = getClosestObjectIterator(uuid, stamp); + if (obj_it_opt.has_value()) { + const auto it = obj_it_opt.value(); + if (it->first == getClosestStamp(stamp)) { + return it->second; + } + } + return std::nullopt; +} + +std::optional> MetricsCalculator::getPreviousObjectByStamp( + const std::string uuid, const rclcpp::Time stamp) const +{ + const auto obj_it_opt = getClosestObjectIterator(uuid, stamp); + if (obj_it_opt.has_value()) { + auto it = obj_it_opt.value(); + if (it != object_map_.at(uuid).begin()) { + // If it is exactly the closest stamp, move one back to get the previous + if (it->first == getClosestStamp(stamp)) { + --it; + } else { + // If it is not the closest stamp, it already points to the previous one due to lower_bound + } + return std::make_pair(it->first, it->second); + } } return std::nullopt; } @@ -313,9 +351,46 @@ Stat MetricsCalculator::calcPredictedPathDeviationMetrics( return stat; } +MetricStatMap MetricsCalculator::calcYawRateMetrics(const ClassObjectsMap & class_objects_map) const +{ + // calculate yaw rate for each object + + 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 previous_object_with_stamp_opt = getPreviousObjectByStamp(uuid, stamp); + if (!previous_object_with_stamp_opt.has_value()) { + continue; + } + const auto [previous_stamp, previous_object] = previous_object_with_stamp_opt.value(); + + const auto time_diff = (stamp - previous_stamp).seconds(); + if (time_diff < 0.01) { + continue; + } + const auto current_yaw = + tf2::getYaw(object.kinematics.initial_pose_with_covariance.pose.orientation); + const auto previous_yaw = + tf2::getYaw(previous_object.kinematics.initial_pose_with_covariance.pose.orientation); + const auto yaw_rate = + std::abs(tier4_autoware_utils::normalizeRadian(current_yaw - previous_yaw) / time_diff); + stat.add(yaw_rate); + } + metric_stat_map["yaw_rate_" + convertLabelToString(label)] = stat; + } + return metric_stat_map; +} + void MetricsCalculator::setPredictedObjects(const PredictedObjects & objects) { - // using TimeStamp = builtin_interfaces::msg::Time; current_stamp_ = objects.header.stamp; // store objects to check deviation diff --git a/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp index cf98ccc4c5460..8820ad16fd8d5 100644 --- a/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp +++ b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp @@ -280,6 +280,8 @@ void PerceptionOnlineEvaluatorNode::initParameter() p->smoothing_window_size = getOrDeclareParameter(*this, "smoothing_window_size"); p->prediction_time_horizons = getOrDeclareParameter>(*this, "prediction_time_horizons"); + p->stopped_velocity_threshold = + getOrDeclareParameter(*this, "stopped_velocity_threshold"); // set metrics const auto selected_metrics = diff --git a/evaluator/perception_online_evaluator/src/utils/objects_filtering.cpp b/evaluator/perception_online_evaluator/src/utils/objects_filtering.cpp index e39ba348ee42b..0c292fcb9a9c4 100644 --- a/evaluator/perception_online_evaluator/src/utils/objects_filtering.cpp +++ b/evaluator/perception_online_evaluator/src/utils/objects_filtering.cpp @@ -16,6 +16,17 @@ namespace perception_diagnostics { +namespace filter +{ +bool velocity_filter(const PredictedObject & object, double velocity_threshold, double max_velocity) +{ + const auto v_norm = std::hypot( + object.kinematics.initial_twist_with_covariance.twist.linear.x, + object.kinematics.initial_twist_with_covariance.twist.linear.y); + return (velocity_threshold < v_norm && v_norm < max_velocity); +} +} // namespace filter + ObjectTypesToCheck getDeviationCheckObjectTypes( const std::unordered_map & params) { @@ -111,4 +122,25 @@ void filterDeviationCheckObjects( filterObjectsByClass(objects, object_types); } +PredictedObjects filterObjectsByVelocity( + const PredictedObjects & objects, const double velocity_threshold, + const bool remove_above_threshold) +{ + if (remove_above_threshold) { + return filterObjectsByVelocity(objects, -velocity_threshold, velocity_threshold); + } + return filterObjectsByVelocity(objects, velocity_threshold, std::numeric_limits::max()); +} + +PredictedObjects filterObjectsByVelocity( + const PredictedObjects & objects, double velocity_threshold, double max_velocity) +{ + const auto filter = [&](const auto & object) { + return filter::velocity_filter(object, velocity_threshold, max_velocity); + }; + + auto filtered = objects; + filterObjects(filtered, filter); + return filtered; +} } // namespace perception_diagnostics 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 16c46d0eecd92..2975a1b4d6df8 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 @@ -580,3 +580,129 @@ TEST_F(EvalTest, testPredictedPathDeviation_deviation0_PEDESTRIAN) EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), mean_deviation, epsilon); } // ========================================================================================== + +// ========================================================================================== +// yaw rate +TEST_F(EvalTest, testYawRate_0) +{ + waitForDummyNode(); + setTargetMetric("yaw_rate_CAR"); + + for (double time = 0; time <= time_delay_; time += time_step_) { + const auto objects = makeStraightPredictedObjects(time); + publishObjects(objects); + } + + const auto last_objects = makeStraightPredictedObjects(time_delay_ + time_step_); + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), 0.0, epsilon); +} + +TEST_F(EvalTest, testYawRate_01) +{ + waitForDummyNode(); + setTargetMetric("yaw_rate_CAR"); + + const double yaw_rate = 0.1; + + for (double time = 0; time <= time_delay_; time += time_step_) { + const auto objects = rotateObjects(makeStraightPredictedObjects(time), yaw_rate * time); + publishObjects(objects); + } + + for (double time = time_delay_ + time_step_; time < time_delay_ * 2; time += time_step_) { + const auto objects = rotateObjects(makeStraightPredictedObjects(time), yaw_rate * time); + EXPECT_NEAR(publishObjectsAndGetMetric(objects), yaw_rate, epsilon); + } +} + +TEST_F(EvalTest, testYawRate_minus_01) +{ + waitForDummyNode(); + setTargetMetric("yaw_rate_CAR"); + + const double yaw_rate = 0.1; + + for (double time = 0; time <= time_delay_; time += time_step_) { + const auto objects = rotateObjects(makeStraightPredictedObjects(time), -yaw_rate * time); + publishObjects(objects); + } + + for (double time = time_delay_ + time_step_; time < time_delay_ * 2; time += time_step_) { + const auto objects = rotateObjects(makeStraightPredictedObjects(time), -yaw_rate * time); + EXPECT_NEAR(publishObjectsAndGetMetric(objects), yaw_rate, epsilon); + } +} + +TEST_F(EvalTest, testYawRate_1) +{ + waitForDummyNode(); + setTargetMetric("yaw_rate_CAR"); + + const double yaw_rate = 1.0; + + for (double time = 0; time <= time_delay_; time += time_step_) { + const auto objects = rotateObjects(makeStraightPredictedObjects(time), yaw_rate * time); + publishObjects(objects); + } + + for (double time = time_delay_ + time_step_; time < time_delay_ * 2; time += time_step_) { + const auto objects = rotateObjects(makeStraightPredictedObjects(time), yaw_rate * time); + EXPECT_NEAR(publishObjectsAndGetMetric(objects), yaw_rate, epsilon); + } +} + +TEST_F(EvalTest, testYawRate_minus_1) +{ + waitForDummyNode(); + setTargetMetric("yaw_rate_CAR"); + + const double yaw_rate = 1.0; + + for (double time = 0; time <= time_delay_; time += time_step_) { + const auto objects = rotateObjects(makeStraightPredictedObjects(time), -yaw_rate * time); + publishObjects(objects); + } + + for (double time = time_delay_ + time_step_; time < time_delay_ * 2; time += time_step_) { + const auto objects = rotateObjects(makeStraightPredictedObjects(time), -yaw_rate * time); + EXPECT_NEAR(publishObjectsAndGetMetric(objects), yaw_rate, epsilon); + } +} + +TEST_F(EvalTest, testYawRate_5) +{ + waitForDummyNode(); + setTargetMetric("yaw_rate_CAR"); + + const double yaw_rate = 5.0; + + for (double time = 0; time <= time_delay_; time += time_step_) { + const auto objects = rotateObjects(makeStraightPredictedObjects(time), yaw_rate * time); + publishObjects(objects); + } + + for (double time = time_delay_ + time_step_; time < time_delay_ * 2; time += time_step_) { + const auto objects = rotateObjects(makeStraightPredictedObjects(time), yaw_rate * time); + EXPECT_NEAR(publishObjectsAndGetMetric(objects), yaw_rate, epsilon); + } +} + +TEST_F(EvalTest, testYawRate_minus_5) +{ + waitForDummyNode(); + setTargetMetric("yaw_rate_CAR"); + + const double yaw_rate = 5.0; + + for (double time = 0; time <= time_delay_; time += time_step_) { + const auto objects = rotateObjects(makeStraightPredictedObjects(time), -yaw_rate * time); + publishObjects(objects); + } + + for (double time = time_delay_ + time_step_; time < time_delay_ * 2; time += time_step_) { + const auto objects = rotateObjects(makeStraightPredictedObjects(time), -yaw_rate * time); + EXPECT_NEAR(publishObjectsAndGetMetric(objects), yaw_rate, epsilon); + } +} +// TEST_F(EvalTest, testYawRate_rate01) +// ==========================================================================================