[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

carma_cooperative_perception/src/multiple_object_tracker_component.cpp

@@ -25,6 +25,7 @@
 #include <multiple_object_tracking/ctra_model.hpp>
 #include <multiple_object_tracking/ctrv_model.hpp>
 #include <multiple_object_tracking/fusing.hpp>
+#include <multiple_object_tracking/gating.hpp>
 #include <multiple_object_tracking/scoring.hpp>
 #include <multiple_object_tracking/temporal_alignment.hpp>
 #include <unordered_map>
@@ -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<Track> 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 <typename Track, typename Detection>
+  auto operator()(const Track & track, const Detection & detection) const -> std::optional<float>
+  {
+    if constexpr (std::is_same_v<decltype(track.state), decltype(detection.state)>) {
+      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 <typename... TrackAlternatives, typename... DetectionAlternatives>
+  auto operator()(
+    const std::variant<TrackAlternatives...> & track,
+    const std::variant<DetectionAlternatives...> & detection) const -> std::optional<float>
+  {
+    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 <typename Detection>
+  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 <typename... Alternatives>
+  auto operator()(mot::Point point, const std::variant<Alternatives...> & 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<double>::max();
+      }};
+
+    return std::visit(visitor, std::variant<mot::Point>{point}, detection);
+  }
+};
+
 auto MultipleObjectTrackerNode::execute_pipeline() -> void
 {
   static constexpr mot::Visitor make_track_visitor{
     [](const mot::CtrvDetection & d) { return Track{mot::make_track<mot::CtrvTrack>(d)}; },
     [](const mot::CtraDetection & d) { return Track{mot::make_track<mot::CtraTrack>(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<mot::Uuid, Detection> 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));