From 0b9739182fa42c223856b230ac843c45848959e0 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Mon, 23 Dec 2024 14:34:04 +0900 Subject: [PATCH 01/18] feat: add traffic light selector node Signed-off-by: badai-nguyen feat: add traffic ligth selector node Signed-off-by: badai-nguyen --- .../CMakeLists.txt | 29 +++++++ .../launch/traffic_light_selector.launch.xml | 17 ++++ .../package.xml | 31 +++++++ .../src/traffic_light_selector_node.cpp | 85 +++++++++++++++++++ .../src/traffic_light_selector_node.hpp | 73 ++++++++++++++++ 5 files changed, 235 insertions(+) create mode 100644 perception/autoware_traffic_light_selector/CMakeLists.txt create mode 100644 perception/autoware_traffic_light_selector/launch/traffic_light_selector.launch.xml create mode 100644 perception/autoware_traffic_light_selector/package.xml create mode 100644 perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp create mode 100644 perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp diff --git a/perception/autoware_traffic_light_selector/CMakeLists.txt b/perception/autoware_traffic_light_selector/CMakeLists.txt new file mode 100644 index 0000000000000..b5860c1376d71 --- /dev/null +++ b/perception/autoware_traffic_light_selector/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.8) +project(autoware_traffic_light_selector) + +# find dependencies +find_package(autoware_cmake REQUIRED) +autoware_package() + +# Targets +ament_auto_add_library(${PROJECT_NAME} SHARED + src/traffic_light_selector_node.cpp +) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::traffic_light::TrafficLightSelectorNode" + EXECUTABLE traffic_light_selector_node) + + +if(BUILD_TESTING) + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_uncrustify) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + +endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/perception/autoware_traffic_light_selector/launch/traffic_light_selector.launch.xml b/perception/autoware_traffic_light_selector/launch/traffic_light_selector.launch.xml new file mode 100644 index 0000000000000..4898d6529738b --- /dev/null +++ b/perception/autoware_traffic_light_selector/launch/traffic_light_selector.launch.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/perception/autoware_traffic_light_selector/package.xml b/perception/autoware_traffic_light_selector/package.xml new file mode 100644 index 0000000000000..d932ea119eed0 --- /dev/null +++ b/perception/autoware_traffic_light_selector/package.xml @@ -0,0 +1,31 @@ + + + + autoware_traffic_light_selector + 0.1.0 + The ROS 2 cluster merger package + Yukihiro Saito + Dai Nguyen + Yoshi Ri + Apache License 2.0 + Dai Nguyen + + ament_cmake_auto + + autoware_test_utils + autoware_universe_utils + geometry_msgs + message_filters + object_recognition_utils + rclcpp + rclcpp_components + tier4_perception_msgs + autoware_cmake + ament_lint_auto + autoware_lint_common + sensor_msgs + + + ament_cmake + + diff --git a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp new file mode 100644 index 0000000000000..8b0865c543abc --- /dev/null +++ b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp @@ -0,0 +1,85 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "traffic_light_selector_node.hpp" + +#include "object_recognition_utils/object_recognition_utils.hpp" +#include "sensor_msgs/msg/region_of_interest.hpp" + +#include +#include +#include + +namespace +{ + bool isInsideRoughRoi(const sensor_msgs::msg::RegionOfInterest & detected_roi, const sensor_msgs::msg::RegionOfInterest & rough_roi) + { + return detected_roi.x_offset >= rough_roi.x_offset && + detected_roi.y_offset >= rough_roi.y_offset && + detected_roi.x_offset + detected_roi.width <= rough_roi.x_offset + rough_roi.width && + detected_roi.y_offset + detected_roi.height <= rough_roi.y_offset + rough_roi.height; + } +} + +namespace autoware::traffic_light +{ + +TrafficLightSelectorNode::TrafficLightSelectorNode(const rclcpp::NodeOptions & node_options) +: rclcpp::Node("traffic_light_selector_node", node_options), + tf_buffer_(get_clock()), + tf_listener_(tf_buffer_), + objects0_sub_(this, "input/detected_rois", rclcpp::QoS{1}.get_rmw_qos_profile()), + objects1_sub_(this, "input/rough_rois", rclcpp::QoS{1}.get_rmw_qos_profile()), + sync_(SyncPolicy(10), objects0_sub_, objects1_sub_) +{ + debug_ = declare_parameter("debug"); + using std::placeholders::_1; + using std::placeholders::_2; + sync_.registerCallback(std::bind(&TrafficLightSelectorNode::objectsCallback, this, _1, _2)); + + // Publisher + pub_traffic_light_rois_ = create_publisher("output/traffic_light_rois", rclcpp::QoS{1}); + +} + +void TrafficLightSelectorNode::objectsCallback( + const DetectedObjectsWithFeature::ConstSharedPtr & detected_traffic_light_msg, + const TrafficLightRoiArray::ConstSharedPtr & rough_rois_msg) +{ + TrafficLightRoiArray output; + output.header = detected_traffic_light_msg->header; + for (const auto & detected_light : detected_traffic_light_msg->feature_objects) + { + const auto detected_roi = detected_light.feature.roi; + for (const auto & rough_roi : rough_rois_msg->rois) + { + if (isInsideRoughRoi(detected_roi, rough_roi.roi)) + { + TrafficLightRoi selected_roi; + selected_roi.roi = detected_roi; + selected_roi.traffic_light_id = rough_roi.traffic_light_id; + output.rois.push_back(selected_roi); + } + } + } + if (debug_){ + pub_traffic_light_rois_->publish(output); + } + return; + +} +} // namespace autoware::traffic_light_selector + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::traffic_light::TrafficLightSelectorNode) diff --git a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp new file mode 100644 index 0000000000000..93e48d532ef6b --- /dev/null +++ b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp @@ -0,0 +1,73 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TRAFFIC_LIGHT_SELECTOR_NODE_HPP_ +#define TRAFFIC_LIGHT_SELECTOR_NODE_HPP_ + +#include "autoware/universe_utils/ros/transform_listener.hpp" +#include "message_filters/subscriber.h" +#include "message_filters/sync_policies/approximate_time.h" +#include "message_filters/synchronizer.h" +#include "rclcpp/rclcpp.hpp" + +#include "tier4_perception_msgs/msg/detected_objects_with_feature.hpp" +#include "tier4_perception_msgs/msg/detected_object_with_feature.hpp" +#include "tier4_perception_msgs/msg/traffic_light_roi_array.hpp" +#include "tier4_perception_msgs/msg/traffic_light_roi.hpp" + +#include +#include + +#include +#include +#include +#include + +namespace autoware::traffic_light +{ +using tier4_perception_msgs::msg::DetectedObjectsWithFeature; +using tier4_perception_msgs::msg::DetectedObjectWithFeature; +using tier4_perception_msgs::msg::TrafficLightRoiArray; +using tier4_perception_msgs::msg::TrafficLightRoi; + + +class TrafficLightSelectorNode : public rclcpp::Node +{ +public: + explicit TrafficLightSelectorNode(const rclcpp::NodeOptions & node_options); + +private: + // Subscriber + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + message_filters::Subscriber objects0_sub_; + message_filters::Subscriber objects1_sub_; + typedef message_filters::sync_policies::ApproximateTime< + DetectedObjectsWithFeature, TrafficLightRoiArray> + SyncPolicy; + typedef message_filters::Synchronizer Sync; + Sync sync_; + void objectsCallback( + const DetectedObjectsWithFeature::ConstSharedPtr & input_objects0_msg, + const TrafficLightRoiArray::ConstSharedPtr & input_objects1_msg); + // Publisher + rclcpp::Publisher::SharedPtr pub_traffic_light_rois_; + bool debug_{false}; +}; + +} // namespace autoware::traffic_light + +#endif // TRAFFIC_LIGHT_SELECTOR_NODE_HPP_ From d78d9dbd2015620fff8200814bba7cfb222fecb9 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Mon, 9 Dec 2024 21:21:39 +0900 Subject: [PATCH 02/18] fix: add check expect roi iou Signed-off-by: badai-nguyen --- .../package.xml | 2 +- .../src/traffic_light_selector_node.cpp | 134 ++++++++++++++---- .../src/traffic_light_selector_node.hpp | 21 +-- 3 files changed, 117 insertions(+), 40 deletions(-) diff --git a/perception/autoware_traffic_light_selector/package.xml b/perception/autoware_traffic_light_selector/package.xml index d932ea119eed0..83068f220fbdf 100644 --- a/perception/autoware_traffic_light_selector/package.xml +++ b/perception/autoware_traffic_light_selector/package.xml @@ -19,11 +19,11 @@ object_recognition_utils rclcpp rclcpp_components + sensor_msgs tier4_perception_msgs autoware_cmake ament_lint_auto autoware_lint_common - sensor_msgs ament_cmake diff --git a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp index 8b0865c543abc..a4e7b15ae2ef9 100644 --- a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp +++ b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp @@ -15,6 +15,7 @@ #include "traffic_light_selector_node.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" + #include "sensor_msgs/msg/region_of_interest.hpp" #include @@ -23,14 +24,57 @@ namespace { - bool isInsideRoughRoi(const sensor_msgs::msg::RegionOfInterest & detected_roi, const sensor_msgs::msg::RegionOfInterest & rough_roi) - { - return detected_roi.x_offset >= rough_roi.x_offset && - detected_roi.y_offset >= rough_roi.y_offset && - detected_roi.x_offset + detected_roi.width <= rough_roi.x_offset + rough_roi.width && - detected_roi.y_offset + detected_roi.height <= rough_roi.y_offset + rough_roi.height; - } +bool isInsideRoughRoi( + const sensor_msgs::msg::RegionOfInterest & detected_roi, + const sensor_msgs::msg::RegionOfInterest & rough_roi) +{ + return detected_roi.x_offset >= rough_roi.x_offset && + detected_roi.y_offset >= rough_roi.y_offset && + detected_roi.x_offset + detected_roi.width <= rough_roi.x_offset + rough_roi.width && + detected_roi.y_offset + detected_roi.height <= rough_roi.y_offset + rough_roi.height; +} + +double getGenIoU( + const sensor_msgs::msg::RegionOfInterest & roi1, const sensor_msgs::msg::RegionOfInterest & roi2) +{ + int rect1_x_min = static_cast(roi1.x_offset); + int rect1_x_max = static_cast(roi1.x_offset + roi1.width); + int rect1_y_min = static_cast(roi1.y_offset); + int rect1_y_max = static_cast(roi1.y_offset + roi1.height); + int rect2_x_min = static_cast(roi2.x_offset); + int rect2_x_max = static_cast(roi2.x_offset + roi2.width); + int rect2_y_min = static_cast(roi2.y_offset); + int rect2_y_max = static_cast(roi2.y_offset + roi2.height); + int rect1_area = roi1.width * roi1.height; + int rect2_area = roi2.width * roi2.height; + int x_min = std::max(rect1_x_min, rect2_x_min); + int y_min = std::max(rect1_y_min, rect2_y_min); + int x_max = std::min(rect1_x_max, rect2_x_max); + int y_max = std::min(rect1_y_max, rect2_y_max); + + auto w = std::max(0, x_max - x_min); + auto h = std::max(0, y_max - y_min); + auto intersect = w * h; + + auto union_area = rect1_area + rect2_area - intersect; + + double iou = static_cast(intersect) / static_cast(union_area); + + // convex shape area + + auto con_x_min = std::min(rect1_x_min, rect2_x_min); + auto con_y_min = std::min(rect1_y_min, rect2_y_min); + auto con_x_max = std::max(rect1_x_max, rect2_x_max); + auto con_y_max = std::max(rect1_y_max, rect2_y_max); + + auto con_area = (con_x_max - con_x_min + 1) * (con_y_max - con_y_min + 1); + + // GIoU calc + double giou = iou - static_cast(con_area - union_area) / static_cast(con_area); + + return giou; } +} // namespace namespace autoware::traffic_light { @@ -39,47 +83,77 @@ TrafficLightSelectorNode::TrafficLightSelectorNode(const rclcpp::NodeOptions & n : rclcpp::Node("traffic_light_selector_node", node_options), tf_buffer_(get_clock()), tf_listener_(tf_buffer_), - objects0_sub_(this, "input/detected_rois", rclcpp::QoS{1}.get_rmw_qos_profile()), - objects1_sub_(this, "input/rough_rois", rclcpp::QoS{1}.get_rmw_qos_profile()), - sync_(SyncPolicy(10), objects0_sub_, objects1_sub_) + detected_rois_sub_(this, "input/detected_rois", rclcpp::QoS{1}.get_rmw_qos_profile()), + rough_rois_sub_(this, "input/rough_rois", rclcpp::QoS{1}.get_rmw_qos_profile()), + expected_rois_sub_(this, "input/expect_rois", rclcpp::QoS{1}.get_rmw_qos_profile()), + sync_(SyncPolicy(10), detected_rois_sub_, rough_rois_sub_, expected_rois_sub_) { debug_ = declare_parameter("debug"); using std::placeholders::_1; using std::placeholders::_2; - sync_.registerCallback(std::bind(&TrafficLightSelectorNode::objectsCallback, this, _1, _2)); + using std::placeholders::_3; + sync_.registerCallback(std::bind(&TrafficLightSelectorNode::objectsCallback, this, _1, _2, _3)); // Publisher - pub_traffic_light_rois_ = create_publisher("output/traffic_light_rois", rclcpp::QoS{1}); - + pub_traffic_light_rois_ = + create_publisher("output/traffic_light_rois", rclcpp::QoS{1}); } void TrafficLightSelectorNode::objectsCallback( const DetectedObjectsWithFeature::ConstSharedPtr & detected_traffic_light_msg, - const TrafficLightRoiArray::ConstSharedPtr & rough_rois_msg) -{ + const TrafficLightRoiArray::ConstSharedPtr & rough_rois_msg, + const TrafficLightRoiArray::ConstSharedPtr & expected_rois_msg) +{ TrafficLightRoiArray output; + + // create map for traffic_roi and traffic_light_id + std::map rough_roi_map; + for (const auto & rough_roi : rough_rois_msg->rois) { + rough_roi_map[rough_roi.traffic_light_id] = rough_roi.roi; + } + std::map expected_roi_map; + for (const auto & expected_roi : expected_rois_msg->rois) { + expected_roi_map[expected_roi.traffic_light_id] = expected_roi.roi; + } + + // declare image to selected_roi and publish + output.header = detected_traffic_light_msg->header; - for (const auto & detected_light : detected_traffic_light_msg->feature_objects) - { - const auto detected_roi = detected_light.feature.roi; - for (const auto & rough_roi : rough_rois_msg->rois) - { - if (isInsideRoughRoi(detected_roi, rough_roi.roi)) - { - TrafficLightRoi selected_roi; - selected_roi.roi = detected_roi; - selected_roi.traffic_light_id = rough_roi.traffic_light_id; - output.rois.push_back(selected_roi); + for (const auto & rough_roi : rough_rois_msg->rois) { + // find expect roi + auto expected_roi = expected_roi_map.find(rough_roi.traffic_light_id); + double max_gen_iou = -1.0; + TrafficLightRoi max_gen_iou_roi; + max_gen_iou_roi.traffic_light_id = rough_roi.traffic_light_id; + + for (const auto & detected_light : detected_traffic_light_msg->feature_objects) { + const auto detected_roi = detected_light.feature.roi; + const auto detected_class = detected_light.object.classification.front().label; + if (detected_class == 0) { + continue; + } + if (!isInsideRoughRoi(detected_roi, rough_roi.roi)) { + continue; + } + double gen_iou = getGenIoU(detected_roi, expected_roi->second); + RCLCPP_INFO(get_logger(), "gen_iou: %f", gen_iou); + if (gen_iou > max_gen_iou) { + max_gen_iou = gen_iou; + max_gen_iou_roi.roi = detected_roi; + max_gen_iou_roi.traffic_light_type = detected_class - 1; } } + if (max_gen_iou > -1.0) { + output.rois.push_back(max_gen_iou_roi); + } } - if (debug_){ - pub_traffic_light_rois_->publish(output); + + pub_traffic_light_rois_->publish(output); + if (debug_) { } return; - } -} // namespace autoware::traffic_light_selector +} // namespace autoware::traffic_light #include "rclcpp_components/register_node_macro.hpp" RCLCPP_COMPONENTS_REGISTER_NODE(autoware::traffic_light::TrafficLightSelectorNode) diff --git a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp index 93e48d532ef6b..859b58247b4b2 100644 --- a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp +++ b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp @@ -21,10 +21,10 @@ #include "message_filters/synchronizer.h" #include "rclcpp/rclcpp.hpp" -#include "tier4_perception_msgs/msg/detected_objects_with_feature.hpp" #include "tier4_perception_msgs/msg/detected_object_with_feature.hpp" -#include "tier4_perception_msgs/msg/traffic_light_roi_array.hpp" +#include "tier4_perception_msgs/msg/detected_objects_with_feature.hpp" #include "tier4_perception_msgs/msg/traffic_light_roi.hpp" +#include "tier4_perception_msgs/msg/traffic_light_roi_array.hpp" #include #include @@ -38,9 +38,8 @@ namespace autoware::traffic_light { using tier4_perception_msgs::msg::DetectedObjectsWithFeature; using tier4_perception_msgs::msg::DetectedObjectWithFeature; -using tier4_perception_msgs::msg::TrafficLightRoiArray; using tier4_perception_msgs::msg::TrafficLightRoi; - +using tier4_perception_msgs::msg::TrafficLightRoiArray; class TrafficLightSelectorNode : public rclcpp::Node { @@ -53,19 +52,23 @@ class TrafficLightSelectorNode : public rclcpp::Node tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; - message_filters::Subscriber objects0_sub_; - message_filters::Subscriber objects1_sub_; + message_filters::Subscriber detected_rois_sub_; + message_filters::Subscriber rough_rois_sub_; + message_filters::Subscriber expected_rois_sub_; typedef message_filters::sync_policies::ApproximateTime< - DetectedObjectsWithFeature, TrafficLightRoiArray> + DetectedObjectsWithFeature, TrafficLightRoiArray, TrafficLightRoiArray> SyncPolicy; typedef message_filters::Synchronizer Sync; Sync sync_; void objectsCallback( - const DetectedObjectsWithFeature::ConstSharedPtr & input_objects0_msg, - const TrafficLightRoiArray::ConstSharedPtr & input_objects1_msg); + const DetectedObjectsWithFeature::ConstSharedPtr & detected_rois_msg, + const TrafficLightRoiArray::ConstSharedPtr & rough_rois_msg, + const TrafficLightRoiArray::ConstSharedPtr & expect_rois_msg); // Publisher rclcpp::Publisher::SharedPtr pub_traffic_light_rois_; bool debug_{false}; + // declare publisher for debug image + // rclcpp::Publisher::SharedPtr pub_debug_image_; }; } // namespace autoware::traffic_light From f36de38bd83f3a0502fc202d9dd6e379ab681152 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Wed, 11 Dec 2024 10:18:40 +0900 Subject: [PATCH 03/18] fix: tl selector Signed-off-by: badai-nguyen --- perception/autoware_traffic_light_selector/CMakeLists.txt | 1 - .../src/traffic_light_selector_node.cpp | 2 -- 2 files changed, 3 deletions(-) diff --git a/perception/autoware_traffic_light_selector/CMakeLists.txt b/perception/autoware_traffic_light_selector/CMakeLists.txt index b5860c1376d71..06b959f9caa27 100644 --- a/perception/autoware_traffic_light_selector/CMakeLists.txt +++ b/perception/autoware_traffic_light_selector/CMakeLists.txt @@ -25,5 +25,4 @@ endif() ament_auto_package( INSTALL_TO_SHARE launch - config ) diff --git a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp index a4e7b15ae2ef9..513ae7e46ed0a 100644 --- a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp +++ b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp @@ -14,8 +14,6 @@ #include "traffic_light_selector_node.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" - #include "sensor_msgs/msg/region_of_interest.hpp" #include From a64a7cc196a4c839260c179839ab88822c4a8fd7 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Mon, 9 Dec 2024 21:23:16 +0900 Subject: [PATCH 04/18] fix: launch file Signed-off-by: badai-nguyen --- .../launch/traffic_light_selector.launch.xml | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/perception/autoware_traffic_light_selector/launch/traffic_light_selector.launch.xml b/perception/autoware_traffic_light_selector/launch/traffic_light_selector.launch.xml index 4898d6529738b..3502781ef7ba9 100644 --- a/perception/autoware_traffic_light_selector/launch/traffic_light_selector.launch.xml +++ b/perception/autoware_traffic_light_selector/launch/traffic_light_selector.launch.xml @@ -3,15 +3,13 @@ - - - + + - From 95d1b2017621ea16ac97f07ffaaf3fdd24eecca0 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Fri, 13 Dec 2024 18:55:31 +0900 Subject: [PATCH 05/18] fix: update matching score Signed-off-by: badai-nguyen --- .../launch/traffic_light_selector.launch.xml | 2 + .../package.xml | 2 + .../src/traffic_light_selector_node.cpp | 267 ++++++++++++------ .../src/traffic_light_selector_node.hpp | 8 + 4 files changed, 200 insertions(+), 79 deletions(-) diff --git a/perception/autoware_traffic_light_selector/launch/traffic_light_selector.launch.xml b/perception/autoware_traffic_light_selector/launch/traffic_light_selector.launch.xml index 3502781ef7ba9..64a6dc8c0b6f8 100644 --- a/perception/autoware_traffic_light_selector/launch/traffic_light_selector.launch.xml +++ b/perception/autoware_traffic_light_selector/launch/traffic_light_selector.launch.xml @@ -3,6 +3,7 @@ + @@ -10,6 +11,7 @@ + diff --git a/perception/autoware_traffic_light_selector/package.xml b/perception/autoware_traffic_light_selector/package.xml index 83068f220fbdf..dff2f4d4a272e 100644 --- a/perception/autoware_traffic_light_selector/package.xml +++ b/perception/autoware_traffic_light_selector/package.xml @@ -14,7 +14,9 @@ autoware_test_utils autoware_universe_utils + cv_bridge geometry_msgs + libopencv-dev message_filters object_recognition_utils rclcpp diff --git a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp index 513ae7e46ed0a..a71a03bd0f34e 100644 --- a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp +++ b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp @@ -14,6 +14,9 @@ #include "traffic_light_selector_node.hpp" +#include +#include + #include "sensor_msgs/msg/region_of_interest.hpp" #include @@ -22,56 +25,124 @@ namespace { -bool isInsideRoughRoi( - const sensor_msgs::msg::RegionOfInterest & detected_roi, - const sensor_msgs::msg::RegionOfInterest & rough_roi) +// bool isInsideRoughRoi( +// const sensor_msgs::msg::RegionOfInterest & detected_roi, +// const sensor_msgs::msg::RegionOfInterest & rough_roi) +// { +// return detected_roi.x_offset >= rough_roi.x_offset && +// detected_roi.y_offset >= rough_roi.y_offset && +// detected_roi.x_offset + detected_roi.width <= rough_roi.x_offset + rough_roi.width && +// detected_roi.y_offset + detected_roi.height <= rough_roi.y_offset + rough_roi.height; +// } + +float calIou( + const sensor_msgs::msg::RegionOfInterest & bbox1, + const sensor_msgs::msg::RegionOfInterest & bbox2) { - return detected_roi.x_offset >= rough_roi.x_offset && - detected_roi.y_offset >= rough_roi.y_offset && - detected_roi.x_offset + detected_roi.width <= rough_roi.x_offset + rough_roi.width && - detected_roi.y_offset + detected_roi.height <= rough_roi.y_offset + rough_roi.height; + int x1 = std::max(bbox1.x_offset, bbox2.x_offset); + int x2 = std::min(bbox1.x_offset + bbox1.width, bbox2.x_offset + bbox2.width); + int y1 = std::max(bbox1.y_offset, bbox2.y_offset); + int y2 = std::min(bbox1.y_offset + bbox1.height, bbox2.y_offset + bbox2.height); + int area1 = std::max(x2 - x1, 0) * std::max(y2 - y1, 0); + int area2 = bbox1.width * bbox1.height + bbox2.width * bbox2.height - area1; + if (area2 == 0) { + return 0.0; + } + return static_cast(area1) / static_cast(area2); } -double getGenIoU( - const sensor_msgs::msg::RegionOfInterest & roi1, const sensor_msgs::msg::RegionOfInterest & roi2) +// double getGenIoU( +// const sensor_msgs::msg::RegionOfInterest & roi1, const sensor_msgs::msg::RegionOfInterest & +// roi2) +// { +// int rect1_x_min = static_cast(roi1.x_offset); +// int rect1_x_max = static_cast(roi1.x_offset + roi1.width); +// int rect1_y_min = static_cast(roi1.y_offset); +// int rect1_y_max = static_cast(roi1.y_offset + roi1.height); +// int rect2_x_min = static_cast(roi2.x_offset); +// int rect2_x_max = static_cast(roi2.x_offset + roi2.width); +// int rect2_y_min = static_cast(roi2.y_offset); +// int rect2_y_max = static_cast(roi2.y_offset + roi2.height); +// int rect1_area = roi1.width * roi1.height; +// int rect2_area = roi2.width * roi2.height; +// int x_min = std::max(rect1_x_min, rect2_x_min); +// int y_min = std::max(rect1_y_min, rect2_y_min); +// int x_max = std::min(rect1_x_max, rect2_x_max); +// int y_max = std::min(rect1_y_max, rect2_y_max); + +// auto w = std::max(0, x_max - x_min); +// auto h = std::max(0, y_max - y_min); +// auto intersect = w * h; + +// auto union_area = rect1_area + rect2_area - intersect; + +// double iou = static_cast(intersect) / static_cast(union_area); + +// // convex shape area + +// auto con_x_min = std::min(rect1_x_min, rect2_x_min); +// auto con_y_min = std::min(rect1_y_min, rect2_y_min); +// auto con_x_max = std::max(rect1_x_max, rect2_x_max); +// auto con_y_max = std::max(rect1_y_max, rect2_y_max); + +// auto con_area = (con_x_max - con_x_min + 1) * (con_y_max - con_y_min + 1); + +// // GIoU calc +// double giou = iou - static_cast(con_area - union_area) / static_cast(con_area); + +// return giou; +// } +// create and function of 2 binary image +int andOf2Images(cv::Mat & img1, cv::Mat & img2) { - int rect1_x_min = static_cast(roi1.x_offset); - int rect1_x_max = static_cast(roi1.x_offset + roi1.width); - int rect1_y_min = static_cast(roi1.y_offset); - int rect1_y_max = static_cast(roi1.y_offset + roi1.height); - int rect2_x_min = static_cast(roi2.x_offset); - int rect2_x_max = static_cast(roi2.x_offset + roi2.width); - int rect2_y_min = static_cast(roi2.y_offset); - int rect2_y_max = static_cast(roi2.y_offset + roi2.height); - int rect1_area = roi1.width * roi1.height; - int rect2_area = roi2.width * roi2.height; - int x_min = std::max(rect1_x_min, rect2_x_min); - int y_min = std::max(rect1_y_min, rect2_y_min); - int x_max = std::min(rect1_x_max, rect2_x_max); - int y_max = std::min(rect1_y_max, rect2_y_max); - - auto w = std::max(0, x_max - x_min); - auto h = std::max(0, y_max - y_min); - auto intersect = w * h; - - auto union_area = rect1_area + rect2_area - intersect; - - double iou = static_cast(intersect) / static_cast(union_area); - - // convex shape area - - auto con_x_min = std::min(rect1_x_min, rect2_x_min); - auto con_y_min = std::min(rect1_y_min, rect2_y_min); - auto con_x_max = std::max(rect1_x_max, rect2_x_max); - auto con_y_max = std::max(rect1_y_max, rect2_y_max); - - auto con_area = (con_x_max - con_x_min + 1) * (con_y_max - con_y_min + 1); - - // GIoU calc - double giou = iou - static_cast(con_area - union_area) / static_cast(con_area); - - return giou; + cv::Mat img_and; + cv::bitwise_and(img1, img2, img_and); + return cv::countNonZero(img_and); +} + +// create OR function of 2 binary image +int orOf2Images(cv::Mat & img1, cv::Mat & img2) +{ + cv::Mat img_or; + cv::bitwise_or(img1, img2, img_or); + return cv::countNonZero(img_or); +} + +// create the overal IOU of 2 binary image +double getIoUOf2BinaryImages(cv::Mat & img1, cv::Mat & img2) +{ + int and_area = andOf2Images(img1, img2); + int or_area = orOf2Images(img1, img2); + return static_cast(and_area) / static_cast(or_area); +} + +// create binary image from list of rois +cv::Mat createBinaryImageFromRois( + const std::vector & rois, const cv::Size & size) +{ + cv::Mat img = cv::Mat::zeros(size, CV_8UC1); + for (const auto & roi : rois) { + // check roi is inside the image + cv::Rect rect(roi.x_offset, roi.y_offset, roi.width, roi.height); + cv::rectangle(img, rect, cv::Scalar(255), cv::FILLED); + } + return img; } + +// shift and padding image by dx, dy +cv::Mat shiftAndPaddingImage(cv::Mat & img, int dx, int dy) +{ + cv::Mat img_shifted = cv::Mat::zeros(img.size(), img.type()); + auto tl_x = std::max(0, dx); + auto tl_y = std::max(0, dy); + auto br_x = std::min(img.cols, img.cols + dx); + auto br_y = std::min(img.rows, img.rows + dy); + cv::Rect img_rect(tl_x, tl_y, br_x - tl_x, br_y - tl_y); + + img(img_rect).copyTo(img_shifted(img_rect)); + return img_shifted; +} + } // namespace namespace autoware::traffic_light @@ -92,60 +163,98 @@ TrafficLightSelectorNode::TrafficLightSelectorNode(const rclcpp::NodeOptions & n using std::placeholders::_3; sync_.registerCallback(std::bind(&TrafficLightSelectorNode::objectsCallback, this, _1, _2, _3)); + camera_info_sub_ = create_subscription( + "input/camera_info", rclcpp::SensorDataQoS(), + std::bind(&TrafficLightSelectorNode::cameraInfoCallback, this, _1)); // Publisher pub_traffic_light_rois_ = create_publisher("output/traffic_light_rois", rclcpp::QoS{1}); } +void TrafficLightSelectorNode::cameraInfoCallback( + const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_msg) +{ + if (camera_info_subscribed_) { + return; + } + RCLCPP_INFO(get_logger(), "camera_info received"); + image_width_ = input_msg->width; + image_height_ = input_msg->height; + camera_info_subscribed_ = true; +} + void TrafficLightSelectorNode::objectsCallback( const DetectedObjectsWithFeature::ConstSharedPtr & detected_traffic_light_msg, const TrafficLightRoiArray::ConstSharedPtr & rough_rois_msg, const TrafficLightRoiArray::ConstSharedPtr & expected_rois_msg) { + (void)rough_rois_msg; + if (!camera_info_subscribed_) { + return; + } TrafficLightRoiArray output; - - // create map for traffic_roi and traffic_light_id - std::map rough_roi_map; - for (const auto & rough_roi : rough_rois_msg->rois) { - rough_roi_map[rough_roi.traffic_light_id] = rough_roi.roi; + output.header = detected_traffic_light_msg->header; + float max_matching_score = 0.0; + int det_roi_shift_x = 0; + int det_roi_shift_y = 0; + std::vector det_rois; + std::vector expect_rois; + for (const auto & detected_roi : detected_traffic_light_msg->feature_objects) { + det_rois.push_back(detected_roi.feature.roi); } - std::map expected_roi_map; for (const auto & expected_roi : expected_rois_msg->rois) { - expected_roi_map[expected_roi.traffic_light_id] = expected_roi.roi; + expect_rois.push_back(expected_roi.roi); + } + cv::Mat expect_roi_img = + createBinaryImageFromRois(expect_rois, cv::Size(image_height_, image_width_)); + cv::Mat det_roi_img = createBinaryImageFromRois(det_rois, cv::Size(image_height_, image_width_)); + for (const auto expect_roi : expect_rois) { + for (const auto detected_roi : det_rois) { + int dx = detected_roi.x_offset - expect_roi.x_offset; + int dy = detected_roi.y_offset - expect_roi.y_offset; + cv::Mat det_roi_shifted = shiftAndPaddingImage(det_roi_img, dx, dy); + double iou = getIoUOf2BinaryImages(expect_roi_img, det_roi_shifted); + if (iou > max_matching_score) { + max_matching_score = iou; + det_roi_shift_x = dx; + det_roi_shift_y = dy; + } + } } - // declare image to selected_roi and publish + // shift all detected rois by dx, dy + // for (auto & detected_roi : det_rois) { + // detected_roi.x_offset -= det_roi_shift_x; + // detected_roi.y_offset -= det_roi_shift_y; + // // detected_roi.x_offset = std::max(, detected_roi.x_offset); + // // detected_roi.y_offset = std::max(0, detected_roi.y_offset); + // // detected_roi.x_offset = std::min(static_cast(image_width_), detected_roi.x_offset); + // // detected_roi.y_offset = std::min(static_cast(image_height_), detected_roi.y_offset); + // } - output.header = detected_traffic_light_msg->header; - for (const auto & rough_roi : rough_rois_msg->rois) { - // find expect roi - auto expected_roi = expected_roi_map.find(rough_roi.traffic_light_id); - double max_gen_iou = -1.0; - TrafficLightRoi max_gen_iou_roi; - max_gen_iou_roi.traffic_light_id = rough_roi.traffic_light_id; - - for (const auto & detected_light : detected_traffic_light_msg->feature_objects) { - const auto detected_roi = detected_light.feature.roi; - const auto detected_class = detected_light.object.classification.front().label; - if (detected_class == 0) { - continue; - } - if (!isInsideRoughRoi(detected_roi, rough_roi.roi)) { - continue; - } - double gen_iou = getGenIoU(detected_roi, expected_roi->second); - RCLCPP_INFO(get_logger(), "gen_iou: %f", gen_iou); - if (gen_iou > max_gen_iou) { - max_gen_iou = gen_iou; - max_gen_iou_roi.roi = detected_roi; - max_gen_iou_roi.traffic_light_type = detected_class - 1; + // shift and matching traffic_light_id for max IOU > 0.0 + + for (const auto & expect_roi : expected_rois_msg->rois) { + // check max IOU after shift + double max_iou = -1.0; + sensor_msgs::msg::RegionOfInterest max_iou_roi; + for (const auto & detected_roi : detected_traffic_light_msg->feature_objects) { + sensor_msgs::msg::RegionOfInterest detected_roi_shifted = detected_roi.feature.roi; + detected_roi_shifted.x_offset -= det_roi_shift_x; + detected_roi_shifted.y_offset -= det_roi_shift_y; + double iou = calIou(detected_roi.feature.roi, expect_roi.roi); + if (iou > max_iou) { + max_iou = iou; + max_iou_roi = detected_roi.feature.roi; } } - if (max_gen_iou > -1.0) { - output.rois.push_back(max_gen_iou_roi); + if (max_iou > 0.0) { + TrafficLightRoi traffic_light_roi; + traffic_light_roi.traffic_light_id = expect_roi.traffic_light_id; + traffic_light_roi.roi = max_iou_roi; + output.rois.push_back(traffic_light_roi); } } - pub_traffic_light_rois_->publish(output); if (debug_) { } diff --git a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp index 859b58247b4b2..0c7d85786f41f 100644 --- a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp +++ b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp @@ -21,6 +21,7 @@ #include "message_filters/synchronizer.h" #include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/camera_info.hpp" #include "tier4_perception_msgs/msg/detected_object_with_feature.hpp" #include "tier4_perception_msgs/msg/detected_objects_with_feature.hpp" #include "tier4_perception_msgs/msg/traffic_light_roi.hpp" @@ -64,9 +65,16 @@ class TrafficLightSelectorNode : public rclcpp::Node const DetectedObjectsWithFeature::ConstSharedPtr & detected_rois_msg, const TrafficLightRoiArray::ConstSharedPtr & rough_rois_msg, const TrafficLightRoiArray::ConstSharedPtr & expect_rois_msg); + + void cameraInfoCallback(const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_msg); // Publisher rclcpp::Publisher::SharedPtr pub_traffic_light_rois_; + // Subscribe camera_info to get width and height of image + rclcpp::Subscription::SharedPtr camera_info_sub_; bool debug_{false}; + bool camera_info_subscribed_; + int image_width_{1280}; + int image_height_{960}; // declare publisher for debug image // rclcpp::Publisher::SharedPtr pub_debug_image_; }; From 4f65762d1d4d28d8e7cf4c8e92a14e374e5cc494 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Mon, 16 Dec 2024 11:00:08 +0900 Subject: [PATCH 06/18] fix: calc sum IOU for whole shifted image Signed-off-by: badai-nguyen --- .../src/traffic_light_selector_node.cpp | 49 +++++++++---------- .../src/traffic_light_selector_node.hpp | 5 +- 2 files changed, 27 insertions(+), 27 deletions(-) diff --git a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp index a71a03bd0f34e..e0f6f2939a411 100644 --- a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp +++ b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp @@ -128,17 +128,18 @@ cv::Mat createBinaryImageFromRois( } return img; } - // shift and padding image by dx, dy cv::Mat shiftAndPaddingImage(cv::Mat & img, int dx, int dy) { cv::Mat img_shifted = cv::Mat::zeros(img.size(), img.type()); - auto tl_x = std::max(0, dx); - auto tl_y = std::max(0, dy); - auto br_x = std::min(img.cols, img.cols + dx); - auto br_y = std::min(img.rows, img.rows + dy); + uint32_t tl_x = static_cast(std::max(0, dx)); + uint32_t tl_y = static_cast(std::max(0, dy)); + uint32_t br_x = std::min(img.cols, (static_cast(img.cols) + dx)); + uint32_t br_y = std::min(img.rows, (static_cast(img.rows) + dy)); + if (br_x <= tl_x || br_y <= tl_y) { + return img_shifted; + } cv::Rect img_rect(tl_x, tl_y, br_x - tl_x, br_y - tl_y); - img(img_rect).copyTo(img_shifted(img_rect)); return img_shifted; } @@ -192,6 +193,9 @@ void TrafficLightSelectorNode::objectsCallback( if (!camera_info_subscribed_) { return; } + + // TODO(badai-nguyen): implement this function on CUDA or refactor the code + TrafficLightRoiArray output; output.header = detected_traffic_light_msg->header; float max_matching_score = 0.0; @@ -206,12 +210,12 @@ void TrafficLightSelectorNode::objectsCallback( expect_rois.push_back(expected_roi.roi); } cv::Mat expect_roi_img = - createBinaryImageFromRois(expect_rois, cv::Size(image_height_, image_width_)); - cv::Mat det_roi_img = createBinaryImageFromRois(det_rois, cv::Size(image_height_, image_width_)); + createBinaryImageFromRois(expect_rois, cv::Size(image_width_, image_height_)); + cv::Mat det_roi_img = createBinaryImageFromRois(det_rois, cv::Size(image_width_, image_height_)); for (const auto expect_roi : expect_rois) { for (const auto detected_roi : det_rois) { - int dx = detected_roi.x_offset - expect_roi.x_offset; - int dy = detected_roi.y_offset - expect_roi.y_offset; + int dx = static_cast(detected_roi.x_offset) - static_cast(expect_roi.x_offset); + int dy = static_cast(detected_roi.y_offset) - static_cast(expect_roi.y_offset); cv::Mat det_roi_shifted = shiftAndPaddingImage(det_roi_img, dx, dy); double iou = getIoUOf2BinaryImages(expect_roi_img, det_roi_shifted); if (iou > max_matching_score) { @@ -222,27 +226,22 @@ void TrafficLightSelectorNode::objectsCallback( } } - // shift all detected rois by dx, dy - // for (auto & detected_roi : det_rois) { - // detected_roi.x_offset -= det_roi_shift_x; - // detected_roi.y_offset -= det_roi_shift_y; - // // detected_roi.x_offset = std::max(, detected_roi.x_offset); - // // detected_roi.y_offset = std::max(0, detected_roi.y_offset); - // // detected_roi.x_offset = std::min(static_cast(image_width_), detected_roi.x_offset); - // // detected_roi.y_offset = std::min(static_cast(image_height_), detected_roi.y_offset); - // } - - // shift and matching traffic_light_id for max IOU > 0.0 - for (const auto & expect_roi : expected_rois_msg->rois) { // check max IOU after shift double max_iou = -1.0; sensor_msgs::msg::RegionOfInterest max_iou_roi; for (const auto & detected_roi : detected_traffic_light_msg->feature_objects) { + // shift detected roi by det_roi_shift_x, det_roi_shift_y and calculate IOU sensor_msgs::msg::RegionOfInterest detected_roi_shifted = detected_roi.feature.roi; - detected_roi_shifted.x_offset -= det_roi_shift_x; - detected_roi_shifted.y_offset -= det_roi_shift_y; - double iou = calIou(detected_roi.feature.roi, expect_roi.roi); + // fit top lef corner of detected roi to inside of image + detected_roi_shifted.x_offset = std::clamp( + static_cast(detected_roi.feature.roi.x_offset) - det_roi_shift_x, 0, + static_cast(image_width_ - detected_roi.feature.roi.width)); + detected_roi_shifted.y_offset = std::clamp( + static_cast(detected_roi.feature.roi.y_offset) - det_roi_shift_y, 0, + static_cast(image_height_ - detected_roi.feature.roi.height)); + + double iou = calIou(expect_roi.roi, detected_roi_shifted); if (iou > max_iou) { max_iou = iou; max_iou_roi = detected_roi.feature.roi; diff --git a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp index 0c7d85786f41f..19d29686044a0 100644 --- a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp +++ b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp @@ -30,6 +30,7 @@ #include #include +#include #include #include #include @@ -73,8 +74,8 @@ class TrafficLightSelectorNode : public rclcpp::Node rclcpp::Subscription::SharedPtr camera_info_sub_; bool debug_{false}; bool camera_info_subscribed_; - int image_width_{1280}; - int image_height_{960}; + uint32_t image_width_{1280}; + uint32_t image_height_{960}; // declare publisher for debug image // rclcpp::Publisher::SharedPtr pub_debug_image_; }; From 06f20150ae3b19953686f83b5527e539766a055c Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Mon, 16 Dec 2024 11:52:05 +0900 Subject: [PATCH 07/18] fix: check inside rough roi Signed-off-by: badai-nguyen --- .../src/traffic_light_selector_node.cpp | 38 ++++++++++++------- 1 file changed, 25 insertions(+), 13 deletions(-) diff --git a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp index e0f6f2939a411..217588a508351 100644 --- a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp +++ b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp @@ -19,21 +19,22 @@ #include "sensor_msgs/msg/region_of_interest.hpp" +#include #include #include #include namespace { -// bool isInsideRoughRoi( -// const sensor_msgs::msg::RegionOfInterest & detected_roi, -// const sensor_msgs::msg::RegionOfInterest & rough_roi) -// { -// return detected_roi.x_offset >= rough_roi.x_offset && -// detected_roi.y_offset >= rough_roi.y_offset && -// detected_roi.x_offset + detected_roi.width <= rough_roi.x_offset + rough_roi.width && -// detected_roi.y_offset + detected_roi.height <= rough_roi.y_offset + rough_roi.height; -// } +bool isInsideRoughRoi( + const sensor_msgs::msg::RegionOfInterest & detected_roi, + const sensor_msgs::msg::RegionOfInterest & rough_roi) +{ + return detected_roi.x_offset >= rough_roi.x_offset && + detected_roi.y_offset >= rough_roi.y_offset && + detected_roi.x_offset + detected_roi.width <= rough_roi.x_offset + rough_roi.width && + detected_roi.y_offset + detected_roi.height <= rough_roi.y_offset + rough_roi.height; +} float calIou( const sensor_msgs::msg::RegionOfInterest & bbox1, @@ -189,11 +190,13 @@ void TrafficLightSelectorNode::objectsCallback( const TrafficLightRoiArray::ConstSharedPtr & rough_rois_msg, const TrafficLightRoiArray::ConstSharedPtr & expected_rois_msg) { - (void)rough_rois_msg; if (!camera_info_subscribed_) { return; } - + std::map rough_rois_map; + for (const auto & roi : rough_rois_msg->rois) { + rough_rois_map[roi.traffic_light_id] = roi.roi; + } // TODO(badai-nguyen): implement this function on CUDA or refactor the code TrafficLightRoiArray output; @@ -212,8 +215,17 @@ void TrafficLightSelectorNode::objectsCallback( cv::Mat expect_roi_img = createBinaryImageFromRois(expect_rois, cv::Size(image_width_, image_height_)); cv::Mat det_roi_img = createBinaryImageFromRois(det_rois, cv::Size(image_width_, image_height_)); - for (const auto expect_roi : expect_rois) { - for (const auto detected_roi : det_rois) { + // for (const auto expect_roi : expect_rois) { + for (const auto & expect_traffic_roi : expected_rois_msg->rois) { + const auto & expect_roi = expect_traffic_roi.roi; + auto traffic_light_id = expect_traffic_roi.traffic_light_id; + const auto & rough_roi = rough_rois_map[traffic_light_id]; + + for (const auto & detected_roi : det_rois) { + // check if the detected roi is inside the rough roi + if (!isInsideRoughRoi(detected_roi, rough_roi)) { + continue; + } int dx = static_cast(detected_roi.x_offset) - static_cast(expect_roi.x_offset); int dy = static_cast(detected_roi.y_offset) - static_cast(expect_roi.y_offset); cv::Mat det_roi_shifted = shiftAndPaddingImage(det_roi_img, dx, dy); From 357967ff06509ab376ae20494711a0a35cd5eb95 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Mon, 16 Dec 2024 11:57:14 +0900 Subject: [PATCH 08/18] fix: check inside function Signed-off-by: badai-nguyen --- .../src/traffic_light_selector_node.cpp | 21 +++++++++++++++---- 1 file changed, 17 insertions(+), 4 deletions(-) diff --git a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp index 217588a508351..7bc158bf1c43f 100644 --- a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp +++ b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp @@ -30,10 +30,23 @@ bool isInsideRoughRoi( const sensor_msgs::msg::RegionOfInterest & detected_roi, const sensor_msgs::msg::RegionOfInterest & rough_roi) { - return detected_roi.x_offset >= rough_roi.x_offset && - detected_roi.y_offset >= rough_roi.y_offset && - detected_roi.x_offset + detected_roi.width <= rough_roi.x_offset + rough_roi.width && - detected_roi.y_offset + detected_roi.height <= rough_roi.y_offset + rough_roi.height; + // check if tl or br of detected roi is inside the rough roi + auto tl_x = detected_roi.x_offset; + auto tl_y = detected_roi.y_offset; + auto br_x = detected_roi.x_offset + detected_roi.width; + auto br_y = detected_roi.y_offset + detected_roi.height; + bool is_tl_inside = rough_roi.x_offset <= tl_x && tl_x <= rough_roi.x_offset + rough_roi.width && + rough_roi.y_offset <= tl_y && tl_y <= rough_roi.y_offset + rough_roi.height; + if (is_tl_inside) { + return true; + } + + bool is_br_inside = rough_roi.x_offset <= br_x && br_x <= rough_roi.x_offset + rough_roi.width && + rough_roi.y_offset <= br_y && br_y <= rough_roi.y_offset + rough_roi.height; + if (is_br_inside) { + return true; + } + return false; } float calIou( From abad27fbdfc5cb96cbee742e374e78852b188030 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Mon, 16 Dec 2024 12:15:06 +0900 Subject: [PATCH 09/18] feat: add max_iou_threshold Signed-off-by: badai-nguyen --- .../src/traffic_light_selector_node.cpp | 4 +++- .../src/traffic_light_selector_node.hpp | 1 + 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp index 7bc158bf1c43f..ea0551037943f 100644 --- a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp +++ b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp @@ -172,6 +172,8 @@ TrafficLightSelectorNode::TrafficLightSelectorNode(const rclcpp::NodeOptions & n expected_rois_sub_(this, "input/expect_rois", rclcpp::QoS{1}.get_rmw_qos_profile()), sync_(SyncPolicy(10), detected_rois_sub_, rough_rois_sub_, expected_rois_sub_) { + max_iou_threshold_ = declare_parameter("max_iou_threshold", 0.0); + RCLCPP_INFO(get_logger(), "max_iou_threshold: %f", max_iou_threshold_); debug_ = declare_parameter("debug"); using std::placeholders::_1; using std::placeholders::_2; @@ -272,7 +274,7 @@ void TrafficLightSelectorNode::objectsCallback( max_iou_roi = detected_roi.feature.roi; } } - if (max_iou > 0.0) { + if (max_iou > max_iou_threshold_) { TrafficLightRoi traffic_light_roi; traffic_light_roi.traffic_light_id = expect_roi.traffic_light_id; traffic_light_roi.roi = max_iou_roi; diff --git a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp index 19d29686044a0..3da72e7b81f5d 100644 --- a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp +++ b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp @@ -76,6 +76,7 @@ class TrafficLightSelectorNode : public rclcpp::Node bool camera_info_subscribed_; uint32_t image_width_{1280}; uint32_t image_height_{960}; + double max_iou_threshold_{0.0}; // declare publisher for debug image // rclcpp::Publisher::SharedPtr pub_debug_image_; }; From 54f85c3ada7ae08f94fa7e10084bec6fe4e24737 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Mon, 23 Dec 2024 14:50:25 +0900 Subject: [PATCH 10/18] chore: pre-commit Signed-off-by: badai-nguyen --- .../package.xml | 1 - .../src/traffic_light_selector_node.cpp | 45 +------------------ .../src/traffic_light_selector_node.hpp | 3 -- 3 files changed, 1 insertion(+), 48 deletions(-) diff --git a/perception/autoware_traffic_light_selector/package.xml b/perception/autoware_traffic_light_selector/package.xml index dff2f4d4a272e..90be360e592b9 100644 --- a/perception/autoware_traffic_light_selector/package.xml +++ b/perception/autoware_traffic_light_selector/package.xml @@ -18,7 +18,6 @@ geometry_msgs libopencv-dev message_filters - object_recognition_utils rclcpp rclcpp_components sensor_msgs diff --git a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp index ea0551037943f..b1f36053afb43 100644 --- a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp +++ b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp @@ -19,6 +19,7 @@ #include "sensor_msgs/msg/region_of_interest.hpp" +#include #include #include #include @@ -65,47 +66,6 @@ float calIou( return static_cast(area1) / static_cast(area2); } -// double getGenIoU( -// const sensor_msgs::msg::RegionOfInterest & roi1, const sensor_msgs::msg::RegionOfInterest & -// roi2) -// { -// int rect1_x_min = static_cast(roi1.x_offset); -// int rect1_x_max = static_cast(roi1.x_offset + roi1.width); -// int rect1_y_min = static_cast(roi1.y_offset); -// int rect1_y_max = static_cast(roi1.y_offset + roi1.height); -// int rect2_x_min = static_cast(roi2.x_offset); -// int rect2_x_max = static_cast(roi2.x_offset + roi2.width); -// int rect2_y_min = static_cast(roi2.y_offset); -// int rect2_y_max = static_cast(roi2.y_offset + roi2.height); -// int rect1_area = roi1.width * roi1.height; -// int rect2_area = roi2.width * roi2.height; -// int x_min = std::max(rect1_x_min, rect2_x_min); -// int y_min = std::max(rect1_y_min, rect2_y_min); -// int x_max = std::min(rect1_x_max, rect2_x_max); -// int y_max = std::min(rect1_y_max, rect2_y_max); - -// auto w = std::max(0, x_max - x_min); -// auto h = std::max(0, y_max - y_min); -// auto intersect = w * h; - -// auto union_area = rect1_area + rect2_area - intersect; - -// double iou = static_cast(intersect) / static_cast(union_area); - -// // convex shape area - -// auto con_x_min = std::min(rect1_x_min, rect2_x_min); -// auto con_y_min = std::min(rect1_y_min, rect2_y_min); -// auto con_x_max = std::max(rect1_x_max, rect2_x_max); -// auto con_y_max = std::max(rect1_y_max, rect2_y_max); - -// auto con_area = (con_x_max - con_x_min + 1) * (con_y_max - con_y_min + 1); - -// // GIoU calc -// double giou = iou - static_cast(con_area - union_area) / static_cast(con_area); - -// return giou; -// } // create and function of 2 binary image int andOf2Images(cv::Mat & img1, cv::Mat & img2) { @@ -174,7 +134,6 @@ TrafficLightSelectorNode::TrafficLightSelectorNode(const rclcpp::NodeOptions & n { max_iou_threshold_ = declare_parameter("max_iou_threshold", 0.0); RCLCPP_INFO(get_logger(), "max_iou_threshold: %f", max_iou_threshold_); - debug_ = declare_parameter("debug"); using std::placeholders::_1; using std::placeholders::_2; using std::placeholders::_3; @@ -282,8 +241,6 @@ void TrafficLightSelectorNode::objectsCallback( } } pub_traffic_light_rois_->publish(output); - if (debug_) { - } return; } } // namespace autoware::traffic_light diff --git a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp index 3da72e7b81f5d..4a4b43e21360f 100644 --- a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp +++ b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp @@ -72,13 +72,10 @@ class TrafficLightSelectorNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_traffic_light_rois_; // Subscribe camera_info to get width and height of image rclcpp::Subscription::SharedPtr camera_info_sub_; - bool debug_{false}; bool camera_info_subscribed_; uint32_t image_width_{1280}; uint32_t image_height_{960}; double max_iou_threshold_{0.0}; - // declare publisher for debug image - // rclcpp::Publisher::SharedPtr pub_debug_image_; }; } // namespace autoware::traffic_light From 92a5c688f7069360e5b2a652b883aa21bbdf8c21 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Mon, 23 Dec 2024 15:23:23 +0900 Subject: [PATCH 11/18] docs: add readme Signed-off-by: badai-nguyen --- .../autoware_traffic_light_selector/README.md | 25 +++++++++++++++++++ 1 file changed, 25 insertions(+) create mode 100644 perception/autoware_traffic_light_selector/README.md diff --git a/perception/autoware_traffic_light_selector/README.md b/perception/autoware_traffic_light_selector/README.md new file mode 100644 index 0000000000000..3a17b0d96e72a --- /dev/null +++ b/perception/autoware_traffic_light_selector/README.md @@ -0,0 +1,25 @@ +# The `autoware_traffic_light_selector` Package + +## Overview + +`autoware_traffic_light_selector` selects the interest traffic light from the list of detected traffic lights by deep learning neural network (DNN) based on the expect ROIs and rough ROIs information and then assign traffic_light_id for them. + + +## Input topics + +| Name | Type | Description | +| -------------------- | ------------------------------------- | ----------------------- | +| `input/detected_rois` | tier4_perception_msgs::msg::DetectedObjectsWithFeature | detected traffic light by DNN | +| `input/rough_rois` | tier4_perception_msgs::msg::TrafficLightRoiArray | location of traffic lights in image corresponding to the camera info | +| `input/expect_rois` | tier4_perception_msgs::msg::TrafficLightRoiArray | location of traffic lights in image without any offset | + +## Output topics + +| Name | Type | Description | +| ---------------- | ------------------------------------------- | -------------------------------------------------------------------- | +| `output/traffic_light_rois` | tier4_perception_msgs::TrafficLightRoiArray | detected traffic light of interest | + + +## Node parameters + +N/A \ No newline at end of file From 4f2a449ae014cd5e2e2f18b5b2b1e30fccd16045 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Mon, 23 Dec 2024 15:23:44 +0900 Subject: [PATCH 12/18] refactor: launch file Signed-off-by: badai-nguyen --- .../launch/traffic_light_selector.launch.xml | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/perception/autoware_traffic_light_selector/launch/traffic_light_selector.launch.xml b/perception/autoware_traffic_light_selector/launch/traffic_light_selector.launch.xml index 64a6dc8c0b6f8..6b96dd1f39f13 100644 --- a/perception/autoware_traffic_light_selector/launch/traffic_light_selector.launch.xml +++ b/perception/autoware_traffic_light_selector/launch/traffic_light_selector.launch.xml @@ -1,17 +1,13 @@ - - - - - + From 2c1214fb4702b770adf98312bf8648b96a40993b Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Mon, 6 Jan 2025 17:14:17 +0900 Subject: [PATCH 13/18] docs: pre-commit Signed-off-by: badai-nguyen --- .../autoware_traffic_light_selector/README.md | 20 +++++++++---------- 1 file changed, 9 insertions(+), 11 deletions(-) diff --git a/perception/autoware_traffic_light_selector/README.md b/perception/autoware_traffic_light_selector/README.md index 3a17b0d96e72a..35a26f63db0a0 100644 --- a/perception/autoware_traffic_light_selector/README.md +++ b/perception/autoware_traffic_light_selector/README.md @@ -4,22 +4,20 @@ `autoware_traffic_light_selector` selects the interest traffic light from the list of detected traffic lights by deep learning neural network (DNN) based on the expect ROIs and rough ROIs information and then assign traffic_light_id for them. - ## Input topics -| Name | Type | Description | -| -------------------- | ------------------------------------- | ----------------------- | -| `input/detected_rois` | tier4_perception_msgs::msg::DetectedObjectsWithFeature | detected traffic light by DNN | -| `input/rough_rois` | tier4_perception_msgs::msg::TrafficLightRoiArray | location of traffic lights in image corresponding to the camera info | -| `input/expect_rois` | tier4_perception_msgs::msg::TrafficLightRoiArray | location of traffic lights in image without any offset | +| Name | Type | Description | +| --------------------- | ------------------------------------------------------ | -------------------------------------------------------------------- | +| `input/detected_rois` | tier4_perception_msgs::msg::DetectedObjectsWithFeature | detected traffic light by DNN | +| `input/rough_rois` | tier4_perception_msgs::msg::TrafficLightRoiArray | location of traffic lights in image corresponding to the camera info | +| `input/expect_rois` | tier4_perception_msgs::msg::TrafficLightRoiArray | location of traffic lights in image without any offset | ## Output topics -| Name | Type | Description | -| ---------------- | ------------------------------------------- | -------------------------------------------------------------------- | -| `output/traffic_light_rois` | tier4_perception_msgs::TrafficLightRoiArray | detected traffic light of interest | - +| Name | Type | Description | +| --------------------------- | ------------------------------------------- | ---------------------------------- | +| `output/traffic_light_rois` | tier4_perception_msgs::TrafficLightRoiArray | detected traffic light of interest | ## Node parameters -N/A \ No newline at end of file +N/A From ff1451f67a1edcd700679a90222ad17c0ce3d9d0 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Mon, 6 Jan 2025 17:16:25 +0900 Subject: [PATCH 14/18] docs Signed-off-by: badai-nguyen --- .../src/traffic_light_selector_node.cpp | 2 +- .../src/traffic_light_selector_node.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp index b1f36053afb43..3cedf28a62d5e 100644 --- a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp +++ b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 TIER IV, Inc. +// Copyright 2025 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp index 4a4b43e21360f..969c84158f763 100644 --- a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp +++ b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 TIER IV, Inc. +// Copyright 2025 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From 3242a34852e644cf13f7c8f8ac9bada2f270b01f Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Mon, 6 Jan 2025 17:25:22 +0900 Subject: [PATCH 15/18] chore: typo Signed-off-by: badai-nguyen --- .../src/traffic_light_selector_node.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp index 3cedf28a62d5e..cffc48245ef7b 100644 --- a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp +++ b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp @@ -50,7 +50,7 @@ bool isInsideRoughRoi( return false; } -float calIou( +float calIOU( const sensor_msgs::msg::RegionOfInterest & bbox1, const sensor_msgs::msg::RegionOfInterest & bbox2) { @@ -82,7 +82,7 @@ int orOf2Images(cv::Mat & img1, cv::Mat & img2) return cv::countNonZero(img_or); } -// create the overal IOU of 2 binary image +// create the overall IOU of 2 binary image double getIoUOf2BinaryImages(cv::Mat & img1, cv::Mat & img2) { int and_area = andOf2Images(img1, img2); @@ -227,7 +227,7 @@ void TrafficLightSelectorNode::objectsCallback( static_cast(detected_roi.feature.roi.y_offset) - det_roi_shift_y, 0, static_cast(image_height_ - detected_roi.feature.roi.height)); - double iou = calIou(expect_roi.roi, detected_roi_shifted); + double iou = calIOU(expect_roi.roi, detected_roi_shifted); if (iou > max_iou) { max_iou = iou; max_iou_roi = detected_roi.feature.roi; From 0462bc2d4a681ea0755941ed79b5b8de1fe810bb Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Tue, 7 Jan 2025 00:39:59 +0900 Subject: [PATCH 16/18] refactor Signed-off-by: badai-nguyen --- .../CMakeLists.txt | 1 + .../launch/traffic_light_selector.launch.xml | 5 +- .../src/traffic_light_selector_node.cpp | 116 ++------------- .../src/traffic_light_selector_node.hpp | 28 ++-- .../src/utils.cpp | 132 ++++++++++++++++++ .../src/utils.hpp | 73 ++++++++++ 6 files changed, 230 insertions(+), 125 deletions(-) create mode 100644 perception/autoware_traffic_light_selector/src/utils.cpp create mode 100644 perception/autoware_traffic_light_selector/src/utils.hpp diff --git a/perception/autoware_traffic_light_selector/CMakeLists.txt b/perception/autoware_traffic_light_selector/CMakeLists.txt index 06b959f9caa27..34f4bce5709bc 100644 --- a/perception/autoware_traffic_light_selector/CMakeLists.txt +++ b/perception/autoware_traffic_light_selector/CMakeLists.txt @@ -8,6 +8,7 @@ autoware_package() # Targets ament_auto_add_library(${PROJECT_NAME} SHARED src/traffic_light_selector_node.cpp + src/utils.cpp ) rclcpp_components_register_node(${PROJECT_NAME} diff --git a/perception/autoware_traffic_light_selector/launch/traffic_light_selector.launch.xml b/perception/autoware_traffic_light_selector/launch/traffic_light_selector.launch.xml index 6b96dd1f39f13..022891de3d457 100644 --- a/perception/autoware_traffic_light_selector/launch/traffic_light_selector.launch.xml +++ b/perception/autoware_traffic_light_selector/launch/traffic_light_selector.launch.xml @@ -1,13 +1,14 @@ - + - + + diff --git a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp index cffc48245ef7b..3c76f17bca3b4 100644 --- a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp +++ b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp @@ -17,109 +17,11 @@ #include #include -#include "sensor_msgs/msg/region_of_interest.hpp" +#include -#include #include -#include -#include #include -namespace -{ -bool isInsideRoughRoi( - const sensor_msgs::msg::RegionOfInterest & detected_roi, - const sensor_msgs::msg::RegionOfInterest & rough_roi) -{ - // check if tl or br of detected roi is inside the rough roi - auto tl_x = detected_roi.x_offset; - auto tl_y = detected_roi.y_offset; - auto br_x = detected_roi.x_offset + detected_roi.width; - auto br_y = detected_roi.y_offset + detected_roi.height; - bool is_tl_inside = rough_roi.x_offset <= tl_x && tl_x <= rough_roi.x_offset + rough_roi.width && - rough_roi.y_offset <= tl_y && tl_y <= rough_roi.y_offset + rough_roi.height; - if (is_tl_inside) { - return true; - } - - bool is_br_inside = rough_roi.x_offset <= br_x && br_x <= rough_roi.x_offset + rough_roi.width && - rough_roi.y_offset <= br_y && br_y <= rough_roi.y_offset + rough_roi.height; - if (is_br_inside) { - return true; - } - return false; -} - -float calIOU( - const sensor_msgs::msg::RegionOfInterest & bbox1, - const sensor_msgs::msg::RegionOfInterest & bbox2) -{ - int x1 = std::max(bbox1.x_offset, bbox2.x_offset); - int x2 = std::min(bbox1.x_offset + bbox1.width, bbox2.x_offset + bbox2.width); - int y1 = std::max(bbox1.y_offset, bbox2.y_offset); - int y2 = std::min(bbox1.y_offset + bbox1.height, bbox2.y_offset + bbox2.height); - int area1 = std::max(x2 - x1, 0) * std::max(y2 - y1, 0); - int area2 = bbox1.width * bbox1.height + bbox2.width * bbox2.height - area1; - if (area2 == 0) { - return 0.0; - } - return static_cast(area1) / static_cast(area2); -} - -// create and function of 2 binary image -int andOf2Images(cv::Mat & img1, cv::Mat & img2) -{ - cv::Mat img_and; - cv::bitwise_and(img1, img2, img_and); - return cv::countNonZero(img_and); -} - -// create OR function of 2 binary image -int orOf2Images(cv::Mat & img1, cv::Mat & img2) -{ - cv::Mat img_or; - cv::bitwise_or(img1, img2, img_or); - return cv::countNonZero(img_or); -} - -// create the overall IOU of 2 binary image -double getIoUOf2BinaryImages(cv::Mat & img1, cv::Mat & img2) -{ - int and_area = andOf2Images(img1, img2); - int or_area = orOf2Images(img1, img2); - return static_cast(and_area) / static_cast(or_area); -} - -// create binary image from list of rois -cv::Mat createBinaryImageFromRois( - const std::vector & rois, const cv::Size & size) -{ - cv::Mat img = cv::Mat::zeros(size, CV_8UC1); - for (const auto & roi : rois) { - // check roi is inside the image - cv::Rect rect(roi.x_offset, roi.y_offset, roi.width, roi.height); - cv::rectangle(img, rect, cv::Scalar(255), cv::FILLED); - } - return img; -} -// shift and padding image by dx, dy -cv::Mat shiftAndPaddingImage(cv::Mat & img, int dx, int dy) -{ - cv::Mat img_shifted = cv::Mat::zeros(img.size(), img.type()); - uint32_t tl_x = static_cast(std::max(0, dx)); - uint32_t tl_y = static_cast(std::max(0, dy)); - uint32_t br_x = std::min(img.cols, (static_cast(img.cols) + dx)); - uint32_t br_y = std::min(img.rows, (static_cast(img.rows) + dy)); - if (br_x <= tl_x || br_y <= tl_y) { - return img_shifted; - } - cv::Rect img_rect(tl_x, tl_y, br_x - tl_x, br_y - tl_y); - img(img_rect).copyTo(img_shifted(img_rect)); - return img_shifted; -} - -} // namespace - namespace autoware::traffic_light { @@ -132,8 +34,7 @@ TrafficLightSelectorNode::TrafficLightSelectorNode(const rclcpp::NodeOptions & n expected_rois_sub_(this, "input/expect_rois", rclcpp::QoS{1}.get_rmw_qos_profile()), sync_(SyncPolicy(10), detected_rois_sub_, rough_rois_sub_, expected_rois_sub_) { - max_iou_threshold_ = declare_parameter("max_iou_threshold", 0.0); - RCLCPP_INFO(get_logger(), "max_iou_threshold: %f", max_iou_threshold_); + max_iou_threshold_ = declare_parameter("max_iou_threshold"); using std::placeholders::_1; using std::placeholders::_2; using std::placeholders::_3; @@ -187,8 +88,9 @@ void TrafficLightSelectorNode::objectsCallback( expect_rois.push_back(expected_roi.roi); } cv::Mat expect_roi_img = - createBinaryImageFromRois(expect_rois, cv::Size(image_width_, image_height_)); - cv::Mat det_roi_img = createBinaryImageFromRois(det_rois, cv::Size(image_width_, image_height_)); + utils::createBinaryImageFromRois(expect_rois, cv::Size(image_width_, image_height_)); + cv::Mat det_roi_img = + utils::createBinaryImageFromRois(det_rois, cv::Size(image_width_, image_height_)); // for (const auto expect_roi : expect_rois) { for (const auto & expect_traffic_roi : expected_rois_msg->rois) { const auto & expect_roi = expect_traffic_roi.roi; @@ -197,13 +99,13 @@ void TrafficLightSelectorNode::objectsCallback( for (const auto & detected_roi : det_rois) { // check if the detected roi is inside the rough roi - if (!isInsideRoughRoi(detected_roi, rough_roi)) { + if (!utils::isInsideRoughRoi(detected_roi, rough_roi)) { continue; } int dx = static_cast(detected_roi.x_offset) - static_cast(expect_roi.x_offset); int dy = static_cast(detected_roi.y_offset) - static_cast(expect_roi.y_offset); - cv::Mat det_roi_shifted = shiftAndPaddingImage(det_roi_img, dx, dy); - double iou = getIoUOf2BinaryImages(expect_roi_img, det_roi_shifted); + cv::Mat det_roi_shifted = utils::shiftAndPaddingImage(det_roi_img, dx, dy); + double iou = utils::getIoUOf2BinaryImages(expect_roi_img, det_roi_shifted); if (iou > max_matching_score) { max_matching_score = iou; det_roi_shift_x = dx; @@ -227,7 +129,7 @@ void TrafficLightSelectorNode::objectsCallback( static_cast(detected_roi.feature.roi.y_offset) - det_roi_shift_y, 0, static_cast(image_height_ - detected_roi.feature.roi.height)); - double iou = calIOU(expect_roi.roi, detected_roi_shifted); + double iou = utils::calIOU(expect_roi.roi, detected_roi_shifted); if (iou > max_iou) { max_iou = iou; max_iou_roi = detected_roi.feature.roi; diff --git a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp index 969c84158f763..4aa645c5f2d9a 100644 --- a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp +++ b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp @@ -15,27 +15,23 @@ #ifndef TRAFFIC_LIGHT_SELECTOR_NODE_HPP_ #define TRAFFIC_LIGHT_SELECTOR_NODE_HPP_ -#include "autoware/universe_utils/ros/transform_listener.hpp" -#include "message_filters/subscriber.h" -#include "message_filters/sync_policies/approximate_time.h" -#include "message_filters/synchronizer.h" -#include "rclcpp/rclcpp.hpp" +#include "utils.hpp" -#include "sensor_msgs/msg/camera_info.hpp" -#include "tier4_perception_msgs/msg/detected_object_with_feature.hpp" -#include "tier4_perception_msgs/msg/detected_objects_with_feature.hpp" -#include "tier4_perception_msgs/msg/traffic_light_roi.hpp" -#include "tier4_perception_msgs/msg/traffic_light_roi_array.hpp" +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include #include #include -#include -#include -#include -#include -#include - namespace autoware::traffic_light { using tier4_perception_msgs::msg::DetectedObjectsWithFeature; diff --git a/perception/autoware_traffic_light_selector/src/utils.cpp b/perception/autoware_traffic_light_selector/src/utils.cpp new file mode 100644 index 0000000000000..1d3e02e15f4aa --- /dev/null +++ b/perception/autoware_traffic_light_selector/src/utils.cpp @@ -0,0 +1,132 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "utils.hpp" + +#include +#include +namespace autoware::traffic_light +{ +namespace utils +{ +bool isInsideRoughRoi( + const sensor_msgs::msg::RegionOfInterest & detected_roi, + const sensor_msgs::msg::RegionOfInterest & rough_roi) +{ + // check if tl or br of detected roi is inside the rough roi + auto tl_x = detected_roi.x_offset; + auto tl_y = detected_roi.y_offset; + auto br_x = detected_roi.x_offset + detected_roi.width; + auto br_y = detected_roi.y_offset + detected_roi.height; + bool is_tl_inside = rough_roi.x_offset <= tl_x && tl_x <= rough_roi.x_offset + rough_roi.width && + rough_roi.y_offset <= tl_y && tl_y <= rough_roi.y_offset + rough_roi.height; + if (is_tl_inside) { + return true; + } + + bool is_br_inside = rough_roi.x_offset <= br_x && br_x <= rough_roi.x_offset + rough_roi.width && + rough_roi.y_offset <= br_y && br_y <= rough_roi.y_offset + rough_roi.height; + if (is_br_inside) { + return true; + } + return false; +} + +float calIOU( + const sensor_msgs::msg::RegionOfInterest & bbox1, + const sensor_msgs::msg::RegionOfInterest & bbox2) +{ + int x1 = std::max(bbox1.x_offset, bbox2.x_offset); + int x2 = std::min(bbox1.x_offset + bbox1.width, bbox2.x_offset + bbox2.width); + int y1 = std::max(bbox1.y_offset, bbox2.y_offset); + int y2 = std::min(bbox1.y_offset + bbox1.height, bbox2.y_offset + bbox2.height); + int area1 = std::max(x2 - x1, 0) * std::max(y2 - y1, 0); + int area2 = bbox1.width * bbox1.height + bbox2.width * bbox2.height - area1; + if (area2 == 0) { + return 0.0; + } + return static_cast(area1) / static_cast(area2); +} + +// create binary image from list of rois +cv::Mat createBinaryImageFromRois( + const std::vector & rois, const cv::Size & size) +{ + cv::Mat img = cv::Mat::zeros(size, CV_8UC1); + for (const auto & roi : rois) { + // check roi is inside the image + cv::Rect rect(roi.x_offset, roi.y_offset, roi.width, roi.height); + cv::rectangle(img, rect, cv::Scalar(255), cv::FILLED); + } + return img; +} +// shift and padding image by dx, dy +cv::Mat shiftAndPaddingImage(cv::Mat & img, int dx, int dy) +{ + cv::Mat img_shifted = cv::Mat::zeros(img.size(), img.type()); + uint32_t tl_x = static_cast(std::max(0, dx)); + uint32_t tl_y = static_cast(std::max(0, dy)); + uint32_t br_x = std::min(img.cols, (static_cast(img.cols) + dx)); + uint32_t br_y = std::min(img.rows, (static_cast(img.rows) + dy)); + if (br_x <= tl_x || br_y <= tl_y) { + return img_shifted; + } + cv::Rect img_rect(tl_x, tl_y, br_x - tl_x, br_y - tl_y); + img(img_rect).copyTo(img_shifted(img_rect)); + return img_shifted; +} + +double getGenIoU( + const sensor_msgs::msg::RegionOfInterest & roi1, const sensor_msgs::msg::RegionOfInterest & roi2) +{ + int rect1_x_min = static_cast(roi1.x_offset); + int rect1_x_max = static_cast(roi1.x_offset + roi1.width); + int rect1_y_min = static_cast(roi1.y_offset); + int rect1_y_max = static_cast(roi1.y_offset + roi1.height); + int rect2_x_min = static_cast(roi2.x_offset); + int rect2_x_max = static_cast(roi2.x_offset + roi2.width); + int rect2_y_min = static_cast(roi2.y_offset); + int rect2_y_max = static_cast(roi2.y_offset + roi2.height); + int rect1_area = roi1.width * roi1.height; + int rect2_area = roi2.width * roi2.height; + int x_min = std::max(rect1_x_min, rect2_x_min); + int y_min = std::max(rect1_y_min, rect2_y_min); + int x_max = std::min(rect1_x_max, rect2_x_max); + int y_max = std::min(rect1_y_max, rect2_y_max); + + auto w = std::max(0, x_max - x_min); + auto h = std::max(0, y_max - y_min); + auto intersect = w * h; + + auto union_area = rect1_area + rect2_area - intersect; + + double iou = static_cast(intersect) / static_cast(union_area); + + // convex shape area + + auto con_x_min = std::min(rect1_x_min, rect2_x_min); + auto con_y_min = std::min(rect1_y_min, rect2_y_min); + auto con_x_max = std::max(rect1_x_max, rect2_x_max); + auto con_y_max = std::max(rect1_y_max, rect2_y_max); + + auto con_area = (con_x_max - con_x_min + 1) * (con_y_max - con_y_min + 1); + + // GIoU calc + double giou = iou - static_cast(con_area - union_area) / static_cast(con_area); + + return giou; +} + +} // namespace utils +} // namespace autoware::traffic_light diff --git a/perception/autoware_traffic_light_selector/src/utils.hpp b/perception/autoware_traffic_light_selector/src/utils.hpp new file mode 100644 index 0000000000000..94f3ff8d97233 --- /dev/null +++ b/perception/autoware_traffic_light_selector/src/utils.hpp @@ -0,0 +1,73 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef UTILS_HPP_ +#define UTILS_HPP_ + +#include +#include + +#include + +#include + +namespace autoware::traffic_light +{ +namespace utils +{ +// create and function of 2 binary image +inline int andOf2Images(cv::Mat & img1, cv::Mat & img2) +{ + cv::Mat img_and; + cv::bitwise_and(img1, img2, img_and); + return cv::countNonZero(img_and); +} + +// create OR function of 2 binary image +inline int orOf2Images(cv::Mat & img1, cv::Mat & img2) +{ + cv::Mat img_or; + cv::bitwise_or(img1, img2, img_or); + return cv::countNonZero(img_or); +} + +// create the overall IOU of 2 binary image +inline double getIoUOf2BinaryImages(cv::Mat & img1, cv::Mat & img2) +{ + int and_area = andOf2Images(img1, img2); + int or_area = orOf2Images(img1, img2); + return static_cast(and_area) / static_cast(or_area); +} + +bool isInsideRoughRoi( + const sensor_msgs::msg::RegionOfInterest & detected_roi, + const sensor_msgs::msg::RegionOfInterest & rough_roi); + +float calIOU( + const sensor_msgs::msg::RegionOfInterest & bbox1, + const sensor_msgs::msg::RegionOfInterest & bbox2); + +double getGenIoU( + const sensor_msgs::msg::RegionOfInterest & roi1, const sensor_msgs::msg::RegionOfInterest & roi2); +// create binary image from list of rois +cv::Mat createBinaryImageFromRois( + const std::vector & rois, const cv::Size & size); + +// shift and padding image by dx, dy +cv::Mat shiftAndPaddingImage(cv::Mat & img, int dx, int dy); + +} // namespace utils +} // namespace autoware::traffic_light + +#endif // UTILS_HPP_ From 99c0c8875b598dd41ca269d7f08e6a8027b7e6cf Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Thu, 26 Dec 2024 17:17:24 +0900 Subject: [PATCH 17/18] fix: add unknown in selector Signed-off-by: badai-nguyen --- .../src/traffic_light_selector_node.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp index 3c76f17bca3b4..367c491f88bca 100644 --- a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp +++ b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp @@ -138,8 +138,14 @@ void TrafficLightSelectorNode::objectsCallback( if (max_iou > max_iou_threshold_) { TrafficLightRoi traffic_light_roi; traffic_light_roi.traffic_light_id = expect_roi.traffic_light_id; + traffic_light_roi.traffic_light_type = expect_roi.traffic_light_type; traffic_light_roi.roi = max_iou_roi; output.rois.push_back(traffic_light_roi); + } else { + TrafficLightRoi traffic_light_roi; + traffic_light_roi.traffic_light_id = expect_roi.traffic_light_id; + traffic_light_roi.traffic_light_type = expect_roi.traffic_light_type; + output.rois.push_back(traffic_light_roi); } } pub_traffic_light_rois_->publish(output); From 25e33d10973324547a71e67ae85518d203a57639 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Thu, 26 Dec 2024 17:17:24 +0900 Subject: [PATCH 18/18] fix: change to GenIOU Signed-off-by: badai-nguyen --- .../launch/traffic_light_selector.launch.xml | 2 +- .../src/traffic_light_selector_node.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/perception/autoware_traffic_light_selector/launch/traffic_light_selector.launch.xml b/perception/autoware_traffic_light_selector/launch/traffic_light_selector.launch.xml index 022891de3d457..342973ddbe348 100644 --- a/perception/autoware_traffic_light_selector/launch/traffic_light_selector.launch.xml +++ b/perception/autoware_traffic_light_selector/launch/traffic_light_selector.launch.xml @@ -9,6 +9,6 @@ - + diff --git a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp index 367c491f88bca..08675a7f84031 100644 --- a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp +++ b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp @@ -129,7 +129,7 @@ void TrafficLightSelectorNode::objectsCallback( static_cast(detected_roi.feature.roi.y_offset) - det_roi_shift_y, 0, static_cast(image_height_ - detected_roi.feature.roi.height)); - double iou = utils::calIOU(expect_roi.roi, detected_roi_shifted); + double iou = utils::getGenIoU(expect_roi.roi, detected_roi_shifted); if (iou > max_iou) { max_iou = iou; max_iou_roi = detected_roi.feature.roi;