From 9ced49aa6af76109699d877f0ff8fed7001694ff Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Wed, 31 Jan 2024 21:46:38 +0900 Subject: [PATCH 1/4] chore(ground_segmentation): rework params (#6209) * chore: remove default params Signed-off-by: badai-nguyen * fix: scan ground gtest Signed-off-by: badai-nguyen * fix: ray ground filter gtest Signed-off-by: badai-nguyen --------- Signed-off-by: badai-nguyen --- perception/ground_segmentation/CMakeLists.txt | 1 + .../config/ground_segmentation.param.yaml | 4 ++ .../config/ransac_ground_filter.param.yaml | 14 +++++ .../config/ray_ground_filter.param.yaml | 14 +++++ .../config/scan_ground_filter.param.yaml | 17 +++++ .../launch/ransac_ground_filter.launch.xml | 11 ++++ .../launch/ray_ground_filter.launch.xml | 11 ++++ .../launch/scan_ground_filter.launch.xml | 15 +++++ .../src/ransac_ground_filter_nodelet.cpp | 24 +++---- .../src/ray_ground_filter_nodelet.cpp | 24 +++---- .../src/scan_ground_filter_nodelet.cpp | 34 +++++----- .../test/test_ray_ground_filter.cpp | 40 ++++++++++-- .../test/test_scan_ground_filter.cpp | 63 +++++++++++-------- 13 files changed, 200 insertions(+), 72 deletions(-) create mode 100644 perception/ground_segmentation/config/ransac_ground_filter.param.yaml create mode 100644 perception/ground_segmentation/config/ray_ground_filter.param.yaml create mode 100644 perception/ground_segmentation/config/scan_ground_filter.param.yaml create mode 100644 perception/ground_segmentation/launch/ransac_ground_filter.launch.xml create mode 100644 perception/ground_segmentation/launch/ray_ground_filter.launch.xml create mode 100644 perception/ground_segmentation/launch/scan_ground_filter.launch.xml diff --git a/perception/ground_segmentation/CMakeLists.txt b/perception/ground_segmentation/CMakeLists.txt index bdd793896062a..656ffec486c3c 100644 --- a/perception/ground_segmentation/CMakeLists.txt +++ b/perception/ground_segmentation/CMakeLists.txt @@ -89,6 +89,7 @@ if(BUILD_TESTING) target_link_libraries(test_ray_ground_filter ground_segmentation + ${YAML_CPP_LIBRARIES} ) ament_add_ros_isolated_gtest(test_scan_ground_filter diff --git a/perception/ground_segmentation/config/ground_segmentation.param.yaml b/perception/ground_segmentation/config/ground_segmentation.param.yaml index 8d56e12244716..756882391183e 100644 --- a/perception/ground_segmentation/config/ground_segmentation.param.yaml +++ b/perception/ground_segmentation/config/ground_segmentation.param.yaml @@ -29,3 +29,7 @@ gnd_grid_buffer_size: 4 detection_range_z_max: 2.5 elevation_grid_mode: true + low_priority_region_x: -20.0 + center_pcl_shift: 0.0 + radial_divider_angle_deg: 1.0 + use_recheck_ground_cluster: true diff --git a/perception/ground_segmentation/config/ransac_ground_filter.param.yaml b/perception/ground_segmentation/config/ransac_ground_filter.param.yaml new file mode 100644 index 0000000000000..0d4a7d574499c --- /dev/null +++ b/perception/ground_segmentation/config/ransac_ground_filter.param.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + base_frame: "base_link" + unit_axis: "z" + max_iterations: 1000 + min_trial: 5000 + min_points: 1000 + outlier_threshold: 0.01 + plane_slope_threshold: 10.0 + voxel_size_x: 0.04 + voxel_size_y: 0.04 + voxel_size_z: 0.04 + height_threshold: 0.01 + debug: false diff --git a/perception/ground_segmentation/config/ray_ground_filter.param.yaml b/perception/ground_segmentation/config/ray_ground_filter.param.yaml new file mode 100644 index 0000000000000..5b9e8c06595f7 --- /dev/null +++ b/perception/ground_segmentation/config/ray_ground_filter.param.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + min_x: -0.01 + max_x: 0.01 + min_y: -0.01 + max_y: 0.01 + use_vehicle_footprint: false + general_max_slope: 8.0 + local_max_slope: 6.0 + initial_max_slope: 3.0 + radial_divider_angle: 1.0 + min_height_threshold: 0.15 + concentric_divider_distance: 0.0 + reclass_distance_threshold: 0.1 diff --git a/perception/ground_segmentation/config/scan_ground_filter.param.yaml b/perception/ground_segmentation/config/scan_ground_filter.param.yaml new file mode 100644 index 0000000000000..bc02a1ab7e6e7 --- /dev/null +++ b/perception/ground_segmentation/config/scan_ground_filter.param.yaml @@ -0,0 +1,17 @@ +/**: + ros__parameters: + global_slope_max_angle_deg: 10.0 + local_slope_max_angle_deg: 13.0 # recommended 30.0 for non elevation_grid_mode + split_points_distance_tolerance: 0.2 + use_virtual_ground_point: True + split_height_distance: 0.2 + non_ground_height_threshold: 0.20 + grid_size_m: 0.1 + grid_mode_switch_radius: 20.0 + gnd_grid_buffer_size: 4 + detection_range_z_max: 2.5 + elevation_grid_mode: true + low_priority_region_x: -20.0 + center_pcl_shift: 0.0 + radial_divider_angle_deg: 1.0 + use_recheck_ground_cluster: true diff --git a/perception/ground_segmentation/launch/ransac_ground_filter.launch.xml b/perception/ground_segmentation/launch/ransac_ground_filter.launch.xml new file mode 100644 index 0000000000000..f8280b774c6b0 --- /dev/null +++ b/perception/ground_segmentation/launch/ransac_ground_filter.launch.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/perception/ground_segmentation/launch/ray_ground_filter.launch.xml b/perception/ground_segmentation/launch/ray_ground_filter.launch.xml new file mode 100644 index 0000000000000..86375c520a426 --- /dev/null +++ b/perception/ground_segmentation/launch/ray_ground_filter.launch.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/perception/ground_segmentation/launch/scan_ground_filter.launch.xml b/perception/ground_segmentation/launch/scan_ground_filter.launch.xml new file mode 100644 index 0000000000000..2a9e4983ecb56 --- /dev/null +++ b/perception/ground_segmentation/launch/scan_ground_filter.launch.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp index ebcb02df2c614..ed5fb6abe7fe7 100644 --- a/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp @@ -85,18 +85,18 @@ using pointcloud_preprocessor::get_param; RANSACGroundFilterComponent::RANSACGroundFilterComponent(const rclcpp::NodeOptions & options) : Filter("RANSACGroundFilter", options) { - base_frame_ = declare_parameter("base_frame", "base_link"); - unit_axis_ = declare_parameter("unit_axis", "z"); - max_iterations_ = declare_parameter("max_iterations", 1000); - min_inliers_ = declare_parameter("min_trial", 5000); - min_points_ = declare_parameter("min_points", 1000); - outlier_threshold_ = declare_parameter("outlier_threshold", 0.01); - plane_slope_threshold_ = declare_parameter("plane_slope_threshold", 10.0); - voxel_size_x_ = declare_parameter("voxel_size_x", 0.04); - voxel_size_y_ = declare_parameter("voxel_size_y", 0.04); - voxel_size_z_ = declare_parameter("voxel_size_z", 0.04); - height_threshold_ = declare_parameter("height_threshold", 0.01); - debug_ = declare_parameter("debug", false); + base_frame_ = declare_parameter("base_frame", "base_link"); + unit_axis_ = declare_parameter("unit_axis"); + max_iterations_ = declare_parameter("max_iterations"); + min_inliers_ = declare_parameter("min_trial"); + min_points_ = declare_parameter("min_points"); + outlier_threshold_ = declare_parameter("outlier_threshold"); + plane_slope_threshold_ = declare_parameter("plane_slope_threshold"); + voxel_size_x_ = declare_parameter("voxel_size_x"); + voxel_size_y_ = declare_parameter("voxel_size_y"); + voxel_size_z_ = declare_parameter("voxel_size_z"); + height_threshold_ = declare_parameter("height_threshold"); + debug_ = declare_parameter("debug"); if (unit_axis_ == "x") { unit_vec_ = Eigen::Vector3d::UnitX(); diff --git a/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp index 4b2e81d272e30..7bcae47fd9f1f 100644 --- a/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp @@ -48,22 +48,22 @@ RayGroundFilterComponent::RayGroundFilterComponent(const rclcpp::NodeOptions & o grid_precision_ = 0.2; ray_ground_filter::generateColors(colors_, color_num_); - min_x_ = declare_parameter("min_x", -0.01); - max_x_ = declare_parameter("max_x", 0.01); - min_y_ = declare_parameter("min_y", -0.01); - max_y_ = declare_parameter("max_y", 0.01); + min_x_ = declare_parameter("min_x"); + max_x_ = declare_parameter("max_x"); + min_y_ = declare_parameter("min_y"); + max_y_ = declare_parameter("max_y"); setVehicleFootprint(min_x_, max_x_, min_y_, max_y_); - use_vehicle_footprint_ = declare_parameter("use_vehicle_footprint", false); + use_vehicle_footprint_ = declare_parameter("use_vehicle_footprint", false); - general_max_slope_ = declare_parameter("general_max_slope", 8.0); - local_max_slope_ = declare_parameter("local_max_slope", 6.0); - initial_max_slope_ = declare_parameter("initial_max_slope", 3.0); - radial_divider_angle_ = declare_parameter("radial_divider_angle", 1.0); - min_height_threshold_ = declare_parameter("min_height_threshold", 0.15); - concentric_divider_distance_ = declare_parameter("concentric_divider_distance", 0.0); - reclass_distance_threshold_ = declare_parameter("reclass_distance_threshold", 0.1); + general_max_slope_ = declare_parameter("general_max_slope"); + local_max_slope_ = declare_parameter("local_max_slope"); + initial_max_slope_ = declare_parameter("initial_max_slope"); + radial_divider_angle_ = declare_parameter("radial_divider_angle"); + min_height_threshold_ = declare_parameter("min_height_threshold"); + concentric_divider_distance_ = declare_parameter("concentric_divider_distance"); + reclass_distance_threshold_ = declare_parameter("reclass_distance_threshold"); } using std::placeholders::_1; diff --git a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp index 953a9feb4d21e..f164e0102e0e9 100644 --- a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp @@ -37,24 +37,22 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & { // set initial parameters { - low_priority_region_x_ = static_cast(declare_parameter("low_priority_region_x", -20.0f)); - detection_range_z_max_ = static_cast(declare_parameter("detection_range_z_max", 2.5f)); - center_pcl_shift_ = static_cast(declare_parameter("center_pcl_shift", 0.0)); - non_ground_height_threshold_ = - static_cast(declare_parameter("non_ground_height_threshold", 0.20)); - grid_mode_switch_radius_ = - static_cast(declare_parameter("grid_mode_switch_radius", 20.0)); - - grid_size_m_ = static_cast(declare_parameter("grid_size_m", 0.5)); - gnd_grid_buffer_size_ = static_cast(declare_parameter("gnd_grid_buffer_size", 4)); - elevation_grid_mode_ = static_cast(declare_parameter("elevation_grid_mode", true)); - global_slope_max_angle_rad_ = deg2rad(declare_parameter("global_slope_max_angle_deg", 8.0)); - local_slope_max_angle_rad_ = deg2rad(declare_parameter("local_slope_max_angle_deg", 10.0)); - radial_divider_angle_rad_ = deg2rad(declare_parameter("radial_divider_angle_deg", 1.0)); - split_points_distance_tolerance_ = declare_parameter("split_points_distance_tolerance", 0.2); - split_height_distance_ = declare_parameter("split_height_distance", 0.2); - use_virtual_ground_point_ = declare_parameter("use_virtual_ground_point", true); - use_recheck_ground_cluster_ = declare_parameter("use_recheck_ground_cluster", true); + low_priority_region_x_ = declare_parameter("low_priority_region_x"); + detection_range_z_max_ = declare_parameter("detection_range_z_max"); + center_pcl_shift_ = declare_parameter("center_pcl_shift"); + non_ground_height_threshold_ = declare_parameter("non_ground_height_threshold"); + grid_mode_switch_radius_ = declare_parameter("grid_mode_switch_radius"); + + grid_size_m_ = declare_parameter("grid_size_m"); + gnd_grid_buffer_size_ = declare_parameter("gnd_grid_buffer_size"); + elevation_grid_mode_ = declare_parameter("elevation_grid_mode"); + global_slope_max_angle_rad_ = deg2rad(declare_parameter("global_slope_max_angle_deg")); + local_slope_max_angle_rad_ = deg2rad(declare_parameter("local_slope_max_angle_deg")); + radial_divider_angle_rad_ = deg2rad(declare_parameter("radial_divider_angle_deg")); + split_points_distance_tolerance_ = declare_parameter("split_points_distance_tolerance"); + split_height_distance_ = declare_parameter("split_height_distance"); + use_virtual_ground_point_ = declare_parameter("use_virtual_ground_point"); + use_recheck_ground_cluster_ = declare_parameter("use_recheck_ground_cluster"); radial_dividers_num_ = std::ceil(2.0 * M_PI / radial_divider_angle_rad_); vehicle_info_ = VehicleInfoUtil(*this).getVehicleInfo(); diff --git a/perception/ground_segmentation/test/test_ray_ground_filter.cpp b/perception/ground_segmentation/test/test_ray_ground_filter.cpp index af8bb29ab12b7..0c1db47a5ae09 100644 --- a/perception/ground_segmentation/test/test_ray_ground_filter.cpp +++ b/perception/ground_segmentation/test/test_ray_ground_filter.cpp @@ -29,7 +29,7 @@ #include #include #endif - +#include class RayGroundFilterComponentTestSuite : public ::testing::Test { protected: @@ -63,8 +63,25 @@ class RayGroundFilterComponentTest : public ground_segmentation::RayGroundFilter TEST_F(RayGroundFilterComponentTestSuite, TestCase1) { - // read pcd to pointcloud const auto share_dir = ament_index_cpp::get_package_share_directory("ground_segmentation"); + const auto config_path = share_dir + "/config/ray_ground_filter.param.yaml"; + // std::cout << "config_path:" << config_path << std::endl; + YAML::Node config = YAML::LoadFile(config_path); + auto params = config["/**"]["ros__parameters"]; + + double min_x_ = params["min_x"].as(); + double max_x_ = params["max_x"].as(); + double min_y_ = params["min_y"].as(); + double max_y_ = params["max_y"].as(); + bool use_vehicle_footprint_ = params["use_vehicle_footprint"].as(); + double general_max_slope_ = params["general_max_slope"].as(); + double local_max_slope_ = params["local_max_slope"].as(); + double initial_max_slope_ = params["initial_max_slope"].as(); + double radial_divider_angle_ = params["radial_divider_angle"].as(); + double min_height_threshold_ = params["min_height_threshold"].as(); + double concentric_divider_distance_ = params["concentric_divider_distance"].as(); + double reclass_distance_threshold_ = params["reclass_distance_threshold"].as(); + const auto pcd_path = share_dir + "/data/test.pcd"; pcl::PointCloud cloud; pcl::io::loadPCDFile(pcd_path, cloud); @@ -94,10 +111,23 @@ TEST_F(RayGroundFilterComponentTestSuite, TestCase1) rclcpp::NodeOptions node_options; std::vector parameters; + parameters.emplace_back(rclcpp::Parameter("base_frame", "base_link")); - parameters.emplace_back(rclcpp::Parameter("general_max_slope", 2.0)); - parameters.emplace_back(rclcpp::Parameter("local_max_slope", 3.0)); - parameters.emplace_back(rclcpp::Parameter("initial_max_slope", 1.0)); + parameters.emplace_back(rclcpp::Parameter("general_max_slope", general_max_slope_)); + parameters.emplace_back(rclcpp::Parameter("local_max_slope", local_max_slope_)); + parameters.emplace_back(rclcpp::Parameter("initial_max_slope", initial_max_slope_)); + parameters.emplace_back(rclcpp::Parameter("radial_divider_angle", radial_divider_angle_)); + parameters.emplace_back(rclcpp::Parameter("min_height_threshold", min_height_threshold_)); + parameters.emplace_back( + rclcpp::Parameter("concentric_divider_distance", concentric_divider_distance_)); + parameters.emplace_back( + rclcpp::Parameter("reclass_distance_threshold", reclass_distance_threshold_)); + parameters.emplace_back(rclcpp::Parameter("min_x", min_x_)); + parameters.emplace_back(rclcpp::Parameter("max_x", max_x_)); + parameters.emplace_back(rclcpp::Parameter("min_y", min_y_)); + parameters.emplace_back(rclcpp::Parameter("max_y", max_y_)); + parameters.emplace_back(rclcpp::Parameter("use_vehicle_footprint", use_vehicle_footprint_)); + node_options.parameter_overrides(parameters); auto ray_ground_filter_test = std::make_shared(node_options); ray_ground_filter_test->input_pointcloud_pub_->publish(*input_msg_ptr); diff --git a/perception/ground_segmentation/test/test_scan_ground_filter.cpp b/perception/ground_segmentation/test/test_scan_ground_filter.cpp index acc81f8817cd8..9df9c7e9c7433 100644 --- a/perception/ground_segmentation/test/test_scan_ground_filter.cpp +++ b/perception/ground_segmentation/test/test_scan_ground_filter.cpp @@ -38,6 +38,8 @@ class ScanGroundFilterTest : public ::testing::Test { rclcpp::init(0, nullptr); + parse_yaml(); + dummy_node_ = std::make_shared("ScanGroundFilterTest"); input_pointcloud_pub_ = rclcpp::create_publisher( dummy_node_, "/test_scan_ground_filter/input_cloud", 1); @@ -58,6 +60,30 @@ class ScanGroundFilterTest : public ::testing::Test parameters.emplace_back(rclcpp::Parameter("right_overhang", 0.1)); parameters.emplace_back(rclcpp::Parameter("vehicle_height", 2.5)); parameters.emplace_back(rclcpp::Parameter("max_steer_angle", 0.7)); + + parameters.emplace_back( + rclcpp::Parameter("global_slope_max_angle_deg", global_slope_max_angle_deg_)); + parameters.emplace_back( + rclcpp::Parameter("local_slope_max_angle_deg", local_slope_max_angle_deg_)); + parameters.emplace_back( + rclcpp::Parameter("split_points_distance_tolerance", split_points_distance_tolerance_)); + parameters.emplace_back( + rclcpp::Parameter("use_virtual_ground_point", use_virtual_ground_point_)); + parameters.emplace_back(rclcpp::Parameter("split_height_distance", split_height_distance_)); + parameters.emplace_back( + rclcpp::Parameter("non_ground_height_threshold", non_ground_height_threshold_)); + parameters.emplace_back(rclcpp::Parameter("grid_size_m", grid_size_m_)); + parameters.emplace_back(rclcpp::Parameter("grid_mode_switch_radius", grid_mode_switch_radius_)); + parameters.emplace_back(rclcpp::Parameter("gnd_grid_buffer_size", gnd_grid_buffer_size_)); + parameters.emplace_back(rclcpp::Parameter("detection_range_z_max", detection_range_z_max_)); + parameters.emplace_back(rclcpp::Parameter("elevation_grid_mode", elevation_grid_mode_)); + parameters.emplace_back(rclcpp::Parameter("low_priority_region_x", low_priority_region_x_)); + parameters.emplace_back(rclcpp::Parameter("center_pcl_shift", center_pcl_shift_)); + parameters.emplace_back( + rclcpp::Parameter("radial_divider_angle_deg", radial_divider_angle_deg_)); + parameters.emplace_back( + rclcpp::Parameter("use_recheck_ground_cluster", use_recheck_ground_cluster_)); + options.parameter_overrides(parameters); scan_ground_filter_ = std::make_shared(options); @@ -88,8 +114,6 @@ class ScanGroundFilterTest : public ::testing::Test t.transform.rotation.w = q.w(); tf2::doTransform(*origin_input_msg_ptr, *input_msg_ptr_, t); - - parse_yaml(); } ScanGroundFilterTest() {} @@ -113,10 +137,10 @@ class ScanGroundFilterTest : public ::testing::Test void parse_yaml() { const auto share_dir = ament_index_cpp::get_package_share_directory("ground_segmentation"); - const auto config_path = share_dir + "/config/ground_segmentation.param.yaml"; + const auto config_path = share_dir + "/config/scan_ground_filter.param.yaml"; // std::cout << "config_path:" << config_path << std::endl; YAML::Node config = YAML::LoadFile(config_path); - auto params = config["/**"]["ros__parameters"]["common_ground_filter"]["parameters"]; + auto params = config["/**"]["ros__parameters"]; global_slope_max_angle_deg_ = params["global_slope_max_angle_deg"].as(); local_slope_max_angle_deg_ = params["local_slope_max_angle_deg"].as(); split_points_distance_tolerance_ = params["split_points_distance_tolerance"].as(); @@ -127,6 +151,11 @@ class ScanGroundFilterTest : public ::testing::Test gnd_grid_buffer_size_ = params["gnd_grid_buffer_size"].as(); detection_range_z_max_ = params["detection_range_z_max"].as(); elevation_grid_mode_ = params["elevation_grid_mode"].as(); + low_priority_region_x_ = params["low_priority_region_x"].as(); + use_virtual_ground_point_ = params["use_virtual_ground_point"].as(); + center_pcl_shift_ = params["center_pcl_shift"].as(); + radial_divider_angle_deg_ = params["radial_divider_angle_deg"].as(); + use_recheck_ground_cluster_ = params["use_recheck_ground_cluster"].as(); } float global_slope_max_angle_deg_ = 0.0; @@ -139,6 +168,11 @@ class ScanGroundFilterTest : public ::testing::Test uint16_t gnd_grid_buffer_size_ = 0; float detection_range_z_max_ = 0.0; bool elevation_grid_mode_ = false; + float low_priority_region_x_ = 0.0; + bool use_virtual_ground_point_; + float center_pcl_shift_; + float radial_divider_angle_deg_; + bool use_recheck_ground_cluster_; }; TEST_F(ScanGroundFilterTest, TestCase1) @@ -146,27 +180,6 @@ TEST_F(ScanGroundFilterTest, TestCase1) input_pointcloud_pub_->publish(*input_msg_ptr_); sensor_msgs::msg::PointCloud2 out_cloud; - // set filter parameter - scan_ground_filter_->set_parameter( - rclcpp::Parameter("global_slope_max_angle_deg", global_slope_max_angle_deg_)); - scan_ground_filter_->set_parameter( - rclcpp::Parameter("local_slope_max_angle_deg", local_slope_max_angle_deg_)); - scan_ground_filter_->set_parameter( - rclcpp::Parameter("split_points_distance_tolerance", split_points_distance_tolerance_)); - scan_ground_filter_->set_parameter( - rclcpp::Parameter("split_height_distance", split_height_distance_)); - scan_ground_filter_->set_parameter( - rclcpp::Parameter("non_ground_height_threshold", non_ground_height_threshold_)); - scan_ground_filter_->set_parameter(rclcpp::Parameter("grid_size_m", grid_size_m_)); - scan_ground_filter_->set_parameter( - rclcpp::Parameter("grid_mode_switch_radius", grid_mode_switch_radius_)); - scan_ground_filter_->set_parameter( - rclcpp::Parameter("gnd_grid_buffer_size", gnd_grid_buffer_size_)); - scan_ground_filter_->set_parameter( - rclcpp::Parameter("detection_range_z_max", detection_range_z_max_)); - scan_ground_filter_->set_parameter( - rclcpp::Parameter("elevation_grid_mode", elevation_grid_mode_)); - filter(out_cloud); output_pointcloud_pub_->publish(out_cloud); From 434cef6953d8183fb067c33bb7d4896104427d4c Mon Sep 17 00:00:00 2001 From: taisa1 <38522559+taisa1@users.noreply.github.com> Date: Mon, 11 Mar 2024 17:14:15 +0900 Subject: [PATCH 2/4] perf(common_ground_filter): performance tuning (#5861) * perf(scan_ground_filter): performance tuning Signed-off-by: taisa1 * perf(scan_ground_filter): improved performance Signed-off-by: taisa1 * chore: refactoring Signed-off-by: taisa1 * chore: refactoring Signed-off-by: taisa1 * refactor: reflect review Signed-off-by: taisa1 * style(pre-commit): autofix * fix: remove unnecessary comment Signed-off-by: taisa1 * test: change unit test to use faster_filter Signed-off-by: taisa1 * test: fixed compile error Signed-off-by: taisa1 * fix: add lacking parameter callback Signed-off-by: taisa1 * fix: uninitialized valuabe Signed-off-by: atsushi421 --------- Signed-off-by: taisa1 Signed-off-by: atsushi421 Co-authored-by: taisa1 Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: atsushi yano <55824710+atsushi421@users.noreply.github.com> Co-authored-by: atsushi421 --- .../scan_ground_filter_nodelet.hpp | 86 ++-- .../src/scan_ground_filter_nodelet.cpp | 402 +++++++++++------- .../test/test_scan_ground_filter.cpp | 3 +- .../pointcloud_preprocessor/src/filter.cpp | 2 +- 4 files changed, 310 insertions(+), 183 deletions(-) diff --git a/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp b/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp index 980565fe2144b..d017d06929702 100644 --- a/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp +++ b/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp @@ -16,6 +16,7 @@ #define GROUND_SEGMENTATION__SCAN_GROUND_FILTER_NODELET_HPP_ #include "pointcloud_preprocessor/filter.hpp" +#include "pointcloud_preprocessor/transform_info.hpp" #include @@ -50,7 +51,7 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter // classified point label // (0: not classified, 1: ground, 2: not ground, 3: follow previous point, // 4: unkown(currently not used), 5: virtual ground) - enum class PointLabel { + enum class PointLabel : uint16_t { INIT = 0, GROUND, NON_GROUND, @@ -59,19 +60,15 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter VIRTUAL_GROUND, OUT_OF_RANGE }; - struct PointRef + + struct PointData { - float grid_size; // radius of grid - uint16_t grid_id; // id of grid in vertical - float radius; // cylindrical coords on XY Plane - float theta; // angle deg on XY plane - size_t radial_div; // index of the radial division to which this point belongs to + float radius; // cylindrical coords on XY Plane PointLabel point_state{PointLabel::INIT}; - + uint16_t grid_id; // id of grid in vertical size_t orig_index; // index of this point in the source pointcloud - pcl::PointXYZ * orig_point; }; - using PointCloudRefVector = std::vector; + using PointCloudVector = std::vector; struct GridCenter { @@ -144,18 +141,39 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter pcl::PointIndices getIndices() { return pcl_indices; } std::vector getHeightList() { return height_list; } + + pcl::PointIndices & getIndicesRef() { return pcl_indices; } + std::vector & getHeightListRef() { return height_list; } }; void filter( const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) override; + // TODO(taisa1): Temporary Implementation: Remove this interface when all the filter nodes + // conform to new API + virtual void faster_filter( + const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output, + const pointcloud_preprocessor::TransformInfo & transform_info); + tf2_ros::Buffer tf_buffer_{get_clock()}; tf2_ros::TransformListener tf_listener_{tf_buffer_}; + int x_offset_; + int y_offset_; + int z_offset_; + int intensity_offset_; + bool offset_initialized_; + + void set_field_offsets(const PointCloud2ConstPtr & input); + + void get_point_from_global_offset( + const PointCloud2ConstPtr & input, pcl::PointXYZ & point, size_t global_offset); + const uint16_t gnd_grid_continual_thresh_ = 3; bool elevation_grid_mode_; float non_ground_height_threshold_; float grid_size_rad_; + float tan_grid_size_rad_; float grid_size_m_; float low_priority_region_x_; uint16_t gnd_grid_buffer_size_; @@ -163,14 +181,17 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter float grid_mode_switch_angle_rad_; float virtual_lidar_z_; float detection_range_z_max_; - float center_pcl_shift_; // virtual center of pcl to center mass - float grid_mode_switch_radius_; // non linear grid size switching distance - double global_slope_max_angle_rad_; // radians - double local_slope_max_angle_rad_; // radians + float center_pcl_shift_; // virtual center of pcl to center mass + float grid_mode_switch_radius_; // non linear grid size switching distance + double global_slope_max_angle_rad_; // radians + double local_slope_max_angle_rad_; // radians + double global_slope_max_ratio_; + double local_slope_max_ratio_; double radial_divider_angle_rad_; // distance in rads between dividers double split_points_distance_tolerance_; // distance in meters between concentric divisions - double // minimum height threshold regardless the slope, - split_height_distance_; // useful for close points + double split_points_distance_tolerance_square_; + double // minimum height threshold regardless the slope, + split_height_distance_; // useful for close points bool use_virtual_ground_point_; bool use_recheck_ground_cluster_; // to enable recheck ground cluster size_t radial_dividers_num_; @@ -186,23 +207,25 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter */ /*! - * Convert pcl::PointCloud to sorted PointCloudRefVector + * Convert sensor_msgs::msg::PointCloud2 to sorted PointCloudVector * @param[in] in_cloud Input Point Cloud to be organized in radial segments * @param[out] out_radial_ordered_points_manager Vector of Points Clouds, * each element will contain the points ordered */ void convertPointcloud( - const pcl::PointCloud::Ptr in_cloud, - std::vector & out_radial_ordered_points_manager); + const PointCloud2ConstPtr & in_cloud, + std::vector & out_radial_ordered_points_manager); void convertPointcloudGridScan( - const pcl::PointCloud::Ptr in_cloud, - std::vector & out_radial_ordered_points_manager); + const PointCloud2ConstPtr & in_cloud, + std::vector & out_radial_ordered_points_manager); /*! * Output ground center of front wheels as the virtual ground point * @param[out] point Virtual ground origin point */ void calcVirtualGroundOrigin(pcl::PointXYZ & point); + float calcGridSize(const PointData & p); + /*! * Classifies Points in the PointCloud as Ground and Not Ground * @param in_radial_ordered_clouds Vector of an Ordered PointsCloud @@ -214,14 +237,19 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter void initializeFirstGndGrids( const float h, const float r, const uint16_t id, std::vector & gnd_grids); - void checkContinuousGndGrid(PointRef & p, const std::vector & gnd_grids_list); - void checkDiscontinuousGndGrid(PointRef & p, const std::vector & gnd_grids_list); - void checkBreakGndGrid(PointRef & p, const std::vector & gnd_grids_list); + void checkContinuousGndGrid( + PointData & p, pcl::PointXYZ & p_orig_point, const std::vector & gnd_grids_list); + void checkDiscontinuousGndGrid( + PointData & p, pcl::PointXYZ & p_orig_point, const std::vector & gnd_grids_list); + void checkBreakGndGrid( + PointData & p, pcl::PointXYZ & p_orig_point, const std::vector & gnd_grids_list); void classifyPointCloud( - std::vector & in_radial_ordered_clouds, + const PointCloud2ConstPtr & in_cloud_ptr, + std::vector & in_radial_ordered_clouds, pcl::PointIndices & out_no_ground_indices); void classifyPointCloudGridScan( - std::vector & in_radial_ordered_clouds, + const PointCloud2ConstPtr & in_cloud_ptr, + std::vector & in_radial_ordered_clouds, pcl::PointIndices & out_no_ground_indices); /*! * Re-classifies point of ground cluster based on their height @@ -237,11 +265,11 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter * and the other removed as indicated in the indices * @param in_cloud_ptr Input PointCloud to which the extraction will be performed * @param in_indices Indices of the points to be both removed and kept - * @param out_object_cloud_ptr Resulting PointCloud with the indices kept + * @param out_object_cloud Resulting PointCloud with the indices kept */ void extractObjectPoints( - const pcl::PointCloud::Ptr in_cloud_ptr, const pcl::PointIndices & in_indices, - pcl::PointCloud::Ptr out_object_cloud_ptr); + const PointCloud2ConstPtr & in_cloud_ptr, const pcl::PointIndices & in_indices, + PointCloud2 & out_object_cloud); /** \brief Parameter service callback result : needed to be hold */ OnSetParametersCallbackHandle::SharedPtr set_param_res_; diff --git a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp index f164e0102e0e9..7a0333d95075b 100644 --- a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp @@ -48,8 +48,12 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & elevation_grid_mode_ = declare_parameter("elevation_grid_mode"); global_slope_max_angle_rad_ = deg2rad(declare_parameter("global_slope_max_angle_deg")); local_slope_max_angle_rad_ = deg2rad(declare_parameter("local_slope_max_angle_deg")); + global_slope_max_ratio_ = std::tan(global_slope_max_angle_rad_); + local_slope_max_ratio_ = std::tan(local_slope_max_angle_rad_); radial_divider_angle_rad_ = deg2rad(declare_parameter("radial_divider_angle_deg")); split_points_distance_tolerance_ = declare_parameter("split_points_distance_tolerance"); + split_points_distance_tolerance_square_ = + split_points_distance_tolerance_ * split_points_distance_tolerance_; split_height_distance_ = declare_parameter("split_height_distance"); use_virtual_ground_point_ = declare_parameter("use_virtual_ground_point"); use_recheck_ground_cluster_ = declare_parameter("use_recheck_ground_cluster"); @@ -60,6 +64,12 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & grid_mode_switch_radius_ / grid_size_m_; // changing the mode of grid division virtual_lidar_z_ = vehicle_info_.vehicle_height_m; grid_mode_switch_angle_rad_ = std::atan2(grid_mode_switch_radius_, virtual_lidar_z_); + + grid_size_rad_ = + normalizeRadian(std::atan2(grid_mode_switch_radius_ + grid_size_m_, virtual_lidar_z_)) - + normalizeRadian(std::atan2(grid_mode_switch_radius_, virtual_lidar_z_)); + tan_grid_size_rad_ = std::tan(grid_size_rad_); + offset_initialized_ = false; } using std::placeholders::_1; @@ -77,92 +87,117 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & } } +inline void ScanGroundFilterComponent::set_field_offsets(const PointCloud2ConstPtr & input) +{ + x_offset_ = input->fields[pcl::getFieldIndex(*input, "x")].offset; + y_offset_ = input->fields[pcl::getFieldIndex(*input, "y")].offset; + z_offset_ = input->fields[pcl::getFieldIndex(*input, "z")].offset; + int intensity_index = pcl::getFieldIndex(*input, "intensity"); + if (intensity_index != -1) { + intensity_offset_ = input->fields[intensity_index].offset; + } else { + intensity_offset_ = z_offset_ + sizeof(float); + } + offset_initialized_ = true; +} + +inline void ScanGroundFilterComponent::get_point_from_global_offset( + const PointCloud2ConstPtr & input, pcl::PointXYZ & point, size_t global_offset) +{ + point.x = *reinterpret_cast(&input->data[global_offset + x_offset_]); + point.y = *reinterpret_cast(&input->data[global_offset + y_offset_]); + point.z = *reinterpret_cast(&input->data[global_offset + z_offset_]); +} + void ScanGroundFilterComponent::convertPointcloudGridScan( - const pcl::PointCloud::Ptr in_cloud, - std::vector & out_radial_ordered_points) + const PointCloud2ConstPtr & in_cloud, std::vector & out_radial_ordered_points) { out_radial_ordered_points.resize(radial_dividers_num_); - PointRef current_point; - uint16_t back_steps_num = 1; + PointData current_point; + + const auto inv_radial_divider_angle_rad = 1.0f / radial_divider_angle_rad_; + const auto inv_grid_size_rad = 1.0f / grid_size_rad_; + const auto inv_grid_size_m = 1.0f / grid_size_m_; + + const auto grid_id_offset = + grid_mode_switch_grid_id_ - grid_mode_switch_angle_rad_ * inv_grid_size_rad; + const auto x_shift = vehicle_info_.wheel_base_m / 2.0f + center_pcl_shift_; - grid_size_rad_ = - normalizeRadian(std::atan2(grid_mode_switch_radius_ + grid_size_m_, virtual_lidar_z_)) - - normalizeRadian(std::atan2(grid_mode_switch_radius_, virtual_lidar_z_)); - for (size_t i = 0; i < in_cloud->points.size(); ++i) { - auto x{ - in_cloud->points[i].x - vehicle_info_.wheel_base_m / 2.0f - - center_pcl_shift_}; // base on front wheel center - // auto y{in_cloud->points[i].y}; - auto radius{static_cast(std::hypot(x, in_cloud->points[i].y))}; - auto theta{normalizeRadian(std::atan2(x, in_cloud->points[i].y), 0.0)}; + const size_t in_cloud_data_size = in_cloud->data.size(); + const size_t in_cloud_point_step = in_cloud->point_step; + + size_t point_index = 0; + pcl::PointXYZ input_point; + for (size_t global_offset = 0; global_offset + in_cloud_point_step <= in_cloud_data_size; + global_offset += in_cloud_point_step) { + get_point_from_global_offset(in_cloud, input_point, global_offset); + + auto x{input_point.x - x_shift}; // base on front wheel center + auto radius{static_cast(std::hypot(x, input_point.y))}; + auto theta{normalizeRadian(std::atan2(x, input_point.y), 0.0)}; // divide by vertical angle - auto gamma{normalizeRadian(std::atan2(radius, virtual_lidar_z_), 0.0f)}; - auto radial_div{ - static_cast(std::floor(normalizeDegree(theta / radial_divider_angle_rad_, 0.0)))}; + auto radial_div{static_cast(std::floor(theta * inv_radial_divider_angle_rad))}; uint16_t grid_id = 0; - float curr_grid_size = 0.0f; if (radius <= grid_mode_switch_radius_) { - grid_id = static_cast(radius / grid_size_m_); - curr_grid_size = grid_size_m_; + grid_id = static_cast(radius * inv_grid_size_m); } else { - grid_id = grid_mode_switch_grid_id_ + (gamma - grid_mode_switch_angle_rad_) / grid_size_rad_; - if (grid_id <= grid_mode_switch_grid_id_ + back_steps_num) { - curr_grid_size = grid_size_m_; - } else { - curr_grid_size = std::tan(gamma) - std::tan(gamma - grid_size_rad_); - curr_grid_size *= virtual_lidar_z_; - } + auto gamma{normalizeRadian(std::atan2(radius, virtual_lidar_z_), 0.0f)}; + grid_id = grid_id_offset + gamma * inv_grid_size_rad; } current_point.grid_id = grid_id; - current_point.grid_size = curr_grid_size; current_point.radius = radius; - current_point.theta = theta; - current_point.radial_div = radial_div; current_point.point_state = PointLabel::INIT; - current_point.orig_index = i; - current_point.orig_point = &in_cloud->points[i]; + current_point.orig_index = point_index; // radial divisions out_radial_ordered_points[radial_div].emplace_back(current_point); + ++point_index; } // sort by distance for (size_t i = 0; i < radial_dividers_num_; ++i) { std::sort( out_radial_ordered_points[i].begin(), out_radial_ordered_points[i].end(), - [](const PointRef & a, const PointRef & b) { return a.radius < b.radius; }); + [](const PointData & a, const PointData & b) { return a.radius < b.radius; }); } } + void ScanGroundFilterComponent::convertPointcloud( - const pcl::PointCloud::Ptr in_cloud, - std::vector & out_radial_ordered_points) + const PointCloud2ConstPtr & in_cloud, std::vector & out_radial_ordered_points) { out_radial_ordered_points.resize(radial_dividers_num_); - PointRef current_point; + PointData current_point; + + const auto inv_radial_divider_angle_rad = 1.0f / radial_divider_angle_rad_; + + const size_t in_cloud_data_size = in_cloud->data.size(); + const size_t in_cloud_point_step = in_cloud->point_step; - for (size_t i = 0; i < in_cloud->points.size(); ++i) { - auto radius{static_cast(std::hypot(in_cloud->points[i].x, in_cloud->points[i].y))}; - auto theta{normalizeRadian(std::atan2(in_cloud->points[i].x, in_cloud->points[i].y), 0.0)}; - auto radial_div{ - static_cast(std::floor(normalizeDegree(theta / radial_divider_angle_rad_, 0.0)))}; + size_t point_index = 0; + pcl::PointXYZ input_point; + for (size_t global_offset = 0; global_offset + in_cloud_point_step <= in_cloud_data_size; + global_offset += in_cloud_point_step) { + // Point + get_point_from_global_offset(in_cloud, input_point, global_offset); + + auto radius{static_cast(std::hypot(input_point.x, input_point.y))}; + auto theta{normalizeRadian(std::atan2(input_point.x, input_point.y), 0.0)}; + auto radial_div{static_cast(std::floor(theta * inv_radial_divider_angle_rad))}; current_point.radius = radius; - current_point.theta = theta; - current_point.radial_div = radial_div; current_point.point_state = PointLabel::INIT; - current_point.orig_index = i; - current_point.orig_point = &in_cloud->points[i]; + current_point.orig_index = point_index; // radial divisions out_radial_ordered_points[radial_div].emplace_back(current_point); + ++point_index; } - // sort by distance for (size_t i = 0; i < radial_dividers_num_; ++i) { std::sort( out_radial_ordered_points[i].begin(), out_radial_ordered_points[i].end(), - [](const PointRef & a, const PointRef & b) { return a.radius < b.radius; }); + [](const PointData & a, const PointData & b) { return a.radius < b.radius; }); } } @@ -173,6 +208,21 @@ void ScanGroundFilterComponent::calcVirtualGroundOrigin(pcl::PointXYZ & point) point.z = 0; } +inline float ScanGroundFilterComponent::calcGridSize(const PointData & p) +{ + float grid_size = grid_size_m_; + uint16_t back_steps_num = 1; + + if ( + p.radius > grid_mode_switch_radius_ && p.grid_id > grid_mode_switch_grid_id_ + back_steps_num) { + // equivalent to grid_size = (std::tan(gamma) - std::tan(gamma - grid_size_rad_)) * + // virtual_lidar_z_ when gamma = normalizeRadian(std::atan2(radius, virtual_lidar_z_), 0.0f) + grid_size = p.radius - (p.radius - tan_grid_size_rad_ * virtual_lidar_z_) / + (1 + p.radius * tan_grid_size_rad_ / virtual_lidar_z_); + } + return grid_size; +} + void ScanGroundFilterComponent::initializeFirstGndGrids( const float h, const float r, const uint16_t id, std::vector & gnd_grids) { @@ -193,96 +243,94 @@ void ScanGroundFilterComponent::initializeFirstGndGrids( } void ScanGroundFilterComponent::checkContinuousGndGrid( - PointRef & p, const std::vector & gnd_grids_list) + PointData & p, pcl::PointXYZ & p_orig_point, const std::vector & gnd_grids_list) { float next_gnd_z = 0.0f; - float curr_gnd_slope_rad = 0.0f; + float curr_gnd_slope_ratio = 0.0f; float gnd_buff_z_mean = 0.0f; - float gnd_buff_z_max = 0.0f; float gnd_buff_radius = 0.0f; for (auto it = gnd_grids_list.end() - gnd_grid_buffer_size_ - 1; it < gnd_grids_list.end() - 1; ++it) { gnd_buff_radius += it->radius; gnd_buff_z_mean += it->avg_height; - gnd_buff_z_max += it->max_height; } gnd_buff_radius /= static_cast(gnd_grid_buffer_size_ - 1); - gnd_buff_z_max /= static_cast(gnd_grid_buffer_size_ - 1); gnd_buff_z_mean /= static_cast(gnd_grid_buffer_size_ - 1); float tmp_delta_mean_z = gnd_grids_list.back().avg_height - gnd_buff_z_mean; float tmp_delta_radius = gnd_grids_list.back().radius - gnd_buff_radius; - curr_gnd_slope_rad = std::atan(tmp_delta_mean_z / tmp_delta_radius); - curr_gnd_slope_rad = curr_gnd_slope_rad < -global_slope_max_angle_rad_ - ? -global_slope_max_angle_rad_ - : curr_gnd_slope_rad; - curr_gnd_slope_rad = curr_gnd_slope_rad > global_slope_max_angle_rad_ - ? global_slope_max_angle_rad_ - : curr_gnd_slope_rad; + curr_gnd_slope_ratio = tmp_delta_mean_z / tmp_delta_radius; + curr_gnd_slope_ratio = curr_gnd_slope_ratio < -global_slope_max_ratio_ ? -global_slope_max_ratio_ + : curr_gnd_slope_ratio; + curr_gnd_slope_ratio = + curr_gnd_slope_ratio > global_slope_max_ratio_ ? global_slope_max_ratio_ : curr_gnd_slope_ratio; - next_gnd_z = std::tan(curr_gnd_slope_rad) * (p.radius - gnd_buff_radius) + gnd_buff_z_mean; + next_gnd_z = curr_gnd_slope_ratio * (p.radius - gnd_buff_radius) + gnd_buff_z_mean; float gnd_z_local_thresh = std::tan(DEG2RAD(5.0)) * (p.radius - gnd_grids_list.back().radius); - tmp_delta_mean_z = p.orig_point->z - (gnd_grids_list.end() - 2)->avg_height; + tmp_delta_mean_z = p_orig_point.z - (gnd_grids_list.end() - 2)->avg_height; tmp_delta_radius = p.radius - (gnd_grids_list.end() - 2)->radius; - float local_slope = std::atan(tmp_delta_mean_z / tmp_delta_radius); + float local_slope_ratio = tmp_delta_mean_z / tmp_delta_radius; if ( - abs(p.orig_point->z - next_gnd_z) <= non_ground_height_threshold_ + gnd_z_local_thresh || - abs(local_slope) <= local_slope_max_angle_rad_) { + abs(p_orig_point.z - next_gnd_z) <= non_ground_height_threshold_ + gnd_z_local_thresh || + abs(local_slope_ratio) <= local_slope_max_ratio_) { p.point_state = PointLabel::GROUND; - } else if (p.orig_point->z - next_gnd_z > non_ground_height_threshold_ + gnd_z_local_thresh) { + } else if (p_orig_point.z - next_gnd_z > non_ground_height_threshold_ + gnd_z_local_thresh) { p.point_state = PointLabel::NON_GROUND; } } + void ScanGroundFilterComponent::checkDiscontinuousGndGrid( - PointRef & p, const std::vector & gnd_grids_list) + PointData & p, pcl::PointXYZ & p_orig_point, const std::vector & gnd_grids_list) { - float tmp_delta_max_z = p.orig_point->z - gnd_grids_list.back().max_height; - float tmp_delta_avg_z = p.orig_point->z - gnd_grids_list.back().avg_height; + float tmp_delta_max_z = p_orig_point.z - gnd_grids_list.back().max_height; + float tmp_delta_avg_z = p_orig_point.z - gnd_grids_list.back().avg_height; float tmp_delta_radius = p.radius - gnd_grids_list.back().radius; - float local_slope = std::atan(tmp_delta_avg_z / tmp_delta_radius); + float local_slope_ratio = tmp_delta_avg_z / tmp_delta_radius; if ( - abs(local_slope) < local_slope_max_angle_rad_ || + abs(local_slope_ratio) < local_slope_max_ratio_ || abs(tmp_delta_avg_z) < non_ground_height_threshold_ || abs(tmp_delta_max_z) < non_ground_height_threshold_) { p.point_state = PointLabel::GROUND; - } else if (local_slope > global_slope_max_angle_rad_) { + } else if (local_slope_ratio > global_slope_max_ratio_) { p.point_state = PointLabel::NON_GROUND; } } void ScanGroundFilterComponent::checkBreakGndGrid( - PointRef & p, const std::vector & gnd_grids_list) + PointData & p, pcl::PointXYZ & p_orig_point, const std::vector & gnd_grids_list) { - float tmp_delta_avg_z = p.orig_point->z - gnd_grids_list.back().avg_height; + float tmp_delta_avg_z = p_orig_point.z - gnd_grids_list.back().avg_height; float tmp_delta_radius = p.radius - gnd_grids_list.back().radius; - float local_slope = std::atan(tmp_delta_avg_z / tmp_delta_radius); - if (abs(local_slope) < global_slope_max_angle_rad_) { + float local_slope_ratio = tmp_delta_avg_z / tmp_delta_radius; + if (abs(local_slope_ratio) < global_slope_max_ratio_) { p.point_state = PointLabel::GROUND; - } else if (local_slope > global_slope_max_angle_rad_) { + } else if (local_slope_ratio > global_slope_max_ratio_) { p.point_state = PointLabel::NON_GROUND; } } + void ScanGroundFilterComponent::recheckGroundCluster( PointsCentroid & gnd_cluster, const float non_ground_threshold, pcl::PointIndices & non_ground_indices) { const float min_gnd_height = gnd_cluster.getMinHeight(); - pcl::PointIndices gnd_indices = gnd_cluster.getIndices(); - std::vector height_list = gnd_cluster.getHeightList(); + const pcl::PointIndices & gnd_indices = gnd_cluster.getIndicesRef(); + const std::vector & height_list = gnd_cluster.getHeightListRef(); for (size_t i = 0; i < height_list.size(); ++i) { if (height_list.at(i) >= min_gnd_height + non_ground_threshold) { non_ground_indices.indices.push_back(gnd_indices.indices.at(i)); } } } + void ScanGroundFilterComponent::classifyPointCloudGridScan( - std::vector & in_radial_ordered_clouds, + const PointCloud2ConstPtr & in_cloud, std::vector & in_radial_ordered_clouds, pcl::PointIndices & out_no_ground_indices) { out_no_ground_indices.indices.clear(); @@ -299,42 +347,41 @@ void ScanGroundFilterComponent::classifyPointCloudGridScan( // check the first point in ray auto * p = &in_radial_ordered_clouds[i][0]; - PointRef * prev_p; - prev_p = &in_radial_ordered_clouds[i][0]; // for checking the distance to prev point + auto * prev_p = &in_radial_ordered_clouds[i][0]; // for checking the distance to prev point bool initialized_first_gnd_grid = false; bool prev_list_init = false; - - for (size_t j = 0; j < in_radial_ordered_clouds[i].size(); ++j) { - p = &in_radial_ordered_clouds[i][j]; - float global_slope_p = std::atan(p->orig_point->z / p->radius); + pcl::PointXYZ p_orig_point, prev_p_orig_point; + for (auto & point : in_radial_ordered_clouds[i]) { + prev_p = p; + prev_p_orig_point = p_orig_point; + p = &point; + get_point_from_global_offset(in_cloud, p_orig_point, in_cloud->point_step * p->orig_index); + float global_slope_ratio_p = p_orig_point.z / p->radius; float non_ground_height_threshold_local = non_ground_height_threshold_; - if (p->orig_point->x < low_priority_region_x_) { + if (p_orig_point.x < low_priority_region_x_) { non_ground_height_threshold_local = - non_ground_height_threshold_ * abs(p->orig_point->x / low_priority_region_x_); + non_ground_height_threshold_ * abs(p_orig_point.x / low_priority_region_x_); } // classify first grid's point cloud if ( - !initialized_first_gnd_grid && global_slope_p >= global_slope_max_angle_rad_ && - p->orig_point->z > non_ground_height_threshold_local) { + !initialized_first_gnd_grid && global_slope_ratio_p >= global_slope_max_ratio_ && + p_orig_point.z > non_ground_height_threshold_local) { out_no_ground_indices.indices.push_back(p->orig_index); p->point_state = PointLabel::NON_GROUND; - prev_p = p; continue; } if ( - !initialized_first_gnd_grid && abs(global_slope_p) < global_slope_max_angle_rad_ && - abs(p->orig_point->z) < non_ground_height_threshold_local) { - ground_cluster.addPoint(p->radius, p->orig_point->z, p->orig_index); + !initialized_first_gnd_grid && abs(global_slope_ratio_p) < global_slope_max_ratio_ && + abs(p_orig_point.z) < non_ground_height_threshold_local) { + ground_cluster.addPoint(p->radius, p_orig_point.z, p->orig_index); p->point_state = PointLabel::GROUND; initialized_first_gnd_grid = static_cast(p->grid_id - prev_p->grid_id); - prev_p = p; continue; } if (!initialized_first_gnd_grid) { - prev_p = p; continue; } @@ -366,53 +413,50 @@ void ScanGroundFilterComponent::classifyPointCloudGridScan( ground_cluster.initialize(); } // classify - if (p->orig_point->z - gnd_grids.back().avg_height > detection_range_z_max_) { + if (p_orig_point.z - gnd_grids.back().avg_height > detection_range_z_max_) { p->point_state = PointLabel::OUT_OF_RANGE; - prev_p = p; continue; } - float points_xy_distance = std::hypot( - p->orig_point->x - prev_p->orig_point->x, p->orig_point->y - prev_p->orig_point->y); + float points_xy_distance_square = + (p_orig_point.x - prev_p_orig_point.x) * (p_orig_point.x - prev_p_orig_point.x) + + (p_orig_point.y - prev_p_orig_point.y) * (p_orig_point.y - prev_p_orig_point.y); if ( prev_p->point_state == PointLabel::NON_GROUND && - points_xy_distance < split_points_distance_tolerance_ && - p->orig_point->z > prev_p->orig_point->z) { + points_xy_distance_square < split_points_distance_tolerance_square_ && + p_orig_point.z > prev_p_orig_point.z) { p->point_state = PointLabel::NON_GROUND; out_no_ground_indices.indices.push_back(p->orig_index); - prev_p = p; continue; } - - if (global_slope_p > global_slope_max_angle_rad_) { + if (global_slope_ratio_p > global_slope_max_ratio_) { out_no_ground_indices.indices.push_back(p->orig_index); - prev_p = p; continue; } // gnd grid is continuous, the last gnd grid is close uint16_t next_gnd_grid_id_thresh = (gnd_grids.end() - gnd_grid_buffer_size_)->grid_id + gnd_grid_buffer_size_ + gnd_grid_continual_thresh_; + float curr_grid_size = calcGridSize(*p); if ( p->grid_id < next_gnd_grid_id_thresh && - p->radius - gnd_grids.back().radius < gnd_grid_continual_thresh_ * p->grid_size) { - checkContinuousGndGrid(*p, gnd_grids); - - } else if (p->radius - gnd_grids.back().radius < gnd_grid_continual_thresh_ * p->grid_size) { - checkDiscontinuousGndGrid(*p, gnd_grids); + p->radius - gnd_grids.back().radius < gnd_grid_continual_thresh_ * curr_grid_size) { + checkContinuousGndGrid(*p, p_orig_point, gnd_grids); + } else if ( + p->radius - gnd_grids.back().radius < gnd_grid_continual_thresh_ * curr_grid_size) { + checkDiscontinuousGndGrid(*p, p_orig_point, gnd_grids); } else { - checkBreakGndGrid(*p, gnd_grids); + checkBreakGndGrid(*p, p_orig_point, gnd_grids); } if (p->point_state == PointLabel::NON_GROUND) { out_no_ground_indices.indices.push_back(p->orig_index); } else if (p->point_state == PointLabel::GROUND) { - ground_cluster.addPoint(p->radius, p->orig_point->z, p->orig_index); + ground_cluster.addPoint(p->radius, p_orig_point.z, p->orig_index); } - prev_p = p; } } } void ScanGroundFilterComponent::classifyPointCloud( - std::vector & in_radial_ordered_clouds, + const PointCloud2ConstPtr & in_cloud, std::vector & in_radial_ordered_clouds, pcl::PointIndices & out_no_ground_indices) { out_no_ground_indices.indices.clear(); @@ -430,16 +474,15 @@ void ScanGroundFilterComponent::classifyPointCloud( PointsCentroid ground_cluster, non_ground_cluster; float local_slope = 0.0f; PointLabel prev_point_label = PointLabel::INIT; - pcl::PointXYZ prev_gnd_point(0, 0, 0); + pcl::PointXYZ prev_gnd_point(0, 0, 0), p_orig_point, prev_p_orig_point; // loop through each point in the radial div for (size_t j = 0; j < in_radial_ordered_clouds[i].size(); ++j) { - const float global_slope_max_angle = global_slope_max_angle_rad_; const float local_slope_max_angle = local_slope_max_angle_rad_; + prev_p_orig_point = p_orig_point; auto * p = &in_radial_ordered_clouds[i][j]; - auto * p_prev = &in_radial_ordered_clouds[i][j - 1]; - + get_point_from_global_offset(in_cloud, p_orig_point, in_cloud->point_step * p->orig_index); if (j == 0) { - bool is_front_side = (p->orig_point->x > virtual_ground_point.x); + bool is_front_side = (p_orig_point.x > virtual_ground_point.x); if (use_virtual_ground_point_ && is_front_side) { prev_gnd_point = virtual_ground_point; } else { @@ -449,22 +492,22 @@ void ScanGroundFilterComponent::classifyPointCloud( prev_gnd_slope = 0.0f; ground_cluster.initialize(); non_ground_cluster.initialize(); - points_distance = calcDistance3d(*p->orig_point, prev_gnd_point); + points_distance = calcDistance3d(p_orig_point, prev_gnd_point); } else { - points_distance = calcDistance3d(*p->orig_point, *p_prev->orig_point); + points_distance = calcDistance3d(p_orig_point, prev_p_orig_point); } float radius_distance_from_gnd = p->radius - prev_gnd_radius; - float height_from_gnd = p->orig_point->z - prev_gnd_point.z; - float height_from_obj = p->orig_point->z - non_ground_cluster.getAverageHeight(); + float height_from_gnd = p_orig_point.z - prev_gnd_point.z; + float height_from_obj = p_orig_point.z - non_ground_cluster.getAverageHeight(); bool calculate_slope = false; bool is_point_close_to_prev = (points_distance < (p->radius * radial_divider_angle_rad_ + split_points_distance_tolerance_)); - float global_slope = std::atan2(p->orig_point->z, p->radius); + float global_slope_ratio = p_orig_point.z / p->radius; // check points which is far enough from previous point - if (global_slope > global_slope_max_angle) { + if (global_slope_ratio > global_slope_max_ratio_) { p->point_state = PointLabel::NON_GROUND; calculate_slope = false; } else if ( @@ -479,7 +522,7 @@ void ScanGroundFilterComponent::classifyPointCloud( calculate_slope = true; } if (is_point_close_to_prev) { - height_from_gnd = p->orig_point->z - ground_cluster.getAverageHeight(); + height_from_gnd = p_orig_point.z - ground_cluster.getAverageHeight(); radius_distance_from_gnd = p->radius - ground_cluster.getAverageRadius(); } if (calculate_slope) { @@ -514,58 +557,66 @@ void ScanGroundFilterComponent::classifyPointCloud( prev_point_label = p->point_state; if (p->point_state == PointLabel::GROUND) { prev_gnd_radius = p->radius; - prev_gnd_point = pcl::PointXYZ(p->orig_point->x, p->orig_point->y, p->orig_point->z); - ground_cluster.addPoint(p->radius, p->orig_point->z); + prev_gnd_point = pcl::PointXYZ(p_orig_point.x, p_orig_point.y, p_orig_point.z); + ground_cluster.addPoint(p->radius, p_orig_point.z); prev_gnd_slope = ground_cluster.getAverageSlope(); } // update the non ground state if (p->point_state == PointLabel::NON_GROUND) { - non_ground_cluster.addPoint(p->radius, p->orig_point->z); + non_ground_cluster.addPoint(p->radius, p_orig_point.z); } } } } void ScanGroundFilterComponent::extractObjectPoints( - const pcl::PointCloud::Ptr in_cloud_ptr, const pcl::PointIndices & in_indices, - pcl::PointCloud::Ptr out_object_cloud_ptr) + const PointCloud2ConstPtr & in_cloud_ptr, const pcl::PointIndices & in_indices, + PointCloud2 & out_object_cloud) { + size_t output_data_size = 0; for (const auto & i : in_indices.indices) { - out_object_cloud_ptr->points.emplace_back(in_cloud_ptr->points[i]); + std::memcpy( + &out_object_cloud.data[output_data_size], &in_cloud_ptr->data[i * in_cloud_ptr->point_step], + in_cloud_ptr->point_step * sizeof(uint8_t)); + *reinterpret_cast(&out_object_cloud.data[output_data_size + intensity_offset_]) = + 1; // set intensity to 1 + output_data_size += in_cloud_ptr->point_step; } } -void ScanGroundFilterComponent::filter( +void ScanGroundFilterComponent::faster_filter( const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, - PointCloud2 & output) + PointCloud2 & output, + [[maybe_unused]] const pointcloud_preprocessor::TransformInfo & transform_info) { std::scoped_lock lock(mutex_); stop_watch_ptr_->toc("processing_time", true); - pcl::PointCloud::Ptr current_sensor_cloud_ptr(new pcl::PointCloud); - pcl::fromROSMsg(*input, *current_sensor_cloud_ptr); - std::vector radial_ordered_points; + if (!offset_initialized_) { + set_field_offsets(input); + } + std::vector radial_ordered_points; pcl::PointIndices no_ground_indices; - pcl::PointCloud::Ptr no_ground_cloud_ptr(new pcl::PointCloud); - no_ground_cloud_ptr->points.reserve(current_sensor_cloud_ptr->points.size()); if (elevation_grid_mode_) { - convertPointcloudGridScan(current_sensor_cloud_ptr, radial_ordered_points); - classifyPointCloudGridScan(radial_ordered_points, no_ground_indices); + convertPointcloudGridScan(input, radial_ordered_points); + classifyPointCloudGridScan(input, radial_ordered_points, no_ground_indices); } else { - convertPointcloud(current_sensor_cloud_ptr, radial_ordered_points); - classifyPointCloud(radial_ordered_points, no_ground_indices); + convertPointcloud(input, radial_ordered_points); + classifyPointCloud(input, radial_ordered_points, no_ground_indices); } - - extractObjectPoints(current_sensor_cloud_ptr, no_ground_indices, no_ground_cloud_ptr); - - auto no_ground_cloud_msg_ptr = std::make_shared(); - pcl::toROSMsg(*no_ground_cloud_ptr, *no_ground_cloud_msg_ptr); - - no_ground_cloud_msg_ptr->header = input->header; - output = *no_ground_cloud_msg_ptr; - + output.row_step = no_ground_indices.indices.size() * input->point_step; + output.data.resize(output.row_step); + output.width = no_ground_indices.indices.size(); + output.fields.assign(input->fields.begin(), input->fields.begin() + 3); + output.is_dense = true; + output.height = input->height; + output.is_bigendian = input->is_bigendian; + output.point_step = input->point_step; + output.header = input->header; + + extractObjectPoints(input, no_ground_indices, output); if (debug_publisher_ptr_ && stop_watch_ptr_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); @@ -576,20 +627,62 @@ void ScanGroundFilterComponent::filter( } } +// TODO(taisa1): Temporary Implementation: Delete this function definition when all the filter +// nodes conform to new API. +void ScanGroundFilterComponent::filter( + const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, + PointCloud2 & output) +{ + (void)input; + (void)indices; + (void)output; +} + rcl_interfaces::msg::SetParametersResult ScanGroundFilterComponent::onParameter( const std::vector & p) { + if (get_param(p, "grid_size_m", grid_size_m_)) { + grid_mode_switch_grid_id_ = grid_mode_switch_radius_ / grid_size_m_; + grid_size_rad_ = + normalizeRadian(std::atan2(grid_mode_switch_radius_ + grid_size_m_, virtual_lidar_z_)) - + normalizeRadian(std::atan2(grid_mode_switch_radius_, virtual_lidar_z_)); + tan_grid_size_rad_ = std::tan(grid_size_rad_); + RCLCPP_DEBUG(get_logger(), "Setting grid_size_m to: %f.", grid_size_m_); + RCLCPP_DEBUG( + get_logger(), "Setting grid_mode_switch_grid_id to: %f.", grid_mode_switch_grid_id_); + RCLCPP_DEBUG(get_logger(), "Setting grid_size_rad to: %f.", grid_size_rad_); + RCLCPP_DEBUG(get_logger(), "Setting tan_grid_size_rad to: %f.", tan_grid_size_rad_); + } + if (get_param(p, "grid_mode_switch_radius", grid_mode_switch_radius_)) { + grid_mode_switch_grid_id_ = grid_mode_switch_radius_ / grid_size_m_; + grid_mode_switch_angle_rad_ = std::atan2(grid_mode_switch_radius_, virtual_lidar_z_); + grid_size_rad_ = + normalizeRadian(std::atan2(grid_mode_switch_radius_ + grid_size_m_, virtual_lidar_z_)) - + normalizeRadian(std::atan2(grid_mode_switch_radius_, virtual_lidar_z_)); + tan_grid_size_rad_ = std::tan(grid_size_rad_); + RCLCPP_DEBUG(get_logger(), "Setting grid_mode_switch_radius to: %f.", grid_mode_switch_radius_); + RCLCPP_DEBUG( + get_logger(), "Setting grid_mode_switch_grid_id to: %f.", grid_mode_switch_grid_id_); + RCLCPP_DEBUG( + get_logger(), "Setting grid_mode_switch_angle_rad to: %f.", grid_mode_switch_angle_rad_); + RCLCPP_DEBUG(get_logger(), "Setting grid_size_rad to: %f.", grid_size_rad_); + RCLCPP_DEBUG(get_logger(), "Setting tan_grid_size_rad to: %f.", tan_grid_size_rad_); + } double global_slope_max_angle_deg{get_parameter("global_slope_max_angle_deg").as_double()}; if (get_param(p, "global_slope_max_angle_deg", global_slope_max_angle_deg)) { global_slope_max_angle_rad_ = deg2rad(global_slope_max_angle_deg); + global_slope_max_ratio_ = std::tan(global_slope_max_angle_rad_); RCLCPP_DEBUG( get_logger(), "Setting global_slope_max_angle_rad to: %f.", global_slope_max_angle_rad_); + RCLCPP_DEBUG(get_logger(), "Setting global_slope_max_ratio to: %f.", global_slope_max_ratio_); } double local_slope_max_angle_deg{get_parameter("local_slope_max_angle_deg").as_double()}; if (get_param(p, "local_slope_max_angle_deg", local_slope_max_angle_deg)) { local_slope_max_angle_rad_ = deg2rad(local_slope_max_angle_deg); + local_slope_max_ratio_ = std::tan(local_slope_max_angle_rad_); RCLCPP_DEBUG( get_logger(), "Setting local_slope_max_angle_rad to: %f.", local_slope_max_angle_rad_); + RCLCPP_DEBUG(get_logger(), "Setting local_slope_max_ratio to: %f.", local_slope_max_ratio_); } double radial_divider_angle_deg{get_parameter("radial_divider_angle_deg").as_double()}; if (get_param(p, "radial_divider_angle_deg", radial_divider_angle_deg)) { @@ -600,9 +693,14 @@ rcl_interfaces::msg::SetParametersResult ScanGroundFilterComponent::onParameter( RCLCPP_DEBUG(get_logger(), "Setting radial_dividers_num to: %zu.", radial_dividers_num_); } if (get_param(p, "split_points_distance_tolerance", split_points_distance_tolerance_)) { + split_points_distance_tolerance_square_ = + split_points_distance_tolerance_ * split_points_distance_tolerance_; RCLCPP_DEBUG( get_logger(), "Setting split_points_distance_tolerance to: %f.", split_points_distance_tolerance_); + RCLCPP_DEBUG( + get_logger(), "Setting split_points_distance_tolerance_square to: %f.", + split_points_distance_tolerance_square_); } if (get_param(p, "split_height_distance", split_height_distance_)) { RCLCPP_DEBUG(get_logger(), "Setting split_height_distance to: %f.", split_height_distance_); diff --git a/perception/ground_segmentation/test/test_scan_ground_filter.cpp b/perception/ground_segmentation/test/test_scan_ground_filter.cpp index 9df9c7e9c7433..444a38ea44754 100644 --- a/perception/ground_segmentation/test/test_scan_ground_filter.cpp +++ b/perception/ground_segmentation/test/test_scan_ground_filter.cpp @@ -131,7 +131,8 @@ class ScanGroundFilterTest : public ::testing::Test // wrapper function to test private function filter void filter(sensor_msgs::msg::PointCloud2 & out_cloud) { - scan_ground_filter_->filter(input_msg_ptr_, nullptr, out_cloud); + pointcloud_preprocessor::TransformInfo transform_info; + scan_ground_filter_->faster_filter(input_msg_ptr_, nullptr, out_cloud, transform_info); } void parse_yaml() diff --git a/sensing/pointcloud_preprocessor/src/filter.cpp b/sensing/pointcloud_preprocessor/src/filter.cpp index d5843be395adf..57a563c28a4a6 100644 --- a/sensing/pointcloud_preprocessor/src/filter.cpp +++ b/sensing/pointcloud_preprocessor/src/filter.cpp @@ -125,7 +125,7 @@ void pointcloud_preprocessor::Filter::subscribe(const std::string & filter_name) // each time a child class supports the faster version. // When all the child classes support the faster version, this workaround is deleted. std::set supported_nodes = { - "CropBoxFilter", "RingOutlierFilter", "VoxelGridDownsampleFilter"}; + "CropBoxFilter", "RingOutlierFilter", "VoxelGridDownsampleFilter", "ScanGroundFilter"}; auto callback = supported_nodes.find(filter_name) != supported_nodes.end() ? &Filter::faster_input_indices_callback : &Filter::input_indices_callback; From 360fd3cef344b64af2bad362b099baaac06e3b95 Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Tue, 9 Apr 2024 20:36:43 +0900 Subject: [PATCH 3/4] chore(ground_segmentation): add tuning param (#6753) * chore(ground_segmentation): add tuning param Signed-off-by: badai-nguyen * fix: and param into yaml file Signed-off-by: badai-nguyen * chore: typo Signed-off-by: badai-nguyen * chore: typo Signed-off-by: badai-nguyen * chore: refactor Signed-off-by: badai-nguyen --------- Signed-off-by: badai-nguyen --- .../config/ground_segmentation.param.yaml | 1 + .../config/scan_ground_filter.param.yaml | 1 + .../ground_segmentation/docs/scan-ground-filter.md | 1 + .../scan_ground_filter_nodelet.hpp | 4 +++- .../src/scan_ground_filter_nodelet.cpp | 11 +++++++---- .../test/test_scan_ground_filter.cpp | 3 +++ 6 files changed, 16 insertions(+), 5 deletions(-) diff --git a/perception/ground_segmentation/config/ground_segmentation.param.yaml b/perception/ground_segmentation/config/ground_segmentation.param.yaml index 756882391183e..594ef1fe974b2 100644 --- a/perception/ground_segmentation/config/ground_segmentation.param.yaml +++ b/perception/ground_segmentation/config/ground_segmentation.param.yaml @@ -33,3 +33,4 @@ center_pcl_shift: 0.0 radial_divider_angle_deg: 1.0 use_recheck_ground_cluster: true + use_lowest_point: true diff --git a/perception/ground_segmentation/config/scan_ground_filter.param.yaml b/perception/ground_segmentation/config/scan_ground_filter.param.yaml index bc02a1ab7e6e7..35d494a620b19 100644 --- a/perception/ground_segmentation/config/scan_ground_filter.param.yaml +++ b/perception/ground_segmentation/config/scan_ground_filter.param.yaml @@ -15,3 +15,4 @@ center_pcl_shift: 0.0 radial_divider_angle_deg: 1.0 use_recheck_ground_cluster: true + use_lowest_point: true diff --git a/perception/ground_segmentation/docs/scan-ground-filter.md b/perception/ground_segmentation/docs/scan-ground-filter.md index 21df6fa6ea1b9..d4eb9c6f3addf 100644 --- a/perception/ground_segmentation/docs/scan-ground-filter.md +++ b/perception/ground_segmentation/docs/scan-ground-filter.md @@ -50,6 +50,7 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref | `low_priority_region_x` | float | -20.0 | The non-zero x threshold in back side from which small objects detection is low priority [m] | | `elevation_grid_mode` | bool | true | Elevation grid scan mode option | | `use_recheck_ground_cluster` | bool | true | Enable recheck ground cluster | +| `use_lowest_point` | bool | true | to select lowest point for reference in recheck ground cluster, otherwise select middle point | ## Assumptions / Known limits diff --git a/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp b/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp index d017d06929702..d97bbaa2118e5 100644 --- a/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp +++ b/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp @@ -194,6 +194,8 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter split_height_distance_; // useful for close points bool use_virtual_ground_point_; bool use_recheck_ground_cluster_; // to enable recheck ground cluster + bool use_lowest_point_; // to select lowest point for reference in recheck ground cluster, + // otherwise select middle point size_t radial_dividers_num_; VehicleInfo vehicle_info_; @@ -258,7 +260,7 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter * @param non_ground_indices Output non-ground PointCloud indices */ void recheckGroundCluster( - PointsCentroid & gnd_cluster, const float non_ground_threshold, + PointsCentroid & gnd_cluster, const float non_ground_threshold, const bool use_lowest_point, pcl::PointIndices & non_ground_indices); /*! * Returns the resulting complementary PointCloud, one with the points kept diff --git a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp index 7a0333d95075b..34573b564fa36 100644 --- a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp @@ -57,6 +57,7 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & split_height_distance_ = declare_parameter("split_height_distance"); use_virtual_ground_point_ = declare_parameter("use_virtual_ground_point"); use_recheck_ground_cluster_ = declare_parameter("use_recheck_ground_cluster"); + use_lowest_point_ = declare_parameter("use_lowest_point"); radial_dividers_num_ = std::ceil(2.0 * M_PI / radial_divider_angle_rad_); vehicle_info_ = VehicleInfoUtil(*this).getVehicleInfo(); @@ -316,14 +317,15 @@ void ScanGroundFilterComponent::checkBreakGndGrid( } void ScanGroundFilterComponent::recheckGroundCluster( - PointsCentroid & gnd_cluster, const float non_ground_threshold, + PointsCentroid & gnd_cluster, const float non_ground_threshold, const bool use_lowest_point, pcl::PointIndices & non_ground_indices) { - const float min_gnd_height = gnd_cluster.getMinHeight(); + float reference_height = + use_lowest_point ? gnd_cluster.getMinHeight() : gnd_cluster.getAverageHeight(); const pcl::PointIndices & gnd_indices = gnd_cluster.getIndicesRef(); const std::vector & height_list = gnd_cluster.getHeightListRef(); for (size_t i = 0; i < height_list.size(); ++i) { - if (height_list.at(i) >= min_gnd_height + non_ground_threshold) { + if (height_list.at(i) >= reference_height + non_ground_threshold) { non_ground_indices.indices.push_back(gnd_indices.indices.at(i)); } } @@ -403,7 +405,8 @@ void ScanGroundFilterComponent::classifyPointCloudGridScan( if (p->grid_id > prev_p->grid_id && ground_cluster.getAverageRadius() > 0.0) { // check if the prev grid have ground point cloud if (use_recheck_ground_cluster_) { - recheckGroundCluster(ground_cluster, non_ground_height_threshold_, out_no_ground_indices); + recheckGroundCluster( + ground_cluster, non_ground_height_threshold_, use_lowest_point_, out_no_ground_indices); } curr_gnd_grid.radius = ground_cluster.getAverageRadius(); curr_gnd_grid.avg_height = ground_cluster.getAverageHeight(); diff --git a/perception/ground_segmentation/test/test_scan_ground_filter.cpp b/perception/ground_segmentation/test/test_scan_ground_filter.cpp index 444a38ea44754..e48bd36d8c54e 100644 --- a/perception/ground_segmentation/test/test_scan_ground_filter.cpp +++ b/perception/ground_segmentation/test/test_scan_ground_filter.cpp @@ -83,6 +83,7 @@ class ScanGroundFilterTest : public ::testing::Test rclcpp::Parameter("radial_divider_angle_deg", radial_divider_angle_deg_)); parameters.emplace_back( rclcpp::Parameter("use_recheck_ground_cluster", use_recheck_ground_cluster_)); + parameters.emplace_back(rclcpp::Parameter("use_lowest_point", use_lowest_point_)); options.parameter_overrides(parameters); @@ -157,6 +158,7 @@ class ScanGroundFilterTest : public ::testing::Test center_pcl_shift_ = params["center_pcl_shift"].as(); radial_divider_angle_deg_ = params["radial_divider_angle_deg"].as(); use_recheck_ground_cluster_ = params["use_recheck_ground_cluster"].as(); + use_lowest_point_ = params["use_lowest_point"].as(); } float global_slope_max_angle_deg_ = 0.0; @@ -174,6 +176,7 @@ class ScanGroundFilterTest : public ::testing::Test float center_pcl_shift_; float radial_divider_angle_deg_; bool use_recheck_ground_cluster_; + bool use_lowest_point_; }; TEST_F(ScanGroundFilterTest, TestCase1) From 1d957c9ecaf59c2a2eca4347ace7ae46598c1e68 Mon Sep 17 00:00:00 2001 From: Taiga <53041471+TakanoTaiga@users.noreply.github.com> Date: Thu, 14 Mar 2024 11:31:18 +0900 Subject: [PATCH 4/4] perf(occupancy_grid_map_outlier_filter): improve performance (#5980) * improve perfomance Signed-off-by: Taiga Takano * style(pre-commit): autofix Signed-off-by: Taiga Takano * fixed the bug and corrected the spelling mistake. Signed-off-by: Taiga Takano * style(pre-commit): autofix Signed-off-by: Taiga Takano * fix bug Signed-off-by: Taiga Takano * style(pre-commit): autofix Signed-off-by: Taiga Takano * change filter Signed-off-by: Taiga Takano --------- Signed-off-by: Taiga Takano Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Yoshi Ri --- ...upancy_grid_map_outlier_filter_nodelet.cpp | 41 +++++++++++++++---- 1 file changed, 32 insertions(+), 9 deletions(-) diff --git a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp index 691c13a6d4701..78f76d4f980c3 100644 --- a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp +++ b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp @@ -239,19 +239,42 @@ OccupancyGridMapOutlierFilterComponent::OccupancyGridMapOutlierFilterComponent( void OccupancyGridMapOutlierFilterComponent::splitPointCloudFrontBack( const PointCloud2::ConstSharedPtr & input_pc, PointCloud2 & front_pc, PointCloud2 & behind_pc) { - PclPointCloud tmp_behind_pc; - PclPointCloud tmp_front_pc; - for (sensor_msgs::PointCloud2ConstIterator x(*input_pc, "x"), y(*input_pc, "y"), - z(*input_pc, "z"); - x != x.end(); ++x, ++y, ++z) { + size_t front_count = 0; + size_t behind_count = 0; + + for (sensor_msgs::PointCloud2ConstIterator x(*input_pc, "x"); x != x.end(); ++x) { if (*x < 0.0) { - tmp_behind_pc.push_back(pcl::PointXYZ(*x, *y, *z)); + behind_count++; } else { - tmp_front_pc.push_back(pcl::PointXYZ(*x, *y, *z)); + front_count++; } } - pcl::toROSMsg(tmp_front_pc, front_pc); - pcl::toROSMsg(tmp_behind_pc, behind_pc); + + sensor_msgs::PointCloud2Modifier front_pc_modifier(front_pc); + sensor_msgs::PointCloud2Modifier behind_pc_modifier(behind_pc); + front_pc_modifier.setPointCloud2FieldsByString(1, "xyz"); + behind_pc_modifier.setPointCloud2FieldsByString(1, "xyz"); + front_pc_modifier.resize(front_count); + behind_pc_modifier.resize(behind_count); + + sensor_msgs::PointCloud2Iterator fr_iter(front_pc, "x"); + sensor_msgs::PointCloud2Iterator be_iter(behind_pc, "x"); + + for (sensor_msgs::PointCloud2ConstIterator in_iter(*input_pc, "x"); + in_iter != in_iter.end(); ++in_iter) { + if (*in_iter < 0.0) { + *be_iter = in_iter[0]; + be_iter[1] = in_iter[1]; + be_iter[2] = in_iter[2]; + ++be_iter; + } else { + *fr_iter = in_iter[0]; + fr_iter[1] = in_iter[1]; + fr_iter[2] = in_iter[2]; + ++fr_iter; + } + } + front_pc.header = input_pc->header; behind_pc.header = input_pc->header; }