diff --git a/perception/traffic_light_fine_detector/CMakeLists.txt b/perception/traffic_light_fine_detector/CMakeLists.txt index fe9fa2ffcaa8b..e8fe6554f937f 100644 --- a/perception/traffic_light_fine_detector/CMakeLists.txt +++ b/perception/traffic_light_fine_detector/CMakeLists.txt @@ -99,6 +99,12 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) EXECUTABLE traffic_light_fine_detector_node ) + if(BUILD_TESTING) + ament_auto_add_gtest(traffic_light_fine_detector_tests + test/test_nodelet.cpp + ) + endif() + ament_auto_package(INSTALL_TO_SHARE launch config diff --git a/perception/traffic_light_fine_detector/include/traffic_light_fine_detector/nodelet.hpp b/perception/traffic_light_fine_detector/include/traffic_light_fine_detector/nodelet.hpp index 5c89c76a11833..bd145e5026409 100644 --- a/perception/traffic_light_fine_detector/include/traffic_light_fine_detector/nodelet.hpp +++ b/perception/traffic_light_fine_detector/include/traffic_light_fine_detector/nodelet.hpp @@ -52,6 +52,9 @@ typedef struct Detection namespace traffic_light { +float calWeightedIou( + const sensor_msgs::msg::RegionOfInterest & bbox1, const tensorrt_yolox::Object & bbox2); + class TrafficLightFineDetectorNodelet : public rclcpp::Node { using TrafficLightRoi = tier4_perception_msgs::msg::TrafficLightRoi; diff --git a/perception/traffic_light_fine_detector/src/nodelet.cpp b/perception/traffic_light_fine_detector/src/nodelet.cpp index 8037dc5472fbe..28f3fafabc6c0 100644 --- a/perception/traffic_light_fine_detector/src/nodelet.cpp +++ b/perception/traffic_light_fine_detector/src/nodelet.cpp @@ -27,7 +27,7 @@ namespace fs = ::std::experimental::filesystem; #include #include -namespace +namespace traffic_light { float calWeightedIou( const sensor_msgs::msg::RegionOfInterest & bbox1, const tensorrt_yolox::Object & bbox2) @@ -44,10 +44,6 @@ float calWeightedIou( return bbox2.score * area1 / area2; } -} // namespace - -namespace traffic_light -{ inline std::vector toFloatVector(const std::vector double_vector) { return std::vector(double_vector.begin(), double_vector.end()); @@ -225,7 +221,7 @@ float TrafficLightFineDetectorNodelet::evalMatchScore( 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]) { - float score = ::calWeightedIou(expected_roi, detection); + float score = calWeightedIou(expected_roi, detection); if (score >= max_score) { max_score = score; id2bestDetection[tlr_id] = detection; diff --git a/perception/traffic_light_fine_detector/test/test_nodelet.cpp b/perception/traffic_light_fine_detector/test/test_nodelet.cpp new file mode 100644 index 0000000000000..f45d3351d025d --- /dev/null +++ b/perception/traffic_light_fine_detector/test/test_nodelet.cpp @@ -0,0 +1,55 @@ +// 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_fine_detector/nodelet.hpp" + +#include + +sensor_msgs::msg::RegionOfInterest createMapBasedBbox( + const uint32_t x_offset, const uint32_t y_offset, const uint32_t width, const uint32_t height) +{ + sensor_msgs::msg::RegionOfInterest bbox; + bbox.x_offset = x_offset; + bbox.y_offset = y_offset; + bbox.width = width; + bbox.height = height; + return bbox; +} + +tensorrt_yolox::Object createYoloxBbox( + const int32_t x_offset, const int32_t y_offset, const int32_t width, const int32_t height, + const float score, const int32_t type) +{ + tensorrt_yolox::Object bbox; + bbox.x_offset = x_offset; + bbox.y_offset = y_offset; + bbox.width = width; + bbox.height = height; + bbox.score = score; + bbox.type = type; + return bbox; +} + +TEST(CalWeightedIouTest, NoOverlap) +{ + const sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(0, 0, 10, 10); + const tensorrt_yolox::Object yolox_bbox = createYoloxBbox(20, 20, 10, 10, 0.9f, 0); + + // check size of map_based_bbox and yolox_bbox + EXPECT_EQ(map_based_bbox.width, 10); + EXPECT_EQ(yolox_bbox.width, 10); + const float iou = traffic_light::calWeightedIou(map_based_bbox, yolox_bbox); + EXPECT_FLOAT_EQ(iou, 0.0f); + // EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.0f); +}