From b59cd8d9826107c7766811e6f12321a3b8528208 Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Mon, 10 Feb 2025 09:38:02 +0900 Subject: [PATCH] feat(traffic_light_selector): add new node for traffic light selection (#9721) * feat: add traffic light selector node Signed-off-by: badai-nguyen feat: add traffic ligth selector node Signed-off-by: badai-nguyen * fix: add check expect roi iou Signed-off-by: badai-nguyen * fix: tl selector Signed-off-by: badai-nguyen * fix: launch file Signed-off-by: badai-nguyen * fix: update matching score Signed-off-by: badai-nguyen * fix: calc sum IOU for whole shifted image Signed-off-by: badai-nguyen * fix: check inside rough roi Signed-off-by: badai-nguyen * fix: check inside function Signed-off-by: badai-nguyen * feat: add max_iou_threshold Signed-off-by: badai-nguyen * chore: pre-commit Signed-off-by: badai-nguyen * docs: add readme Signed-off-by: badai-nguyen * refactor: launch file Signed-off-by: badai-nguyen * docs: pre-commit Signed-off-by: badai-nguyen * docs Signed-off-by: badai-nguyen * chore: typo Signed-off-by: badai-nguyen * refactor Signed-off-by: badai-nguyen * fix: add unknown in selector Signed-off-by: badai-nguyen * fix: change to GenIOU Signed-off-by: badai-nguyen * feat: add debug topic Signed-off-by: badai-nguyen * fix: add maintainer Signed-off-by: badai-nguyen * chore: pre-commit Signed-off-by: badai-nguyen * fix:cmake Signed-off-by: badai-nguyen * fix: move param to yaml file Signed-off-by: badai-nguyen * fix: typo Signed-off-by: badai-nguyen * fix: add schema Signed-off-by: badai-nguyen * fix Signed-off-by: MasatoSaeki * style(pre-commit): autofix * fix typo Signed-off-by: MasatoSaeki --------- Signed-off-by: badai-nguyen Signed-off-by: MasatoSaeki Co-authored-by: Masato Saeki <78376491+MasatoSaeki@users.noreply.github.com> Co-authored-by: MasatoSaeki Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../CMakeLists.txt | 47 +++++ .../autoware_traffic_light_selector/README.md | 23 +++ .../config/traffic_light_selector.param.yaml | 3 + .../launch/traffic_light_selector.launch.xml | 15 ++ .../package.xml | 33 ++++ .../schema/traffic_light_selector.schema.json | 32 +++ .../src/traffic_light_selector_node.cpp | 182 ++++++++++++++++++ .../src/traffic_light_selector_node.hpp | 86 +++++++++ .../src/utils.cpp | 132 +++++++++++++ .../src/utils.hpp | 73 +++++++ 10 files changed, 626 insertions(+) create mode 100644 perception/autoware_traffic_light_selector/CMakeLists.txt create mode 100644 perception/autoware_traffic_light_selector/README.md create mode 100644 perception/autoware_traffic_light_selector/config/traffic_light_selector.param.yaml 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/schema/traffic_light_selector.schema.json 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 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 new file mode 100644 index 0000000000000..3d391f2e2c360 --- /dev/null +++ b/perception/autoware_traffic_light_selector/CMakeLists.txt @@ -0,0 +1,47 @@ +cmake_minimum_required(VERSION 3.8) +project(autoware_traffic_light_selector) + +# find dependencies +find_package(autoware_cmake REQUIRED) +find_package(OpenCV REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(Sophus REQUIRED) +find_package(Boost REQUIRED) +find_package(PCL REQUIRED) +find_package(CGAL REQUIRED COMPONENTS Core) + +include_directories( + include + SYSTEM + ${Boost_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} + ${Sophus_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} +) + +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} + 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/README.md b/perception/autoware_traffic_light_selector/README.md new file mode 100644 index 0000000000000..1b2ef2c4c89d1 --- /dev/null +++ b/perception/autoware_traffic_light_selector/README.md @@ -0,0 +1,23 @@ +`autoware_traffic_light_selector` + +## Overview + +`autoware_traffic_light_selector` selects the interest traffic light from the list of accurately detected traffic lights by something (e.g. deep learning neural network) 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 | accurately detected traffic light | +| `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_rois` | tier4_perception_msgs::TrafficLightRoiArray | detected traffic light of interest with id | + +## Node parameters + +{{json_to_markdown("perception/autoware_traffic_light_selector/schema/traffic_light_selector.schema.json")}} diff --git a/perception/autoware_traffic_light_selector/config/traffic_light_selector.param.yaml b/perception/autoware_traffic_light_selector/config/traffic_light_selector.param.yaml new file mode 100644 index 0000000000000..0ca36be8c3ddb --- /dev/null +++ b/perception/autoware_traffic_light_selector/config/traffic_light_selector.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + max_iou_threshold: -0.5 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..a4f572b9055ba --- /dev/null +++ b/perception/autoware_traffic_light_selector/launch/traffic_light_selector.launch.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/perception/autoware_traffic_light_selector/package.xml b/perception/autoware_traffic_light_selector/package.xml new file mode 100644 index 0000000000000..417a4f3b31495 --- /dev/null +++ b/perception/autoware_traffic_light_selector/package.xml @@ -0,0 +1,33 @@ + + + + autoware_traffic_light_selector + 0.1.0 + The ROS 2 autoware_traffic_light_selector package + Yukihiro Saito + Dai Nguyen + Yoshi Ri + Masato Saeki + Apache License 2.0 + Dai Nguyen + + ament_cmake_auto + + autoware_test_utils + autoware_universe_utils + cv_bridge + geometry_msgs + libopencv-dev + message_filters + rclcpp + rclcpp_components + sensor_msgs + tier4_perception_msgs + autoware_cmake + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/perception/autoware_traffic_light_selector/schema/traffic_light_selector.schema.json b/perception/autoware_traffic_light_selector/schema/traffic_light_selector.schema.json new file mode 100644 index 0000000000000..0491740974324 --- /dev/null +++ b/perception/autoware_traffic_light_selector/schema/traffic_light_selector.schema.json @@ -0,0 +1,32 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for traffic_light_selector", + "type": "object", + "definitions": { + "traffic_light_selector": { + "type": "object", + "properties": { + "max_iou_threshold": { + "type": "number", + "description": "Threshold of generalized IOU for detected traffic light selection", + "default": -0.5, + "minimum": -1.0, + "maximum": 1.0 + } + }, + "required": ["max_iou_threshold"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/traffic_light_selector" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} 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..80e2450a287f7 --- /dev/null +++ b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp @@ -0,0 +1,182 @@ +// 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 "traffic_light_selector_node.hpp" + +#include +#include + +#include + +#include +#include +#include + +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_), + 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_) +{ + { + stop_watch_ptr_ = + std::make_unique>(); + debug_publisher_ = + std::make_unique(this, this->get_name()); + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); + } + + max_iou_threshold_ = declare_parameter("max_iou_threshold"); + using std::placeholders::_1; + using std::placeholders::_2; + 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_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) +{ + stop_watch_ptr_->toc("processing_time", true); + 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; + 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); + } + for (const auto & expected_roi : expected_rois_msg->rois) { + expect_rois.push_back(expected_roi.roi); + } + cv::Mat expect_roi_img = + 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; + 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 (!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 = 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; + det_roi_shift_y = dy; + } + } + } + + 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; + // 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 = utils::getGenIoU(expect_roi.roi, detected_roi_shifted); + if (iou > max_iou) { + max_iou = iou; + max_iou_roi = detected_roi.feature.roi; + } + } + 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); + if (debug_publisher_) { + const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); + const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + const double pipeline_latency_ms = + std::chrono::duration( + std::chrono::nanoseconds((this->get_clock()->now() - output.header.stamp).nanoseconds())) + .count(); + debug_publisher_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + debug_publisher_->publish( + "debug/processing_time_ms", processing_time_ms); + debug_publisher_->publish( + "debug/pipeline_latency_ms", pipeline_latency_ms); + } + return; +} +} // 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 new file mode 100644 index 0000000000000..9dab987b7523d --- /dev/null +++ b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp @@ -0,0 +1,86 @@ +// 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 TRAFFIC_LIGHT_SELECTOR_NODE_HPP_ +#define TRAFFIC_LIGHT_SELECTOR_NODE_HPP_ + +#include "utils.hpp" + +#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; +using tier4_perception_msgs::msg::DetectedObjectWithFeature; +using tier4_perception_msgs::msg::TrafficLightRoi; +using tier4_perception_msgs::msg::TrafficLightRoiArray; + +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 detected_rois_sub_; + message_filters::Subscriber rough_rois_sub_; + message_filters::Subscriber expected_rois_sub_; + typedef message_filters::sync_policies::ApproximateTime< + DetectedObjectsWithFeature, TrafficLightRoiArray, TrafficLightRoiArray> + SyncPolicy; + typedef message_filters::Synchronizer Sync; + Sync sync_; + void objectsCallback( + 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 camera_info_subscribed_; + uint32_t image_width_{1280}; + uint32_t image_height_{960}; + double max_iou_threshold_{0.0}; + + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; +}; + +} // namespace autoware::traffic_light + +#endif // TRAFFIC_LIGHT_SELECTOR_NODE_HPP_ 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_