feat: add test to utilities and fix code

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

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"

sensing/pointcloud_preprocessor/src/utility/utilities.cpp

@@ -53,13 +53,12 @@ void remove_polygon_cgal_from_cloud(
   for (sensor_msgs::PointCloud2ConstIterator<float> 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<float> 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<pcl::PointXYZ> 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);

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 <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <CGAL/Polygon_2.h>
+#include <CGAL/Simple_cartesian.h>
+#include <gtest/gtest.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+
+#include <optional>
+
+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::PointXYZ> 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::PointXYZ> 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::PointXYZ> 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<float> max_z = 1.0f;
+  pointcloud_preprocessor::utils::remove_polygon_cgal_from_cloud(
+    cloud_in, polygon, cloud_out, max_z);
+
+  pcl::PointCloud<pcl::PointXYZ> 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<float> max_z = 1.0f;
+  pointcloud_preprocessor::utils::remove_polygon_cgal_from_cloud(
+    cloud_in, polygon, cloud_out, max_z);
+
+  pcl::PointCloud<pcl::PointXYZ> 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();
+}