Skip to content

Commit

Permalink
refactor: apply autoware namespace to tensorrt_yolox
Browse files Browse the repository at this point in the history
Signed-off-by: ktro2828 <[email protected]>
  • Loading branch information
ktro2828 committed Jul 12, 2024
1 parent 1b3f5aa commit de56e97
Show file tree
Hide file tree
Showing 3 changed files with 21 additions and 19 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -15,12 +15,12 @@
#ifndef TRAFFIC_LIGHT_FINE_DETECTOR__NODELET_HPP_
#define TRAFFIC_LIGHT_FINE_DETECTOR__NODELET_HPP_

#include <autoware/tensorrt_yolox/tensorrt_yolox.hpp>
#include <image_transport/image_transport.hpp>
#include <image_transport/subscriber_filter.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <rclcpp/rclcpp.hpp>
#include <tensorrt_yolox/tensorrt_yolox.hpp>

#include <sensor_msgs/image_encodings.hpp>
#include <sensor_msgs/msg/image.hpp>
Expand Down Expand Up @@ -94,8 +94,8 @@ class TrafficLightFineDetectorNodelet : public rclcpp::Node
*/
float evalMatchScore(
std::map<int, TrafficLightRoi> & id2expectRoi,
std::map<int, tensorrt_yolox::ObjectArray> & id2detections,
std::map<int, tensorrt_yolox::Object> & id2bestDetection);
std::map<int, autoware::tensorrt_yolox::ObjectArray> & id2detections,
std::map<int, autoware::tensorrt_yolox::Object> & id2bestDetection);
/**
* @brief Every traffic light roi might have several possible detections. This function
* is designed to select the best detection for every traffic light by making use of
Expand All @@ -114,7 +114,8 @@ class TrafficLightFineDetectorNodelet : public rclcpp::Node
*/
void detectionMatch(
std::map<int, TrafficLightRoi> & id2expectRoi,
std::map<int, tensorrt_yolox::ObjectArray> & id2detections, TrafficLightRoiArray & out_rois);
std::map<int, autoware::tensorrt_yolox::ObjectArray> & id2detections,
TrafficLightRoiArray & out_rois);

/**
* @brief convert image message to cv::Mat
Expand Down Expand Up @@ -167,7 +168,7 @@ class TrafficLightFineDetectorNodelet : public rclcpp::Node
std::vector<int> tlr_label_id_;

int batch_size_;
std::unique_ptr<tensorrt_yolox::TrtYoloX> trt_yolox_;
std::unique_ptr<autoware::tensorrt_yolox::TrtYoloX> trt_yolox_;
}; // TrafficLightFineDetectorNodelet

} // namespace traffic_light
Expand Down
2 changes: 1 addition & 1 deletion perception/traffic_light_fine_detector/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -13,13 +13,13 @@

<build_depend>autoware_cmake</build_depend>

<depend>autoware_tensorrt_yolox</depend>
<depend>cv_bridge</depend>
<depend>image_transport</depend>
<depend>message_filters</depend>
<depend>rclcpp</depend>
<depend>rclcpp_components</depend>
<depend>sensor_msgs</depend>
<depend>tensorrt_yolox</depend>
<depend>tier4_debug_msgs</depend>
<depend>tier4_perception_msgs</depend>

Expand Down
27 changes: 14 additions & 13 deletions perception/traffic_light_fine_detector/src/nodelet.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@ namespace fs = ::std::experimental::filesystem;
namespace
{
float calWeightedIou(
const sensor_msgs::msg::RegionOfInterest & bbox1, const tensorrt_yolox::Object & bbox2)
const sensor_msgs::msg::RegionOfInterest & bbox1, const autoware::tensorrt_yolox::Object & bbox2)
{
int x1 = std::max(static_cast<int>(bbox1.x_offset), bbox2.x_offset);
int x2 = std::min(static_cast<int>(bbox1.x_offset + bbox1.width), bbox2.x_offset + bbox2.width);
Expand Down Expand Up @@ -89,7 +89,7 @@ TrafficLightFineDetectorNodelet::TrafficLightFineDetectorNodelet(
batch_size_ = input_dim.d[0];
const tensorrt_common::BatchConfig batch_config{batch_size_, batch_size_, batch_size_};

trt_yolox_ = std::make_unique<tensorrt_yolox::TrtYoloX>(
trt_yolox_ = std::make_unique<autoware::tensorrt_yolox::TrtYoloX>(
model_path, precision, num_class, score_thresh_, nms_threshold, build_config, cuda_preprocess,
calib_image_list, scale, cache_dir, batch_config);

Expand Down Expand Up @@ -147,14 +147,14 @@ void TrafficLightFineDetectorNodelet::callback(
cv::Mat original_image;
TrafficLightRoiArray out_rois;
std::map<int, TrafficLightRoi> id2expectRoi;
std::map<int, tensorrt_yolox::ObjectArray> id2detections;
std::map<int, autoware::tensorrt_yolox::ObjectArray> id2detections;
for (const auto & expect_roi : expect_roi_msg->rois) {
id2expectRoi[expect_roi.traffic_light_id] = expect_roi;
}

rosMsg2CvMat(in_image_msg, original_image, "bgr8");
std::vector<cv::Rect> rois;
tensorrt_yolox::ObjectArrays inference_results;
autoware::tensorrt_yolox::ObjectArrays inference_results;
std::vector<cv::Point> lts;
std::vector<size_t> roi_ids;

Expand All @@ -178,15 +178,15 @@ void TrafficLightFineDetectorNodelet::callback(
if (static_cast<int>(rois.size()) == batch_size_) {
trt_yolox_->doMultiScaleInference(original_image, inference_results, rois);
for (size_t batch_i = 0; batch_i < true_batch_size; batch_i++) {
for (const tensorrt_yolox::Object & detection : inference_results[batch_i]) {
for (const autoware::tensorrt_yolox::Object & detection : inference_results[batch_i]) {
if (detection.score < score_thresh_) {
continue;
}
cv::Point lt_roi(
lts[batch_i].x + detection.x_offset, lts[batch_i].y + detection.y_offset);
cv::Point rb_roi(lt_roi.x + detection.width, lt_roi.y + detection.height);
fitInFrame(lt_roi, rb_roi, cv::Size(original_image.size()));
tensorrt_yolox::Object det = detection;
autoware::tensorrt_yolox::Object det = detection;
det.x_offset = lt_roi.x;
det.y_offset = lt_roi.y;
det.width = rb_roi.x - lt_roi.x;
Expand Down Expand Up @@ -215,16 +215,16 @@ void TrafficLightFineDetectorNodelet::callback(

float TrafficLightFineDetectorNodelet::evalMatchScore(
std::map<int, TrafficLightRoi> & id2expectRoi,
std::map<int, tensorrt_yolox::ObjectArray> & id2detections,
std::map<int, tensorrt_yolox::Object> & id2bestDetection)
std::map<int, autoware::tensorrt_yolox::ObjectArray> & id2detections,
std::map<int, autoware::tensorrt_yolox::Object> & id2bestDetection)
{
float score_sum = 0.0f;
id2bestDetection.clear();
for (const auto & roi_p : id2expectRoi) {
int tlr_id = roi_p.first;
float max_score = 0.0f;
const sensor_msgs::msg::RegionOfInterest & expected_roi = roi_p.second.roi;
for (const tensorrt_yolox::Object & detection : id2detections[tlr_id]) {
for (const autoware::tensorrt_yolox::Object & detection : id2detections[tlr_id]) {
float score = ::calWeightedIou(expected_roi, detection);
if (score >= max_score) {
max_score = score;
Expand All @@ -238,17 +238,18 @@ float TrafficLightFineDetectorNodelet::evalMatchScore(

void TrafficLightFineDetectorNodelet::detectionMatch(
std::map<int, TrafficLightRoi> & id2expectRoi,
std::map<int, tensorrt_yolox::ObjectArray> & id2detections, TrafficLightRoiArray & out_rois)
std::map<int, autoware::tensorrt_yolox::ObjectArray> & id2detections,
TrafficLightRoiArray & out_rois)
{
float max_score = 0.0f;
std::map<int, tensorrt_yolox::Object> bestDetections;
std::map<int, autoware::tensorrt_yolox::Object> bestDetections;
for (const auto & roi_pair : id2expectRoi) {
int tlr_id = roi_pair.first;
// the expected roi calculated from tf information
const sensor_msgs::msg::RegionOfInterest & expect_roi = roi_pair.second.roi;
int expect_cx = expect_roi.x_offset + expect_roi.width / 2;
int expect_cy = expect_roi.y_offset + expect_roi.height / 2;
for (const tensorrt_yolox::Object & det : id2detections[tlr_id]) {
for (const autoware::tensorrt_yolox::Object & det : id2detections[tlr_id]) {
// for every detection, calculate the center offset between the detection and the
// corresponding expected roi
int det_cx = det.x_offset + det.width / 2;
Expand All @@ -262,7 +263,7 @@ void TrafficLightFineDetectorNodelet::detectionMatch(
p.second.roi.y_offset += dy;
}
// calculate the "match score" between expected rois and id2detections_copy
std::map<int, tensorrt_yolox::Object> id2bestDetection;
std::map<int, autoware::tensorrt_yolox::Object> id2bestDetection;
float score = evalMatchScore(id2expectRoi_copy, id2detections, id2bestDetection);
if (score > max_score) {
max_score = score;
Expand Down

0 comments on commit de56e97

Please sign in to comment.