feat: replace measurements label when size is not proper

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

Author: YoshiRi

perception/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp

@@ -54,6 +54,10 @@ class DataAssociation
   void assign(
     const Eigen::MatrixXd & src, std::unordered_map<int, int> & direct_assignment,
     std::unordered_map<int, int> & reverse_assignment);
+  void filterMeasurementsBySize(
+    const autoware_auto_perception_msgs::msg::DetectedObjects & measurements,
+    autoware_auto_perception_msgs::msg::DetectedObjects & filtered_measurements,
+    autoware_auto_perception_msgs::msg::DetectedObjects & unexpected_measurements);
   Eigen::MatrixXd calcScoreMatrix(
     const autoware_auto_perception_msgs::msg::DetectedObjects & measurements,
     const std::list<std::shared_ptr<Tracker>> & trackers);

perception/multi_object_tracker/src/data_association/data_association.cpp

@@ -146,6 +146,47 @@ void DataAssociation::assign(
   }
 }
 
+/**
+ * @brief filter measurements by its size, large/small objects are converted to UNKNOWN
+ *
+ * @param measurements
+ * @param filtered_measurements
+ * @param unexpected_measurements
+ */
+void DataAssociation::filterMeasurementsBySize(
+  const autoware_auto_perception_msgs::msg::DetectedObjects & measurements,
+  autoware_auto_perception_msgs::msg::DetectedObjects & filtered_measurements,
+  autoware_auto_perception_msgs::msg::DetectedObjects & unexpected_measurements)
+{
+  // copy header
+  filtered_measurements.header = measurements.header;
+  unexpected_measurements.header = measurements.header;
+  filtered_measurements.objects.clear();
+  unexpected_measurements.objects.clear();
+
+  // filter objects by its size
+  const double max_unknown_area = max_area_matrix_(0, 0);
+  const double min_unknown_area = min_area_matrix_(0, 0);
+  const auto unknown_classifications =
+    object_recognition_utils::toObjectClassifications("UNKNOWN", 1.0);  // for label replace
+  for (const auto & measurement : measurements.objects) {
+    const std::uint8_t measurement_label =
+      object_recognition_utils::getHighestProbLabel(measurement.classification);
+    const double area = tier4_autoware_utils::getArea(measurement.shape);
+    const double max_area = max_area_matrix_(measurement_label, measurement_label);
+    const double min_area = min_area_matrix_(measurement_label, measurement_label);
+    if (area > min_area && max_area > area) {
+      filtered_measurements.objects.push_back(measurement);
+    } else if (area > min_unknown_area && max_unknown_area > area) {
+      auto label_fixed_measurement = measurement;
+      label_fixed_measurement.classification = unknown_classifications;
+      filtered_measurements.objects.push_back(label_fixed_measurement);
+    } else {
+      unexpected_measurements.objects.push_back(measurement);
+    }
+  }
+}
+
 Eigen::MatrixXd DataAssociation::calcScoreMatrix(
   const autoware_auto_perception_msgs::msg::DetectedObjects & measurements,
   const std::list<std::shared_ptr<Tracker>> & trackers)

perception/multi_object_tracker/src/multi_object_tracker_core.cpp

@@ -261,10 +261,16 @@ void MultiObjectTracker::onMeasurement(
     (*itr)->predict(measurement_time);
   }
 
+  /* filter by objects size before association */
+  autoware_auto_perception_msgs::msg::DetectedObjects filtered_objects;
+  autoware_auto_perception_msgs::msg::DetectedObjects unexpected_objects;
+  data_association_->filterMeasurementsBySize(
+    transformed_objects, filtered_objects, unexpected_objects);
+
   /* global nearest neighbor */
   std::unordered_map<int, int> direct_assignment, reverse_assignment;
   Eigen::MatrixXd score_matrix = data_association_->calcScoreMatrix(
-    transformed_objects, list_tracker_);  // row : tracker, col : measurement
+    filtered_objects, list_tracker_);  // row : tracker, col : measurement
   data_association_->assign(score_matrix, direct_assignment, reverse_assignment);
 
   /* tracker measurement update */
@@ -274,7 +280,7 @@ void MultiObjectTracker::onMeasurement(
     if (direct_assignment.find(tracker_idx) != direct_assignment.end()) {  // found
       (*(tracker_itr))
         ->updateWithMeasurement(
-          transformed_objects.objects.at(direct_assignment.find(tracker_idx)->second),
+          filtered_objects.objects.at(direct_assignment.find(tracker_idx)->second),
           measurement_time, *self_transform);
     } else {  // not found
       (*(tracker_itr))->updateWithoutMeasurement();
@@ -287,12 +293,12 @@ void MultiObjectTracker::onMeasurement(
   sanitizeTracker(list_tracker_, measurement_time);
 
   /* new tracker */
-  for (size_t i = 0; i < transformed_objects.objects.size(); ++i) {
+  for (size_t i = 0; i < filtered_objects.objects.size(); ++i) {
     if (reverse_assignment.find(i) != reverse_assignment.end()) {  // found
       continue;
     }
     std::shared_ptr<Tracker> tracker =
-      createNewTracker(transformed_objects.objects.at(i), measurement_time, *self_transform);
+      createNewTracker(filtered_objects.objects.at(i), measurement_time, *self_transform);
     if (tracker) list_tracker_.push_back(tracker);
   }