diff --git a/perception/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp b/perception/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp index a29fcdaa4e1e5..1cfe4e2842a10 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp +++ b/perception/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 & direct_assignment, std::unordered_map & 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> & 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 index f367e19c11f2a..eaa1095ac9d8b 100644 --- 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> & 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 index 2acc4f447cb50..3989c60605ff5 100644 --- 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 direct_assignment, reverse_assignment; Eigen::MatrixXd score_matrix = data_association_->calcScoreMatrix(