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];