From aabd7cd103e428860e4c8d348d79b0c30c0e5028 Mon Sep 17 00:00:00 2001 From: TadaKazuto Date: Tue, 11 Mar 2025 11:53:11 +0900 Subject: [PATCH 1/5] Further improvements for the sanitization process --- .../src/multi_object_tracker_core.cpp | 59 ++++++++++--------- 1 file changed, 31 insertions(+), 28 deletions(-) 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..2776e0be3610c 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -386,13 +386,30 @@ void MultiObjectTracker::sanitizeTracker( constexpr float min_iou = 0.1; constexpr float min_iou_for_unknown_object = 0.001; constexpr double distance_threshold = 5.0; + + // Create sorted list with non-UNKNOWN objects first, then by measurement count + std::vector> sorted_list_tracker(list_tracker.begin(), list_tracker.end()); + std::sort(sorted_list_tracker.begin(), sorted_list_tracker.end(), + [](const std::shared_ptr& a, const std::shared_ptr& b) { + bool a_unknown = (a->getHighestProbLabel() == Label::UNKNOWN); + bool b_unknown = (b->getHighestProbLabel() == Label::UNKNOWN); + if (a_unknown != b_unknown) { + return b_unknown; // Put non-UNKNOWN objects first + } + return a->getTotalMeasurementCount() > b->getTotalMeasurementCount(); // Then sort by measurement count + }); + /* delete collision tracker */ - for (auto itr1 = list_tracker.begin(); itr1 != list_tracker.end(); ++itr1) { + for (size_t i = 0; i < sorted_list_tracker.size(); ++i) { + /* block erasing if no measurement */ + if (sorted_list_tracker[i]->getNoMeasurementCount() > 0) { + continue; + } autoware_auto_perception_msgs::msg::TrackedObject object1; - (*itr1)->getTrackedObject(time, object1); - for (auto itr2 = std::next(itr1); itr2 != list_tracker.end(); ++itr2) { + sorted_list_tracker[i]->getTrackedObject(time, object1); + for (size_t j = i + 1; j < sorted_list_tracker.size(); ++j) { autoware_auto_perception_msgs::msg::TrackedObject object2; - (*itr2)->getTrackedObject(time, object2); + sorted_list_tracker[j]->getTrackedObject(time, object2); const double distance = std::hypot( object1.kinematics.pose_with_covariance.pose.position.x - object2.kinematics.pose_with_covariance.pose.position.x, @@ -404,44 +421,30 @@ void MultiObjectTracker::sanitizeTracker( const double min_union_iou_area = 1e-2; const auto iou = object_recognition_utils::get2dIoU(object1, object2, min_union_iou_area); - const auto & label1 = (*itr1)->getHighestProbLabel(); - const auto & label2 = (*itr2)->getHighestProbLabel(); - bool should_delete_tracker1 = false; + const auto & label1 = sorted_list_tracker[i]->getHighestProbLabel(); + const auto & label2 = sorted_list_tracker[j]->getHighestProbLabel(); bool should_delete_tracker2 = false; // If at least one of them is UNKNOWN, delete the one with lower IOU. Because the UNKNOWN // objects are not reliable. if (label1 == Label::UNKNOWN || label2 == Label::UNKNOWN) { if (min_iou_for_unknown_object < iou) { - if (label1 == Label::UNKNOWN && label2 == Label::UNKNOWN) { - if ((*itr1)->getTotalMeasurementCount() < (*itr2)->getTotalMeasurementCount()) { - should_delete_tracker1 = true; - } else { - should_delete_tracker2 = true; - } - } else if (label1 == Label::UNKNOWN) { - should_delete_tracker1 = true; - } else if (label2 == Label::UNKNOWN) { + if (label2 == Label::UNKNOWN) { should_delete_tracker2 = true; } } } else { // If neither is UNKNOWN, delete the one with lower IOU. if (min_iou < iou) { - if ((*itr1)->getTotalMeasurementCount() < (*itr2)->getTotalMeasurementCount()) { - should_delete_tracker1 = true; - } else { - should_delete_tracker2 = true; - } + should_delete_tracker2 = true; } } - if (should_delete_tracker1) { - itr1 = list_tracker.erase(itr1); - --itr1; - break; - } else if (should_delete_tracker2) { - itr2 = list_tracker.erase(itr2); - --itr2; + if (should_delete_tracker2) { + // Remove from original list_tracker + list_tracker.remove(sorted_list_tracker[j]); + // Remove from sorted list + sorted_list_tracker.erase(sorted_list_tracker.begin() + j); + --j; } } } From 81ab80efc9d62bcbccd57b55d4919c157fc14d3c Mon Sep 17 00:00:00 2001 From: TadaKazuto Date: Tue, 11 Mar 2025 15:24:12 +0900 Subject: [PATCH 2/5] Allow no-spawn obj to erase another no-spawn one --- .../src/multi_object_tracker_core.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) 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 2776e0be3610c..628a0a988be8a 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -401,10 +401,6 @@ void MultiObjectTracker::sanitizeTracker( /* delete collision tracker */ for (size_t i = 0; i < sorted_list_tracker.size(); ++i) { - /* block erasing if no measurement */ - if (sorted_list_tracker[i]->getNoMeasurementCount() > 0) { - continue; - } autoware_auto_perception_msgs::msg::TrackedObject object1; sorted_list_tracker[i]->getTrackedObject(time, object1); for (size_t j = i + 1; j < sorted_list_tracker.size(); ++j) { @@ -430,7 +426,11 @@ void MultiObjectTracker::sanitizeTracker( if (label1 == Label::UNKNOWN || label2 == Label::UNKNOWN) { if (min_iou_for_unknown_object < iou) { if (label2 == Label::UNKNOWN) { - should_delete_tracker2 = true; + /* erase only when prioritized one has a measurement or the other one doesn't */ + if (sorted_list_tracker[i]->getNoMeasurementCount() <= 0 or + sorted_list_tracker[j]->getNoMeasurementCount() > 0) { + should_delete_tracker2 = true; + } } } } else { // If neither is UNKNOWN, delete the one with lower IOU. From 67560e4961279e593701cd05e72bc5bf8a3ea680 Mon Sep 17 00:00:00 2001 From: TadaKazuto Date: Tue, 11 Mar 2025 16:34:03 +0900 Subject: [PATCH 3/5] Applied to known -> known case as well --- .../multi_object_tracker/src/multi_object_tracker_core.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) 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 628a0a988be8a..db92020616eb3 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -435,7 +435,10 @@ void MultiObjectTracker::sanitizeTracker( } } else { // If neither is UNKNOWN, delete the one with lower IOU. if (min_iou < iou) { - should_delete_tracker2 = true; + /* erase only when prioritized one has a measurement */ + if (sorted_list_tracker[i]->getNoMeasurementCount() <= 0) { + should_delete_tracker2 = true; + } } } From f8d4663644671f9bfebec4b3e13c9ef5473ed4bb Mon Sep 17 00:00:00 2001 From: TadaKazuto Date: Tue, 11 Mar 2025 18:42:32 +0900 Subject: [PATCH 4/5] Update the condition of erasing and small refactor --- .../src/multi_object_tracker_core.cpp | 28 +++++++++---------- 1 file changed, 13 insertions(+), 15 deletions(-) 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 db92020616eb3..6ca229d08b69e 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -419,35 +419,33 @@ void MultiObjectTracker::sanitizeTracker( const auto iou = object_recognition_utils::get2dIoU(object1, object2, min_union_iou_area); const auto & label1 = sorted_list_tracker[i]->getHighestProbLabel(); const auto & label2 = sorted_list_tracker[j]->getHighestProbLabel(); - bool should_delete_tracker2 = false; + bool delete_candidate_tracker = false; // If at least one of them is UNKNOWN, delete the one with lower IOU. Because the UNKNOWN // objects are not reliable. if (label1 == Label::UNKNOWN || label2 == Label::UNKNOWN) { if (min_iou_for_unknown_object < iou) { if (label2 == Label::UNKNOWN) { - /* erase only when prioritized one has a measurement or the other one doesn't */ - if (sorted_list_tracker[i]->getNoMeasurementCount() <= 0 or - sorted_list_tracker[j]->getNoMeasurementCount() > 0) { - should_delete_tracker2 = true; - } + delete_candidate_tracker = true; } } } else { // If neither is UNKNOWN, delete the one with lower IOU. if (min_iou < iou) { /* erase only when prioritized one has a measurement */ - if (sorted_list_tracker[i]->getNoMeasurementCount() <= 0) { - should_delete_tracker2 = true; - } + delete_candidate_tracker = true; } } - if (should_delete_tracker2) { - // Remove from original list_tracker - list_tracker.remove(sorted_list_tracker[j]); - // Remove from sorted list - sorted_list_tracker.erase(sorted_list_tracker.begin() + j); - --j; + if (delete_candidate_tracker) { + /* erase only when prioritized one has a measurement or the other one doesn't */ + if (sorted_list_tracker[i]->getNoMeasurementCount() <= 0 or + sorted_list_tracker[j]->getNoMeasurementCount() > 0) { + // Remove from original list_tracker + list_tracker.remove(sorted_list_tracker[j]); + // Remove from sorted list + sorted_list_tracker.erase(sorted_list_tracker.begin() + j); + --j; + } } } } From a0e57c1ef3e9a8443e83df84f526d98969d50943 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 11 Mar 2025 10:58:23 +0000 Subject: [PATCH 5/5] style(pre-commit): autofix --- .../src/multi_object_tracker_core.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) 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 6ca229d08b69e..10a3e583e5a4e 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -388,15 +388,18 @@ void MultiObjectTracker::sanitizeTracker( constexpr double distance_threshold = 5.0; // Create sorted list with non-UNKNOWN objects first, then by measurement count - std::vector> sorted_list_tracker(list_tracker.begin(), list_tracker.end()); - std::sort(sorted_list_tracker.begin(), sorted_list_tracker.end(), - [](const std::shared_ptr& a, const std::shared_ptr& b) { + std::vector> sorted_list_tracker( + list_tracker.begin(), list_tracker.end()); + std::sort( + sorted_list_tracker.begin(), sorted_list_tracker.end(), + [](const std::shared_ptr & a, const std::shared_ptr & b) { bool a_unknown = (a->getHighestProbLabel() == Label::UNKNOWN); bool b_unknown = (b->getHighestProbLabel() == Label::UNKNOWN); if (a_unknown != b_unknown) { return b_unknown; // Put non-UNKNOWN objects first } - return a->getTotalMeasurementCount() > b->getTotalMeasurementCount(); // Then sort by measurement count + return a->getTotalMeasurementCount() > + b->getTotalMeasurementCount(); // Then sort by measurement count }); /* delete collision tracker */ @@ -438,8 +441,9 @@ void MultiObjectTracker::sanitizeTracker( if (delete_candidate_tracker) { /* erase only when prioritized one has a measurement or the other one doesn't */ - if (sorted_list_tracker[i]->getNoMeasurementCount() <= 0 or - sorted_list_tracker[j]->getNoMeasurementCount() > 0) { + if ( + sorted_list_tracker[i]->getNoMeasurementCount() <= 0 or + sorted_list_tracker[j]->getNoMeasurementCount() > 0) { // Remove from original list_tracker list_tracker.remove(sorted_list_tracker[j]); // Remove from sorted list