diff --git a/perception/autoware_bevfusion/CMakeLists.txt b/perception/autoware_bevfusion/CMakeLists.txt index 01e1e79731b19..2ec05c057e21b 100644 --- a/perception/autoware_bevfusion/CMakeLists.txt +++ b/perception/autoware_bevfusion/CMakeLists.txt @@ -165,6 +165,26 @@ if(TRT_AVAIL AND CUDA_AVAIL AND SPCONV_AVAIL) DESTINATION lib ) + if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(test_detection_class_remapper + test/test_detection_class_remapper.cpp + lib/detection_class_remapper.cpp + ) + target_include_directories(test_detection_class_remapper PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/include + ) + target_link_libraries(test_detection_class_remapper + Eigen3::Eigen + ) + ament_target_dependencies(test_detection_class_remapper + autoware_perception_msgs + ) + endif() + ament_auto_package( INSTALL_TO_SHARE launch @@ -175,6 +195,27 @@ else() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() + if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + find_package(Eigen3 3.3 REQUIRED NO_MODULE) + ament_add_gtest(test_detection_class_remapper + test/test_detection_class_remapper.cpp + lib/detection_class_remapper.cpp + ) + target_include_directories(test_detection_class_remapper PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/include + ) + target_link_libraries(test_detection_class_remapper + Eigen3::Eigen + ) + ament_target_dependencies(test_detection_class_remapper + autoware_perception_msgs + ) + endif() + ament_auto_package( INSTALL_TO_SHARE launch diff --git a/perception/autoware_bevfusion/include/autoware/bevfusion/detection_class_remapper.hpp b/perception/autoware_bevfusion/include/autoware/bevfusion/detection_class_remapper.hpp index 021134476078b..bf08848a8f2a5 100644 --- a/perception/autoware_bevfusion/include/autoware/bevfusion/detection_class_remapper.hpp +++ b/perception/autoware_bevfusion/include/autoware/bevfusion/detection_class_remapper.hpp @@ -40,7 +40,7 @@ class DetectionClassRemapper Eigen::Matrix allow_remapping_by_area_matrix_; Eigen::MatrixXd min_area_matrix_; Eigen::MatrixXd max_area_matrix_; - int num_labels_; + int num_labels_{0}; }; } // namespace autoware::bevfusion diff --git a/perception/autoware_bevfusion/lib/detection_class_remapper.cpp b/perception/autoware_bevfusion/lib/detection_class_remapper.cpp index 32ef66c477acf..e818873289267 100644 --- a/perception/autoware_bevfusion/lib/detection_class_remapper.cpp +++ b/perception/autoware_bevfusion/lib/detection_class_remapper.cpp @@ -14,22 +14,106 @@ #include +#include #include #include +#include +#include #include + namespace autoware::bevfusion { -void DetectionClassRemapper::setParameters( +namespace +{ + +void validateSameSize( + const std::size_t lhs_size, const std::size_t rhs_size, const char * lhs_name, + const char * rhs_name) +{ + if (lhs_size != rhs_size) { + throw std::invalid_argument( + std::string(lhs_name) + " and " + rhs_name + " must have the same size"); + } +} + +std::size_t validateNonEmptyMatrixSize(const std::size_t matrix_size) +{ + if (matrix_size == 0U) { + throw std::invalid_argument("Detection class remapper matrices must not be empty"); + } + + return matrix_size; +} + +int getSquareMatrixDimension(const std::size_t matrix_size) +{ + const auto num_labels = static_cast(std::sqrt(matrix_size)); + if (static_cast(num_labels) * static_cast(num_labels) != matrix_size) { + throw std::invalid_argument("Detection class remapper matrices must define a square matrix"); + } + + return num_labels; +} + +int validateAndGetNumLabels( const std::vector & allow_remapping_by_area_matrix, const std::vector & min_area_matrix, const std::vector & max_area_matrix) { - assert(allow_remapping_by_area_matrix.size() == min_area_matrix.size()); - assert(allow_remapping_by_area_matrix.size() == max_area_matrix.size()); - assert( - std::pow(static_cast(std::sqrt(min_area_matrix.size())), 2) == min_area_matrix.size()); + validateSameSize( + allow_remapping_by_area_matrix.size(), min_area_matrix.size(), "allow_remapping_by_area_matrix", + "min_area_matrix"); + validateSameSize( + allow_remapping_by_area_matrix.size(), max_area_matrix.size(), "allow_remapping_by_area_matrix", + "max_area_matrix"); + + return getSquareMatrixDimension(validateNonEmptyMatrixSize(min_area_matrix.size())); +} + +struct RemapContext +{ + int num_labels; + const Eigen::Matrix & allow_remapping_by_area_matrix; + const Eigen::MatrixXd & min_area_matrix; + const Eigen::MatrixXd & max_area_matrix; + + bool isLabelOutsideConfiguredMatrix(const std::uint8_t label) const + { + return static_cast(label) >= num_labels; + } + + bool shouldRemapLabel( + const int source_label, const int target_label, const float bev_area) const + { + return allow_remapping_by_area_matrix(source_label, target_label) && + bev_area >= min_area_matrix(source_label, target_label) && + bev_area <= max_area_matrix(source_label, target_label); + } + + std::uint8_t remapLabel(const std::uint8_t label, const float bev_area) const + { + if (isLabelOutsideConfiguredMatrix(label)) { + return label; + } + + for (int target_label = 0; target_label < num_labels; ++target_label) { + if (shouldRemapLabel(label, target_label, bev_area)) { + return static_cast(target_label); + } + } + + return label; + } +}; - num_labels_ = static_cast(std::sqrt(min_area_matrix.size())); +} // namespace + +void DetectionClassRemapper::setParameters( + const std::vector & allow_remapping_by_area_matrix, + const std::vector & min_area_matrix, const std::vector & max_area_matrix) +{ + num_labels_ = + validateAndGetNumLabels(allow_remapping_by_area_matrix, min_area_matrix, max_area_matrix); Eigen::Map> allow_remapping_by_area_matrix_tmp( @@ -53,20 +137,14 @@ void DetectionClassRemapper::setParameters( void DetectionClassRemapper::mapClasses(autoware_perception_msgs::msg::DetectedObjects & msg) { + const RemapContext context{ + num_labels_, allow_remapping_by_area_matrix_, min_area_matrix_, max_area_matrix_}; + for (auto & object : msg.objects) { const float bev_area = object.shape.dimensions.x * object.shape.dimensions.y; for (auto & classification : object.classification) { - auto & label = classification.label; - - for (int i = 0; i < num_labels_; ++i) { - if ( - allow_remapping_by_area_matrix_(label, i) && bev_area >= min_area_matrix_(label, i) && - bev_area <= max_area_matrix_(label, i)) { - label = i; - break; - } - } + classification.label = context.remapLabel(classification.label, bev_area); } } } diff --git a/perception/autoware_bevfusion/test/test_detection_class_remapper.cpp b/perception/autoware_bevfusion/test/test_detection_class_remapper.cpp new file mode 100644 index 0000000000000..11dacf8dff04c --- /dev/null +++ b/perception/autoware_bevfusion/test/test_detection_class_remapper.cpp @@ -0,0 +1,175 @@ +// Copyright 2026 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 + +#include +#include + +#include +#include +#include +#include +#include + +namespace +{ + +using autoware::bevfusion::DetectionClassRemapper; +using autoware_perception_msgs::msg::DetectedObject; +using autoware_perception_msgs::msg::DetectedObjects; +using autoware_perception_msgs::msg::ObjectClassification; + +struct InvalidRemapperConfig +{ + std::vector allow_remapping_by_area_matrix; + std::vector min_area_matrix; + std::vector max_area_matrix; + std::string expected_error_substring; +}; + +std::tuple, std::vector, std::vector> +makeVehicleRemapperParameters() +{ + constexpr std::size_t kNumClasses = 8; + std::vector allow_remapping_by_area_matrix(kNumClasses * kNumClasses, 0); + std::vector min_area_matrix(kNumClasses * kNumClasses, 0.0); + std::vector max_area_matrix(kNumClasses * kNumClasses, 0.0); + + auto set_rule = + [&](std::uint8_t source_label, std::uint8_t target_label, double min_area, double max_area) { + const auto index = static_cast(source_label) * kNumClasses + target_label; + allow_remapping_by_area_matrix.at(index) = 1; + min_area_matrix.at(index) = min_area; + max_area_matrix.at(index) = max_area; + }; + + set_rule(ObjectClassification::CAR, ObjectClassification::TRUCK, 12.1, 36.0); + set_rule(ObjectClassification::CAR, ObjectClassification::TRAILER, 36.0, 999.0); + set_rule(ObjectClassification::TRUCK, ObjectClassification::TRAILER, 36.0, 999.0); + + return { + allow_remapping_by_area_matrix, + min_area_matrix, + max_area_matrix}; +} + +DetectedObject makeObject(double length, double width, std::uint8_t label) +{ + DetectedObject object; + object.shape.dimensions.x = length; + object.shape.dimensions.y = width; + + ObjectClassification classification; + classification.label = label; + classification.probability = 1.0f; + object.classification = {classification}; + + return object; +} + +void expectSetParametersThrows( + const InvalidRemapperConfig & invalid_config, const std::string & expected_error_substring) +{ + DetectionClassRemapper remapper; + + try { + remapper.setParameters( + invalid_config.allow_remapping_by_area_matrix, invalid_config.min_area_matrix, + invalid_config.max_area_matrix); + FAIL() << "Expected std::invalid_argument"; + } catch (const std::invalid_argument & error) { + EXPECT_NE(std::string(error.what()).find(expected_error_substring), std::string::npos); + } +} + +class DetectionClassRemapperInvalidConfigTest +: public ::testing::TestWithParam +{ +}; + +TEST(DetectionClassRemapperTest, MapClasses) +{ + DetectionClassRemapper remapper; + + const auto [allow_remapping_by_area_matrix, min_area_matrix, max_area_matrix] = + makeVehicleRemapperParameters(); + + remapper.setParameters(allow_remapping_by_area_matrix, min_area_matrix, max_area_matrix); + + DetectedObjects msg; + msg.objects.push_back(makeObject(2.0, 2.0, ObjectClassification::CAR)); + msg.objects.push_back(makeObject(8.0, 2.0, ObjectClassification::CAR)); + msg.objects.push_back(makeObject(10.0, 4.0, ObjectClassification::TRUCK)); + msg.objects.push_back(makeObject(1.0, 1.0, ObjectClassification::PEDESTRIAN)); + + remapper.mapClasses(msg); + + EXPECT_EQ(msg.objects[0].classification[0].label, ObjectClassification::CAR); + EXPECT_EQ(msg.objects[1].classification[0].label, ObjectClassification::TRUCK); + EXPECT_EQ(msg.objects[2].classification[0].label, ObjectClassification::TRAILER); + EXPECT_EQ(msg.objects[3].classification[0].label, ObjectClassification::PEDESTRIAN); +} + +TEST_P(DetectionClassRemapperInvalidConfigTest, SetParametersThrowsOnInvalidConfig) +{ + const auto & invalid_config = GetParam(); + expectSetParametersThrows(invalid_config, invalid_config.expected_error_substring); +} + +TEST(DetectionClassRemapperTest, MapClassesSkipsLabelsOutsideConfiguredMatrix) +{ + DetectionClassRemapper remapper; + + const auto [allow_remapping_by_area_matrix, min_area_matrix, max_area_matrix] = + makeVehicleRemapperParameters(); + + remapper.setParameters(allow_remapping_by_area_matrix, min_area_matrix, max_area_matrix); + + DetectedObjects msg; + msg.objects.push_back(makeObject(6.0, 2.0, ObjectClassification::HAZARD)); + msg.objects.push_back(makeObject(8.0, 2.0, ObjectClassification::CAR)); + + remapper.mapClasses(msg); + + EXPECT_EQ(msg.objects[0].classification[0].label, ObjectClassification::HAZARD); + EXPECT_EQ(msg.objects[1].classification[0].label, ObjectClassification::TRUCK); +} + +INSTANTIATE_TEST_SUITE_P( + InvalidConfigs, DetectionClassRemapperInvalidConfigTest, + ::testing::Values( + InvalidRemapperConfig{ + {0, 1, 0, 1}, + {0.0, 1.0, 2.0}, + {0.0, 1.0, 2.0, 3.0}, + "allow_remapping_by_area_matrix and min_area_matrix"}, + InvalidRemapperConfig{ + {}, + {}, + {}, + "must not be empty"}, + InvalidRemapperConfig{ + {0, 1, 0}, + {0.0, 1.0, 2.0}, + {0.0, 1.0, 2.0}, + "square matrix"})); + +} // namespace + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}