Skip to content

Commit b383b41

Browse files
MasatoSaekipre-commit-ci[bot]
authored and
KhalilSelyan
committed
test(image_projection_based_fusion): add unit test code for geometry (#7096)
* add geometry utility test code * style(pre-commit): autofix * fix scope and declare * change declare name * style(pre-commit): autofix --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
1 parent dd6f6c4 commit b383b41

File tree

2 files changed

+209
-0
lines changed

2 files changed

+209
-0
lines changed

‎perception/image_projection_based_fusion/CMakeLists.txt

+3
Original file line numberDiff line numberDiff line change
@@ -168,6 +168,9 @@ if(BUILD_TESTING)
168168
ament_auto_add_gtest(test_utils
169169
test/test_utils.cpp
170170
)
171+
ament_auto_add_gtest(test_geometry
172+
test/test_geometry.cpp
173+
)
171174
endif()
172175

173176
ament_auto_package(INSTALL_TO_SHARE

‎perception/image_projection_based_fusion/test/test_geometry.cpp

+206
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,206 @@
1+
// Copyright 2024 TIER IV, inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include "image_projection_based_fusion/utils/geometry.hpp"
16+
17+
#include <gtest/gtest.h>
18+
19+
TEST(objectToVertices, test_objectToVertices)
20+
{
21+
// Test `boundingBoxToVertices()` and `cylinderToVertices()` simultaneously
22+
// Test at Shape::BOUNDING_BOX
23+
geometry_msgs::msg::Pose pose;
24+
pose.position.x = 1.0;
25+
pose.position.y = 2.0;
26+
pose.position.z = 3.0;
27+
const double angle = M_PI / 12;
28+
pose.orientation.x = 0.0;
29+
pose.orientation.y = 0.0;
30+
pose.orientation.z = std::sin(angle);
31+
pose.orientation.w = std::cos(angle);
32+
{
33+
autoware_auto_perception_msgs::msg::Shape shape;
34+
shape.type = 0;
35+
shape.dimensions.x = 4.0;
36+
shape.dimensions.y = 6.0;
37+
shape.dimensions.z = 8.0;
38+
std::vector<Eigen::Vector3d> vertices;
39+
40+
image_projection_based_fusion::objectToVertices(pose, shape, vertices);
41+
42+
EXPECT_FALSE(vertices.empty());
43+
EXPECT_NEAR(vertices.at(0).x(), 1.2320508075688772935274, 1e-6);
44+
EXPECT_NEAR(vertices.at(0).y(), 5.598076211353315940291, 1e-6);
45+
EXPECT_NEAR(vertices.at(0).z(), -1.0, 1e-6);
46+
EXPECT_NEAR(vertices.at(7).x(), -2.232050807568877293527, 1e-6);
47+
EXPECT_NEAR(vertices.at(7).y(), 3.598076211353315940291, 1e-6);
48+
EXPECT_NEAR(vertices.at(7).z(), 7.0, 1e-6);
49+
}
50+
51+
{
52+
// Test at Shape::CYLINDER
53+
autoware_auto_perception_msgs::msg::Shape shape;
54+
shape.type = 1;
55+
shape.dimensions.x = 4.0;
56+
shape.dimensions.y = 6.0;
57+
shape.dimensions.z = 8.0;
58+
std::vector<Eigen::Vector3d> vertices;
59+
60+
image_projection_based_fusion::objectToVertices(pose, shape, vertices);
61+
62+
EXPECT_FALSE(vertices.empty());
63+
EXPECT_NEAR(vertices.at(0).x(), 2.732050807568877293528, 1e-6);
64+
EXPECT_NEAR(vertices.at(0).y(), 3.0, 1e-6);
65+
EXPECT_NEAR(vertices.at(0).z(), 7.0, 1e-6);
66+
EXPECT_NEAR(vertices.at(11).x(), 2.732050807568877293528, 1e-6);
67+
EXPECT_NEAR(vertices.at(11).y(), 1.0, 1e-6);
68+
EXPECT_NEAR(vertices.at(11).z(), -1.0, 1e-6);
69+
}
70+
71+
{
72+
// Test at Shape::POLYGON (Nothing to do)
73+
autoware_auto_perception_msgs::msg::Shape shape;
74+
shape.type = 2;
75+
std::vector<Eigen::Vector3d> vertices;
76+
77+
image_projection_based_fusion::objectToVertices(pose, shape, vertices);
78+
79+
EXPECT_TRUE(vertices.empty());
80+
}
81+
}
82+
83+
TEST(transformPoints, test_transformPoints)
84+
{
85+
std::vector<Eigen::Vector3d> input_points;
86+
Eigen::Vector3d point(0.0, 0.0, 0.0);
87+
input_points.push_back(point);
88+
Eigen::Translation<double, 3> translation(1.0, 1.0, 1.0);
89+
Eigen::Matrix3d rotation = (Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitX()) *
90+
Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitY()) *
91+
Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitZ()))
92+
.toRotationMatrix();
93+
Eigen::Affine3d affine_transform = rotation * translation;
94+
std::vector<Eigen::Vector3d> output_points;
95+
96+
image_projection_based_fusion::transformPoints(input_points, affine_transform, output_points);
97+
98+
EXPECT_FALSE(output_points.empty());
99+
EXPECT_NEAR(output_points.at(0).x(), 0.7071067811865475244008, 1e-6);
100+
EXPECT_NEAR(output_points.at(0).y(), 0.5, 1e-6);
101+
EXPECT_NEAR(output_points.at(0).z(), 1.5, 1e-6);
102+
}
103+
104+
TEST(is_inside, test_is_inside)
105+
{
106+
// Test default pattern
107+
sensor_msgs::msg::RegionOfInterest outer;
108+
outer.x_offset = 30;
109+
outer.y_offset = 40;
110+
outer.height = 400;
111+
outer.width = 300;
112+
const double outer_offset_scale = 1.0;
113+
{
114+
sensor_msgs::msg::RegionOfInterest inner;
115+
inner.x_offset = 31;
116+
inner.y_offset = 41;
117+
inner.height = 399;
118+
inner.width = 299;
119+
120+
const bool inside_flag =
121+
image_projection_based_fusion::is_inside(outer, inner, outer_offset_scale);
122+
123+
EXPECT_TRUE(inside_flag);
124+
}
125+
126+
{
127+
// Test left-top outside pattern
128+
sensor_msgs::msg::RegionOfInterest inner;
129+
inner.x_offset = 29;
130+
inner.y_offset = 39;
131+
132+
const bool inside_flag =
133+
image_projection_based_fusion::is_inside(outer, inner, outer_offset_scale);
134+
135+
EXPECT_FALSE(inside_flag);
136+
}
137+
138+
{
139+
// Test right-bottom outside pattern
140+
sensor_msgs::msg::RegionOfInterest inner;
141+
inner.x_offset = 31;
142+
inner.y_offset = 41;
143+
inner.height = 401;
144+
inner.width = 301;
145+
146+
const bool inside_flag =
147+
image_projection_based_fusion::is_inside(outer, inner, outer_offset_scale);
148+
149+
EXPECT_FALSE(inside_flag);
150+
}
151+
}
152+
153+
TEST(sanitizeROI, test_sanitizeROI)
154+
{
155+
{
156+
// Test default pattern
157+
sensor_msgs::msg::RegionOfInterest roi;
158+
roi.x_offset = 10;
159+
roi.y_offset = 20;
160+
roi.height = 200;
161+
roi.width = 100;
162+
int height = 400; // image height
163+
int width = 300; // image width
164+
165+
image_projection_based_fusion::sanitizeROI(roi, width, height);
166+
167+
EXPECT_EQ(roi.height, 200);
168+
EXPECT_EQ(roi.width, 100);
169+
}
170+
171+
{
172+
// Test pattern that x_offset or y_offset is not in image
173+
sensor_msgs::msg::RegionOfInterest roi;
174+
roi.x_offset = 100;
175+
roi.y_offset = 200;
176+
int height = 100;
177+
int width = 50;
178+
179+
image_projection_based_fusion::sanitizeROI(roi, width, height);
180+
181+
EXPECT_EQ(roi.height, 0);
182+
EXPECT_EQ(roi.width, 0);
183+
}
184+
185+
{
186+
// Test patten that roi does not fit within image
187+
sensor_msgs::msg::RegionOfInterest roi;
188+
roi.x_offset = 10;
189+
roi.y_offset = 20;
190+
roi.height = 500;
191+
roi.width = 400;
192+
int height = 100;
193+
int width = 50;
194+
195+
image_projection_based_fusion::sanitizeROI(roi, width, height);
196+
197+
EXPECT_EQ(roi.height, 80);
198+
EXPECT_EQ(roi.width, 40);
199+
}
200+
}
201+
202+
int main(int argc, char * argv[])
203+
{
204+
testing::InitGoogleTest(&argc, argv);
205+
return RUN_ALL_TESTS();
206+
}

0 commit comments

Comments
 (0)