feat(pointcloud_preprocessor): add z filter in vector map inside

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/docs/vector-map-inside-area-filter.md

@@ -10,6 +10,7 @@ The `vector_map_inside_area_filter` is a node that removes points inside the vec
 - Extract the vector map area that intersects with the bounding box of input points to reduce the calculation cost
 - Create the 2D polygon from the extracted vector map area
 - Remove input points inside the polygon
+- If the z value is used for filtering, remove points that are below the z threshold
 
 ![vector_map_inside_area_filter_figure](./image/vector_map_inside_area_filter_overview.svg)
 
@@ -35,5 +36,7 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, so please
 | Name           | Type   | Description                 |
 | -------------- | ------ | --------------------------- |
 | `polygon_type` | string | polygon type to be filtered |
+| `use_z`        | bool   | use z value for filtering   |
+| `z_threshold`  | float  | z threshold for filtering   |
 
 ## Assumptions / Known limits

sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp

@@ -26,6 +26,7 @@
 #include <pcl/point_types.h>
 #include <pcl_conversions/pcl_conversions.h>
 
+#include <optional>
 #include <vector>
 
 using K = CGAL::Exact_predicates_inexact_constructions_kernel;
@@ -49,28 +50,30 @@ void to_cgal_polygon(const lanelet::BasicPolygon2d & polygon_in, PolygonCgal & p
  */
 void remove_polygon_cgal_from_cloud(
   const sensor_msgs::msg::PointCloud2 & cloud_in, const PolygonCgal & polyline_polygon,
-  sensor_msgs::msg::PointCloud2 & cloud_out);
+  sensor_msgs::msg::PointCloud2 & cloud_out, const std::optional<float> & max_z = std::nullopt);
 
 /**
  * @brief remove points in the given polygon
  */
 void remove_polygon_cgal_from_cloud(
   const pcl::PointCloud<pcl::PointXYZ> & cloud_in, const PolygonCgal & polyline_polygon,
-  pcl::PointCloud<pcl::PointXYZ> & cloud_out);
+  pcl::PointCloud<pcl::PointXYZ> & cloud_out, const std::optional<float> & max_z = std::nullopt);
 
 /**
  * @brief remove points in the given polygons
  */
 void remove_polygon_cgal_from_cloud(
   const sensor_msgs::msg::PointCloud2 & cloud_in,
-  const std::vector<PolygonCgal> & polyline_polygons, sensor_msgs::msg::PointCloud2 & cloud_out);
+  const std::vector<PolygonCgal> & polyline_polygons, sensor_msgs::msg::PointCloud2 & cloud_out,
+  const std::optional<float> & max_z = std::nullopt);
 
 /**
  * @brief remove points in the given polygons
  */
 void remove_polygon_cgal_from_cloud(
   const pcl::PointCloud<pcl::PointXYZ> & cloud_in,
-  const std::vector<PolygonCgal> & polyline_polygons, pcl::PointCloud<pcl::PointXYZ> & cloud_out);
+  const std::vector<PolygonCgal> & polyline_polygons, pcl::PointCloud<pcl::PointXYZ> & cloud_out,
+  const std::optional<float> & max_z = std::nullopt);
 
 /**
  * @brief return true if the given point is inside the at least one of the polygons

sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp

@@ -43,6 +43,8 @@ class VectorMapInsideAreaFilterComponent : public pointcloud_preprocessor::Filte
 
   // parameter
   std::string polygon_type_;
+  bool use_z_filter_ = false;
+  float z_threshold_;
 
 public:
   PCL_MAKE_ALIGNED_OPERATOR_NEW

sensing/pointcloud_preprocessor/src/utility/utilities.cpp

@@ -1,4 +1,4 @@
 // Copyright 2022 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.
@@ -46,18 +46,19 @@
 
 void remove_polygon_cgal_from_cloud(
   const sensor_msgs::msg::PointCloud2 & cloud_in, const PolygonCgal & polyline_polygon,
-  sensor_msgs::msg::PointCloud2 & cloud_out)
+  sensor_msgs::msg::PointCloud2 & cloud_out, const std::optional<float> & max_z)
 {
   pcl::PointCloud<pcl::PointXYZ> pcl_output;
 
   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) {
-    // check if the point is inside the polygon
-    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;
@@ -72,25 +73,27 @@
 
 void remove_polygon_cgal_from_cloud(
   const pcl::PointCloud<pcl::PointXYZ> & cloud_in, const PolygonCgal & polyline_polygon,
-  pcl::PointCloud<pcl::PointXYZ> & cloud_out)
+  pcl::PointCloud<pcl::PointXYZ> & cloud_out, const std::optional<float> & max_z)
 {
   cloud_out.clear();
   cloud_out.header = cloud_in.header;
 
   for (const auto & p : cloud_in) {
-    // check if the point is inside the polygon
-    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);
     }
   }
 }
 
 void remove_polygon_cgal_from_cloud(
   const sensor_msgs::msg::PointCloud2 & cloud_in,
-  const std::vector<PolygonCgal> & polyline_polygons, sensor_msgs::msg::PointCloud2 & cloud_out)
+  const std::vector<PolygonCgal> & polyline_polygons, sensor_msgs::msg::PointCloud2 & cloud_out,
+  const std::optional<float> & max_z)
 {
   if (polyline_polygons.empty()) {
     cloud_out = cloud_in;
@@ -101,9 +104,11 @@
   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 the point is inside the polygon, skip inserting and check the next point
-    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);
@@ -115,7 +120,8 @@
 
 void remove_polygon_cgal_from_cloud(
   const pcl::PointCloud<pcl::PointXYZ> & cloud_in,
-  const std::vector<PolygonCgal> & polyline_polygons, pcl::PointCloud<pcl::PointXYZ> & cloud_out)
+  const std::vector<PolygonCgal> & polyline_polygons, pcl::PointCloud<pcl::PointXYZ> & cloud_out,
+  const std::optional<float> & max_z)
 {
   if (polyline_polygons.empty()) {
     cloud_out = cloud_in;
@@ -124,8 +130,10 @@
 
   pcl::PointCloud<pcl::PointXYZ> filtered_cloud;
   for (const auto & p : cloud_in) {
-    // if the point is inside the polygon, skip inserting and check the next point
-    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/src/vector_map_filter/vector_map_inside_area_filter.cpp

@@ -40,7 +40,8 @@ lanelet::ConstPolygons3d calcIntersectedPolygons(
 }
 
 pcl::PointCloud<pcl::PointXYZ> removePointsWithinPolygons(
-  const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud_in, const lanelet::ConstPolygons3d & polygons)
+  const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud_in, const lanelet::ConstPolygons3d & polygons,
+  const std::optional<float> & z_threshold_)
 {
   std::vector<PolygonCgal> cgal_polys;
 
@@ -53,7 +54,7 @@ pcl::PointCloud<pcl::PointXYZ> removePointsWithinPolygons(
 
   pcl::PointCloud<pcl::PointXYZ> filtered_cloud;
   pointcloud_preprocessor::utils::remove_polygon_cgal_from_cloud(
-    *cloud_in, cgal_polys, filtered_cloud);
+    *cloud_in, cgal_polys, filtered_cloud, z_threshold_);
 
   return filtered_cloud;
 }
@@ -74,6 +75,10 @@ VectorMapInsideAreaFilterComponent::VectorMapInsideAreaFilterComponent(
   map_sub_ = this->create_subscription<autoware_auto_mapping_msgs::msg::HADMapBin>(
     "input/vector_map", rclcpp::QoS{1}.transient_local(),
     std::bind(&VectorMapInsideAreaFilterComponent::mapCallback, this, _1));
+
+  // Set parameters
+  use_z_filter_ = declare_parameter<bool>("use_z_filter", false);
+  z_threshold_ = declare_parameter<float>("z_threshold", 0.0f);  // defined in the base_link frame
 }
 
 void VectorMapInsideAreaFilterComponent::filter(
@@ -99,7 +104,25 @@ void VectorMapInsideAreaFilterComponent::filter(
   const auto intersected_lanelets = calcIntersectedPolygons(bounding_box, polygon_lanelets_);
 
   // filter pointcloud by lanelet
-  const auto filtered_pc = removePointsWithinPolygons(pc_input, intersected_lanelets);
+  std::optional<float> z_threshold_in_base_link = std::nullopt;
+  if (use_z_filter_) {
+    // assume z_max is defined in the base_link frame
+    const std::string base_link_frame = "base_link";
+    z_threshold_in_base_link = z_threshold_;
+    if (input->header.frame_id != base_link_frame) {
+      try {
+        // get z difference from baselink to input frame
+        const auto transform =
+          tf_buffer_->lookupTransform(input->header.frame_id, base_link_frame, input->header.stamp);
+        *z_threshold_in_base_link += transform.transform.translation.z;
+      } catch (const tf2::TransformException & e) {
+        RCLCPP_WARN(get_logger(), "Failed to transform z_threshold to base_link frame");
+        z_threshold_in_base_link = std::nullopt;
+      }
+    }
+  }
+  const auto filtered_pc =
+    removePointsWithinPolygons(pc_input, intersected_lanelets, z_threshold_in_base_link);
 
   // convert to ROS message
   pcl::toROSMsg(filtered_pc, output);

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();
+}