From 3b1f60c7b74af82c4428a55939cb8616186783ce Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 21 May 2024 09:39:46 +0900 Subject: [PATCH] test(shape_estimation): add unit test on shape_estimation (#7073) * feat: add unit test Signed-off-by: Taekjin LEE * fix: adjust input cluster, expected value Signed-off-by: Taekjin LEE * feat: add testing data generation function Signed-off-by: Taekjin LEE * fix: remove unused glog Signed-off-by: Taekjin LEE --------- Signed-off-by: Taekjin LEE --- perception/shape_estimation/CMakeLists.txt | 16 ++ perception/shape_estimation/package.xml | 2 +- .../shape_estimation/test_shape_estimator.cpp | 236 ++++++++++++++++++ .../test/test_shape_estimation.cpp | 26 ++ 4 files changed, 279 insertions(+), 1 deletion(-) create mode 100644 perception/shape_estimation/test/shape_estimation/test_shape_estimator.cpp create mode 100644 perception/shape_estimation/test/test_shape_estimation.cpp diff --git a/perception/shape_estimation/CMakeLists.txt b/perception/shape_estimation/CMakeLists.txt index 8eb7a15b5a885..527c565b91e05 100644 --- a/perception/shape_estimation/CMakeLists.txt +++ b/perception/shape_estimation/CMakeLists.txt @@ -66,3 +66,19 @@ ament_auto_package(INSTALL_TO_SHARE launch config ) + +## Tests +if(BUILD_TESTING) + find_package(ament_cmake_ros REQUIRED) + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_uncrustify) + + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + file(GLOB_RECURSE test_files test/**/*.cpp) + ament_add_ros_isolated_gtest(test_shape_estimation ${test_files}) + + target_link_libraries(test_shape_estimation + shape_estimation_node + ) +endif() diff --git a/perception/shape_estimation/package.xml b/perception/shape_estimation/package.xml index 334b6cc2af123..137d925de6d54 100644 --- a/perception/shape_estimation/package.xml +++ b/perception/shape_estimation/package.xml @@ -12,7 +12,6 @@ autoware_cmake autoware_auto_perception_msgs - builtin_interfaces eigen libopencv-dev libpcl-all-dev @@ -25,6 +24,7 @@ tier4_autoware_utils tier4_perception_msgs + ament_cmake_gtest ament_lint_auto autoware_lint_common diff --git a/perception/shape_estimation/test/shape_estimation/test_shape_estimator.cpp b/perception/shape_estimation/test/shape_estimation/test_shape_estimator.cpp new file mode 100644 index 0000000000000..d59cfd11b69df --- /dev/null +++ b/perception/shape_estimation/test/shape_estimation/test_shape_estimator.cpp @@ -0,0 +1,236 @@ + +// 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 "shape_estimation/corrector/corrector.hpp" +#include "shape_estimation/filter/filter.hpp" +#include "shape_estimation/model/model.hpp" +#include "shape_estimation/shape_estimator.hpp" + +#include +#include + +namespace +{ +double yawFromQuaternion(const geometry_msgs::msg::Quaternion & q) +{ + return atan2(2.0 * (q.w * q.z + q.x * q.y), 1.0 - 2.0 * (q.y * q.y + q.z * q.z)); +} + +double deg2rad(const double deg) +{ + return deg / 180.0 * M_PI; +} + +pcl::PointCloud createLShapeCluster( + const double length, const double width, const double height, const double yaw, + const double offset_x, const double offset_y) +{ + pcl::PointCloud cluster; + for (double x = -length / 2; x < length / 2; x += 0.4) { + cluster.push_back(pcl::PointXYZ(x, width / 2, 0.0)); + } + for (double y = -width / 2; y < width / 2; y += 0.2) { + cluster.push_back(pcl::PointXYZ(-length / 2, y, 0.0)); + } + cluster.push_back(pcl::PointXYZ(length / 2, -width / 2, 0.0)); + cluster.push_back(pcl::PointXYZ(length / 2, width / 2, 0.0)); + cluster.push_back(pcl::PointXYZ(-length / 2, -width / 2, 0.0)); + cluster.push_back(pcl::PointXYZ(-length / 2, width / 2, 0.0)); + cluster.push_back(pcl::PointXYZ(0.0, 0.0, height)); + + for (auto & point : cluster) { + const double x = point.x; + const double y = point.y; + // rotate + point.x = x * cos(yaw) - y * sin(yaw); + point.y = x * sin(yaw) + y * cos(yaw); + // offset + point.x += offset_x; + point.y += offset_y; + } + + return cluster; +} +} // namespace + +// test BoundingBoxShapeModel +// 1. base case +TEST(BoundingBoxShapeModel, test_estimateShape) +{ + // Generate BoundingBoxShapeModel + BoundingBoxShapeModel bbox_shape_model = BoundingBoxShapeModel(); + + // Generate cluster + const double length = 4.0; + const double width = 2.0; + const double height = 1.0; + + pcl::PointCloud cluster = + createLShapeCluster(length, width, height, 0.0, 0.0, 0.0); + + // Generate shape and pose output + autoware_auto_perception_msgs::msg::Shape shape_output; + geometry_msgs::msg::Pose pose_output; + + // Test estimateShape + const bool result = bbox_shape_model.estimate(cluster, shape_output, pose_output); + EXPECT_TRUE(result); + + // Check shape_output + EXPECT_EQ(shape_output.type, autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX); + EXPECT_NEAR(shape_output.dimensions.x, length, length * 0.1); + EXPECT_NEAR(shape_output.dimensions.y, width, width * 0.1); + EXPECT_NEAR(shape_output.dimensions.z, height, height * 0.1); + + // Check pose_output + EXPECT_NEAR(pose_output.position.x, 0.0, 1e-2); + EXPECT_NEAR(pose_output.position.y, 0.0, 1e-2); + EXPECT_NEAR(pose_output.position.z, height / 2, 1e-2); + + // transform quaternion to yaw + const double pose_output_yaw = yawFromQuaternion(pose_output.orientation); + EXPECT_NEAR(pose_output_yaw, 0, 1e-3); +} + +// 2. rotated case +TEST(BoundingBoxShapeModel, test_estimateShape_rotated) +{ + // Generate cluster + const double length = 4.0; + const double width = 2.0; + const double height = 1.0; + const double yaw = deg2rad(30.0); + const double offset_x = 10.0; + const double offset_y = 1.0; + pcl::PointCloud cluster = + createLShapeCluster(length, width, height, yaw, offset_x, offset_y); + + const auto ref_yaw_info = + ReferenceYawInfo{static_cast(yaw), static_cast(deg2rad(10.0))}; + const bool use_boost_bbox_optimizer = true; + // Generate BoundingBoxShapeModel + BoundingBoxShapeModel bbox_shape_model = + BoundingBoxShapeModel(ref_yaw_info, use_boost_bbox_optimizer); + + // Generate shape and pose output + autoware_auto_perception_msgs::msg::Shape shape_output; + geometry_msgs::msg::Pose pose_output; + + // Test estimateShape + const bool result = bbox_shape_model.estimate(cluster, shape_output, pose_output); + EXPECT_TRUE(result); + + // Check shape_output + EXPECT_EQ(shape_output.type, autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX); + EXPECT_NEAR(shape_output.dimensions.x, length, length * 0.1); + EXPECT_NEAR(shape_output.dimensions.y, width, width * 0.1); + EXPECT_NEAR(shape_output.dimensions.z, height, height * 0.1); + + // Check pose_output + EXPECT_NEAR(pose_output.position.x, offset_x, 1.0); + EXPECT_NEAR(pose_output.position.y, offset_y, 1.0); + EXPECT_NEAR(pose_output.position.z, height / 2, 1.0); + + // transform quaternion to yaw + const double pose_output_yaw = yawFromQuaternion(pose_output.orientation); + EXPECT_NEAR(pose_output_yaw, yaw, deg2rad(15.0)); +} + +// test CylinderShapeModel +TEST(CylinderShapeModel, test_estimateShape) +{ + // Generate CylinderShapeModel + CylinderShapeModel cylinder_shape_model = CylinderShapeModel(); + + // Generate cluster + pcl::PointCloud cluster = createLShapeCluster(4.0, 2.0, 1.0, 0.0, 0.0, 0.0); + // Generate shape and pose output + autoware_auto_perception_msgs::msg::Shape shape_output; + geometry_msgs::msg::Pose pose_output; + + // Test estimateShape + const bool result = cylinder_shape_model.estimate(cluster, shape_output, pose_output); + EXPECT_TRUE(result); +} + +// test ConvexHullShapeModel +TEST(ConvexHullShapeModel, test_estimateShape) +{ + // Generate ConvexHullShapeModel + ConvexHullShapeModel convex_hull_shape_model = ConvexHullShapeModel(); + + // Generate cluster + pcl::PointCloud cluster = createLShapeCluster(2.0, 1.0, 1.0, 0.0, 0.0, 0.0); + + // Generate shape and pose output + autoware_auto_perception_msgs::msg::Shape shape_output; + geometry_msgs::msg::Pose pose_output; + + // Test estimateShape + const bool result = convex_hull_shape_model.estimate(cluster, shape_output, pose_output); + EXPECT_TRUE(result); +} + +// test ShapeEstimator +TEST(ShapeEstimator, test_estimateShapeAndPose) +{ + // Generate cluster + double length = 4.0; + double width = 2.0; + double height = 1.0; + const double yaw = deg2rad(60.0); + const double offset_x = 6.0; + const double offset_y = -2.0; + pcl::PointCloud cluster = + createLShapeCluster(length, width, height, yaw, offset_x, offset_y); + + // Generate ShapeEstimator + const bool use_corrector = true; + const bool use_filter = true; + const bool use_boost_bbox_optimizer = true; + ShapeEstimator shape_estimator = + ShapeEstimator(use_corrector, use_filter, use_boost_bbox_optimizer); + + // Generate ref_yaw_info + boost::optional ref_yaw_info = boost::none; + boost::optional ref_shape_size_info = boost::none; + + ref_yaw_info = ReferenceYawInfo{static_cast(yaw), static_cast(deg2rad(10.0))}; + const auto label = autoware_auto_perception_msgs::msg::ObjectClassification::CAR; + + // Generate shape and pose output + autoware_auto_perception_msgs::msg::Shape shape_output; + geometry_msgs::msg::Pose pose_output; + + // Test estimateShapeAndPose + const bool result = shape_estimator.estimateShapeAndPose( + label, cluster, ref_yaw_info, ref_shape_size_info, shape_output, pose_output); + EXPECT_TRUE(result); + + // Check shape_output + EXPECT_EQ(shape_output.type, autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX); + EXPECT_NEAR(shape_output.dimensions.x, length, length * 0.1); + EXPECT_NEAR(shape_output.dimensions.y, width, width * 0.1); + EXPECT_NEAR(shape_output.dimensions.z, height, height * 0.1); + + // Check pose_output + EXPECT_NEAR(pose_output.position.x, offset_x, 1.0); + EXPECT_NEAR(pose_output.position.y, offset_y, 1.0); + EXPECT_NEAR(pose_output.position.z, height / 2, 1.0); + + // transform quaternion to yaw + const double pose_output_yaw = yawFromQuaternion(pose_output.orientation); + EXPECT_NEAR(pose_output_yaw, yaw, deg2rad(15.0)); +} diff --git a/perception/shape_estimation/test/test_shape_estimation.cpp b/perception/shape_estimation/test/test_shape_estimation.cpp new file mode 100644 index 0000000000000..4c8ad7dd25916 --- /dev/null +++ b/perception/shape_estimation/test/test_shape_estimation.cpp @@ -0,0 +1,26 @@ +// 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 + +#include + +int main(int argc, char * argv[]) +{ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + bool result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +}