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..a45cab8078aca 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,10 @@ class DataAssociation void assign( const Eigen::MatrixXd & src, std::unordered_map & direct_assignment, std::unordered_map & 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> & 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..7337dcc9116af 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,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> & trackers) 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 1d4d50c7eab4c..3f5c1b317c566 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/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 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 = - 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); }