From 0323f33c45ac42b05d92a6b092f2dad8c1a8a710 Mon Sep 17 00:00:00 2001 From: yoshiri Date: Thu, 6 Jun 2024 14:29:15 +0900 Subject: [PATCH] feat: add test to utilities and fix code Signed-off-by: yoshiri --- .../pointcloud_preprocessor/CMakeLists.txt | 8 ++ .../src/utility/utilities.cpp | 44 +++--- .../test/test_utilities.cpp | 125 ++++++++++++++++++ 3 files changed, 154 insertions(+), 23 deletions(-) create mode 100644 sensing/pointcloud_preprocessor/test/test_utilities.cpp diff --git a/sensing/pointcloud_preprocessor/CMakeLists.txt b/sensing/pointcloud_preprocessor/CMakeLists.txt index 0cbb04345477e..ecb84c8a7f3b3 100644 --- a/sensing/pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/pointcloud_preprocessor/CMakeLists.txt @@ -221,6 +221,14 @@ ament_auto_package(INSTALL_TO_SHARE ) if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + find_package(ament_cmake_gtest) + ament_lint_auto_find_test_dependencies() + ament_add_gtest(test_utilities + test/test_utilities.cpp + ) + target_link_libraries(test_utilities pointcloud_preprocessor_filter) + add_ros_test( test/test_distortion_corrector.py TIMEOUT "30" diff --git a/sensing/pointcloud_preprocessor/src/utility/utilities.cpp b/sensing/pointcloud_preprocessor/src/utility/utilities.cpp index 6783bf8f80f14..ec84a2467db78 100644 --- a/sensing/pointcloud_preprocessor/src/utility/utilities.cpp +++ b/sensing/pointcloud_preprocessor/src/utility/utilities.cpp @@ -53,13 +53,12 @@ void remove_polygon_cgal_from_cloud( for (sensor_msgs::PointCloud2ConstIterator iter_x(cloud_in, "x"), iter_y(cloud_in, "y"), iter_z(cloud_in, "z"); iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { - if (max_z && *iter_z > *max_z) { - continue; - } - if ( - CGAL::bounded_side_2( - polyline_polygon.begin(), polyline_polygon.end(), PointCgal(*iter_x, *iter_y), K()) == - CGAL::ON_UNBOUNDED_SIDE) { + const bool within_max_z = max_z ? *iter_z <= *max_z : true; + const bool within_polygon = CGAL::bounded_side_2( + polyline_polygon.begin(), polyline_polygon.end(), + PointCgal(*iter_x, *iter_y), K()) == CGAL::ON_BOUNDED_SIDE; + // remove points within the polygon and max_z + if (!(within_max_z && within_polygon)) { pcl::PointXYZ p; p.x = *iter_x; p.y = *iter_y; @@ -80,13 +79,12 @@ void remove_polygon_cgal_from_cloud( cloud_out.header = cloud_in.header; for (const auto & p : cloud_in) { - if (max_z && p.z > *max_z) { - continue; - } - if ( - CGAL::bounded_side_2( - polyline_polygon.begin(), polyline_polygon.end(), PointCgal(p.x, p.y), K()) == - CGAL::ON_UNBOUNDED_SIDE) { + const bool within_max_z = max_z ? p.z <= *max_z : true; + const bool within_polygon = CGAL::bounded_side_2( + polyline_polygon.begin(), polyline_polygon.end(), + PointCgal(p.x, p.y), K()) == CGAL::ON_BOUNDED_SIDE; + // remove points within the polygon and max_z + if (!(within_max_z && within_polygon)) { cloud_out.emplace_back(p); } } @@ -106,11 +104,11 @@ void remove_polygon_cgal_from_cloud( for (sensor_msgs::PointCloud2ConstIterator iter_x(cloud_in, "x"), iter_y(cloud_in, "y"), iter_z(cloud_in, "z"); iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { - if (max_z && *iter_z > *max_z) { - continue; - } - pcl::PointXYZ p(*iter_x, *iter_y, *iter_z); - if (point_within_cgal_polys(p, polyline_polygons)) { + const bool within_max_z = max_z ? *iter_z <= *max_z : true; + const pcl::PointXYZ p(*iter_x, *iter_y, *iter_z); + const bool within_polygon = point_within_cgal_polys(p, polyline_polygons); + // remove points within the polygon and max_z + if (within_max_z && within_polygon) { continue; } filtered_cloud.emplace_back(p); @@ -132,10 +130,10 @@ void remove_polygon_cgal_from_cloud( pcl::PointCloud filtered_cloud; for (const auto & p : cloud_in) { - if (max_z && p.z > *max_z) { - continue; - } - if (point_within_cgal_polys(p, polyline_polygons)) { + const bool within_max_z = max_z ? p.z <= *max_z : true; + const bool within_polygon = point_within_cgal_polys(p, polyline_polygons); + // remove points within the polygon and max_z + if (within_max_z && within_polygon) { continue; } filtered_cloud.emplace_back(p); diff --git a/sensing/pointcloud_preprocessor/test/test_utilities.cpp b/sensing/pointcloud_preprocessor/test/test_utilities.cpp new file mode 100644 index 0000000000000..68c86dfbd0505 --- /dev/null +++ b/sensing/pointcloud_preprocessor/test/test_utilities.cpp @@ -0,0 +1,125 @@ +// 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 "pointcloud_preprocessor/utility/utilities.hpp" + +#include + +#include +#include +#include +#include +#include +#include + +#include + +constexpr double EPSILON = 1e-6; + +class RemovePolygonCgalFromCloudTest : public ::testing::Test +{ +protected: + PolygonCgal polygon; + sensor_msgs::msg::PointCloud2 cloud_out; + + virtual void SetUp() + { + // Set up a simple square polygon + polygon.push_back(PointCgal(0.0, 0.0)); + polygon.push_back(PointCgal(0.0, 1.0)); + polygon.push_back(PointCgal(1.0, 1.0)); + polygon.push_back(PointCgal(1.0, 0.0)); + polygon.push_back(PointCgal(0.0, 0.0)); + } + + void CreatePointCloud2(sensor_msgs::msg::PointCloud2 & cloud, double x, double y, double z) + { + pcl::PointCloud pcl_cloud; + pcl_cloud.push_back(pcl::PointXYZ(x, y, z)); + pcl::toROSMsg(pcl_cloud, cloud); + cloud.header.frame_id = "map"; + } +}; + +// Test case 1: without max_z, points inside the polygon are removed +TEST_F(RemovePolygonCgalFromCloudTest, PointsInsidePolygonAreRemoved) +{ + sensor_msgs::msg::PointCloud2 cloud_in; + CreatePointCloud2(cloud_in, 0.5, 0.5, 0.1); // point inside the polygon + + pointcloud_preprocessor::utils::remove_polygon_cgal_from_cloud(cloud_in, polygon, cloud_out); + + pcl::PointCloud pcl_output; + pcl::fromROSMsg(cloud_out, pcl_output); + + EXPECT_EQ(pcl_output.size(), 0); +} + +// Test case 2: without max_z, points outside the polygon remain +TEST_F(RemovePolygonCgalFromCloudTest, PointsOutsidePolygonRemain) +{ + sensor_msgs::msg::PointCloud2 cloud_in; + CreatePointCloud2(cloud_in, 1.5, 1.5, 0.1); // point outside the polygon + + pointcloud_preprocessor::utils::remove_polygon_cgal_from_cloud(cloud_in, polygon, cloud_out); + + pcl::PointCloud pcl_output; + pcl::fromROSMsg(cloud_out, pcl_output); + + EXPECT_EQ(pcl_output.size(), 1); + EXPECT_NEAR(pcl_output.points[0].x, 1.5, EPSILON); + EXPECT_NEAR(pcl_output.points[0].y, 1.5, EPSILON); + EXPECT_NEAR(pcl_output.points[0].z, 0.1, EPSILON); +} + +// Test case 3: with max_z, points inside the polygon and below max_z are removed +TEST_F(RemovePolygonCgalFromCloudTest, PointsBelowMaxZAreRemoved) +{ + sensor_msgs::msg::PointCloud2 cloud_in; + CreatePointCloud2(cloud_in, 0.5, 0.5, 0.1); // point inside the polygon, below max_z + + std::optional max_z = 1.0f; + pointcloud_preprocessor::utils::remove_polygon_cgal_from_cloud( + cloud_in, polygon, cloud_out, max_z); + + pcl::PointCloud pcl_output; + pcl::fromROSMsg(cloud_out, pcl_output); + + EXPECT_EQ(pcl_output.size(), 0); +} + +// Test case 4: with max_z, points inside the polygon but above max_z remain +TEST_F(RemovePolygonCgalFromCloudTest, PointsAboveMaxZRemain) +{ + sensor_msgs::msg::PointCloud2 cloud_in; + CreatePointCloud2(cloud_in, 0.5, 0.5, 1.5); // point inside the polygon, above max_z + + std::optional max_z = 1.0f; + pointcloud_preprocessor::utils::remove_polygon_cgal_from_cloud( + cloud_in, polygon, cloud_out, max_z); + + pcl::PointCloud pcl_output; + pcl::fromROSMsg(cloud_out, pcl_output); + + EXPECT_EQ(pcl_output.size(), 1); + EXPECT_NEAR(pcl_output.points[0].x, 0.5, EPSILON); + EXPECT_NEAR(pcl_output.points[0].y, 0.5, EPSILON); + EXPECT_NEAR(pcl_output.points[0].z, 1.5, EPSILON); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}