From 28ec1dd0c4aeaef3fad1c6147aa09b394b964503 Mon Sep 17 00:00:00 2001 From: emuemuJP Date: Mon, 2 Sep 2024 16:56:45 +0900 Subject: [PATCH 1/4] add feat:merge known object Signed-off-by: emuemuJP --- .../object_association_merger_node.hpp | 1 + .../src/object_association_merger_node.cpp | 70 +++++++++++++++++++ 2 files changed, 71 insertions(+) diff --git a/perception/autoware_object_merger/include/autoware/object_merger/object_association_merger_node.hpp b/perception/autoware_object_merger/include/autoware/object_merger/object_association_merger_node.hpp index 81fa34803d6c..09d7f6f52c78 100644 --- a/perception/autoware_object_merger/include/autoware/object_merger/object_association_merger_node.hpp +++ b/perception/autoware_object_merger/include/autoware/object_merger/object_association_merger_node.hpp @@ -75,6 +75,7 @@ class ObjectAssociationMergerNode : public rclcpp::Node PriorityMode priority_mode_; bool remove_overlapped_unknown_objects_; + bool remove_overlapped_known_objects_; struct { double precision_threshold; diff --git a/perception/autoware_object_merger/src/object_association_merger_node.cpp b/perception/autoware_object_merger/src/object_association_merger_node.cpp index 663a338649d3..3bfee3f0c9e2 100644 --- a/perception/autoware_object_merger/src/object_association_merger_node.cpp +++ b/perception/autoware_object_merger/src/object_association_merger_node.cpp @@ -55,6 +55,37 @@ bool isUnknownObjectOverlapped( return precision > precision_threshold || recall > recall_threshold || generalized_iou > generalized_iou_threshold; } + +bool isKnownObjectOverlapped( + const autoware_auto_perception_msgs::msg::DetectedObject & object1, + const autoware_auto_perception_msgs::msg::DetectedObject & object2, + const double precision_threshold, const double recall_threshold, + const std::map& distance_threshold_map, + const std::map& generalized_iou_threshold_map) +{ + int object1_label = object_recognition_utils::getHighestProbLabel(object1.classification); + int object2_label = object_recognition_utils::getHighestProbLabel(object2.classification); + + const double generalized_iou_threshold = std::min( + generalized_iou_threshold_map.at(object1_label), + generalized_iou_threshold_map.at(object2_label)); + const double distance_threshold = std::min( + distance_threshold_map.at(object1_label), + distance_threshold_map.at(object2_label)); + + const double sq_distance_threshold = std::pow(distance_threshold, 2.0); + const double sq_distance = tier4_autoware_utils::calcSquaredDistance2d( + object1.kinematics.pose_with_covariance.pose, + object2.kinematics.pose_with_covariance.pose); + if (sq_distance_threshold < sq_distance) return false; + + const auto precision = object_recognition_utils::get2dPrecision(object1, object2); + const auto recall = object_recognition_utils::get2dRecall(object1, object2); + const auto generalized_iou = object_recognition_utils::get2dGeneralizedIoU(object1, object2); + + return precision > precision_threshold || recall > recall_threshold || + generalized_iou > generalized_iou_threshold; +} } // namespace namespace @@ -224,6 +255,45 @@ void ObjectAssociationMergerNode::objectsCallback( } } + // Remove overlapped known object + if (remove_overlapped_known_objects_) { + std::vector unknown_objects, known_objects; + unknown_objects.reserve(output_msg.objects.size()); + known_objects.reserve(output_msg.objects.size()); + for (const auto & object : output_msg.objects) { + if (object_recognition_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { + unknown_objects.push_back(object); + } else { + known_objects.push_back(object); + } + } + output_msg.objects.clear(); + output_msg.objects = unknown_objects; + for (size_t i = 0; i < known_objects.size(); ++i) { + bool should_keep = true; + for (size_t j = 0; j < known_objects.size(); ++j) { + if (i == j) { + continue; + } + if (areObjectsOverlapped(known_objects[i], known_objects[j], + overlapped_judge_param_.precision_threshold, + overlapped_judge_param_.recall_threshold, + overlapped_judge_param_.distance_threshold_map, + overlapped_judge_param_.generalized_iou_threshold)) { + auto label_i = object_recognition_utils::getHighestProbClassification(known_objects[i].classification); + auto label_j = object_recognition_utils::getHighestProbClassification(known_objects[j].classification); + if (label_i.probability * known_objects[i].existence_probability <= label_j.probability * known_objects[j].existence_probability) { + should_keep = false; + break; + } + } + } + if (should_keep) { + output_msg.objects.push_back(known_objects[i]); + } + } + } + // publish output msg merged_object_pub_->publish(output_msg); published_time_publisher_->publish_if_subscribed(merged_object_pub_, output_msg.header.stamp); From e8caf2703642724abcee0dd49d9635e72bd70d5c Mon Sep 17 00:00:00 2001 From: emuemuJP Date: Tue, 3 Sep 2024 00:15:14 +0900 Subject: [PATCH 2/4] fix class name diff Signed-off-by: emuemuJP --- .../src/object_association_merger_node.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/perception/autoware_object_merger/src/object_association_merger_node.cpp b/perception/autoware_object_merger/src/object_association_merger_node.cpp index 3bfee3f0c9e2..cec06a998709 100644 --- a/perception/autoware_object_merger/src/object_association_merger_node.cpp +++ b/perception/autoware_object_merger/src/object_association_merger_node.cpp @@ -57,8 +57,8 @@ bool isUnknownObjectOverlapped( } bool isKnownObjectOverlapped( - const autoware_auto_perception_msgs::msg::DetectedObject & object1, - const autoware_auto_perception_msgs::msg::DetectedObject & object2, + const autoware_perception_msgs::msg::DetectedObject & object1, + const autoware_perception_msgs::msg::DetectedObject & object2, const double precision_threshold, const double recall_threshold, const std::map& distance_threshold_map, const std::map& generalized_iou_threshold_map) @@ -74,7 +74,7 @@ bool isKnownObjectOverlapped( distance_threshold_map.at(object2_label)); const double sq_distance_threshold = std::pow(distance_threshold, 2.0); - const double sq_distance = tier4_autoware_utils::calcSquaredDistance2d( + const double sq_distance = autoware::universe_utils::calcSquaredDistance2d( object1.kinematics.pose_with_covariance.pose, object2.kinematics.pose_with_covariance.pose); if (sq_distance_threshold < sq_distance) return false; @@ -257,7 +257,7 @@ void ObjectAssociationMergerNode::objectsCallback( // Remove overlapped known object if (remove_overlapped_known_objects_) { - std::vector unknown_objects, known_objects; + std::vector unknown_objects, known_objects; unknown_objects.reserve(output_msg.objects.size()); known_objects.reserve(output_msg.objects.size()); for (const auto & object : output_msg.objects) { @@ -275,7 +275,7 @@ void ObjectAssociationMergerNode::objectsCallback( if (i == j) { continue; } - if (areObjectsOverlapped(known_objects[i], known_objects[j], + if (isKnownObjectOverlapped(known_objects[i], known_objects[j], overlapped_judge_param_.precision_threshold, overlapped_judge_param_.recall_threshold, overlapped_judge_param_.distance_threshold_map, From 4dc278e25a1cc27433b58d377cb180c5b6e16b2c Mon Sep 17 00:00:00 2001 From: emuemuJP Date: Tue, 3 Sep 2024 02:14:20 +0900 Subject: [PATCH 3/4] Apply clang-format changes Signed-off-by: emuemuJP --- .../src/object_association_merger_node.cpp | 35 ++++++++++--------- 1 file changed, 18 insertions(+), 17 deletions(-) diff --git a/perception/autoware_object_merger/src/object_association_merger_node.cpp b/perception/autoware_object_merger/src/object_association_merger_node.cpp index cec06a998709..32b55be366c8 100644 --- a/perception/autoware_object_merger/src/object_association_merger_node.cpp +++ b/perception/autoware_object_merger/src/object_association_merger_node.cpp @@ -58,10 +58,9 @@ bool isUnknownObjectOverlapped( bool isKnownObjectOverlapped( const autoware_perception_msgs::msg::DetectedObject & object1, - const autoware_perception_msgs::msg::DetectedObject & object2, - const double precision_threshold, const double recall_threshold, - const std::map& distance_threshold_map, - const std::map& generalized_iou_threshold_map) + const autoware_perception_msgs::msg::DetectedObject & object2, const double precision_threshold, + const double recall_threshold, const std::map & distance_threshold_map, + const std::map & generalized_iou_threshold_map) { int object1_label = object_recognition_utils::getHighestProbLabel(object1.classification); int object2_label = object_recognition_utils::getHighestProbLabel(object2.classification); @@ -69,14 +68,12 @@ bool isKnownObjectOverlapped( const double generalized_iou_threshold = std::min( generalized_iou_threshold_map.at(object1_label), generalized_iou_threshold_map.at(object2_label)); - const double distance_threshold = std::min( - distance_threshold_map.at(object1_label), - distance_threshold_map.at(object2_label)); + const double distance_threshold = + std::min(distance_threshold_map.at(object1_label), distance_threshold_map.at(object2_label)); const double sq_distance_threshold = std::pow(distance_threshold, 2.0); const double sq_distance = autoware::universe_utils::calcSquaredDistance2d( - object1.kinematics.pose_with_covariance.pose, - object2.kinematics.pose_with_covariance.pose); + object1.kinematics.pose_with_covariance.pose, object2.kinematics.pose_with_covariance.pose); if (sq_distance_threshold < sq_distance) return false; const auto precision = object_recognition_utils::get2dPrecision(object1, object2); @@ -275,14 +272,18 @@ void ObjectAssociationMergerNode::objectsCallback( if (i == j) { continue; } - if (isKnownObjectOverlapped(known_objects[i], known_objects[j], - overlapped_judge_param_.precision_threshold, - overlapped_judge_param_.recall_threshold, - overlapped_judge_param_.distance_threshold_map, - overlapped_judge_param_.generalized_iou_threshold)) { - auto label_i = object_recognition_utils::getHighestProbClassification(known_objects[i].classification); - auto label_j = object_recognition_utils::getHighestProbClassification(known_objects[j].classification); - if (label_i.probability * known_objects[i].existence_probability <= label_j.probability * known_objects[j].existence_probability) { + if (isKnownObjectOverlapped( + known_objects[i], known_objects[j], overlapped_judge_param_.precision_threshold, + overlapped_judge_param_.recall_threshold, + overlapped_judge_param_.distance_threshold_map, + overlapped_judge_param_.generalized_iou_threshold)) { + auto label_i = + object_recognition_utils::getHighestProbClassification(known_objects[i].classification); + auto label_j = + object_recognition_utils::getHighestProbClassification(known_objects[j].classification); + if ( + label_i.probability * known_objects[i].existence_probability <= + label_j.probability * known_objects[j].existence_probability) { should_keep = false; break; } From d534972764d78a96d62ea744b543f456bad38337 Mon Sep 17 00:00:00 2001 From: emuemuJP Date: Sat, 7 Sep 2024 21:25:38 +0900 Subject: [PATCH 4/4] add params for known object merge Signed-off-by: emuemuJP --- .../object_association_merger.param.yaml | 7 +- .../config/overlapped_judge.param.yaml | 11 ++- .../object_association_merger_node.hpp | 6 +- .../src/object_association_merger_node.cpp | 73 +++++++++++-------- 4 files changed, 60 insertions(+), 37 deletions(-) diff --git a/perception/autoware_object_merger/config/object_association_merger.param.yaml b/perception/autoware_object_merger/config/object_association_merger.param.yaml index e9b86cd64be2..259f1e0244c0 100644 --- a/perception/autoware_object_merger/config/object_association_merger.param.yaml +++ b/perception/autoware_object_merger/config/object_association_merger.param.yaml @@ -1,8 +1,11 @@ /**: ros__parameters: sync_queue_size: 20 - precision_threshold_to_judge_overlapped: 0.4 - recall_threshold_to_judge_overlapped: 0.5 + precision_threshold_to_judge_overlapped_unknown: 0.4 + precision_threshold_to_judge_overlapped_known: 0.4 + recall_threshold_to_judge_overlapped_unknown: 0.5 + recall_threshold_to_judge_overlapped_known: 0.5 remove_overlapped_unknown_objects: true + remove_overlapped_known_objects: true base_link_frame_id: base_link priority_mode: 3 # PriorityMode::Confidence diff --git a/perception/autoware_object_merger/config/overlapped_judge.param.yaml b/perception/autoware_object_merger/config/overlapped_judge.param.yaml index 0410b7739018..b1a4057d09e0 100644 --- a/perception/autoware_object_merger/config/overlapped_judge.param.yaml +++ b/perception/autoware_object_merger/config/overlapped_judge.param.yaml @@ -1,8 +1,15 @@ /**: ros__parameters: - distance_threshold_list: + unknown_obj_distance_threshold_list: #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN [9.0, 9.0, 9.0, 9.0, 9.0, 9.0, 9.0, 9.0] #UNKNOWN - generalized_iou_threshold: + unknown_obj_generalized_iou_threshold: + [-0.1, -0.1, -0.1, -0.6, -0.6, -0.1, -0.1, -0.1] + + known_obj_distance_threshold_list: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN + [9.0, 9.0, 9.0, 9.0, 9.0, 9.0, 9.0, 9.0] #KNOWN + + known_obj_generalized_iou_threshold: [-0.1, -0.1, -0.1, -0.6, -0.6, -0.1, -0.1, -0.1] diff --git a/perception/autoware_object_merger/include/autoware/object_merger/object_association_merger_node.hpp b/perception/autoware_object_merger/include/autoware/object_merger/object_association_merger_node.hpp index 09d7f6f52c78..7e54a6ee41c8 100644 --- a/perception/autoware_object_merger/include/autoware/object_merger/object_association_merger_node.hpp +++ b/perception/autoware_object_merger/include/autoware/object_merger/object_association_merger_node.hpp @@ -76,13 +76,15 @@ class ObjectAssociationMergerNode : public rclcpp::Node PriorityMode priority_mode_; bool remove_overlapped_unknown_objects_; bool remove_overlapped_known_objects_; - struct + using OverlappedJudgeParam = struct { double precision_threshold; double recall_threshold; std::map generalized_iou_threshold; std::map distance_threshold_map; - } overlapped_judge_param_; + }; + + OverlappedJudgeParam overlapped_unknown_obj_judge_param_, overlapped_known_obj_judge_param_; // debug publisher std::unique_ptr processing_time_publisher_; diff --git a/perception/autoware_object_merger/src/object_association_merger_node.cpp b/perception/autoware_object_merger/src/object_association_merger_node.cpp index 32b55be366c8..34a31ddb9e8f 100644 --- a/perception/autoware_object_merger/src/object_association_merger_node.cpp +++ b/perception/autoware_object_merger/src/object_association_merger_node.cpp @@ -113,20 +113,32 @@ ObjectAssociationMergerNode::ObjectAssociationMergerNode(const rclcpp::NodeOptio priority_mode_ = static_cast(declare_parameter("priority_mode")); sync_queue_size_ = declare_parameter("sync_queue_size"); remove_overlapped_unknown_objects_ = declare_parameter("remove_overlapped_unknown_objects"); - overlapped_judge_param_.precision_threshold = - declare_parameter("precision_threshold_to_judge_overlapped"); - overlapped_judge_param_.recall_threshold = - declare_parameter("recall_threshold_to_judge_overlapped"); - overlapped_judge_param_.generalized_iou_threshold = - convertListToClassMap(declare_parameter>("generalized_iou_threshold")); + + // unknown obj param + overlapped_unknown_obj_judge_param_.precision_threshold = + declare_parameter("precision_threshold_to_judge_overlapped_unknown"); + overlapped_unknown_obj_judge_param_.recall_threshold = + declare_parameter("recall_threshold_to_judge_overlapped_unknown", 0.5); + overlapped_unknown_obj_judge_param_.generalized_iou_threshold = + convertListToClassMap(declare_parameter>("unknown_obj_generalized_iou_threshold")); // get distance_threshold_map from distance_threshold_list /** TODO(Shin-kyoto): * this implementation assumes index of vector shows class_label. * if param supports map, refactor this code. */ - overlapped_judge_param_.distance_threshold_map = - convertListToClassMap(declare_parameter>("distance_threshold_list")); + overlapped_unknown_obj_judge_param_.distance_threshold_map = + convertListToClassMap(declare_parameter>("unknown_obj_distance_threshold_list")); + + // known obj param + overlapped_known_obj_judge_param_.precision_threshold = + declare_parameter("precision_threshold_to_judge_overlapped_known"); + overlapped_known_obj_judge_param_.recall_threshold = + declare_parameter("recall_threshold_to_judge_overlapped_known", 0.5); + overlapped_known_obj_judge_param_.generalized_iou_threshold = + convertListToClassMap(declare_parameter>("known_obj_generalized_iou_threshold")); + overlapped_known_obj_judge_param_.distance_threshold_map = + convertListToClassMap(declare_parameter>("known_obj_distance_threshold_list")); const auto tmp = this->declare_parameter>("can_assign_matrix"); const std::vector can_assign_matrix(tmp.begin(), tmp.end()); @@ -238,10 +250,10 @@ void ObjectAssociationMergerNode::objectsCallback( bool is_overlapped = false; for (const auto & known_object : known_objects) { if (isUnknownObjectOverlapped( - unknown_object, known_object, overlapped_judge_param_.precision_threshold, - overlapped_judge_param_.recall_threshold, - overlapped_judge_param_.distance_threshold_map, - overlapped_judge_param_.generalized_iou_threshold)) { + unknown_object, known_object, overlapped_unknown_obj_judge_param_.precision_threshold, + overlapped_unknown_obj_judge_param_.recall_threshold, + overlapped_unknown_obj_judge_param_.distance_threshold_map, + overlapped_unknown_obj_judge_param_.generalized_iou_threshold)) { is_overlapped = true; break; } @@ -264,33 +276,32 @@ void ObjectAssociationMergerNode::objectsCallback( known_objects.push_back(object); } } + + std::sort(known_objects.begin(), known_objects.end(), + [](const auto& a, const auto& b) { + return a.kinematics.pose_with_covariance.pose.position.x < b.kinematics.pose_with_covariance.pose.position.x; + }); + output_msg.objects.clear(); output_msg.objects = unknown_objects; for (size_t i = 0; i < known_objects.size(); ++i) { bool should_keep = true; - for (size_t j = 0; j < known_objects.size(); ++j) { - if (i == j) { - continue; - } - if (isKnownObjectOverlapped( - known_objects[i], known_objects[j], overlapped_judge_param_.precision_threshold, - overlapped_judge_param_.recall_threshold, - overlapped_judge_param_.distance_threshold_map, - overlapped_judge_param_.generalized_iou_threshold)) { - auto label_i = - object_recognition_utils::getHighestProbClassification(known_objects[i].classification); - auto label_j = - object_recognition_utils::getHighestProbClassification(known_objects[j].classification); - if ( - label_i.probability * known_objects[i].existence_probability <= - label_j.probability * known_objects[j].existence_probability) { + for (size_t j = i + 1; j < known_objects.size(); ++j) { + if (isKnownObjectOverlapped(known_objects[i], known_objects[j], + overlapped_known_obj_judge_param_.precision_threshold, + overlapped_known_obj_judge_param_.recall_threshold, + overlapped_known_obj_judge_param_.distance_threshold_map, + overlapped_known_obj_judge_param_.generalized_iou_threshold)) { + auto label_i = object_recognition_utils::getHighestProbClassification(known_objects[i].classification); + auto label_j = object_recognition_utils::getHighestProbClassification(known_objects[j].classification); + if (label_i.probability * known_objects[i].existence_probability <= label_j.probability * known_objects[j].existence_probability) { should_keep = false; break; } } - } - if (should_keep) { - output_msg.objects.push_back(known_objects[i]); + if (should_keep) { + output_msg.objects.push_back(known_objects[i]); + } } } }