From a8baff3372108a12245097c84e17193908d198da Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Wed, 2 Aug 2023 10:39:12 +0900 Subject: [PATCH 1/2] fix(object_merger): separate GIoU Signed-off-by: badai-nguyen --- perception/object_merger/README.md | 2 +- .../include/object_association_merger/node.hpp | 2 +- .../launch/object_association_merger.launch.xml | 2 +- .../object_merger/src/object_association_merger/node.cpp | 7 +++++-- 4 files changed, 8 insertions(+), 5 deletions(-) diff --git a/perception/object_merger/README.md b/perception/object_merger/README.md index 62a95ada62b09..18bf12fa8cdb8 100644 --- a/perception/object_merger/README.md +++ b/perception/object_merger/README.md @@ -34,7 +34,7 @@ The successive shortest path algorithm is used to solve the data association pro | `max_rad_matrix` | double | Maximum angle table for data association | | `base_link_frame_id` | double | association frame | | `distance_threshold_list` | `std::vector` | Distance threshold for each class used in judging overlap. The class order depends on [ObjectClassification](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_perception_msgs/msg/ObjectClassification.idl). | -| `generalized_iou_threshold` | double | Generalized IoU threshold | +| `generalized_iou_threshold` | `std::vector` | Generalized IoU threshold for each class | ## Tips diff --git a/perception/object_merger/include/object_association_merger/node.hpp b/perception/object_merger/include/object_association_merger/node.hpp index 704f7859f6672..1e5b9fad9c9ca 100644 --- a/perception/object_merger/include/object_association_merger/node.hpp +++ b/perception/object_merger/include/object_association_merger/node.hpp @@ -76,7 +76,7 @@ class ObjectAssociationMergerNode : public rclcpp::Node { double precision_threshold; double recall_threshold; - double generalized_iou_threshold; + std::map generalized_iou_threshold; std::map distance_threshold_map; } overlapped_judge_param_; }; diff --git a/perception/object_merger/launch/object_association_merger.launch.xml b/perception/object_merger/launch/object_association_merger.launch.xml index 1b89693b0f9f7..3418f7d5a5e61 100644 --- a/perception/object_merger/launch/object_association_merger.launch.xml +++ b/perception/object_merger/launch/object_association_merger.launch.xml @@ -14,7 +14,7 @@ - + diff --git a/perception/object_merger/src/object_association_merger/node.cpp b/perception/object_merger/src/object_association_merger/node.cpp index 17397b782892b..4f600ce8a4948 100644 --- a/perception/object_merger/src/object_association_merger/node.cpp +++ b/perception/object_merger/src/object_association_merger/node.cpp @@ -35,8 +35,11 @@ bool isUnknownObjectOverlapped( const autoware_auto_perception_msgs::msg::DetectedObject & unknown_object, const autoware_auto_perception_msgs::msg::DetectedObject & known_object, const double precision_threshold, const double recall_threshold, - std::map distance_threshold_map, const double generalized_iou_threshold) + std::map distance_threshold_map, + const std::map generalized_iou_threshold_map) { + const double generalized_iou_threshold = generalized_iou_threshold_map.at( + object_recognition_utils::getHighestProbLabel(known_object.classification)); const double distance_threshold = distance_threshold_map.at( object_recognition_utils::getHighestProbLabel(known_object.classification)); const double sq_distance_threshold = std::pow(distance_threshold, 2.0); @@ -95,7 +98,7 @@ ObjectAssociationMergerNode::ObjectAssociationMergerNode(const rclcpp::NodeOptio overlapped_judge_param_.recall_threshold = declare_parameter("recall_threshold_to_judge_overlapped", 0.5); overlapped_judge_param_.generalized_iou_threshold = - declare_parameter("generalized_iou_threshold"); + convertListToClassMap(declare_parameter>("generalized_iou_threshold")); // get distance_threshold_map from distance_threshold_list /** TODO(Shin-kyoto): From ad121ab0facf468bc6bbab7cd526acbc870069ff Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Wed, 2 Aug 2023 18:30:21 +0900 Subject: [PATCH 2/2] fix: update GIoU threshold Signed-off-by: badai-nguyen --- perception/object_merger/config/overlapped_judge.param.yaml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/perception/object_merger/config/overlapped_judge.param.yaml b/perception/object_merger/config/overlapped_judge.param.yaml index 94882fae4f282..0410b77390187 100644 --- a/perception/object_merger/config/overlapped_judge.param.yaml +++ b/perception/object_merger/config/overlapped_judge.param.yaml @@ -3,3 +3,6 @@ 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: + [-0.1, -0.1, -0.1, -0.6, -0.6, -0.1, -0.1, -0.1]