From d173d2db1be0748e2e6593a41351cdd2236d7823 Mon Sep 17 00:00:00 2001 From: Adam Morrissett <22103567+adamlm@users.noreply.github.com> Date: Wed, 10 Jan 2024 11:08:25 -0500 Subject: [PATCH] [cooperative perception] Add new scoring metrics (#2265) * #2264 Add new metrics The SemanticDistance2dScore combines the 2D Euclidean distance with the semantic classes between detections and tracks. The MetricSe2 is the SE(2) distance. * #2264 Update comments * #2264 Clean up SemanticDistance2dScore There were some unnecessary if statements/wrapping. * #2264 Add clarification/justification comment * #2264 Update comment * #2264 Add comment about clustering distance * #2264 Add "why" comments to visitors --- .../src/multiple_object_tracker_component.cpp | 129 +++++++++++++++++- 1 file changed, 123 insertions(+), 6 deletions(-) diff --git a/carma_cooperative_perception/src/multiple_object_tracker_component.cpp b/carma_cooperative_perception/src/multiple_object_tracker_component.cpp index 0fe47cc0a2..a9e5685b50 100644 --- a/carma_cooperative_perception/src/multiple_object_tracker_component.cpp +++ b/carma_cooperative_perception/src/multiple_object_tracker_component.cpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include #include @@ -232,7 +233,10 @@ static auto to_ros_msg(const Track & track) static constexpr mot::Visitor visitor{ [](const mot::CtrvTrack & t) { return to_ros_msg(t); }, [](const mot::CtraTrack & t) { return to_ros_msg(t); }, - [](const auto &) { throw std::runtime_error{"cannot make ROS 2 message from track type"}; }}; + [](const auto &) { + // Currently on support CTRV and CTRA + throw std::runtime_error{"cannot make ROS 2 message from track type"}; + }}; return std::visit(visitor, track); } @@ -452,18 +456,123 @@ static auto predict_track_states(std::vector tracks, units::time::second_ return tracks; } +/** + * @brief Calculate 2D Euclidean distance between track and detection + * + * If both the track and detection semantic classes are known, this function + * object returns the 2D Euclidean distance between their states. Otherwise, + * it returns std::nullopt. +*/ +struct SemanticDistance2dScore +{ + template + auto operator()(const Track & track, const Detection & detection) const -> std::optional + { + if constexpr (std::is_same_v) { + const auto dist{two_dimensional_distance(track.state, detection.state)}; + + // Fall back to 2D Euclidean distance if either semantic class if unknown. The unknown + // track/detection may actually be the same other track/detection we are scoring against. + if ( + track.semantic_class == mot::SemanticClass::kUnknown || + detection.semantic_class == mot::SemanticClass::kUnknown) { + return dist; + } + + if (track.semantic_class == detection.semantic_class) { + return dist; + } + } + + return std::nullopt; + } + + template + auto operator()( + const std::variant & track, + const std::variant & detection) const -> std::optional + { + return std::visit( + [this](const auto & t, const auto & d) { return (*this)(t, d); }, track, detection); + } + +private: + static auto two_dimensional_distance(const mot::CtrvState & lhs, const mot::CtrvState & rhs) + -> float + { + const auto x_diff_sq{ + std::pow(mot::remove_units(lhs.position_x) - mot::remove_units(rhs.position_x), 2)}; + const auto y_diff_sq{ + std::pow(mot::remove_units(lhs.position_y) - mot::remove_units(rhs.position_y), 2)}; + + return std::sqrt(x_diff_sq + y_diff_sq); + } + + static auto two_dimensional_distance(const mot::CtraState & lhs, const mot::CtraState & rhs) + -> float + { + const auto x_diff_sq{ + std::pow(mot::remove_units(lhs.position_x) - mot::remove_units(rhs.position_x), 2)}; + const auto y_diff_sq{ + std::pow(mot::remove_units(lhs.position_y) - mot::remove_units(rhs.position_y), 2)}; + + return std::sqrt(x_diff_sq + y_diff_sq); + } +}; + +/** + * @brief Calculates distance between a point and detection in SE(2) (special Euclidean) space +*/ +struct MetricSe2 +{ + template + auto operator()(const mot::Point & point, const Detection & detection) const -> double + { + double sum{0}; + + sum += std::pow(mot::remove_units(point.position_x - detection.state.position_x), 2); + sum += std::pow(mot::remove_units(point.position_y - detection.state.position_y), 2); + + const auto p_yaw_rad{mot::remove_units(point.yaw.get_angle())}; + const auto d_yaw_rad{mot::remove_units(detection.state.yaw.get_angle())}; + const auto abs_diff{std::abs(p_yaw_rad - d_yaw_rad)}; + sum += std::min(abs_diff, 2 * 3.14159265359 - abs_diff); + + return sum; + } + + template + auto operator()(mot::Point point, const std::variant & detection) const -> double + { + const mot::Visitor visitor{ + [this](const mot::Point & p, const mot::CtrvDetection & d) { return this->operator()(p, d); }, + [this](const mot::Point & p, const mot::CtraDetection & d) { return this->operator()(p, d); }, + [](const mot::Point &, const auto &) { + // Currently on support CTRV and CTRA + return std::numeric_limits::max(); + }}; + + return std::visit(visitor, std::variant{point}, detection); + } +}; + auto MultipleObjectTrackerNode::execute_pipeline() -> void { static constexpr mot::Visitor make_track_visitor{ [](const mot::CtrvDetection & d) { return Track{mot::make_track(d)}; }, [](const mot::CtraDetection & d) { return Track{mot::make_track(d)}; }, - [](const auto &) { throw std::runtime_error("cannot make track from given detection"); }, + [](const auto &) { + // Currently on support CTRV and CTRA + throw std::runtime_error("cannot make track from given detection"); + }, }; if (track_manager_.get_all_tracks().empty()) { RCLCPP_DEBUG( get_logger(), "List of tracks is empty. Converting detections to tentative tracks"); + // This clustering distance is an arbitrarily-chosen heuristic. It is working well for our + // current purposes, but there's no reason it couldn't be restricted or loosened. const auto clusters{mot::cluster_detections(detections_, 0.75)}; for (const auto & cluster : clusters) { const auto detection{std::cbegin(cluster.get_detections())->second}; @@ -482,10 +591,16 @@ auto MultipleObjectTrackerNode::execute_pipeline() -> void temporally_align_detections(detections_, current_time); const auto predicted_tracks{predict_track_states(track_manager_.get_all_tracks(), current_time)}; - const auto scores{ - mot::score_tracks_and_detections(predicted_tracks, detections_, mot::euclidean_distance_score)}; + auto scores{ + mot::score_tracks_and_detections(predicted_tracks, detections_, SemanticDistance2dScore{})}; + + // This pruning distance is an arbitrarily-chosen heuristic. It is working well for our + // current purposes, but there's no reason it couldn't be restricted or loosened. + mot::prune_track_and_detection_scores_if(scores, [](const auto & score) { return score > 5.0; }); + + const auto associations{ + mot::associate_detections_to_tracks(scores, mot::gnn_association_visitor)}; - const auto associations{mot::associate_detections_to_tracks(scores, mot::gnn_associator)}; track_manager_.update_track_lists(associations); std::unordered_map detection_map; @@ -515,7 +630,9 @@ auto MultipleObjectTrackerNode::execute_pipeline() -> void } } - const auto clusters{mot::cluster_detections(unassociated_detections, 0.75)}; + // This clustering distance is an arbitrarily-chosen heuristic. It is working well for our + // current purposes, but there's no reason it couldn't be restricted or loosened. + const auto clusters{mot::cluster_detections(unassociated_detections, 0.75, MetricSe2{})}; for (const auto & cluster : clusters) { const auto detection{std::cbegin(cluster.get_detections())->second}; track_manager_.add_tentative_track(std::visit(make_track_visitor, detection));