Skip to content

Commit

Permalink
Merge pull request #329 from tier4/chore/cherry-pick/ground_segmentation
Browse files Browse the repository at this point in the history 
chore: cherry pick for ground segmentation
  • Loading branch information
@0x126
0x126 authored Mar 20, 2023
2 parents c619db5 + a75b1cc commit 687c348
Show file tree
Hide file tree
Showing 3 changed files with 70 additions and 26 deletions.
1 change: 1 addition & 0 deletions perception/ground_segmentation/docs/scan-ground-filter.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,<br /> 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

Expand Down
37 changes: 31 additions & 6 deletions perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<float> height_list;

PointsCentroid()
: radius_sum(0.0f), height_sum(0.0f), radius_avg(0.0f), height_avg(0.0f), point_num(0)
Expand All @@ -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)
Expand All @@ -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); }
Expand All @@ -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<float> getHeightList() { return height_list; }
};

void filter(
Expand Down Expand Up @@ -152,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_;

Expand Down Expand Up @@ -193,18 +212,24 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
void initializeFirstGndGrids(
const float h, const float r, const uint16_t id, std::vector<GridCenter> & gnd_grids);

void checkContinuousGndGrid(
PointRef & p, const std::vector<GridCenter> & gnd_grids_list, PointsCentroid & gnd_cluster);
void checkDiscontinuousGndGrid(
PointRef & p, const std::vector<GridCenter> & gnd_grids_list, PointsCentroid & gnd_cluster);
void checkBreakGndGrid(
PointRef & p, const std::vector<GridCenter> & gnd_grids_list, PointsCentroid & gnd_cluster);
void checkContinuousGndGrid(PointRef & p, const std::vector<GridCenter> & gnd_grids_list);
void checkDiscontinuousGndGrid(PointRef & p, const std::vector<GridCenter> & gnd_grids_list);
void checkBreakGndGrid(PointRef & p, const std::vector<GridCenter> & gnd_grids_list);
void classifyPointCloud(
std::vector<PointCloudRefVector> & in_radial_ordered_clouds,
pcl::PointIndices & out_no_ground_indices);
void classifyPointCloudGridScan(
std::vector<PointCloudRefVector> & 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
Expand Down
58 changes: 38 additions & 20 deletions perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();

Expand Down Expand Up @@ -178,7 +179,7 @@ void ScanGroundFilterComponent::initializeFirstGndGrids(
const float h, const float r, const uint16_t id, std::vector<GridCenter> & 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<float>(gnd_grid_buffer_size_);

Expand All @@ -194,7 +195,7 @@ void ScanGroundFilterComponent::initializeFirstGndGrids(
}

void ScanGroundFilterComponent::checkContinuousGndGrid(
PointRef & p, const std::vector<GridCenter> & gnd_grids_list, PointsCentroid & gnd_cluster)
PointRef & p, const std::vector<GridCenter> & gnd_grids_list)
{
float next_gnd_z = 0.0f;
float curr_gnd_slope_rad = 0.0f;
Expand All @@ -203,7 +204,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;
Expand Down Expand Up @@ -234,14 +235,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<GridCenter> & gnd_grids_list, PointsCentroid & gnd_cluster)
PointRef & p, const std::vector<GridCenter> & 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;
Expand All @@ -252,32 +252,43 @@ 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;
}
}

void ScanGroundFilterComponent::checkBreakGndGrid(
PointRef & p, const std::vector<GridCenter> & gnd_grids_list, PointsCentroid & gnd_cluster)
PointRef & p, const std::vector<GridCenter> & 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<float> 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<PointCloudRefVector> & 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<GridCenter> gnd_grids;
Expand All @@ -296,7 +307,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_;
Expand All @@ -317,7 +328,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<bool>(p->grid_id - prev_p->grid_id);
prev_p = p;
Expand Down Expand Up @@ -346,6 +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
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();
Expand Down Expand Up @@ -382,17 +396,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;
}
Expand All @@ -411,7 +425,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;
Expand All @@ -420,7 +434,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];
Expand Down Expand Up @@ -600,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";
Expand Down

0 comments on commit 687c348

Please sign in to comment.