From 060a6ef9f1983971f8f8b6e0ff912dfb4d40792d Mon Sep 17 00:00:00 2001 From: Yoshi Ri Date: Fri, 7 Jun 2024 10:47:57 +0900 Subject: [PATCH] feat(pointcloud_preprocessor): add z filter in vector map inside (#7300) * feat: add utils to handle z height filter Signed-off-by: yoshiri * feat: add test to utilities and fix code Signed-off-by: yoshiri * feat: add node param to filter by z Signed-off-by: yoshiri * docs: update readme Signed-off-by: yoshiri --------- Signed-off-by: yoshiri --- .../pointcloud_preprocessor/CMakeLists.txt | 8 ++ .../docs/vector-map-inside-area-filter.md | 3 + .../utility/utilities.hpp | 11 +- .../vector_map_inside_area_filter.hpp | 2 + .../src/utility/utilities.cpp | 46 ++++--- .../vector_map_inside_area_filter.cpp | 29 +++- .../test/test_utilities.cpp | 125 ++++++++++++++++++ 7 files changed, 198 insertions(+), 26 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/docs/vector-map-inside-area-filter.md b/sensing/pointcloud_preprocessor/docs/vector-map-inside-area-filter.md index bd2f7d98919d5..8e31dfb203d2b 100644 --- a/sensing/pointcloud_preprocessor/docs/vector-map-inside-area-filter.md +++ b/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 diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp index 5de644fd8ffd1..654057e7bb8d7 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp @@ -26,6 +26,7 @@ #include #include +#include #include 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 & max_z = std::nullopt); /** * @brief remove points in the given polygon */ void remove_polygon_cgal_from_cloud( const pcl::PointCloud & cloud_in, const PolygonCgal & polyline_polygon, - pcl::PointCloud & cloud_out); + pcl::PointCloud & cloud_out, const std::optional & 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 & polyline_polygons, sensor_msgs::msg::PointCloud2 & cloud_out); + const std::vector & polyline_polygons, sensor_msgs::msg::PointCloud2 & cloud_out, + const std::optional & max_z = std::nullopt); /** * @brief remove points in the given polygons */ void remove_polygon_cgal_from_cloud( const pcl::PointCloud & cloud_in, - const std::vector & polyline_polygons, pcl::PointCloud & cloud_out); + const std::vector & polyline_polygons, pcl::PointCloud & cloud_out, + const std::optional & max_z = std::nullopt); /** * @brief return true if the given point is inside the at least one of the polygons diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp index 894768d385438..760ffbe1eb1dd 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp +++ b/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 diff --git a/sensing/pointcloud_preprocessor/src/utility/utilities.cpp b/sensing/pointcloud_preprocessor/src/utility/utilities.cpp index fe9fde7301fd7..ec84a2467db78 100644 --- a/sensing/pointcloud_preprocessor/src/utility/utilities.cpp +++ b/sensing/pointcloud_preprocessor/src/utility/utilities.cpp @@ -46,18 +46,19 @@ 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 & max_z) { pcl::PointCloud pcl_output; 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) { - // 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,17 +73,18 @@ void remove_polygon_cgal_from_cloud( void remove_polygon_cgal_from_cloud( const pcl::PointCloud & cloud_in, const PolygonCgal & polyline_polygon, - pcl::PointCloud & cloud_out) + pcl::PointCloud & cloud_out, const std::optional & 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); } } @@ -90,7 +92,8 @@ void remove_polygon_cgal_from_cloud( void remove_polygon_cgal_from_cloud( const sensor_msgs::msg::PointCloud2 & cloud_in, - const std::vector & polyline_polygons, sensor_msgs::msg::PointCloud2 & cloud_out) + const std::vector & polyline_polygons, sensor_msgs::msg::PointCloud2 & cloud_out, + const std::optional & max_z) { if (polyline_polygons.empty()) { cloud_out = cloud_in; @@ -101,9 +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 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( void remove_polygon_cgal_from_cloud( const pcl::PointCloud & cloud_in, - const std::vector & polyline_polygons, pcl::PointCloud & cloud_out) + const std::vector & polyline_polygons, pcl::PointCloud & cloud_out, + const std::optional & max_z) { if (polyline_polygons.empty()) { cloud_out = cloud_in; @@ -124,8 +130,10 @@ void remove_polygon_cgal_from_cloud( pcl::PointCloud 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); diff --git a/sensing/pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp b/sensing/pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp index 8fcf15326829f..6b4e90a697cd6 100644 --- a/sensing/pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp +++ b/sensing/pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp @@ -40,7 +40,8 @@ lanelet::ConstPolygons3d calcIntersectedPolygons( } pcl::PointCloud removePointsWithinPolygons( - const pcl::PointCloud::Ptr & cloud_in, const lanelet::ConstPolygons3d & polygons) + const pcl::PointCloud::Ptr & cloud_in, const lanelet::ConstPolygons3d & polygons, + const std::optional & z_threshold_) { std::vector cgal_polys; @@ -53,7 +54,7 @@ pcl::PointCloud removePointsWithinPolygons( pcl::PointCloud 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( "input/vector_map", rclcpp::QoS{1}.transient_local(), std::bind(&VectorMapInsideAreaFilterComponent::mapCallback, this, _1)); + + // Set parameters + use_z_filter_ = declare_parameter("use_z_filter", false); + z_threshold_ = declare_parameter("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 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); 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(); +}