Skip to content

Commit

Permalink
fix(ground-segmentation): recheck gnd cluster pointcloud (autowarefou…
Browse files Browse the repository at this point in the history 
…ndation#2448)

* fix: reclassify ground cluster pcl

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* fix: add lowest-based recheck

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* chore: refactoring

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* chore: refactoring

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com>
  • Loading branch information
@badai-nguyen @miursh
badai-nguyen and miursh committed Dec 9, 2022
1 parent 8906a1e commit b2ded82
Show file tree
Hide file tree
Showing 2 changed files with 60 additions and 25 deletions.
36 changes: 30 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 @@ -193,18 +211,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
49 changes: 30 additions & 19 deletions perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -178,7 +178,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 +194,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 +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;
Expand Down Expand Up @@ -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<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 +251,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 +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_;
Expand All @@ -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<bool>(p->grid_id - prev_p->grid_id);
prev_p = p;
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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;
}
Expand All @@ -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;
Expand All @@ -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];
Expand Down

0 comments on commit b2ded82

Please sign in to comment.