From d1de548c63e8861c8a21ca9267ed4628dee84d0a Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Fri, 9 Dec 2022 17:12:13 +0900 Subject: [PATCH 1/2] fix(ground-segmentation): recheck gnd cluster pointcloud (#2448) * fix: reclassify ground cluster pcl Signed-off-by: badai-nguyen * fix: add lowest-based recheck Signed-off-by: badai-nguyen * chore: refactoring Signed-off-by: badai-nguyen * chore: refactoring Signed-off-by: badai-nguyen Signed-off-by: badai-nguyen Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com> --- .../scan_ground_filter_nodelet.hpp | 36 +++++++++++--- .../src/scan_ground_filter_nodelet.cpp | 49 ++++++++++++------- 2 files changed, 60 insertions(+), 25 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 fcf96f828fc9b..f6a8ba4c8a89a 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 @@ -86,8 +86,11 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter float radius_avg; float height_avg; float height_max; + float height_min; uint32_t point_num; uint16_t grid_id; + pcl::PointIndices pcl_indices; + std::vector height_list; PointsCentroid() : radius_sum(0.0f), height_sum(0.0f), radius_avg(0.0f), height_avg(0.0f), point_num(0) @@ -101,8 +104,11 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter radius_avg = 0.0f; height_avg = 0.0f; height_max = 0.0f; + height_min = 10.0f; point_num = 0; grid_id = 0; + pcl_indices.indices.clear(); + height_list.clear(); } void addPoint(const float radius, const float height) @@ -113,6 +119,13 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter radius_avg = radius_sum / point_num; height_avg = height_sum / point_num; height_max = height_max < height ? height : height_max; + height_min = height_min > height ? height : height_min; + } + void addPoint(const float radius, const float height, const uint index) + { + pcl_indices.indices.push_back(index); + height_list.push_back(height); + addPoint(radius, height); } float getAverageSlope() { return std::atan2(height_avg, radius_avg); } @@ -123,7 +136,12 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter float getMaxHeight() { return height_max; } + float getMinHeight() { return height_min; } + uint16_t getGridId() { return grid_id; } + + pcl::PointIndices getIndices() { return pcl_indices; } + std::vector getHeightList() { return height_list; } }; void filter( @@ -193,18 +211,24 @@ 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, PointsCentroid & gnd_cluster); - void checkDiscontinuousGndGrid( - PointRef & p, const std::vector & gnd_grids_list, PointsCentroid & gnd_cluster); - void checkBreakGndGrid( - PointRef & p, const std::vector & gnd_grids_list, PointsCentroid & gnd_cluster); + 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 classifyPointCloud( std::vector & in_radial_ordered_clouds, pcl::PointIndices & out_no_ground_indices); void classifyPointCloudGridScan( std::vector & in_radial_ordered_clouds, pcl::PointIndices & out_no_ground_indices); + /*! + * Re-classifies point of ground cluster based on their height + * @param gnd_cluster Input ground cluster for re-checking + * @param non_ground_threshold Height threshold for ground and non-ground points classification + * @param non_ground_indices Output non-ground PointCloud indices + */ + void recheckGroundCluster( + PointsCentroid & gnd_cluster, const float non_ground_threshold, + pcl::PointIndices & non_ground_indices); /*! * Returns the resulting complementary PointCloud, one with the points kept * and the other removed as indicated in the indices diff --git a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp index 16a6859d09ad9..fa9d3d6d83665 100644 --- a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp @@ -178,7 +178,7 @@ void ScanGroundFilterComponent::initializeFirstGndGrids( const float h, const float r, const uint16_t id, std::vector & gnd_grids) { GridCenter curr_gnd_grid; - for (int ind_grid = id - 1 - gnd_grid_buffer_size_; ind_grid < id - 1; ind_grid++) { + for (int ind_grid = id - 1 - gnd_grid_buffer_size_; ind_grid < id - 1; ++ind_grid) { float ind_gnd_z = ind_grid - id + 1 + gnd_grid_buffer_size_; ind_gnd_z *= h / static_cast(gnd_grid_buffer_size_); @@ -194,7 +194,7 @@ void ScanGroundFilterComponent::initializeFirstGndGrids( } void ScanGroundFilterComponent::checkContinuousGndGrid( - PointRef & p, const std::vector & gnd_grids_list, PointsCentroid & gnd_cluster) + PointRef & p, const std::vector & gnd_grids_list) { float next_gnd_z = 0.0f; float curr_gnd_slope_rad = 0.0f; @@ -203,7 +203,7 @@ void ScanGroundFilterComponent::checkContinuousGndGrid( 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++) { + ++it) { gnd_buff_radius += it->radius; gnd_buff_z_mean += it->avg_height; gnd_buff_z_max += it->max_height; @@ -234,14 +234,13 @@ void ScanGroundFilterComponent::checkContinuousGndGrid( 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_) { - gnd_cluster.addPoint(p.radius, p.orig_point->z); p.point_state = PointLabel::GROUND; } 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, PointsCentroid & gnd_cluster) + PointRef & p, 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; @@ -252,7 +251,6 @@ void ScanGroundFilterComponent::checkDiscontinuousGndGrid( abs(local_slope) < local_slope_max_angle_rad_ || abs(tmp_delta_avg_z) < non_ground_height_threshold_ || abs(tmp_delta_max_z) < non_ground_height_threshold_) { - gnd_cluster.addPoint(p.radius, p.orig_point->z); p.point_state = PointLabel::GROUND; } else if (local_slope > global_slope_max_angle_rad_) { p.point_state = PointLabel::NON_GROUND; @@ -260,24 +258,36 @@ void ScanGroundFilterComponent::checkDiscontinuousGndGrid( } void ScanGroundFilterComponent::checkBreakGndGrid( - PointRef & p, const std::vector & gnd_grids_list, PointsCentroid & gnd_cluster) + PointRef & p, const std::vector & gnd_grids_list) { 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_) { - gnd_cluster.addPoint(p.radius, p.orig_point->z); p.point_state = PointLabel::GROUND; } else if (local_slope > global_slope_max_angle_rad_) { 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(); + 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, pcl::PointIndices & out_no_ground_indices) { out_no_ground_indices.indices.clear(); - for (size_t i = 0; i < in_radial_ordered_clouds.size(); i++) { + for (size_t i = 0; i < in_radial_ordered_clouds.size(); ++i) { PointsCentroid ground_cluster; ground_cluster.initialize(); std::vector gnd_grids; @@ -296,7 +306,7 @@ void ScanGroundFilterComponent::classifyPointCloudGridScan( bool initialized_first_gnd_grid = false; bool prev_list_init = false; - for (size_t j = 0; j < in_radial_ordered_clouds[i].size(); j++) { + 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); float non_ground_height_threshold_local = non_ground_height_threshold_; @@ -317,7 +327,7 @@ void ScanGroundFilterComponent::classifyPointCloudGridScan( 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); + 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; @@ -346,6 +356,7 @@ void ScanGroundFilterComponent::classifyPointCloudGridScan( // move to new grid if (p->grid_id > prev_p->grid_id && ground_cluster.getAverageRadius() > 0.0) { // check if the prev grid have ground point cloud + recheckGroundCluster(ground_cluster, non_ground_height_threshold_, out_no_ground_indices); curr_gnd_grid.radius = ground_cluster.getAverageRadius(); curr_gnd_grid.avg_height = ground_cluster.getAverageHeight(); curr_gnd_grid.max_height = ground_cluster.getMaxHeight(); @@ -382,17 +393,17 @@ void ScanGroundFilterComponent::classifyPointCloudGridScan( 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, ground_cluster); + checkContinuousGndGrid(*p, gnd_grids); - } else if ( - p->radius - gnd_grids.back().radius < gnd_grid_continual_thresh_ * p->grid_size || - p->radius < grid_mode_switch_radius_ * 2.0f) { - checkDiscontinuousGndGrid(*p, gnd_grids, ground_cluster); + } else if (p->radius - gnd_grids.back().radius < gnd_grid_continual_thresh_ * p->grid_size) { + checkDiscontinuousGndGrid(*p, gnd_grids); } else { - checkBreakGndGrid(*p, gnd_grids, ground_cluster); + checkBreakGndGrid(*p, 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); } prev_p = p; } @@ -411,7 +422,7 @@ void ScanGroundFilterComponent::classifyPointCloud( // point classification algorithm // sweep through each radial division - for (size_t i = 0; i < in_radial_ordered_clouds.size(); i++) { + for (size_t i = 0; i < in_radial_ordered_clouds.size(); ++i) { float prev_gnd_radius = 0.0f; float prev_gnd_slope = 0.0f; float points_distance = 0.0f; @@ -420,7 +431,7 @@ void ScanGroundFilterComponent::classifyPointCloud( PointLabel prev_point_label = PointLabel::INIT; pcl::PointXYZ prev_gnd_point(0, 0, 0); // loop through each point in the radial div - for (size_t j = 0; j < in_radial_ordered_clouds[i].size(); j++) { + 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_; auto * p = &in_radial_ordered_clouds[i][j]; From a75b1cc88b25a0d260b18d54b277a2a84798568c Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Sat, 18 Mar 2023 19:47:33 +0900 Subject: [PATCH 2/2] chore(ground_segmentation): add recheck ground cluster option param (#3106) Signed-off-by: badai-nguyen --- .../ground_segmentation/docs/scan-ground-filter.md | 1 + .../scan_ground_filter_nodelet.hpp | 1 + .../src/scan_ground_filter_nodelet.cpp | 11 +++++++++-- 3 files changed, 11 insertions(+), 2 deletions(-) diff --git a/perception/ground_segmentation/docs/scan-ground-filter.md b/perception/ground_segmentation/docs/scan-ground-filter.md index 6d044d6b47c3a..0c07ce600f625 100644 --- a/perception/ground_segmentation/docs/scan-ground-filter.md +++ b/perception/ground_segmentation/docs/scan-ground-filter.md @@ -47,6 +47,7 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref | `gnd_grid_buffer_size` | uint16 | 4 | Number of grids using to estimate local ground slope,
applied only for elevation_grid_mode | | `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 | ## 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 f6a8ba4c8a89a..11572772a0d0e 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 @@ -170,6 +170,7 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter 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_; VehicleInfo vehicle_info_; diff --git a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp index fa9d3d6d83665..953a9feb4d21e 100644 --- a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp @@ -54,6 +54,7 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & 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); radial_dividers_num_ = std::ceil(2.0 * M_PI / radial_divider_angle_rad_); vehicle_info_ = VehicleInfoUtil(*this).getVehicleInfo(); @@ -356,7 +357,9 @@ void ScanGroundFilterComponent::classifyPointCloudGridScan( // move to new grid if (p->grid_id > prev_p->grid_id && ground_cluster.getAverageRadius() > 0.0) { // check if the prev grid have ground point cloud - recheckGroundCluster(ground_cluster, non_ground_height_threshold_, out_no_ground_indices); + if (use_recheck_ground_cluster_) { + recheckGroundCluster(ground_cluster, non_ground_height_threshold_, out_no_ground_indices); + } curr_gnd_grid.radius = ground_cluster.getAverageRadius(); curr_gnd_grid.avg_height = ground_cluster.getAverageHeight(); curr_gnd_grid.max_height = ground_cluster.getMaxHeight(); @@ -611,7 +614,11 @@ rcl_interfaces::msg::SetParametersResult ScanGroundFilterComponent::onParameter( get_logger(), "Setting use_virtual_ground_point to: " << std::boolalpha << use_virtual_ground_point_); } - + if (get_param(p, "use_recheck_ground_cluster", use_recheck_ground_cluster_)) { + RCLCPP_DEBUG_STREAM( + get_logger(), + "Setting use_recheck_ground_cluster to: " << std::boolalpha << use_recheck_ground_cluster_); + } rcl_interfaces::msg::SetParametersResult result; result.successful = true; result.reason = "success";