Skip to content

Commit 7571ec7

Browse files
technolojinkarishma1911
technolojin
authored and
karishma1911
committed
test(shape_estimation): add unit test on shape_estimation (autowarefoundation#7073)
* feat: add unit test Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: adjust input cluster, expected value Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * feat: add testing data generation function Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: remove unused glog Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> --------- Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
1 parent e5bed84 commit 7571ec7

File tree

4 files changed

+279
-1
lines changed

4 files changed

+279
-1
lines changed

‎perception/shape_estimation/CMakeLists.txt

+16
Original file line numberDiff line numberDiff line change
@@ -66,3 +66,19 @@ ament_auto_package(INSTALL_TO_SHARE
6666
launch
6767
config
6868
)
69+
70+
## Tests
71+
if(BUILD_TESTING)
72+
find_package(ament_cmake_ros REQUIRED)
73+
list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_uncrustify)
74+
75+
find_package(ament_lint_auto REQUIRED)
76+
ament_lint_auto_find_test_dependencies()
77+
78+
file(GLOB_RECURSE test_files test/**/*.cpp)
79+
ament_add_ros_isolated_gtest(test_shape_estimation ${test_files})
80+
81+
target_link_libraries(test_shape_estimation
82+
shape_estimation_node
83+
)
84+
endif()

‎perception/shape_estimation/package.xml

+1-1
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,6 @@
1212
<buildtool_depend>autoware_cmake</buildtool_depend>
1313

1414
<depend>autoware_auto_perception_msgs</depend>
15-
<depend>builtin_interfaces</depend>
1615
<depend>eigen</depend>
1716
<depend>libopencv-dev</depend>
1817
<depend>libpcl-all-dev</depend>
@@ -25,6 +24,7 @@
2524
<depend>tier4_autoware_utils</depend>
2625
<depend>tier4_perception_msgs</depend>
2726

27+
<test_depend>ament_cmake_gtest</test_depend>
2828
<test_depend>ament_lint_auto</test_depend>
2929
<test_depend>autoware_lint_common</test_depend>
3030

‎perception/shape_estimation/test/shape_estimation/test_shape_estimator.cpp

+236
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,236 @@
1+
2+
// Copyright 2024 TIER IV, inc.
3+
//
4+
// Licensed under the Apache License, Version 2.0 (the "License");
5+
// you may not use this file except in compliance with the License.
6+
// You may obtain a copy of the License at
7+
//
8+
// http://www.apache.org/licenses/LICENSE-2.0
9+
//
10+
// Unless required by applicable law or agreed to in writing, software
11+
// distributed under the License is distributed on an "AS IS" BASIS,
12+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
// See the License for the specific language governing permissions and
14+
// limitations under the License.
15+
16+
#include "shape_estimation/corrector/corrector.hpp"
17+
#include "shape_estimation/filter/filter.hpp"
18+
#include "shape_estimation/model/model.hpp"
19+
#include "shape_estimation/shape_estimator.hpp"
20+
21+
#include <gtest/gtest.h>
22+
#include <math.h>
23+
24+
namespace
25+
{
26+
double yawFromQuaternion(const geometry_msgs::msg::Quaternion & q)
27+
{
28+
return atan2(2.0 * (q.w * q.z + q.x * q.y), 1.0 - 2.0 * (q.y * q.y + q.z * q.z));
29+
}
30+
31+
double deg2rad(const double deg)
32+
{
33+
return deg / 180.0 * M_PI;
34+
}
35+
36+
pcl::PointCloud<pcl::PointXYZ> createLShapeCluster(
37+
const double length, const double width, const double height, const double yaw,
38+
const double offset_x, const double offset_y)
39+
{
40+
pcl::PointCloud<pcl::PointXYZ> cluster;
41+
for (double x = -length / 2; x < length / 2; x += 0.4) {
42+
cluster.push_back(pcl::PointXYZ(x, width / 2, 0.0));
43+
}
44+
for (double y = -width / 2; y < width / 2; y += 0.2) {
45+
cluster.push_back(pcl::PointXYZ(-length / 2, y, 0.0));
46+
}
47+
cluster.push_back(pcl::PointXYZ(length / 2, -width / 2, 0.0));
48+
cluster.push_back(pcl::PointXYZ(length / 2, width / 2, 0.0));
49+
cluster.push_back(pcl::PointXYZ(-length / 2, -width / 2, 0.0));
50+
cluster.push_back(pcl::PointXYZ(-length / 2, width / 2, 0.0));
51+
cluster.push_back(pcl::PointXYZ(0.0, 0.0, height));
52+
53+
for (auto & point : cluster) {
54+
const double x = point.x;
55+
const double y = point.y;
56+
// rotate
57+
point.x = x * cos(yaw) - y * sin(yaw);
58+
point.y = x * sin(yaw) + y * cos(yaw);
59+
// offset
60+
point.x += offset_x;
61+
point.y += offset_y;
62+
}
63+
64+
return cluster;
65+
}
66+
} // namespace
67+
68+
// test BoundingBoxShapeModel
69+
// 1. base case
70+
TEST(BoundingBoxShapeModel, test_estimateShape)
71+
{
72+
// Generate BoundingBoxShapeModel
73+
BoundingBoxShapeModel bbox_shape_model = BoundingBoxShapeModel();
74+
75+
// Generate cluster
76+
const double length = 4.0;
77+
const double width = 2.0;
78+
const double height = 1.0;
79+
80+
pcl::PointCloud<pcl::PointXYZ> cluster =
81+
createLShapeCluster(length, width, height, 0.0, 0.0, 0.0);
82+
83+
// Generate shape and pose output
84+
autoware_auto_perception_msgs::msg::Shape shape_output;
85+
geometry_msgs::msg::Pose pose_output;
86+
87+
// Test estimateShape
88+
const bool result = bbox_shape_model.estimate(cluster, shape_output, pose_output);
89+
EXPECT_TRUE(result);
90+
91+
// Check shape_output
92+
EXPECT_EQ(shape_output.type, autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX);
93+
EXPECT_NEAR(shape_output.dimensions.x, length, length * 0.1);
94+
EXPECT_NEAR(shape_output.dimensions.y, width, width * 0.1);
95+
EXPECT_NEAR(shape_output.dimensions.z, height, height * 0.1);
96+
97+
// Check pose_output
98+
EXPECT_NEAR(pose_output.position.x, 0.0, 1e-2);
99+
EXPECT_NEAR(pose_output.position.y, 0.0, 1e-2);
100+
EXPECT_NEAR(pose_output.position.z, height / 2, 1e-2);
101+
102+
// transform quaternion to yaw
103+
const double pose_output_yaw = yawFromQuaternion(pose_output.orientation);
104+
EXPECT_NEAR(pose_output_yaw, 0, 1e-3);
105+
}
106+
107+
// 2. rotated case
108+
TEST(BoundingBoxShapeModel, test_estimateShape_rotated)
109+
{
110+
// Generate cluster
111+
const double length = 4.0;
112+
const double width = 2.0;
113+
const double height = 1.0;
114+
const double yaw = deg2rad(30.0);
115+
const double offset_x = 10.0;
116+
const double offset_y = 1.0;
117+
pcl::PointCloud<pcl::PointXYZ> cluster =
118+
createLShapeCluster(length, width, height, yaw, offset_x, offset_y);
119+
120+
const auto ref_yaw_info =
121+
ReferenceYawInfo{static_cast<float>(yaw), static_cast<float>(deg2rad(10.0))};
122+
const bool use_boost_bbox_optimizer = true;
123+
// Generate BoundingBoxShapeModel
124+
BoundingBoxShapeModel bbox_shape_model =
125+
BoundingBoxShapeModel(ref_yaw_info, use_boost_bbox_optimizer);
126+
127+
// Generate shape and pose output
128+
autoware_auto_perception_msgs::msg::Shape shape_output;
129+
geometry_msgs::msg::Pose pose_output;
130+
131+
// Test estimateShape
132+
const bool result = bbox_shape_model.estimate(cluster, shape_output, pose_output);
133+
EXPECT_TRUE(result);
134+
135+
// Check shape_output
136+
EXPECT_EQ(shape_output.type, autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX);
137+
EXPECT_NEAR(shape_output.dimensions.x, length, length * 0.1);
138+
EXPECT_NEAR(shape_output.dimensions.y, width, width * 0.1);
139+
EXPECT_NEAR(shape_output.dimensions.z, height, height * 0.1);
140+
141+
// Check pose_output
142+
EXPECT_NEAR(pose_output.position.x, offset_x, 1.0);
143+
EXPECT_NEAR(pose_output.position.y, offset_y, 1.0);
144+
EXPECT_NEAR(pose_output.position.z, height / 2, 1.0);
145+
146+
// transform quaternion to yaw
147+
const double pose_output_yaw = yawFromQuaternion(pose_output.orientation);
148+
EXPECT_NEAR(pose_output_yaw, yaw, deg2rad(15.0));
149+
}
150+
151+
// test CylinderShapeModel
152+
TEST(CylinderShapeModel, test_estimateShape)
153+
{
154+
// Generate CylinderShapeModel
155+
CylinderShapeModel cylinder_shape_model = CylinderShapeModel();
156+
157+
// Generate cluster
158+
pcl::PointCloud<pcl::PointXYZ> cluster = createLShapeCluster(4.0, 2.0, 1.0, 0.0, 0.0, 0.0);
159+
// Generate shape and pose output
160+
autoware_auto_perception_msgs::msg::Shape shape_output;
161+
geometry_msgs::msg::Pose pose_output;
162+
163+
// Test estimateShape
164+
const bool result = cylinder_shape_model.estimate(cluster, shape_output, pose_output);
165+
EXPECT_TRUE(result);
166+
}
167+
168+
// test ConvexHullShapeModel
169+
TEST(ConvexHullShapeModel, test_estimateShape)
170+
{
171+
// Generate ConvexHullShapeModel
172+
ConvexHullShapeModel convex_hull_shape_model = ConvexHullShapeModel();
173+
174+
// Generate cluster
175+
pcl::PointCloud<pcl::PointXYZ> cluster = createLShapeCluster(2.0, 1.0, 1.0, 0.0, 0.0, 0.0);
176+
177+
// Generate shape and pose output
178+
autoware_auto_perception_msgs::msg::Shape shape_output;
179+
geometry_msgs::msg::Pose pose_output;
180+
181+
// Test estimateShape
182+
const bool result = convex_hull_shape_model.estimate(cluster, shape_output, pose_output);
183+
EXPECT_TRUE(result);
184+
}
185+
186+
// test ShapeEstimator
187+
TEST(ShapeEstimator, test_estimateShapeAndPose)
188+
{
189+
// Generate cluster
190+
double length = 4.0;
191+
double width = 2.0;
192+
double height = 1.0;
193+
const double yaw = deg2rad(60.0);
194+
const double offset_x = 6.0;
195+
const double offset_y = -2.0;
196+
pcl::PointCloud<pcl::PointXYZ> cluster =
197+
createLShapeCluster(length, width, height, yaw, offset_x, offset_y);
198+
199+
// Generate ShapeEstimator
200+
const bool use_corrector = true;
201+
const bool use_filter = true;
202+
const bool use_boost_bbox_optimizer = true;
203+
ShapeEstimator shape_estimator =
204+
ShapeEstimator(use_corrector, use_filter, use_boost_bbox_optimizer);
205+
206+
// Generate ref_yaw_info
207+
boost::optional<ReferenceYawInfo> ref_yaw_info = boost::none;
208+
boost::optional<ReferenceShapeSizeInfo> ref_shape_size_info = boost::none;
209+
210+
ref_yaw_info = ReferenceYawInfo{static_cast<float>(yaw), static_cast<float>(deg2rad(10.0))};
211+
const auto label = autoware_auto_perception_msgs::msg::ObjectClassification::CAR;
212+
213+
// Generate shape and pose output
214+
autoware_auto_perception_msgs::msg::Shape shape_output;
215+
geometry_msgs::msg::Pose pose_output;
216+
217+
// Test estimateShapeAndPose
218+
const bool result = shape_estimator.estimateShapeAndPose(
219+
label, cluster, ref_yaw_info, ref_shape_size_info, shape_output, pose_output);
220+
EXPECT_TRUE(result);
221+
222+
// Check shape_output
223+
EXPECT_EQ(shape_output.type, autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX);
224+
EXPECT_NEAR(shape_output.dimensions.x, length, length * 0.1);
225+
EXPECT_NEAR(shape_output.dimensions.y, width, width * 0.1);
226+
EXPECT_NEAR(shape_output.dimensions.z, height, height * 0.1);
227+
228+
// Check pose_output
229+
EXPECT_NEAR(pose_output.position.x, offset_x, 1.0);
230+
EXPECT_NEAR(pose_output.position.y, offset_y, 1.0);
231+
EXPECT_NEAR(pose_output.position.z, height / 2, 1.0);
232+
233+
// transform quaternion to yaw
234+
const double pose_output_yaw = yawFromQuaternion(pose_output.orientation);
235+
EXPECT_NEAR(pose_output_yaw, yaw, deg2rad(15.0));
236+
}

‎perception/shape_estimation/test/test_shape_estimation.cpp

+26
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,26 @@
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 <rclcpp/rclcpp.hpp>
16+
17+
#include <gtest/gtest.h>
18+
19+
int main(int argc, char * argv[])
20+
{
21+
testing::InitGoogleTest(&argc, argv);
22+
rclcpp::init(argc, argv);
23+
bool result = RUN_ALL_TESTS();
24+
rclcpp::shutdown();
25+
return result;
26+
}

0 commit comments

Comments
 (0)