chore: Add object filtering to data association

technolojin · Oct 25, 2024 · 1adb6a9 · 1adb6a9
1 parent 5de95b0
commit 1adb6a9
Show file tree

Hide file tree

Showing 3 changed files with 34 additions and 7 deletions.
diff --git a/...n/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp b/...n/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp
@@ -54,6 +54,7 @@ class DataAssociation
   void assign(
     const Eigen::MatrixXd & src, std::unordered_map<int, int> & direct_assignment,
     std::unordered_map<int, int> & reverse_assignment);
+  void objectFilter(autoware_auto_perception_msgs::msg::DetectedObjects & measurements) const;
   Eigen::MatrixXd calcScoreMatrix(
     const autoware_auto_perception_msgs::msg::DetectedObjects & measurements,
     const std::list<std::shared_ptr<Tracker>> & trackers);

diff --git a/perception/multi_object_tracker/src/data_association/data_association.cpp b/perception/multi_object_tracker/src/data_association/data_association.cpp
@@ -146,6 +146,37 @@ void DataAssociation::assign(
   }
 }
 
+void DataAssociation::objectFilter(
+  autoware_auto_perception_msgs::msg::DetectedObjects & measurements) const
+{
+  using Label = autoware_auto_perception_msgs::msg::ObjectClassification;
+
+  for (auto & measurement_object : measurements.objects) {
+    auto & obj_class_list = measurement_object.classification;
+    const std::uint8_t measurement_label =
+      object_recognition_utils::getHighestProbLabel(obj_class_list);
+    bool passed_gate = true;
+    const double max_area = max_area_matrix_(measurement_label, measurement_label);
+    const double min_area = min_area_matrix_(measurement_label, measurement_label);
+    const double area = tier4_autoware_utils::getArea(measurement_object.shape);
+    if (area < min_area || max_area < area) passed_gate = false;
+
+    //
+    if (!passed_gate) {
+      // change top-rate label to unknown, step back the existing labels
+      constexpr float RATE_REDUCED = 0.3f;
+      constexpr float RATE_OVERWRITE = 0.7f;
+      for (auto & obj_class : obj_class_list) {
+        obj_class.probability *= RATE_REDUCED;
+      }
+      Label new_obj_class;
+      new_obj_class.label = Label::UNKNOWN;
+      new_obj_class.probability = RATE_OVERWRITE;
+      obj_class_list.emplace_back(new_obj_class);
+    }
+  }
+}
+
 Eigen::MatrixXd DataAssociation::calcScoreMatrix(
   const autoware_auto_perception_msgs::msg::DetectedObjects & measurements,
   const std::list<std::shared_ptr<Tracker>> & trackers)
@@ -179,13 +210,6 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix(
         if (passed_gate) {
           if (max_dist < dist) passed_gate = false;
         }
-        // area gate
-        if (passed_gate) {
-          const double max_area = max_area_matrix_(tracker_label, measurement_label);
-          const double min_area = min_area_matrix_(tracker_label, measurement_label);
-          const double area = tier4_autoware_utils::getArea(measurement_object.shape);
-          if (area < min_area || max_area < area) passed_gate = false;
-        }
         // angle gate
         if (passed_gate) {
           const double max_rad = max_rad_matrix_(tracker_label, measurement_label);

diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
@@ -264,6 +264,8 @@ void MultiObjectTracker::onMeasurement(
     (*itr)->predict(measurement_time);
   }
 
+  data_association_->objectFilter(transformed_objects);
+
   /* global nearest neighbor */
   std::unordered_map<int, int> direct_assignment, reverse_assignment;
   Eigen::MatrixXd score_matrix = data_association_->calcScoreMatrix(