From 40e5c534f03423719f1161d574616b939426a806 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 13 Mar 2024 17:01:30 +0900 Subject: [PATCH 1/8] feat: object class filter Signed-off-by: Taekjin LEE --- .../tracker/model/tracker_base.hpp | 6 ++ .../model/pedestrian_and_bicycle_tracker.cpp | 3 +- .../src/tracker/model/tracker_base.cpp | 80 +++++++++++++++++++ 3 files changed, 88 insertions(+), 1 deletion(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp index e7bf8030cddf8..c026c57a0fef1 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp +++ b/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 & classification); private: unique_identifier_msgs::msg::UUID uuid_; @@ -51,6 +53,9 @@ class Tracker int total_measurement_count_; rclcpp::Time last_update_with_measurement_time_; +public: + autoware_auto_perception_msgs::msg::ObjectClassification last_filtered_class_; + public: Tracker( const rclcpp::Time & time, @@ -68,6 +73,7 @@ class Tracker { return object_recognition_utils::getHighestProbLabel(classification_); } + std::uint8_t getFilteredLabel() const { return last_filtered_class_.label; } int getNoMeasurementCount() const { return no_measurement_count_; } int getTotalNoMeasurementCount() const { return total_no_measurement_count_; } int getTotalMeasurementCount() const { return total_measurement_count_; } diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp index eed9d05359b77..33e5b93685791 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp @@ -43,7 +43,8 @@ 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); + // setClassification(object.classification); + updateClassification(object.classification); return true; } diff --git a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp b/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp index ba684e4777947..c25291400935a 100644 --- a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp +++ b/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp @@ -34,6 +34,9 @@ Tracker::Tracker( std::mt19937 gen(std::random_device{}()); std::independent_bits_engine bit_eng(gen); std::generate(uuid_.uuid.begin(), uuid_.uuid.end(), bit_eng); + + // initialize last_filtered_class_ + last_filtered_class_ = object_recognition_utils::getHighestProbClassification(classification_); } bool Tracker::updateWithMeasurement( @@ -54,6 +57,83 @@ bool Tracker::updateWithoutMeasurement() return true; } +void Tracker::updateClassification( + const std::vector & classification) +{ + // Update classification + // 1. Match classification label + // 2. Update the matched classification probability with a gain + // 3. If the label is not found, add it to the classification list + // 4. If the old class probability is not found, decay the probability + // 5. Normalize the probability + + const double gain = 0.05; + const double gain_inv = 1.0 - gain; + const double decay = gain_inv; + + for (const auto & new_class : classification) { + bool found = false; + for (auto & old_class : classification_) { + // Update the matched classification probability with a gain + if (new_class.label == old_class.label) { + old_class.probability = old_class.probability * gain_inv + new_class.probability * gain; + found = true; + break; + } + } + // If the label is not found, add it to the classification list + if (!found) { + classification_.push_back(new_class); + } + } + // If the old class probability is not found, decay the probability + for (auto & old_class : classification_) { + bool found = false; + for (const auto & new_class : classification) { + if (new_class.label == old_class.label) { + found = true; + break; + } + } + if (!found) { + old_class.probability *= decay; + } + } + + // Normalize + double sum = 0.0; + for (const auto & class_ : classification_) { + sum += class_.probability; + } + for (auto & class_ : classification_) { + class_.probability /= sum; + } + + // If the probability is too small, remove the class + classification_.erase( + std::remove_if( + classification_.begin(), classification_.end(), + [](const auto & class_) { return class_.probability < 0.001; }), + classification_.end()); + + // Set the last filtered class + // if the highest probability class is not overcome a certain hysteresis, the last + // filtered class stays the same + + for (const auto & class_ : classification_) { + if (class_.label == last_filtered_class_.label) { + last_filtered_class_.probability = class_.probability; + break; + } + } + const double hysteresis = 0.1; + autoware_auto_perception_msgs::msg::ObjectClassification const new_classification = + object_recognition_utils::getHighestProbClassification(classification_); + if (new_classification.probability > last_filtered_class_.probability + hysteresis) { + last_filtered_class_ = new_classification; + } +} + geometry_msgs::msg::PoseWithCovariance Tracker::getPoseWithCovariance( const rclcpp::Time & time) const { From e4e88fdd27d89895476c8f105b2b3995c94e33dd Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 13 Mar 2024 18:14:04 +0900 Subject: [PATCH 2/8] fix: set a member private Signed-off-by: Taekjin LEE --- .../include/multi_object_tracker/tracker/model/tracker_base.hpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp index c026c57a0fef1..b4b348e002e10 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp @@ -52,8 +52,6 @@ class Tracker int total_no_measurement_count_; int total_measurement_count_; rclcpp::Time last_update_with_measurement_time_; - -public: autoware_auto_perception_msgs::msg::ObjectClassification last_filtered_class_; public: From 6745406ddb3a407d6da9f94990312fa871e676d2 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 14 Mar 2024 14:37:10 +0900 Subject: [PATCH 3/8] fix: last filtered label is not useful, remove Signed-off-by: Taekjin LEE --- .../tracker/model/tracker_base.hpp | 2 -- .../model/multiple_vehicle_tracker.cpp | 2 +- .../model/pedestrian_and_bicycle_tracker.cpp | 1 - .../src/tracker/model/tracker_base.cpp | 19 ------------------- 4 files changed, 1 insertion(+), 23 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp index b4b348e002e10..ab6a747288d4a 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp @@ -52,7 +52,6 @@ class Tracker int total_no_measurement_count_; int total_measurement_count_; rclcpp::Time last_update_with_measurement_time_; - autoware_auto_perception_msgs::msg::ObjectClassification last_filtered_class_; public: Tracker( @@ -71,7 +70,6 @@ class Tracker { return object_recognition_utils::getHighestProbLabel(classification_); } - std::uint8_t getFilteredLabel() const { return last_filtered_class_.label; } int getNoMeasurementCount() const { return no_measurement_count_; } int getTotalNoMeasurementCount() const { return total_no_measurement_count_; } int getTotalMeasurementCount() const { return total_measurement_count_; } diff --git a/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp index 51adca7e69b56..4f0fb4d7871f2 100644 --- a/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp +++ b/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; } diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp index 33e5b93685791..d61a9a02ccd80 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp @@ -43,7 +43,6 @@ 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; } diff --git a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp b/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp index c25291400935a..1f157dc17c566 100644 --- a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp +++ b/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp @@ -34,9 +34,6 @@ Tracker::Tracker( std::mt19937 gen(std::random_device{}()); std::independent_bits_engine bit_eng(gen); std::generate(uuid_.uuid.begin(), uuid_.uuid.end(), bit_eng); - - // initialize last_filtered_class_ - last_filtered_class_ = object_recognition_utils::getHighestProbClassification(classification_); } bool Tracker::updateWithMeasurement( @@ -116,22 +113,6 @@ void Tracker::updateClassification( [](const auto & class_) { return class_.probability < 0.001; }), classification_.end()); - // Set the last filtered class - // if the highest probability class is not overcome a certain hysteresis, the last - // filtered class stays the same - - for (const auto & class_ : classification_) { - if (class_.label == last_filtered_class_.label) { - last_filtered_class_.probability = class_.probability; - break; - } - } - const double hysteresis = 0.1; - autoware_auto_perception_msgs::msg::ObjectClassification const new_classification = - object_recognition_utils::getHighestProbClassification(classification_); - if (new_classification.probability > last_filtered_class_.probability + hysteresis) { - last_filtered_class_ = new_classification; - } } geometry_msgs::msg::PoseWithCovariance Tracker::getPoseWithCovariance( From 49e9cc30d3dc8527ff706e95946626234287dd06 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 14 Mar 2024 05:39:19 +0000 Subject: [PATCH 4/8] style(pre-commit): autofix Signed-off-by: Taekjin LEE --- .../multi_object_tracker/src/tracker/model/tracker_base.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp b/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp index 1f157dc17c566..a1efe7f4aca2a 100644 --- a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp +++ b/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp @@ -112,7 +112,6 @@ void Tracker::updateClassification( classification_.begin(), classification_.end(), [](const auto & class_) { return class_.probability < 0.001; }), classification_.end()); - } geometry_msgs::msg::PoseWithCovariance Tracker::getPoseWithCovariance( From d505311ff81ecab6342f101368b55b4d6cddb18b Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 14 Mar 2024 15:09:36 +0900 Subject: [PATCH 5/8] fix: multiply gain for new class Signed-off-by: Taekjin LEE --- .../src/tracker/model/tracker_base.cpp | 27 +++++++------------ 1 file changed, 9 insertions(+), 18 deletions(-) diff --git a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp b/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp index a1efe7f4aca2a..ba399acf06d31 100644 --- a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp +++ b/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp @@ -65,35 +65,26 @@ void Tracker::updateClassification( // 5. Normalize the probability const double gain = 0.05; - const double gain_inv = 1.0 - gain; - const double decay = gain_inv; - + const double decay = 1.0 - gain; + // decal all existing probability + for (auto & class_ : classification_) { + class_.probability *= decay; + } for (const auto & new_class : classification) { bool found = false; for (auto & old_class : classification_) { // Update the matched classification probability with a gain if (new_class.label == old_class.label) { - old_class.probability = old_class.probability * gain_inv + new_class.probability * gain; + old_class.probability += new_class.probability * gain; found = true; break; } } // If the label is not found, add it to the classification list if (!found) { - classification_.push_back(new_class); - } - } - // If the old class probability is not found, decay the probability - for (auto & old_class : classification_) { - bool found = false; - for (const auto & new_class : classification) { - if (new_class.label == old_class.label) { - found = true; - break; - } - } - if (!found) { - old_class.probability *= decay; + auto adding_class = new_class; + adding_class.probability *= gain; + classification_.push_back(adding_class); } } From 2669c61e9c997c9603fdc8d4523e6dc2d5b6b3eb Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 14 Mar 2024 06:17:49 +0000 Subject: [PATCH 6/8] style(pre-commit): autofix Signed-off-by: Taekjin LEE --- .../multi_object_tracker/src/tracker/model/tracker_base.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp b/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp index ba399acf06d31..38622072381ee 100644 --- a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp +++ b/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp @@ -83,7 +83,7 @@ void Tracker::updateClassification( // If the label is not found, add it to the classification list if (!found) { auto adding_class = new_class; - adding_class.probability *= gain; + adding_class.probability *= gain; classification_.push_back(adding_class); } } From 9ab9c30b8e50023acd518558343563e3b6f5007a Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 14 Mar 2024 15:23:07 +0900 Subject: [PATCH 7/8] chore: algorithm explanation Signed-off-by: Taekjin LEE --- .../src/tracker/model/tracker_base.cpp | 29 ++++++++++--------- 1 file changed, 16 insertions(+), 13 deletions(-) diff --git a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp b/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp index 38622072381ee..66b72ccbd6f56 100644 --- a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp +++ b/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp @@ -57,23 +57,33 @@ bool Tracker::updateWithoutMeasurement() void Tracker::updateClassification( const std::vector & classification) { - // Update classification - // 1. Match classification label + // 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. If the old class probability is not found, decay the probability - // 5. Normalize the probability + // 4. Normalize + // Gain and decay const double gain = 0.05; const double decay = 1.0 - gain; - // decal all existing probability + + // 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_) { - // Update the matched classification probability with a gain if (new_class.label == old_class.label) { old_class.probability += new_class.probability * gain; found = true; @@ -96,13 +106,6 @@ void Tracker::updateClassification( for (auto & class_ : classification_) { class_.probability /= sum; } - - // If the probability is too small, remove the class - classification_.erase( - std::remove_if( - classification_.begin(), classification_.end(), - [](const auto & class_) { return class_.probability < 0.001; }), - classification_.end()); } geometry_msgs::msg::PoseWithCovariance Tracker::getPoseWithCovariance( From 6c00b63f137fccf80c09b06297d8f5619351e776 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 14 Mar 2024 18:11:14 +0900 Subject: [PATCH 8/8] fix: revise the filtering process flow Signed-off-by: Taekjin LEE --- .../src/tracker/model/tracker_base.cpp | 59 +++++++++++-------- 1 file changed, 33 insertions(+), 26 deletions(-) diff --git a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp b/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp index 66b72ccbd6f56..a3320ff54afcb 100644 --- a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp +++ b/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp @@ -58,30 +58,36 @@ void Tracker::updateClassification( const std::vector & 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 + // 0. Normalize the input classification + // 1-1. Update the matched classification probability with a gain (ratio of 0.05) + // 1-2. If the label is not found, add it to the classification list + // 2. Remove the class with probability < remove_threshold (0.001) + // 3. Normalize tracking classification - // Gain and decay + // Parameters + // if the remove_threshold is too high (compare to the gain), the classification will be removed + // immediately const double gain = 0.05; - const double decay = 1.0 - gain; + constexpr double remove_threshold = 0.001; - // 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()); + // Normalization function + auto normalizeProbabilities = + [](std::vector & classification) { + double sum = 0.0; + for (const auto & class_ : classification) { + sum += class_.probability; + } + for (auto & class_ : classification) { + class_.probability /= sum; + } + }; - // Decay all existing probability - for (auto & class_ : classification_) { - class_.probability *= decay; - } + // Normalize the input + auto classification_input = classification; + normalizeProbabilities(classification_input); // Update the matched classification probability with a gain - for (const auto & new_class : classification) { + for (const auto & new_class : classification_input) { bool found = false; for (auto & old_class : classification_) { if (new_class.label == old_class.label) { @@ -98,14 +104,15 @@ void Tracker::updateClassification( } } - // Normalize - double sum = 0.0; - for (const auto & class_ : classification_) { - sum += class_.probability; - } - for (auto & class_ : classification_) { - class_.probability /= sum; - } + // If the probability is less than the threshold, remove the class + classification_.erase( + std::remove_if( + classification_.begin(), classification_.end(), + [remove_threshold](const auto & class_) { return class_.probability < remove_threshold; }), + classification_.end()); + + // Normalize tracking classification + normalizeProbabilities(classification_); } geometry_msgs::msg::PoseWithCovariance Tracker::getPoseWithCovariance(