Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

chore: cherry pick for ground segmentation #329

Merged
merged 2 commits into from
Mar 20, 2023
Merged

chore: cherry pick for ground segmentation #329

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
d1de548
fix(ground-segmentation): recheck gnd cluster pointcloud (#2448)
badai-nguyen Dec 9, 2022
a75b1cc
chore(ground_segmentation): add recheck ground cluster option param (…
badai-nguyen Mar 18, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions perception/ground_segmentation/docs/scan-ground-filter.md
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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