feat(multi_object_tracker): add object class filtering in tracking process

perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp

@@ -42,6 +42,8 @@ class Tracker
   {
     classification_ = classification;
   }
+  void updateClassification(
+    const std::vector<autoware_auto_perception_msgs::msg::ObjectClassification> & classification);
 
 private:
   unique_identifier_msgs::msg::UUID uuid_;

perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp

@@ -43,7 +43,7 @@ bool MultipleVehicleTracker::measure(
   big_vehicle_tracker_.measure(object, time, self_transform);
   normal_vehicle_tracker_.measure(object, time, self_transform);
   if (object_recognition_utils::getHighestProbLabel(object.classification) != Label::UNKNOWN)
-    setClassification(object.classification);
+    updateClassification(object.classification);
   return true;
 }
 

perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp

@@ -43,7 +43,7 @@ bool PedestrianAndBicycleTracker::measure(
   pedestrian_tracker_.measure(object, time, self_transform);
   bicycle_tracker_.measure(object, time, self_transform);
   if (object_recognition_utils::getHighestProbLabel(object.classification) != Label::UNKNOWN)
-    setClassification(object.classification);
+    updateClassification(object.classification);
   return true;
 }
 

perception/multi_object_tracker/src/tracker/model/tracker_base.cpp

@@ -54,6 +54,60 @@ bool Tracker::updateWithoutMeasurement()
   return true;
 }
 
+void Tracker::updateClassification(
+  const std::vector<autoware_auto_perception_msgs::msg::ObjectClassification> & classification)
+{
+  // classification algorithm:
+  // 0. Remove the class with probability < 0.005
+  // 1. Decay all existing probability
+  // 2. Update the matched classification probability with a gain
+  // 3. If the label is not found, add it to the classification list
+  // 4. Normalize
+
+  // Gain and decay
+  const double gain = 0.05;
+  const double decay = 1.0 - gain;
+
+  // If the probability is less than 0.005, remove the class
+  classification_.erase(
+    std::remove_if(
+      classification_.begin(), classification_.end(),
+      [](const auto & class_) { return class_.probability < 0.005; }),
+    classification_.end());
+
+  // Decay all existing probability
+  for (auto & class_ : classification_) {
+    class_.probability *= decay;
+  }
+
+  // Update the matched classification probability with a gain
+  for (const auto & new_class : classification) {
+    bool found = false;
+    for (auto & old_class : classification_) {
+      if (new_class.label == old_class.label) {
+        old_class.probability += new_class.probability * gain;
+        found = true;
+        break;
+      }
+    }
+    // If the label is not found, add it to the classification list
+    if (!found) {
+      auto adding_class = new_class;
+      adding_class.probability *= gain;
+      classification_.push_back(adding_class);
+    }
+  }
+
+  // Normalize
+  double sum = 0.0;
+  for (const auto & class_ : classification_) {
+    sum += class_.probability;
+  }
+  for (auto & class_ : classification_) {
+    class_.probability /= sum;
+  }
+}
+
 geometry_msgs::msg::PoseWithCovariance Tracker::getPoseWithCovariance(
   const rclcpp::Time & time) const
 {