Skip to content

Commit

Permalink
fix: update matching score
Browse files Browse the repository at this point in the history 
Signed-off-by: badai-nguyen <[email protected]>
  • Loading branch information
@badai-nguyen
badai-nguyen committed Dec 13, 2024
1 parent 6bebe46 commit 25ab1f6
Show file tree
Hide file tree
Showing 5 changed files with 202 additions and 79 deletions.
2 changes: 2 additions & 0 deletions ...ier4_perception_launch/launch/traffic_light_recognition/traffic_light_detector.launch.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,12 +55,14 @@
<arg name="input/rough_rois" default="/perception/traffic_light_recognition/$(var namespace)/detection/rough/rois"/>
<arg name="output/traffic_light_rois" default="/perception/traffic_light_recognition/$(var namespace)/detection/rois"/>
<arg name="input/expect_rois" default="/perception/traffic_light_recognition/$(var namespace)/detection/expect/rois"/>
<arg name="input/camera_info" default="/sensing/camera/$(var namespace)/camera_info"/>
<!-- Node -->
<node pkg="autoware_traffic_light_selector" exec="traffic_light_selector_node" name="autoware_traffic_light_selector_$(var namespace)" output="screen">
<remap from="input/detected_rois" to="$(var yolox_detected_traffic/rois)"/>
<remap from="input/rough_rois" to="$(var input/rough_rois)"/>
<remap from="input/expect_rois" to="$(var input/expect_rois)"/>
<remap from="output/traffic_light_rois" to="$(var output/traffic_light_rois)"/>
<remap from="input/camera_info" to="$(var input/camera_info)"/>
<param name="debug" value="true"/>
</node>
</group>
Expand Down
2 changes: 2 additions & 0 deletions perception/autoware_traffic_light_selector/launch/traffic_light_selector.launch.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,13 +3,15 @@
<arg name="output/clusters" default="output/clusters"/>
<arg name="input/detected_rois" default="input/detected_rois"/>
<arg name="input/rough_rois" default="input/rough_rois"/>
<arg name="input/camera_info" default="input/camera_info"/>
<!-- <arg name="param_path" default="$(find-pkg-share autoware_traffic_light_selector)/config/traffic_light_selector.param.yaml"/> -->

<!-- Node -->
<node pkg="autoware_traffic_light_selector" exec="traffic_light_selector_node" name="autoware_traffic_light_selector" output="screen">
<remap from="output/clusters" to="$(var output/clusters)"/>
<remap from="input/detected_rois" to="$(var input/detected_rois)"/>
<remap from="input/rough_rois" to="$(var input/rough_rois)"/>
<remap from"input/camera_info" to="input/camera_info"/>
<param name="debug" value="true"/>
</node>
</launch>
2 changes: 2 additions & 0 deletions perception/autoware_traffic_light_selector/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,9 @@

<depend>autoware_test_utils</depend>
<depend>autoware_universe_utils</depend>
<depend>cv_bridge</depend>
<depend>geometry_msgs</depend>
<depend>libopencv-dev</depend>
<depend>message_filters</depend>
<depend>rclcpp</depend>
<depend>rclcpp_components</depend>
Expand Down
267 changes: 188 additions & 79 deletions perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,9 @@

#include "traffic_light_selector_node.hpp"

#include <opencv2/core/types.hpp>
#include <opencv2/opencv.hpp>

#include "sensor_msgs/msg/region_of_interest.hpp"

#include <memory>
Expand All @@ -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<float>(area1) / static_cast<float>(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<int>(roi1.x_offset);
// int rect1_x_max = static_cast<int>(roi1.x_offset + roi1.width);
// int rect1_y_min = static_cast<int>(roi1.y_offset);
// int rect1_y_max = static_cast<int>(roi1.y_offset + roi1.height);
// int rect2_x_min = static_cast<int>(roi2.x_offset);
// int rect2_x_max = static_cast<int>(roi2.x_offset + roi2.width);
// int rect2_y_min = static_cast<int>(roi2.y_offset);
// int rect2_y_max = static_cast<int>(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<double>(intersect) / static_cast<double>(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<double>(con_area - union_area) / static_cast<double>(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<int>(roi1.x_offset);
int rect1_x_max = static_cast<int>(roi1.x_offset + roi1.width);
int rect1_y_min = static_cast<int>(roi1.y_offset);
int rect1_y_max = static_cast<int>(roi1.y_offset + roi1.height);
int rect2_x_min = static_cast<int>(roi2.x_offset);
int rect2_x_max = static_cast<int>(roi2.x_offset + roi2.width);
int rect2_y_min = static_cast<int>(roi2.y_offset);
int rect2_y_max = static_cast<int>(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<double>(intersect) / static_cast<double>(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<double>(con_area - union_area) / static_cast<double>(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<double>(and_area) / static_cast<double>(or_area);
}

// create binary image from list of rois
cv::Mat createBinaryImageFromRois(
const std::vector<sensor_msgs::msg::RegionOfInterest> & 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
Expand All @@ -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<sensor_msgs::msg::CameraInfo>(
"input/camera_info", rclcpp::SensorDataQoS(),
std::bind(&TrafficLightSelectorNode::cameraInfoCallback, this, _1));
// Publisher
pub_traffic_light_rois_ =
create_publisher<TrafficLightRoiArray>("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<int, sensor_msgs::msg::RegionOfInterest> 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<sensor_msgs::msg::RegionOfInterest> det_rois;
std::vector<sensor_msgs::msg::RegionOfInterest> expect_rois;
for (const auto & detected_roi : detected_traffic_light_msg->feature_objects) {
det_rois.push_back(detected_roi.feature.roi);
}
std::map<int, sensor_msgs::msg::RegionOfInterest> 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<int>(image_width_), detected_roi.x_offset);
// // detected_roi.y_offset = std::min(static_cast<int>(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_) {
}
Expand Down
8 changes: 8 additions & 0 deletions perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -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<TrafficLightRoiArray>::SharedPtr pub_traffic_light_rois_;
// Subscribe camera_info to get width and height of image
rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::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<sensor_msgs::msg::Image>::SharedPtr pub_debug_image_;
};
Expand Down

0 comments on commit 25ab1f6

Please sign in to comment.