add geometry utility test code

Author: MasatoSaeki

perception/image_projection_based_fusion/CMakeLists.txt

@@ -165,6 +165,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

perception/image_projection_based_fusion/test/test_geometry.cpp

@@ -0,0 +1,176 @@
+// 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 <glog/logging.h>
+#include <gtest/gtest.h>
+
+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 q_scalar = std::sin(M_PI / 12);
+  pose.orientation.x = 0.0;
+  pose.orientation.y = 0.0;
+  pose.orientation.z = q_scalar;
+  pose.orientation.w = std::cos(M_PI / 12);
+  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<Eigen::Vector3d> 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
+  shape.type = 1;
+  vertices.clear();
+
+  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)
+  shape.type = 2;
+  vertices.clear();
+
+  image_projection_based_fusion::objectToVertices(pose, shape, vertices);
+
+  EXPECT_TRUE(vertices.empty());
+}
+
+TEST(transformPoints,
+test_transformPoints)
+{
+  std::vector<Eigen::Vector3d> input_points;
+  Eigen::Vector3d point(0.0, 0.0, 0.0);
+  input_points.push_back(point);
+  Eigen::Translation<double, 3> 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<Eigen::Vector3d> 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;
+  sensor_msgs::msg::RegionOfInterest inner;
+  inner.x_offset = 31;
+  inner.y_offset = 41;
+  inner.height = 399;
+  inner.width = 299;
+  const double outer_offset_scale = 1.0;
+
+  bool inside_flag = image_projection_based_fusion::is_inside(outer, inner, outer_offset_scale);
+
+  EXPECT_TRUE(inside_flag);
+
+  // Test left-top outside pattern
+  inner.x_offset = 29;
+  inner.y_offset = 39;
+
+  inside_flag = image_projection_based_fusion::is_inside(outer, inner, outer_offset_scale);
+
+  EXPECT_FALSE(inside_flag);
+
+  // Test right-bottom outside pattern
+  inner.x_offset = 31;
+  inner.y_offset = 41;
+  inner.height = 401;
+  inner.width = 301;
+
+  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
+  roi.x_offset = 100;
+  roi.y_offset = 200;
+  height_ = 100;
+  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
+  roi.x_offset = 10;
+  roi.y_offset = 20;
+  roi.height = 500;
+  roi.width = 400;
+
+  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();
+}