From 30467aa3c9e2badf39e06bcbda69af5288f8dfde Mon Sep 17 00:00:00 2001 From: Shin-kyoto Date: Mon, 20 May 2024 12:48:08 +0900 Subject: [PATCH 01/28] feat(traffic_light_fine_detector): update function for waited IoU Signed-off-by: Shin-kyoto --- .../traffic_light_fine_detector/nodelet.hpp | 15 +++++++++++++ .../src/nodelet.cpp | 21 +------------------ 2 files changed, 16 insertions(+), 20 deletions(-) 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..d117e200e0c45 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,21 @@ typedef struct Detection namespace traffic_light { +float calWeightedIou( + const sensor_msgs::msg::RegionOfInterest & bbox1, const tensorrt_yolox::Object & bbox2) +{ + int x1 = std::max(static_cast(bbox1.x_offset), bbox2.x_offset); + int x2 = std::min(static_cast(bbox1.x_offset + bbox1.width), bbox2.x_offset + bbox2.width); + int y1 = std::max(static_cast(bbox1.y_offset), bbox2.y_offset); + int y2 = std::min(static_cast(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 bbox2.score * area1 / area2; +} + 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..b4c815b62ba01 100644 --- a/perception/traffic_light_fine_detector/src/nodelet.cpp +++ b/perception/traffic_light_fine_detector/src/nodelet.cpp @@ -27,25 +27,6 @@ namespace fs = ::std::experimental::filesystem; #include #include -namespace -{ -float calWeightedIou( - const sensor_msgs::msg::RegionOfInterest & bbox1, const tensorrt_yolox::Object & bbox2) -{ - int x1 = std::max(static_cast(bbox1.x_offset), bbox2.x_offset); - int x2 = std::min(static_cast(bbox1.x_offset + bbox1.width), bbox2.x_offset + bbox2.width); - int y1 = std::max(static_cast(bbox1.y_offset), bbox2.y_offset); - int y2 = std::min(static_cast(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 bbox2.score * area1 / area2; -} - -} // namespace - namespace traffic_light { inline std::vector toFloatVector(const std::vector double_vector) @@ -225,7 +206,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 = traffic_light::calWeightedIou(expected_roi, detection); if (score >= max_score) { max_score = score; id2bestDetection[tlr_id] = detection; From 8c28620ef4118615a5d60f411eb56d015eb34415 Mon Sep 17 00:00:00 2001 From: Shin-kyoto Date: Mon, 20 May 2024 12:49:34 +0900 Subject: [PATCH 02/28] test(traffic_light_fine_detector): add test for util function Signed-off-by: Shin-kyoto --- .../CMakeLists.txt | 19 ++++ .../test/test_nodelet.cpp | 95 +++++++++++++++++++ 2 files changed, 114 insertions(+) create mode 100644 perception/traffic_light_fine_detector/test/test_nodelet.cpp diff --git a/perception/traffic_light_fine_detector/CMakeLists.txt b/perception/traffic_light_fine_detector/CMakeLists.txt index fe9fa2ffcaa8b..07e9cca6a2412 100644 --- a/perception/traffic_light_fine_detector/CMakeLists.txt +++ b/perception/traffic_light_fine_detector/CMakeLists.txt @@ -99,6 +99,25 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) EXECUTABLE traffic_light_fine_detector_node ) + function(add_testcase filepath) + get_filename_component(filename ${filepath} NAME) + string(REGEX REPLACE ".cpp" "" test_name ${filename}) + + ament_add_gtest(${test_name} ${filepath}) + target_link_libraries("${test_name}" traffic_light_fine_detector_nodelet) + ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) + endfunction() + + if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + file(GLOB_RECURSE TEST_FILES test/*.cpp) + + foreach(filepath ${TEST_FILES}) + add_testcase(${filepath}) + endforeach() + endif() + ament_auto_package(INSTALL_TO_SHARE launch config 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..51a4e230c4843 --- /dev/null +++ b/perception/traffic_light_fine_detector/test/test_nodelet.cpp @@ -0,0 +1,95 @@ +// 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(uint32_t x_offset, uint32_t y_offset, uint32_t width, 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(int32_t x_offset, int32_t y_offset, int32_t width, int32_t height, float score, 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) { + sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(0, 0, 10, 10); + tensorrt_yolox::Object yolox_bbox = createYoloxBbox(20, 20, 10, 10, 0.9f, 0); + + EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.0f); +} + +TEST(CalWeightedIouTest, PartiallyOverlap1) { + sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(15, 15, 10, 10); + tensorrt_yolox::Object yolox_bbox = createYoloxBbox(20, 20, 10, 10, 0.7f, 0); + + EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.1f); +} + +TEST(CalWeightedIouTest, PartiallyOverlap2) { + sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(0, 0, 10, 10); + tensorrt_yolox::Object yolox_bbox = createYoloxBbox(-5, -5, 10, 10, 0.7f, 0); + + EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.1f); +} + +TEST(CalWeightedIouTest, Included1) { + sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(20, 20, 10, 10); + tensorrt_yolox::Object yolox_bbox = createYoloxBbox(20, 20, 10, 10, 0.5f, 0); + + EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.5f); +} + +TEST(CalWeightedIouTest, Included2) { + sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(20, 20, 100, 100); + tensorrt_yolox::Object yolox_bbox = createYoloxBbox(20, 20, 10, 10, 0.5f, 0); + + EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.005f); +} + +TEST(CalWeightedIouTest, Zero) { + sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(0, 0, 0, 0); + tensorrt_yolox::Object yolox_bbox = createYoloxBbox(0, 0, 0, 0, 0.5f, 0); + + EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.0f); +} + +TEST(CalWeightedIouTest, Negative) { + sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(0, 0, 10, 10); + tensorrt_yolox::Object yolox_bbox = createYoloxBbox(-5, -5, -5, 10, 0.5f, 0); + + EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.0f); +} + +// This test case should be passed but it fails because of unappropriate type casting. Current result is 0.1097561, but it should be 0.0. +// TEST(CalWeightedIouTest, Uint32Max) { +// sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(UINT32_MAX, UINT32_MAX, 10, 10); +// tensorrt_yolox::Object yolox_bbox = createYoloxBbox(-5, -5, 10, 10, 0.5f, 0); + +// EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.0f); +// } From fa3113434be6c39f724d1ee3044d0a724e6753d2 Mon Sep 17 00:00:00 2001 From: Shin-kyoto Date: Mon, 20 May 2024 20:40:32 +0900 Subject: [PATCH 03/28] test(traffic_light_fine_detector): update CMakeLists.txt with short expression Signed-off-by: Shin-kyoto --- .../traffic_light_fine_detector/CMakeLists.txt | 18 +++--------------- 1 file changed, 3 insertions(+), 15 deletions(-) diff --git a/perception/traffic_light_fine_detector/CMakeLists.txt b/perception/traffic_light_fine_detector/CMakeLists.txt index 07e9cca6a2412..d6fccc9b2a311 100644 --- a/perception/traffic_light_fine_detector/CMakeLists.txt +++ b/perception/traffic_light_fine_detector/CMakeLists.txt @@ -99,23 +99,11 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) EXECUTABLE traffic_light_fine_detector_node ) - function(add_testcase filepath) - get_filename_component(filename ${filepath} NAME) - string(REGEX REPLACE ".cpp" "" test_name ${filename}) - - ament_add_gtest(${test_name} ${filepath}) - target_link_libraries("${test_name}" traffic_light_fine_detector_nodelet) - ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) - endfunction() - if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) - - file(GLOB_RECURSE TEST_FILES test/*.cpp) - - foreach(filepath ${TEST_FILES}) - add_testcase(${filepath}) - endforeach() + ament_add_gtest(traffic_light_fine_detector_tests test/test_nodelet.cpp) + target_link_libraries(traffic_light_fine_detector_tests traffic_light_fine_detector_nodelet) + ament_target_dependencies(traffic_light_fine_detector_tests ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) endif() ament_auto_package(INSTALL_TO_SHARE From b929b1373cb55489cd094ff1f855e3fb6b6042ff Mon Sep 17 00:00:00 2001 From: Shin-kyoto Date: Mon, 20 May 2024 20:43:35 +0900 Subject: [PATCH 04/28] chore(traffic_light_fine_detector): apply pre-commit Signed-off-by: Shin-kyoto --- .../test/test_nodelet.cpp | 37 +++++++++++++------ 1 file changed, 25 insertions(+), 12 deletions(-) diff --git a/perception/traffic_light_fine_detector/test/test_nodelet.cpp b/perception/traffic_light_fine_detector/test/test_nodelet.cpp index 51a4e230c4843..7bc6d68fac594 100644 --- a/perception/traffic_light_fine_detector/test/test_nodelet.cpp +++ b/perception/traffic_light_fine_detector/test/test_nodelet.cpp @@ -13,9 +13,11 @@ // limitations under the License. #include "traffic_light_fine_detector/nodelet.hpp" + #include -sensor_msgs::msg::RegionOfInterest createMapBasedBbox(uint32_t x_offset, uint32_t y_offset, uint32_t width, uint32_t height) +sensor_msgs::msg::RegionOfInterest createMapBasedBbox( + uint32_t x_offset, uint32_t y_offset, uint32_t width, uint32_t height) { sensor_msgs::msg::RegionOfInterest bbox; bbox.x_offset = x_offset; @@ -25,7 +27,8 @@ sensor_msgs::msg::RegionOfInterest createMapBasedBbox(uint32_t x_offset, uint32_ return bbox; } -tensorrt_yolox::Object createYoloxBbox(int32_t x_offset, int32_t y_offset, int32_t width, int32_t height, float score, int32_t type) +tensorrt_yolox::Object createYoloxBbox( + int32_t x_offset, int32_t y_offset, int32_t width, int32_t height, float score, int32_t type) { tensorrt_yolox::Object bbox; bbox.x_offset = x_offset; @@ -37,58 +40,68 @@ tensorrt_yolox::Object createYoloxBbox(int32_t x_offset, int32_t y_offset, int32 return bbox; } -TEST(CalWeightedIouTest, NoOverlap) { +TEST(CalWeightedIouTest, NoOverlap) +{ sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(0, 0, 10, 10); tensorrt_yolox::Object yolox_bbox = createYoloxBbox(20, 20, 10, 10, 0.9f, 0); EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.0f); } -TEST(CalWeightedIouTest, PartiallyOverlap1) { +TEST(CalWeightedIouTest, PartiallyOverlap1) +{ sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(15, 15, 10, 10); tensorrt_yolox::Object yolox_bbox = createYoloxBbox(20, 20, 10, 10, 0.7f, 0); EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.1f); } -TEST(CalWeightedIouTest, PartiallyOverlap2) { +TEST(CalWeightedIouTest, PartiallyOverlap2) +{ sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(0, 0, 10, 10); tensorrt_yolox::Object yolox_bbox = createYoloxBbox(-5, -5, 10, 10, 0.7f, 0); EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.1f); } -TEST(CalWeightedIouTest, Included1) { +TEST(CalWeightedIouTest, Included1) +{ sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(20, 20, 10, 10); tensorrt_yolox::Object yolox_bbox = createYoloxBbox(20, 20, 10, 10, 0.5f, 0); EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.5f); } -TEST(CalWeightedIouTest, Included2) { +TEST(CalWeightedIouTest, Included2) +{ sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(20, 20, 100, 100); tensorrt_yolox::Object yolox_bbox = createYoloxBbox(20, 20, 10, 10, 0.5f, 0); EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.005f); } -TEST(CalWeightedIouTest, Zero) { +TEST(CalWeightedIouTest, Zero) +{ sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(0, 0, 0, 0); tensorrt_yolox::Object yolox_bbox = createYoloxBbox(0, 0, 0, 0, 0.5f, 0); EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.0f); } -TEST(CalWeightedIouTest, Negative) { +TEST(CalWeightedIouTest, Negative) +{ sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(0, 0, 10, 10); tensorrt_yolox::Object yolox_bbox = createYoloxBbox(-5, -5, -5, 10, 0.5f, 0); EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.0f); } -// This test case should be passed but it fails because of unappropriate type casting. Current result is 0.1097561, but it should be 0.0. -// TEST(CalWeightedIouTest, Uint32Max) { -// sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(UINT32_MAX, UINT32_MAX, 10, 10); +// This test case should be passed but it fails because of unappropriate type casting. Current +// result is 0.1097561, but it should be 0.0. +// TEST(CalWeightedIouTest, Uint32Max) +// { +// sensor_msgs::msg::RegionOfInterest map_based_bbox = +// createMapBasedBbox(UINT32_MAX, UINT32_MAX, 10, 10); // tensorrt_yolox::Object yolox_bbox = createYoloxBbox(-5, -5, 10, 10, 0.5f, 0); // EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.0f); From 3365dcb450dcfe8e915fa485f8c8d5b3523f48c2 Mon Sep 17 00:00:00 2001 From: Shin-kyoto Date: Mon, 20 May 2024 21:10:28 +0900 Subject: [PATCH 05/28] chore(traffic_light_fine_detector): remove misspelling Signed-off-by: Shin-kyoto --- perception/traffic_light_fine_detector/test/test_nodelet.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/traffic_light_fine_detector/test/test_nodelet.cpp b/perception/traffic_light_fine_detector/test/test_nodelet.cpp index 7bc6d68fac594..d7db39f5d513b 100644 --- a/perception/traffic_light_fine_detector/test/test_nodelet.cpp +++ b/perception/traffic_light_fine_detector/test/test_nodelet.cpp @@ -96,7 +96,7 @@ TEST(CalWeightedIouTest, Negative) EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.0f); } -// This test case should be passed but it fails because of unappropriate type casting. Current +// This test case should be passed but it fails because of inappropriate type casting. Current // result is 0.1097561, but it should be 0.0. // TEST(CalWeightedIouTest, Uint32Max) // { From 761a221985287b0e480bcf6a0bedf2399c76bd54 Mon Sep 17 00:00:00 2001 From: Shin-kyoto Date: Tue, 21 May 2024 16:06:17 +0900 Subject: [PATCH 06/28] feat(traffic_light_fine_detector): move definition of util function to cpp Signed-off-by: Shin-kyoto --- .../traffic_light_fine_detector/nodelet.hpp | 14 +------------- .../traffic_light_fine_detector/src/nodelet.cpp | 17 ++++++++++++++++- 2 files changed, 17 insertions(+), 14 deletions(-) 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 d117e200e0c45..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 @@ -53,19 +53,7 @@ typedef struct Detection namespace traffic_light { float calWeightedIou( - const sensor_msgs::msg::RegionOfInterest & bbox1, const tensorrt_yolox::Object & bbox2) -{ - int x1 = std::max(static_cast(bbox1.x_offset), bbox2.x_offset); - int x2 = std::min(static_cast(bbox1.x_offset + bbox1.width), bbox2.x_offset + bbox2.width); - int y1 = std::max(static_cast(bbox1.y_offset), bbox2.y_offset); - int y2 = std::min(static_cast(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 bbox2.score * area1 / area2; -} + const sensor_msgs::msg::RegionOfInterest & bbox1, const tensorrt_yolox::Object & bbox2); class TrafficLightFineDetectorNodelet : public rclcpp::Node { diff --git a/perception/traffic_light_fine_detector/src/nodelet.cpp b/perception/traffic_light_fine_detector/src/nodelet.cpp index b4c815b62ba01..28f3fafabc6c0 100644 --- a/perception/traffic_light_fine_detector/src/nodelet.cpp +++ b/perception/traffic_light_fine_detector/src/nodelet.cpp @@ -29,6 +29,21 @@ namespace fs = ::std::experimental::filesystem; namespace traffic_light { +float calWeightedIou( + const sensor_msgs::msg::RegionOfInterest & bbox1, const tensorrt_yolox::Object & bbox2) +{ + int x1 = std::max(static_cast(bbox1.x_offset), bbox2.x_offset); + int x2 = std::min(static_cast(bbox1.x_offset + bbox1.width), bbox2.x_offset + bbox2.width); + int y1 = std::max(static_cast(bbox1.y_offset), bbox2.y_offset); + int y2 = std::min(static_cast(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 bbox2.score * area1 / area2; +} + inline std::vector toFloatVector(const std::vector double_vector) { return std::vector(double_vector.begin(), double_vector.end()); @@ -206,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 = traffic_light::calWeightedIou(expected_roi, detection); + float score = calWeightedIou(expected_roi, detection); if (score >= max_score) { max_score = score; id2bestDetection[tlr_id] = detection; From a948ae3fa8be7b3622e524f608c664b5ce09ac52 Mon Sep 17 00:00:00 2001 From: Shin-kyoto Date: Fri, 24 May 2024 16:30:42 +0900 Subject: [PATCH 07/28] test(traffic_light_fine_detector): make CMakeLists shorter by using ament_auto_add_gtest Signed-off-by: Shin-kyoto --- perception/traffic_light_fine_detector/CMakeLists.txt | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/perception/traffic_light_fine_detector/CMakeLists.txt b/perception/traffic_light_fine_detector/CMakeLists.txt index d6fccc9b2a311..82dfcf1c4a7ef 100644 --- a/perception/traffic_light_fine_detector/CMakeLists.txt +++ b/perception/traffic_light_fine_detector/CMakeLists.txt @@ -100,10 +100,10 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) ) if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) - ament_add_gtest(traffic_light_fine_detector_tests test/test_nodelet.cpp) - target_link_libraries(traffic_light_fine_detector_tests traffic_light_fine_detector_nodelet) - ament_target_dependencies(traffic_light_fine_detector_tests ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) + ament_auto_add_gtest(traffic_light_fine_detector_tests + test/test_nodelet.cpp + src/nodelet.cpp + ) endif() ament_auto_package(INSTALL_TO_SHARE From 9f1ab616d7b3455cccc1e757d0c181b5f0bc728f Mon Sep 17 00:00:00 2001 From: Shin-kyoto Date: Sun, 26 May 2024 18:46:51 +0900 Subject: [PATCH 08/28] test(traffic_light_fine_detector): remove unnecessary compiling Signed-off-by: Shin-kyoto --- perception/traffic_light_fine_detector/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/perception/traffic_light_fine_detector/CMakeLists.txt b/perception/traffic_light_fine_detector/CMakeLists.txt index 82dfcf1c4a7ef..e8fe6554f937f 100644 --- a/perception/traffic_light_fine_detector/CMakeLists.txt +++ b/perception/traffic_light_fine_detector/CMakeLists.txt @@ -102,7 +102,6 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) if(BUILD_TESTING) ament_auto_add_gtest(traffic_light_fine_detector_tests test/test_nodelet.cpp - src/nodelet.cpp ) endif() From a4160d52cce8a1e006642c4704e987619f8e8a4b Mon Sep 17 00:00:00 2001 From: Shin-kyoto Date: Wed, 29 May 2024 12:03:25 +0900 Subject: [PATCH 09/28] test(traffic_light_fine_detector): disable test that fails now Signed-off-by: Shin-kyoto --- .../test/test_nodelet.cpp | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/perception/traffic_light_fine_detector/test/test_nodelet.cpp b/perception/traffic_light_fine_detector/test/test_nodelet.cpp index d7db39f5d513b..20b8de4e92724 100644 --- a/perception/traffic_light_fine_detector/test/test_nodelet.cpp +++ b/perception/traffic_light_fine_detector/test/test_nodelet.cpp @@ -96,13 +96,13 @@ TEST(CalWeightedIouTest, Negative) EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.0f); } -// This test case should be passed but it fails because of inappropriate type casting. Current -// result is 0.1097561, but it should be 0.0. -// TEST(CalWeightedIouTest, Uint32Max) -// { -// sensor_msgs::msg::RegionOfInterest map_based_bbox = -// createMapBasedBbox(UINT32_MAX, UINT32_MAX, 10, 10); -// tensorrt_yolox::Object yolox_bbox = createYoloxBbox(-5, -5, 10, 10, 0.5f, 0); - -// EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.0f); -// } +// This test case should be passed but it fails because of inappropriate type casting. +// So it is disabled now. Current result is 0.1097561, but it should be 0.0. +TEST(CalWeightedIouTest, DISABLED_Uint32Max) +{ + sensor_msgs::msg::RegionOfInterest map_based_bbox = + createMapBasedBbox(UINT32_MAX, UINT32_MAX, 10, 10); + tensorrt_yolox::Object yolox_bbox = createYoloxBbox(-5, -5, 10, 10, 0.5f, 0); + + EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.0f); +} From c76b5fd5a337ce0a47966628423f8e1113cbaffb Mon Sep 17 00:00:00 2001 From: Shin-kyoto Date: Wed, 29 May 2024 13:29:41 +0900 Subject: [PATCH 10/28] test(traffic_light_fine_detector): add const to variables that do not change Signed-off-by: Shin-kyoto --- .../test/test_nodelet.cpp | 32 +++++++++---------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/perception/traffic_light_fine_detector/test/test_nodelet.cpp b/perception/traffic_light_fine_detector/test/test_nodelet.cpp index 20b8de4e92724..acbf123c06670 100644 --- a/perception/traffic_light_fine_detector/test/test_nodelet.cpp +++ b/perception/traffic_light_fine_detector/test/test_nodelet.cpp @@ -42,56 +42,56 @@ tensorrt_yolox::Object createYoloxBbox( TEST(CalWeightedIouTest, NoOverlap) { - sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(0, 0, 10, 10); - tensorrt_yolox::Object yolox_bbox = createYoloxBbox(20, 20, 10, 10, 0.9f, 0); + 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); EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.0f); } TEST(CalWeightedIouTest, PartiallyOverlap1) { - sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(15, 15, 10, 10); - tensorrt_yolox::Object yolox_bbox = createYoloxBbox(20, 20, 10, 10, 0.7f, 0); + const sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(15, 15, 10, 10); + const tensorrt_yolox::Object yolox_bbox = createYoloxBbox(20, 20, 10, 10, 0.7f, 0); EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.1f); } TEST(CalWeightedIouTest, PartiallyOverlap2) { - sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(0, 0, 10, 10); - tensorrt_yolox::Object yolox_bbox = createYoloxBbox(-5, -5, 10, 10, 0.7f, 0); + const sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(0, 0, 10, 10); + const tensorrt_yolox::Object yolox_bbox = createYoloxBbox(-5, -5, 10, 10, 0.7f, 0); EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.1f); } TEST(CalWeightedIouTest, Included1) { - sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(20, 20, 10, 10); - tensorrt_yolox::Object yolox_bbox = createYoloxBbox(20, 20, 10, 10, 0.5f, 0); + const sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(20, 20, 10, 10); + const tensorrt_yolox::Object yolox_bbox = createYoloxBbox(20, 20, 10, 10, 0.5f, 0); EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.5f); } TEST(CalWeightedIouTest, Included2) { - sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(20, 20, 100, 100); - tensorrt_yolox::Object yolox_bbox = createYoloxBbox(20, 20, 10, 10, 0.5f, 0); + const sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(20, 20, 100, 100); + const tensorrt_yolox::Object yolox_bbox = createYoloxBbox(20, 20, 10, 10, 0.5f, 0); EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.005f); } TEST(CalWeightedIouTest, Zero) { - sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(0, 0, 0, 0); - tensorrt_yolox::Object yolox_bbox = createYoloxBbox(0, 0, 0, 0, 0.5f, 0); + const sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(0, 0, 0, 0); + const tensorrt_yolox::Object yolox_bbox = createYoloxBbox(0, 0, 0, 0, 0.5f, 0); EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.0f); } TEST(CalWeightedIouTest, Negative) { - sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(0, 0, 10, 10); - tensorrt_yolox::Object yolox_bbox = createYoloxBbox(-5, -5, -5, 10, 0.5f, 0); + const sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(0, 0, 10, 10); + const tensorrt_yolox::Object yolox_bbox = createYoloxBbox(-5, -5, -5, 10, 0.5f, 0); EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.0f); } @@ -100,9 +100,9 @@ TEST(CalWeightedIouTest, Negative) // So it is disabled now. Current result is 0.1097561, but it should be 0.0. TEST(CalWeightedIouTest, DISABLED_Uint32Max) { - sensor_msgs::msg::RegionOfInterest map_based_bbox = + const sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(UINT32_MAX, UINT32_MAX, 10, 10); - tensorrt_yolox::Object yolox_bbox = createYoloxBbox(-5, -5, 10, 10, 0.5f, 0); + const tensorrt_yolox::Object yolox_bbox = createYoloxBbox(-5, -5, 10, 10, 0.5f, 0); EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.0f); } From 441a6ff41449bf8b12f805267e2cd1004429b0f6 Mon Sep 17 00:00:00 2001 From: Shin-kyoto Date: Wed, 29 May 2024 13:33:15 +0900 Subject: [PATCH 11/28] test(traffic_light_fine_detector): add const to the arguments Signed-off-by: Shin-kyoto --- perception/traffic_light_fine_detector/test/test_nodelet.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/perception/traffic_light_fine_detector/test/test_nodelet.cpp b/perception/traffic_light_fine_detector/test/test_nodelet.cpp index acbf123c06670..67345cd6d1a70 100644 --- a/perception/traffic_light_fine_detector/test/test_nodelet.cpp +++ b/perception/traffic_light_fine_detector/test/test_nodelet.cpp @@ -17,7 +17,7 @@ #include sensor_msgs::msg::RegionOfInterest createMapBasedBbox( - uint32_t x_offset, uint32_t y_offset, uint32_t width, uint32_t height) + 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; @@ -28,7 +28,8 @@ sensor_msgs::msg::RegionOfInterest createMapBasedBbox( } tensorrt_yolox::Object createYoloxBbox( - int32_t x_offset, int32_t y_offset, int32_t width, int32_t height, float score, int32_t type) + 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; From 327c3580f170f23690aa8ddf6d98467ec6e1b052 Mon Sep 17 00:00:00 2001 From: Shin-kyoto Date: Thu, 30 May 2024 15:29:14 +0900 Subject: [PATCH 12/28] chore: set CUDA_ARCHITECTURE. This commit should not be merged. Signed-off-by: Shin-kyoto --- perception/tensorrt_yolox/CMakeLists.txt | 3 +++ perception/traffic_light_fine_detector/CMakeLists.txt | 3 +++ 2 files changed, 6 insertions(+) diff --git a/perception/tensorrt_yolox/CMakeLists.txt b/perception/tensorrt_yolox/CMakeLists.txt index a5498a845e62e..740508d7b040c 100644 --- a/perception/tensorrt_yolox/CMakeLists.txt +++ b/perception/tensorrt_yolox/CMakeLists.txt @@ -1,6 +1,9 @@ cmake_minimum_required(VERSION 3.17) project(tensorrt_yolox) +cmake_policy(SET CMP0104 NEW) +set(CMAKE_CUDA_ARCHITECTURES 86) + find_package(tensorrt_common) if(NOT ${tensorrt_common_FOUND}) message(WARNING "The tensorrt_common package is not found. Please check its dependencies.") diff --git a/perception/traffic_light_fine_detector/CMakeLists.txt b/perception/traffic_light_fine_detector/CMakeLists.txt index e8fe6554f937f..53410148c5a65 100644 --- a/perception/traffic_light_fine_detector/CMakeLists.txt +++ b/perception/traffic_light_fine_detector/CMakeLists.txt @@ -1,6 +1,9 @@ cmake_minimum_required(VERSION 3.14) project(traffic_light_fine_detector) +cmake_policy(SET CMP0104 NEW) +set(CMAKE_CUDA_ARCHITECTURES 86) + find_package(autoware_cmake REQUIRED) autoware_package() From 3cabf1242277d5cf0a70f9025eb01ff2c264b99e Mon Sep 17 00:00:00 2001 From: Shin-kyoto Date: Thu, 30 May 2024 15:57:10 +0900 Subject: [PATCH 13/28] Revert "chore: set CUDA_ARCHITECTURE. This commit should not be merged." This reverts commit 327c3580f170f23690aa8ddf6d98467ec6e1b052. --- perception/tensorrt_yolox/CMakeLists.txt | 3 --- perception/traffic_light_fine_detector/CMakeLists.txt | 3 --- 2 files changed, 6 deletions(-) diff --git a/perception/tensorrt_yolox/CMakeLists.txt b/perception/tensorrt_yolox/CMakeLists.txt index 740508d7b040c..a5498a845e62e 100644 --- a/perception/tensorrt_yolox/CMakeLists.txt +++ b/perception/tensorrt_yolox/CMakeLists.txt @@ -1,9 +1,6 @@ cmake_minimum_required(VERSION 3.17) project(tensorrt_yolox) -cmake_policy(SET CMP0104 NEW) -set(CMAKE_CUDA_ARCHITECTURES 86) - find_package(tensorrt_common) if(NOT ${tensorrt_common_FOUND}) message(WARNING "The tensorrt_common package is not found. Please check its dependencies.") diff --git a/perception/traffic_light_fine_detector/CMakeLists.txt b/perception/traffic_light_fine_detector/CMakeLists.txt index 53410148c5a65..e8fe6554f937f 100644 --- a/perception/traffic_light_fine_detector/CMakeLists.txt +++ b/perception/traffic_light_fine_detector/CMakeLists.txt @@ -1,9 +1,6 @@ cmake_minimum_required(VERSION 3.14) project(traffic_light_fine_detector) -cmake_policy(SET CMP0104 NEW) -set(CMAKE_CUDA_ARCHITECTURES 86) - find_package(autoware_cmake REQUIRED) autoware_package() From a2477c7d4e796c0a11b6b9a6c4e818c165291db1 Mon Sep 17 00:00:00 2001 From: Shin-kyoto Date: Thu, 30 May 2024 16:56:15 +0900 Subject: [PATCH 14/28] chore: remove test code to check CI. this commit should not be merged Signed-off-by: Shin-kyoto --- .../CMakeLists.txt | 6 - .../test/test_nodelet.cpp | 109 ------------------ 2 files changed, 115 deletions(-) delete mode 100644 perception/traffic_light_fine_detector/test/test_nodelet.cpp diff --git a/perception/traffic_light_fine_detector/CMakeLists.txt b/perception/traffic_light_fine_detector/CMakeLists.txt index e8fe6554f937f..fe9fa2ffcaa8b 100644 --- a/perception/traffic_light_fine_detector/CMakeLists.txt +++ b/perception/traffic_light_fine_detector/CMakeLists.txt @@ -99,12 +99,6 @@ 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/test/test_nodelet.cpp b/perception/traffic_light_fine_detector/test/test_nodelet.cpp deleted file mode 100644 index 67345cd6d1a70..0000000000000 --- a/perception/traffic_light_fine_detector/test/test_nodelet.cpp +++ /dev/null @@ -1,109 +0,0 @@ -// 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); - - EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.0f); -} - -TEST(CalWeightedIouTest, PartiallyOverlap1) -{ - const sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(15, 15, 10, 10); - const tensorrt_yolox::Object yolox_bbox = createYoloxBbox(20, 20, 10, 10, 0.7f, 0); - - EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.1f); -} - -TEST(CalWeightedIouTest, PartiallyOverlap2) -{ - const sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(0, 0, 10, 10); - const tensorrt_yolox::Object yolox_bbox = createYoloxBbox(-5, -5, 10, 10, 0.7f, 0); - - EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.1f); -} - -TEST(CalWeightedIouTest, Included1) -{ - const sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(20, 20, 10, 10); - const tensorrt_yolox::Object yolox_bbox = createYoloxBbox(20, 20, 10, 10, 0.5f, 0); - - EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.5f); -} - -TEST(CalWeightedIouTest, Included2) -{ - const sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(20, 20, 100, 100); - const tensorrt_yolox::Object yolox_bbox = createYoloxBbox(20, 20, 10, 10, 0.5f, 0); - - EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.005f); -} - -TEST(CalWeightedIouTest, Zero) -{ - const sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(0, 0, 0, 0); - const tensorrt_yolox::Object yolox_bbox = createYoloxBbox(0, 0, 0, 0, 0.5f, 0); - - EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.0f); -} - -TEST(CalWeightedIouTest, Negative) -{ - const sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(0, 0, 10, 10); - const tensorrt_yolox::Object yolox_bbox = createYoloxBbox(-5, -5, -5, 10, 0.5f, 0); - - EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.0f); -} - -// This test case should be passed but it fails because of inappropriate type casting. -// So it is disabled now. Current result is 0.1097561, but it should be 0.0. -TEST(CalWeightedIouTest, DISABLED_Uint32Max) -{ - const sensor_msgs::msg::RegionOfInterest map_based_bbox = - createMapBasedBbox(UINT32_MAX, UINT32_MAX, 10, 10); - const tensorrt_yolox::Object yolox_bbox = createYoloxBbox(-5, -5, 10, 10, 0.5f, 0); - - EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.0f); -} From 0ff8bed383c1bb689beb5663986b71df3cc0ca58 Mon Sep 17 00:00:00 2001 From: Shin-kyoto Date: Thu, 30 May 2024 17:00:54 +0900 Subject: [PATCH 15/28] Revert "chore: remove test code to check CI. this commit should not be merged" This reverts commit a2477c7d4e796c0a11b6b9a6c4e818c165291db1. --- .../CMakeLists.txt | 6 + .../test/test_nodelet.cpp | 109 ++++++++++++++++++ 2 files changed, 115 insertions(+) create mode 100644 perception/traffic_light_fine_detector/test/test_nodelet.cpp 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/test/test_nodelet.cpp b/perception/traffic_light_fine_detector/test/test_nodelet.cpp new file mode 100644 index 0000000000000..67345cd6d1a70 --- /dev/null +++ b/perception/traffic_light_fine_detector/test/test_nodelet.cpp @@ -0,0 +1,109 @@ +// 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); + + EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.0f); +} + +TEST(CalWeightedIouTest, PartiallyOverlap1) +{ + const sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(15, 15, 10, 10); + const tensorrt_yolox::Object yolox_bbox = createYoloxBbox(20, 20, 10, 10, 0.7f, 0); + + EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.1f); +} + +TEST(CalWeightedIouTest, PartiallyOverlap2) +{ + const sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(0, 0, 10, 10); + const tensorrt_yolox::Object yolox_bbox = createYoloxBbox(-5, -5, 10, 10, 0.7f, 0); + + EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.1f); +} + +TEST(CalWeightedIouTest, Included1) +{ + const sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(20, 20, 10, 10); + const tensorrt_yolox::Object yolox_bbox = createYoloxBbox(20, 20, 10, 10, 0.5f, 0); + + EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.5f); +} + +TEST(CalWeightedIouTest, Included2) +{ + const sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(20, 20, 100, 100); + const tensorrt_yolox::Object yolox_bbox = createYoloxBbox(20, 20, 10, 10, 0.5f, 0); + + EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.005f); +} + +TEST(CalWeightedIouTest, Zero) +{ + const sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(0, 0, 0, 0); + const tensorrt_yolox::Object yolox_bbox = createYoloxBbox(0, 0, 0, 0, 0.5f, 0); + + EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.0f); +} + +TEST(CalWeightedIouTest, Negative) +{ + const sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(0, 0, 10, 10); + const tensorrt_yolox::Object yolox_bbox = createYoloxBbox(-5, -5, -5, 10, 0.5f, 0); + + EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.0f); +} + +// This test case should be passed but it fails because of inappropriate type casting. +// So it is disabled now. Current result is 0.1097561, but it should be 0.0. +TEST(CalWeightedIouTest, DISABLED_Uint32Max) +{ + const sensor_msgs::msg::RegionOfInterest map_based_bbox = + createMapBasedBbox(UINT32_MAX, UINT32_MAX, 10, 10); + const tensorrt_yolox::Object yolox_bbox = createYoloxBbox(-5, -5, 10, 10, 0.5f, 0); + + EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.0f); +} From 145bf55df3eeedad5866d379bd8d153bb3367831 Mon Sep 17 00:00:00 2001 From: Shin-kyoto Date: Thu, 30 May 2024 17:10:51 +0900 Subject: [PATCH 16/28] feat(traffic_light_fine_detector): add link library for test Signed-off-by: Shin-kyoto --- perception/traffic_light_fine_detector/CMakeLists.txt | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/perception/traffic_light_fine_detector/CMakeLists.txt b/perception/traffic_light_fine_detector/CMakeLists.txt index e8fe6554f937f..39c23768102ce 100644 --- a/perception/traffic_light_fine_detector/CMakeLists.txt +++ b/perception/traffic_light_fine_detector/CMakeLists.txt @@ -103,6 +103,16 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) ament_auto_add_gtest(traffic_light_fine_detector_tests test/test_nodelet.cpp ) + target_link_libraries(traffic_light_fine_detector_tests + ${NVINFER} + ${NVONNXPARSER} + ${NVINFER_PLUGIN} + ${CUDA_LIBRARIES} + ${CUBLAS_LIBRARIES} + ${CUDNN_LIBRARY} + ${OpenCV_LIB} + stdc++fs + ) endif() ament_auto_package(INSTALL_TO_SHARE From 7777709224765af9d86a5e133dc0def0cd289a05 Mon Sep 17 00:00:00 2001 From: Shin-kyoto Date: Thu, 30 May 2024 18:20:20 +0900 Subject: [PATCH 17/28] feat(traffic_light_fine_detector): add target directories for test Signed-off-by: Shin-kyoto --- perception/traffic_light_fine_detector/CMakeLists.txt | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/perception/traffic_light_fine_detector/CMakeLists.txt b/perception/traffic_light_fine_detector/CMakeLists.txt index 39c23768102ce..27488e14fb9b8 100644 --- a/perception/traffic_light_fine_detector/CMakeLists.txt +++ b/perception/traffic_light_fine_detector/CMakeLists.txt @@ -103,6 +103,11 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) ament_auto_add_gtest(traffic_light_fine_detector_tests test/test_nodelet.cpp ) + + target_include_directories(traffic_light_fine_detector_tests PUBLIC + lib/include + ) + target_link_libraries(traffic_light_fine_detector_tests ${NVINFER} ${NVONNXPARSER} From 85aeaa43718c4b69f4c42653163dd8a14e7b56ac Mon Sep 17 00:00:00 2001 From: Shin-kyoto Date: Thu, 30 May 2024 19:03:10 +0900 Subject: [PATCH 18/28] Revert "feat(traffic_light_fine_detector): move definition of util function to cpp" This reverts commit 761a221985287b0e480bcf6a0bedf2399c76bd54. --- .../traffic_light_fine_detector/nodelet.hpp | 14 +++++++++++++- .../traffic_light_fine_detector/src/nodelet.cpp | 17 +---------------- 2 files changed, 14 insertions(+), 17 deletions(-) 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 bd145e5026409..d117e200e0c45 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 @@ -53,7 +53,19 @@ typedef struct Detection namespace traffic_light { float calWeightedIou( - const sensor_msgs::msg::RegionOfInterest & bbox1, const tensorrt_yolox::Object & bbox2); + const sensor_msgs::msg::RegionOfInterest & bbox1, const tensorrt_yolox::Object & bbox2) +{ + int x1 = std::max(static_cast(bbox1.x_offset), bbox2.x_offset); + int x2 = std::min(static_cast(bbox1.x_offset + bbox1.width), bbox2.x_offset + bbox2.width); + int y1 = std::max(static_cast(bbox1.y_offset), bbox2.y_offset); + int y2 = std::min(static_cast(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 bbox2.score * area1 / area2; +} class TrafficLightFineDetectorNodelet : public rclcpp::Node { diff --git a/perception/traffic_light_fine_detector/src/nodelet.cpp b/perception/traffic_light_fine_detector/src/nodelet.cpp index 28f3fafabc6c0..b4c815b62ba01 100644 --- a/perception/traffic_light_fine_detector/src/nodelet.cpp +++ b/perception/traffic_light_fine_detector/src/nodelet.cpp @@ -29,21 +29,6 @@ namespace fs = ::std::experimental::filesystem; namespace traffic_light { -float calWeightedIou( - const sensor_msgs::msg::RegionOfInterest & bbox1, const tensorrt_yolox::Object & bbox2) -{ - int x1 = std::max(static_cast(bbox1.x_offset), bbox2.x_offset); - int x2 = std::min(static_cast(bbox1.x_offset + bbox1.width), bbox2.x_offset + bbox2.width); - int y1 = std::max(static_cast(bbox1.y_offset), bbox2.y_offset); - int y2 = std::min(static_cast(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 bbox2.score * area1 / area2; -} - inline std::vector toFloatVector(const std::vector double_vector) { return std::vector(double_vector.begin(), double_vector.end()); @@ -221,7 +206,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 = traffic_light::calWeightedIou(expected_roi, detection); if (score >= max_score) { max_score = score; id2bestDetection[tlr_id] = detection; From 0ffe4dba322cbd5ab1f92833f166925659541758 Mon Sep 17 00:00:00 2001 From: Shin-kyoto Date: Thu, 30 May 2024 19:03:33 +0900 Subject: [PATCH 19/28] Revert "feat(traffic_light_fine_detector): update function for waited IoU" This reverts commit 30467aa3c9e2badf39e06bcbda69af5288f8dfde. --- .../traffic_light_fine_detector/nodelet.hpp | 15 ------------- .../src/nodelet.cpp | 21 ++++++++++++++++++- 2 files changed, 20 insertions(+), 16 deletions(-) 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 d117e200e0c45..5c89c76a11833 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,21 +52,6 @@ typedef struct Detection namespace traffic_light { -float calWeightedIou( - const sensor_msgs::msg::RegionOfInterest & bbox1, const tensorrt_yolox::Object & bbox2) -{ - int x1 = std::max(static_cast(bbox1.x_offset), bbox2.x_offset); - int x2 = std::min(static_cast(bbox1.x_offset + bbox1.width), bbox2.x_offset + bbox2.width); - int y1 = std::max(static_cast(bbox1.y_offset), bbox2.y_offset); - int y2 = std::min(static_cast(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 bbox2.score * area1 / area2; -} - 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 b4c815b62ba01..8037dc5472fbe 100644 --- a/perception/traffic_light_fine_detector/src/nodelet.cpp +++ b/perception/traffic_light_fine_detector/src/nodelet.cpp @@ -27,6 +27,25 @@ namespace fs = ::std::experimental::filesystem; #include #include +namespace +{ +float calWeightedIou( + const sensor_msgs::msg::RegionOfInterest & bbox1, const tensorrt_yolox::Object & bbox2) +{ + int x1 = std::max(static_cast(bbox1.x_offset), bbox2.x_offset); + int x2 = std::min(static_cast(bbox1.x_offset + bbox1.width), bbox2.x_offset + bbox2.width); + int y1 = std::max(static_cast(bbox1.y_offset), bbox2.y_offset); + int y2 = std::min(static_cast(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 bbox2.score * area1 / area2; +} + +} // namespace + namespace traffic_light { inline std::vector toFloatVector(const std::vector double_vector) @@ -206,7 +225,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 = traffic_light::calWeightedIou(expected_roi, detection); + float score = ::calWeightedIou(expected_roi, detection); if (score >= max_score) { max_score = score; id2bestDetection[tlr_id] = detection; From 0129f28702907e8a185dc1a9b335fa2417764808 Mon Sep 17 00:00:00 2001 From: Shin-kyoto Date: Thu, 30 May 2024 19:04:36 +0900 Subject: [PATCH 20/28] Revert "feat(traffic_light_fine_detector): add target directories for test" This reverts commit 7777709224765af9d86a5e133dc0def0cd289a05. --- perception/traffic_light_fine_detector/CMakeLists.txt | 5 ----- 1 file changed, 5 deletions(-) diff --git a/perception/traffic_light_fine_detector/CMakeLists.txt b/perception/traffic_light_fine_detector/CMakeLists.txt index 27488e14fb9b8..39c23768102ce 100644 --- a/perception/traffic_light_fine_detector/CMakeLists.txt +++ b/perception/traffic_light_fine_detector/CMakeLists.txt @@ -103,11 +103,6 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) ament_auto_add_gtest(traffic_light_fine_detector_tests test/test_nodelet.cpp ) - - target_include_directories(traffic_light_fine_detector_tests PUBLIC - lib/include - ) - target_link_libraries(traffic_light_fine_detector_tests ${NVINFER} ${NVONNXPARSER} From f54a2bb920ac04eae78f1671d846a7100cb80a0b Mon Sep 17 00:00:00 2001 From: Shin-kyoto Date: Thu, 30 May 2024 19:04:37 +0900 Subject: [PATCH 21/28] Revert "feat(traffic_light_fine_detector): add link library for test" This reverts commit 145bf55df3eeedad5866d379bd8d153bb3367831. --- perception/traffic_light_fine_detector/CMakeLists.txt | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/perception/traffic_light_fine_detector/CMakeLists.txt b/perception/traffic_light_fine_detector/CMakeLists.txt index 39c23768102ce..e8fe6554f937f 100644 --- a/perception/traffic_light_fine_detector/CMakeLists.txt +++ b/perception/traffic_light_fine_detector/CMakeLists.txt @@ -103,16 +103,6 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) ament_auto_add_gtest(traffic_light_fine_detector_tests test/test_nodelet.cpp ) - target_link_libraries(traffic_light_fine_detector_tests - ${NVINFER} - ${NVONNXPARSER} - ${NVINFER_PLUGIN} - ${CUDA_LIBRARIES} - ${CUBLAS_LIBRARIES} - ${CUDNN_LIBRARY} - ${OpenCV_LIB} - stdc++fs - ) endif() ament_auto_package(INSTALL_TO_SHARE From db9db2a23e29a7c57e7c56db521744c2bc2b3764 Mon Sep 17 00:00:00 2001 From: Shin-kyoto Date: Thu, 30 May 2024 19:05:31 +0900 Subject: [PATCH 22/28] feat(traffic_light_fine_detector): remove test cases Signed-off-by: Shin-kyoto --- .../test/test_nodelet.cpp | 80 ------------------- 1 file changed, 80 deletions(-) diff --git a/perception/traffic_light_fine_detector/test/test_nodelet.cpp b/perception/traffic_light_fine_detector/test/test_nodelet.cpp index 67345cd6d1a70..0912c32e2c204 100644 --- a/perception/traffic_light_fine_detector/test/test_nodelet.cpp +++ b/perception/traffic_light_fine_detector/test/test_nodelet.cpp @@ -14,19 +14,6 @@ #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) @@ -40,70 +27,3 @@ tensorrt_yolox::Object createYoloxBbox( 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); - - EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.0f); -} - -TEST(CalWeightedIouTest, PartiallyOverlap1) -{ - const sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(15, 15, 10, 10); - const tensorrt_yolox::Object yolox_bbox = createYoloxBbox(20, 20, 10, 10, 0.7f, 0); - - EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.1f); -} - -TEST(CalWeightedIouTest, PartiallyOverlap2) -{ - const sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(0, 0, 10, 10); - const tensorrt_yolox::Object yolox_bbox = createYoloxBbox(-5, -5, 10, 10, 0.7f, 0); - - EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.1f); -} - -TEST(CalWeightedIouTest, Included1) -{ - const sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(20, 20, 10, 10); - const tensorrt_yolox::Object yolox_bbox = createYoloxBbox(20, 20, 10, 10, 0.5f, 0); - - EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.5f); -} - -TEST(CalWeightedIouTest, Included2) -{ - const sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(20, 20, 100, 100); - const tensorrt_yolox::Object yolox_bbox = createYoloxBbox(20, 20, 10, 10, 0.5f, 0); - - EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.005f); -} - -TEST(CalWeightedIouTest, Zero) -{ - const sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(0, 0, 0, 0); - const tensorrt_yolox::Object yolox_bbox = createYoloxBbox(0, 0, 0, 0, 0.5f, 0); - - EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.0f); -} - -TEST(CalWeightedIouTest, Negative) -{ - const sensor_msgs::msg::RegionOfInterest map_based_bbox = createMapBasedBbox(0, 0, 10, 10); - const tensorrt_yolox::Object yolox_bbox = createYoloxBbox(-5, -5, -5, 10, 0.5f, 0); - - EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.0f); -} - -// This test case should be passed but it fails because of inappropriate type casting. -// So it is disabled now. Current result is 0.1097561, but it should be 0.0. -TEST(CalWeightedIouTest, DISABLED_Uint32Max) -{ - const sensor_msgs::msg::RegionOfInterest map_based_bbox = - createMapBasedBbox(UINT32_MAX, UINT32_MAX, 10, 10); - const tensorrt_yolox::Object yolox_bbox = createYoloxBbox(-5, -5, 10, 10, 0.5f, 0); - - EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.0f); -} From a542a98f1e66e0f594a462290b116818887a342b Mon Sep 17 00:00:00 2001 From: Shin-kyoto Date: Thu, 30 May 2024 19:39:10 +0900 Subject: [PATCH 23/28] feat(traffic_light_fine_detector): add test function Signed-off-by: Shin-kyoto --- .../test/test_nodelet.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/perception/traffic_light_fine_detector/test/test_nodelet.cpp b/perception/traffic_light_fine_detector/test/test_nodelet.cpp index 0912c32e2c204..99b90f8b74881 100644 --- a/perception/traffic_light_fine_detector/test/test_nodelet.cpp +++ b/perception/traffic_light_fine_detector/test/test_nodelet.cpp @@ -14,6 +14,19 @@ #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) From 2f6f80cc78d768cd0e78b7b9201b1f0b7d3391ee Mon Sep 17 00:00:00 2001 From: Shin-kyoto Date: Thu, 30 May 2024 20:11:23 +0900 Subject: [PATCH 24/28] Revert "Revert "feat(traffic_light_fine_detector): update function for waited IoU"" This reverts commit 0ffe4dba322cbd5ab1f92833f166925659541758. --- .../traffic_light_fine_detector/nodelet.hpp | 15 +++++++++++++ .../src/nodelet.cpp | 21 +------------------ 2 files changed, 16 insertions(+), 20 deletions(-) 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..d117e200e0c45 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,21 @@ typedef struct Detection namespace traffic_light { +float calWeightedIou( + const sensor_msgs::msg::RegionOfInterest & bbox1, const tensorrt_yolox::Object & bbox2) +{ + int x1 = std::max(static_cast(bbox1.x_offset), bbox2.x_offset); + int x2 = std::min(static_cast(bbox1.x_offset + bbox1.width), bbox2.x_offset + bbox2.width); + int y1 = std::max(static_cast(bbox1.y_offset), bbox2.y_offset); + int y2 = std::min(static_cast(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 bbox2.score * area1 / area2; +} + 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..b4c815b62ba01 100644 --- a/perception/traffic_light_fine_detector/src/nodelet.cpp +++ b/perception/traffic_light_fine_detector/src/nodelet.cpp @@ -27,25 +27,6 @@ namespace fs = ::std::experimental::filesystem; #include #include -namespace -{ -float calWeightedIou( - const sensor_msgs::msg::RegionOfInterest & bbox1, const tensorrt_yolox::Object & bbox2) -{ - int x1 = std::max(static_cast(bbox1.x_offset), bbox2.x_offset); - int x2 = std::min(static_cast(bbox1.x_offset + bbox1.width), bbox2.x_offset + bbox2.width); - int y1 = std::max(static_cast(bbox1.y_offset), bbox2.y_offset); - int y2 = std::min(static_cast(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 bbox2.score * area1 / area2; -} - -} // namespace - namespace traffic_light { inline std::vector toFloatVector(const std::vector double_vector) @@ -225,7 +206,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 = traffic_light::calWeightedIou(expected_roi, detection); if (score >= max_score) { max_score = score; id2bestDetection[tlr_id] = detection; From 4c941e37424d7040742735640927f99146b7dbc4 Mon Sep 17 00:00:00 2001 From: Shin-kyoto Date: Thu, 30 May 2024 20:11:24 +0900 Subject: [PATCH 25/28] Revert "Revert "feat(traffic_light_fine_detector): move definition of util function to cpp"" This reverts commit 85aeaa43718c4b69f4c42653163dd8a14e7b56ac. --- .../traffic_light_fine_detector/nodelet.hpp | 14 +------------- .../traffic_light_fine_detector/src/nodelet.cpp | 17 ++++++++++++++++- 2 files changed, 17 insertions(+), 14 deletions(-) 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 d117e200e0c45..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 @@ -53,19 +53,7 @@ typedef struct Detection namespace traffic_light { float calWeightedIou( - const sensor_msgs::msg::RegionOfInterest & bbox1, const tensorrt_yolox::Object & bbox2) -{ - int x1 = std::max(static_cast(bbox1.x_offset), bbox2.x_offset); - int x2 = std::min(static_cast(bbox1.x_offset + bbox1.width), bbox2.x_offset + bbox2.width); - int y1 = std::max(static_cast(bbox1.y_offset), bbox2.y_offset); - int y2 = std::min(static_cast(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 bbox2.score * area1 / area2; -} + const sensor_msgs::msg::RegionOfInterest & bbox1, const tensorrt_yolox::Object & bbox2); class TrafficLightFineDetectorNodelet : public rclcpp::Node { diff --git a/perception/traffic_light_fine_detector/src/nodelet.cpp b/perception/traffic_light_fine_detector/src/nodelet.cpp index b4c815b62ba01..28f3fafabc6c0 100644 --- a/perception/traffic_light_fine_detector/src/nodelet.cpp +++ b/perception/traffic_light_fine_detector/src/nodelet.cpp @@ -29,6 +29,21 @@ namespace fs = ::std::experimental::filesystem; namespace traffic_light { +float calWeightedIou( + const sensor_msgs::msg::RegionOfInterest & bbox1, const tensorrt_yolox::Object & bbox2) +{ + int x1 = std::max(static_cast(bbox1.x_offset), bbox2.x_offset); + int x2 = std::min(static_cast(bbox1.x_offset + bbox1.width), bbox2.x_offset + bbox2.width); + int y1 = std::max(static_cast(bbox1.y_offset), bbox2.y_offset); + int y2 = std::min(static_cast(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 bbox2.score * area1 / area2; +} + inline std::vector toFloatVector(const std::vector double_vector) { return std::vector(double_vector.begin(), double_vector.end()); @@ -206,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 = traffic_light::calWeightedIou(expected_roi, detection); + float score = calWeightedIou(expected_roi, detection); if (score >= max_score) { max_score = score; id2bestDetection[tlr_id] = detection; From afb99539898beb287bd24bb41354e9027e74c07e Mon Sep 17 00:00:00 2001 From: Shin-kyoto Date: Thu, 30 May 2024 20:38:13 +0900 Subject: [PATCH 26/28] feat(traffic_light_fine_detector): add test case Signed-off-by: Shin-kyoto --- .../traffic_light_fine_detector/test/test_nodelet.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/perception/traffic_light_fine_detector/test/test_nodelet.cpp b/perception/traffic_light_fine_detector/test/test_nodelet.cpp index 99b90f8b74881..7d50c1c4068ee 100644 --- a/perception/traffic_light_fine_detector/test/test_nodelet.cpp +++ b/perception/traffic_light_fine_detector/test/test_nodelet.cpp @@ -40,3 +40,11 @@ tensorrt_yolox::Object createYoloxBbox( 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); + + EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.0f); +} From ff5aed1bdfa6cee65e4b4cc6bc94236737764df7 Mon Sep 17 00:00:00 2001 From: Shin-kyoto Date: Fri, 31 May 2024 13:55:50 +0900 Subject: [PATCH 27/28] feat(traffic_light_fine_detector): add code for checking ci Signed-off-by: Shin-kyoto --- perception/traffic_light_fine_detector/test/test_nodelet.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/perception/traffic_light_fine_detector/test/test_nodelet.cpp b/perception/traffic_light_fine_detector/test/test_nodelet.cpp index 7d50c1c4068ee..42a5bdd9a8608 100644 --- a/perception/traffic_light_fine_detector/test/test_nodelet.cpp +++ b/perception/traffic_light_fine_detector/test/test_nodelet.cpp @@ -46,5 +46,8 @@ 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); - EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.0f); + // check size of map_based_bbox and yolox_bbox + EXPECT_EQ(map_based_bbox.width, 10); + EXPECT_EQ(yolox_bbox.width, 10); + // EXPECT_FLOAT_EQ(traffic_light::calWeightedIou(map_based_bbox, yolox_bbox), 0.0f); } From 9870ac998b35a12e17ac9b9e6ab06d9d3db8199c Mon Sep 17 00:00:00 2001 From: Shin-kyoto Date: Fri, 31 May 2024 14:39:23 +0900 Subject: [PATCH 28/28] feat(traffic_light_fine_detector): add code for checking ci Signed-off-by: Shin-kyoto --- perception/traffic_light_fine_detector/test/test_nodelet.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/perception/traffic_light_fine_detector/test/test_nodelet.cpp b/perception/traffic_light_fine_detector/test/test_nodelet.cpp index 42a5bdd9a8608..f45d3351d025d 100644 --- a/perception/traffic_light_fine_detector/test/test_nodelet.cpp +++ b/perception/traffic_light_fine_detector/test/test_nodelet.cpp @@ -49,5 +49,7 @@ TEST(CalWeightedIouTest, NoOverlap) // 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); }