diff --git a/perception/image_projection_based_fusion/CMakeLists.txt b/perception/image_projection_based_fusion/CMakeLists.txt index 825c4888ac7ea..0f4cb7112f74a 100644 --- a/perception/image_projection_based_fusion/CMakeLists.txt +++ b/perception/image_projection_based_fusion/CMakeLists.txt @@ -168,6 +168,9 @@ if(BUILD_TESTING) ament_auto_add_gtest(test_utils test/test_utils.cpp ) + ament_auto_add_gtest(test_geometry + test/test_geometry.cpp + ) endif() ament_auto_package(INSTALL_TO_SHARE diff --git a/perception/image_projection_based_fusion/test/test_geometry.cpp b/perception/image_projection_based_fusion/test/test_geometry.cpp new file mode 100644 index 0000000000000..ac8160fe89124 --- /dev/null +++ b/perception/image_projection_based_fusion/test/test_geometry.cpp @@ -0,0 +1,206 @@ +// 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 "image_projection_based_fusion/utils/geometry.hpp" + +#include + +TEST(objectToVertices, test_objectToVertices) +{ + // Test `boundingBoxToVertices()` and `cylinderToVertices()` simultaneously + // Test at Shape::BOUNDING_BOX + geometry_msgs::msg::Pose pose; + pose.position.x = 1.0; + pose.position.y = 2.0; + pose.position.z = 3.0; + const double angle = M_PI / 12; + pose.orientation.x = 0.0; + pose.orientation.y = 0.0; + pose.orientation.z = std::sin(angle); + pose.orientation.w = std::cos(angle); + { + autoware_auto_perception_msgs::msg::Shape shape; + shape.type = 0; + shape.dimensions.x = 4.0; + shape.dimensions.y = 6.0; + shape.dimensions.z = 8.0; + std::vector vertices; + + image_projection_based_fusion::objectToVertices(pose, shape, vertices); + + EXPECT_FALSE(vertices.empty()); + EXPECT_NEAR(vertices.at(0).x(), 1.2320508075688772935274, 1e-6); + EXPECT_NEAR(vertices.at(0).y(), 5.598076211353315940291, 1e-6); + EXPECT_NEAR(vertices.at(0).z(), -1.0, 1e-6); + EXPECT_NEAR(vertices.at(7).x(), -2.232050807568877293527, 1e-6); + EXPECT_NEAR(vertices.at(7).y(), 3.598076211353315940291, 1e-6); + EXPECT_NEAR(vertices.at(7).z(), 7.0, 1e-6); + } + + { + // Test at Shape::CYLINDER + autoware_auto_perception_msgs::msg::Shape shape; + shape.type = 1; + shape.dimensions.x = 4.0; + shape.dimensions.y = 6.0; + shape.dimensions.z = 8.0; + std::vector vertices; + + image_projection_based_fusion::objectToVertices(pose, shape, vertices); + + EXPECT_FALSE(vertices.empty()); + EXPECT_NEAR(vertices.at(0).x(), 2.732050807568877293528, 1e-6); + EXPECT_NEAR(vertices.at(0).y(), 3.0, 1e-6); + EXPECT_NEAR(vertices.at(0).z(), 7.0, 1e-6); + EXPECT_NEAR(vertices.at(11).x(), 2.732050807568877293528, 1e-6); + EXPECT_NEAR(vertices.at(11).y(), 1.0, 1e-6); + EXPECT_NEAR(vertices.at(11).z(), -1.0, 1e-6); + } + + { + // Test at Shape::POLYGON (Nothing to do) + autoware_auto_perception_msgs::msg::Shape shape; + shape.type = 2; + std::vector vertices; + + image_projection_based_fusion::objectToVertices(pose, shape, vertices); + + EXPECT_TRUE(vertices.empty()); + } +} + +TEST(transformPoints, test_transformPoints) +{ + std::vector input_points; + Eigen::Vector3d point(0.0, 0.0, 0.0); + input_points.push_back(point); + Eigen::Translation translation(1.0, 1.0, 1.0); + Eigen::Matrix3d rotation = (Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitX()) * + Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitZ())) + .toRotationMatrix(); + Eigen::Affine3d affine_transform = rotation * translation; + std::vector output_points; + + image_projection_based_fusion::transformPoints(input_points, affine_transform, output_points); + + EXPECT_FALSE(output_points.empty()); + EXPECT_NEAR(output_points.at(0).x(), 0.7071067811865475244008, 1e-6); + EXPECT_NEAR(output_points.at(0).y(), 0.5, 1e-6); + EXPECT_NEAR(output_points.at(0).z(), 1.5, 1e-6); +} + +TEST(is_inside, test_is_inside) +{ + // Test default pattern + sensor_msgs::msg::RegionOfInterest outer; + outer.x_offset = 30; + outer.y_offset = 40; + outer.height = 400; + outer.width = 300; + const double outer_offset_scale = 1.0; + { + sensor_msgs::msg::RegionOfInterest inner; + inner.x_offset = 31; + inner.y_offset = 41; + inner.height = 399; + inner.width = 299; + + const bool inside_flag = + image_projection_based_fusion::is_inside(outer, inner, outer_offset_scale); + + EXPECT_TRUE(inside_flag); + } + + { + // Test left-top outside pattern + sensor_msgs::msg::RegionOfInterest inner; + inner.x_offset = 29; + inner.y_offset = 39; + + const bool inside_flag = + image_projection_based_fusion::is_inside(outer, inner, outer_offset_scale); + + EXPECT_FALSE(inside_flag); + } + + { + // Test right-bottom outside pattern + sensor_msgs::msg::RegionOfInterest inner; + inner.x_offset = 31; + inner.y_offset = 41; + inner.height = 401; + inner.width = 301; + + const bool inside_flag = + image_projection_based_fusion::is_inside(outer, inner, outer_offset_scale); + + EXPECT_FALSE(inside_flag); + } +} + +TEST(sanitizeROI, test_sanitizeROI) +{ + { + // Test default pattern + sensor_msgs::msg::RegionOfInterest roi; + roi.x_offset = 10; + roi.y_offset = 20; + roi.height = 200; + roi.width = 100; + int height = 400; // image height + int width = 300; // image width + + image_projection_based_fusion::sanitizeROI(roi, width, height); + + EXPECT_EQ(roi.height, 200); + EXPECT_EQ(roi.width, 100); + } + + { + // Test pattern that x_offset or y_offset is not in image + sensor_msgs::msg::RegionOfInterest roi; + roi.x_offset = 100; + roi.y_offset = 200; + int height = 100; + int width = 50; + + image_projection_based_fusion::sanitizeROI(roi, width, height); + + EXPECT_EQ(roi.height, 0); + EXPECT_EQ(roi.width, 0); + } + + { + // Test patten that roi does not fit within image + sensor_msgs::msg::RegionOfInterest roi; + roi.x_offset = 10; + roi.y_offset = 20; + roi.height = 500; + roi.width = 400; + int height = 100; + int width = 50; + + image_projection_based_fusion::sanitizeROI(roi, width, height); + + EXPECT_EQ(roi.height, 80); + EXPECT_EQ(roi.width, 40); + } +} + +int main(int argc, char * argv[]) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}