Skip to content

Commit

Permalink
fix(autoware_ground_segmentation): fix scan ground filter logic (auto…
Browse files Browse the repository at this point in the history 
…warefoundation#9084)

* refactor: initialize gnd_grids in ScanGroundFilterComponent::initializeFirstGndGrids

Initialize gnd_grids vector in the ScanGroundFilterComponent::initializeFirstGndGrids function to ensure it is empty and has the correct capacity. This improves the efficiency of the function and ensures accurate grid initialization.

Signed-off-by: Taekjin LEE <[email protected]>

* refactor: initialize gnd_grids vector in initializeFirstGndGrids function

Initialize the gnd_grids vector in the initializeFirstGndGrids function to ensure it is empty and has the correct capacity. This improves the efficiency of the function and ensures accurate grid initialization.

Signed-off-by: Taekjin LEE <[email protected]>

* refactor: improve efficiency and accuracy of grid initialization

Initialize the gnd_grids vector in the initializeFirstGndGrids function to ensure it is empty and has the correct capacity. This refactor improves the efficiency of the function and ensures accurate grid initialization.

Signed-off-by: Taekjin LEE <[email protected]>

* refactor: improve efficiency of checkDiscontinuousGndGrid function

Refactor the checkDiscontinuousGndGrid function in node.cpp to improve its efficiency. The changes include optimizing the conditional statements and reducing unnecessary calculations.

Signed-off-by: Taekjin LEE <[email protected]>

* refactor: improve efficiency of checkDiscontinuousGndGrid function

Signed-off-by: Taekjin LEE <[email protected]>

* fix: add missing condition

Signed-off-by: Taekjin LEE <[email protected]>

* style(pre-commit): autofix

* refactor: fix height_max initialization in node.hpp

Signed-off-by: Taekjin LEE <[email protected]>

* fix: bring back inequality sign

Signed-off-by: Taekjin LEE <[email protected]>

* fix: parameters from float to double

following the guideline https://docs.ros.org/en/foxy/Concepts/About-ROS-2-Parameters.html#overview

Signed-off-by: Taekjin LEE <[email protected]>

* refactor: fix logic description comment

Signed-off-by: Taekjin LEE <[email protected]>

---------

Signed-off-by: Taekjin LEE <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
  • Loading branch information
@technolojin @pre-commit-ci
technolojin and pre-commit-ci[bot] authored Oct 17, 2024
1 parent 2b9138d commit dd6e677
Show file tree
Hide file tree
Showing 2 changed files with 156 additions and 102 deletions.
238 changes: 146 additions & 92 deletions perception/autoware_ground_segmentation/src/scan_ground_filter/node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,15 +42,19 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions &
elevation_grid_mode_ = declare_parameter<bool>("elevation_grid_mode");

// common parameters
radial_divider_angle_rad_ = deg2rad(declare_parameter<float>("radial_divider_angle_deg"));
radial_divider_angle_rad_ =
static_cast<float>(deg2rad(declare_parameter<double>("radial_divider_angle_deg")));
radial_dividers_num_ = std::ceil(2.0 * M_PI / radial_divider_angle_rad_);

// common thresholds
global_slope_max_angle_rad_ = deg2rad(declare_parameter<float>("global_slope_max_angle_deg"));
local_slope_max_angle_rad_ = deg2rad(declare_parameter<float>("local_slope_max_angle_deg"));
global_slope_max_angle_rad_ =
static_cast<float>(deg2rad(declare_parameter<double>("global_slope_max_angle_deg")));
local_slope_max_angle_rad_ =
static_cast<float>(deg2rad(declare_parameter<double>("local_slope_max_angle_deg")));
global_slope_max_ratio_ = std::tan(global_slope_max_angle_rad_);
local_slope_max_ratio_ = std::tan(local_slope_max_angle_rad_);
split_points_distance_tolerance_ = declare_parameter<float>("split_points_distance_tolerance");
split_points_distance_tolerance_ =
static_cast<float>(declare_parameter<double>("split_points_distance_tolerance"));
split_points_distance_tolerance_square_ =
split_points_distance_tolerance_ * split_points_distance_tolerance_;

Expand All @@ -59,19 +63,21 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions &

// non-grid parameters
use_virtual_ground_point_ = declare_parameter<bool>("use_virtual_ground_point");
split_height_distance_ = declare_parameter<float>("split_height_distance");
split_height_distance_ = static_cast<float>(declare_parameter<double>("split_height_distance"));

// grid mode parameters
use_recheck_ground_cluster_ = declare_parameter<bool>("use_recheck_ground_cluster");
use_lowest_point_ = declare_parameter<bool>("use_lowest_point");
detection_range_z_max_ = declare_parameter<float>("detection_range_z_max");
low_priority_region_x_ = declare_parameter<float>("low_priority_region_x");
center_pcl_shift_ = declare_parameter<float>("center_pcl_shift");
non_ground_height_threshold_ = declare_parameter<float>("non_ground_height_threshold");
detection_range_z_max_ = static_cast<float>(declare_parameter<double>("detection_range_z_max"));
low_priority_region_x_ = static_cast<float>(declare_parameter<double>("low_priority_region_x"));
center_pcl_shift_ = static_cast<float>(declare_parameter<double>("center_pcl_shift"));
non_ground_height_threshold_ =
static_cast<float>(declare_parameter<double>("non_ground_height_threshold"));

// grid parameters
grid_size_m_ = declare_parameter<float>("grid_size_m");
grid_mode_switch_radius_ = declare_parameter<float>("grid_mode_switch_radius");
grid_size_m_ = static_cast<float>(declare_parameter<double>("grid_size_m"));
grid_mode_switch_radius_ =
static_cast<float>(declare_parameter<double>("grid_mode_switch_radius"));
gnd_grid_buffer_size_ = declare_parameter<int>("gnd_grid_buffer_size");
virtual_lidar_z_ = vehicle_info_.vehicle_height_m;

Expand Down Expand Up @@ -250,17 +256,29 @@ void ScanGroundFilterComponent::calcVirtualGroundOrigin(pcl::PointXYZ & point) c
void ScanGroundFilterComponent::initializeFirstGndGrids(
const float h, const float r, const uint16_t id, std::vector<GridCenter> & gnd_grids) const
{
// initialize gnd_grids
gnd_grids.clear();
gnd_grids.reserve(gnd_grid_buffer_size_);

// Index of grids
// id is the first grid, will be filled by initial centroid_bin
// id - gnd_grid_buffer_size_ is the first grid of zero-zero position
// the intermediate grids are generated by linear interpolation
const uint16_t estimating_grid_num = gnd_grid_buffer_size_ + 1;
const uint16_t idx_estimate_from = id - estimating_grid_num;
const float gradient = h / r;

GridCenter curr_gnd_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_);
for (uint16_t idx = 1; idx < estimating_grid_num; ++idx) {
float interpolation_ratio = static_cast<float>(idx) / static_cast<float>(estimating_grid_num);
const uint16_t ind_grid = idx_estimate_from + idx;

float ind_gnd_radius = ind_grid - id + 1 + gnd_grid_buffer_size_;
ind_gnd_radius *= r / static_cast<float>(gnd_grid_buffer_size_);
const float interpolated_r = r * interpolation_ratio;
const float interpolated_z = gradient * interpolated_r;

curr_gnd_grid.radius = ind_gnd_radius;
curr_gnd_grid.avg_height = ind_gnd_z;
curr_gnd_grid.max_height = ind_gnd_z;
curr_gnd_grid.radius = interpolated_r;
curr_gnd_grid.avg_height = interpolated_z;
curr_gnd_grid.max_height = interpolated_z;
curr_gnd_grid.grid_id = ind_grid;
gnd_grids.push_back(curr_gnd_grid);
}
Expand All @@ -270,83 +288,107 @@ void ScanGroundFilterComponent::checkContinuousGndGrid(
PointData & pd, const pcl::PointXYZ & point_curr,
const std::vector<GridCenter> & gnd_grids_list) const
{
float next_gnd_z = 0.0f;
float curr_gnd_slope_ratio = 0.0f;
// 1. check local slope
{
// reference gird is the last-1
const auto reference_grid_it = gnd_grids_list.end() - 2;
const float delta_z = point_curr.z - reference_grid_it->avg_height;
const float delta_radius = pd.radius - reference_grid_it->radius;
const float local_slope_ratio = delta_z / delta_radius;

if (abs(local_slope_ratio) < local_slope_max_ratio_) {
pd.point_state = PointLabel::GROUND;
return;
}
}

// 2. mean of grid buffer(filtering)
// get mean of buffer except the last grid
float gnd_buff_z_mean = 0.0f;
float gnd_buff_radius = 0.0f;

for (auto it = gnd_grids_list.end() - gnd_grid_buffer_size_ - 1; it < gnd_grids_list.end() - 1;
for (auto it = gnd_grids_list.end() - 1 - gnd_grid_buffer_size_; it < gnd_grids_list.end() - 1;
++it) {
gnd_buff_radius += it->radius;
gnd_buff_z_mean += it->avg_height;
}

gnd_buff_radius /= static_cast<float>(gnd_grid_buffer_size_ - 1);
gnd_buff_z_mean /= static_cast<float>(gnd_grid_buffer_size_ - 1);

float tmp_delta_mean_z = gnd_grids_list.back().avg_height - gnd_buff_z_mean;
float tmp_delta_radius = gnd_grids_list.back().radius - gnd_buff_radius;

curr_gnd_slope_ratio = tmp_delta_mean_z / tmp_delta_radius;
curr_gnd_slope_ratio = curr_gnd_slope_ratio < -global_slope_max_ratio_ ? -global_slope_max_ratio_
: curr_gnd_slope_ratio;
gnd_buff_radius /= static_cast<float>(gnd_grid_buffer_size_);
gnd_buff_z_mean /= static_cast<float>(gnd_grid_buffer_size_);

// reference gradient(slope) from mean of previous gnd grids
// reference position is the last grid
const float delta_z = gnd_grids_list.back().avg_height - gnd_buff_z_mean;
const float delta_radius = gnd_grids_list.back().radius - gnd_buff_radius;
float curr_gnd_slope_ratio = delta_z / delta_radius;
curr_gnd_slope_ratio =
curr_gnd_slope_ratio > global_slope_max_ratio_ ? global_slope_max_ratio_ : curr_gnd_slope_ratio;
std::clamp(curr_gnd_slope_ratio, -global_slope_max_ratio_, global_slope_max_ratio_);

next_gnd_z = curr_gnd_slope_ratio * (pd.radius - gnd_buff_radius) + gnd_buff_z_mean;
// extrapolate next ground height
const float next_gnd_z = curr_gnd_slope_ratio * (pd.radius - gnd_buff_radius) + gnd_buff_z_mean;

float gnd_z_local_thresh = std::tan(DEG2RAD(5.0)) * (pd.radius - gnd_grids_list.back().radius);
// calculate fixed angular threshold from the reference position
const float gnd_z_local_thresh =
std::tan(DEG2RAD(5.0)) * (pd.radius - gnd_grids_list.back().radius);

tmp_delta_mean_z = point_curr.z - (gnd_grids_list.end() - 2)->avg_height;
tmp_delta_radius = pd.radius - (gnd_grids_list.end() - 2)->radius;
float local_slope_ratio = tmp_delta_mean_z / tmp_delta_radius;
if (
abs(point_curr.z - next_gnd_z) <= non_ground_height_threshold_ + gnd_z_local_thresh ||
abs(local_slope_ratio) <= local_slope_max_ratio_) {
if (abs(point_curr.z - next_gnd_z) <= non_ground_height_threshold_ + gnd_z_local_thresh) {
pd.point_state = PointLabel::GROUND;
} else if (point_curr.z - next_gnd_z > non_ground_height_threshold_ + gnd_z_local_thresh) {
return;
}
if (point_curr.z - next_gnd_z >= non_ground_height_threshold_ + gnd_z_local_thresh) {
pd.point_state = PointLabel::NON_GROUND;
return;
}
}

void ScanGroundFilterComponent::checkDiscontinuousGndGrid(
PointData & pd, const pcl::PointXYZ & point_curr,
const std::vector<GridCenter> & gnd_grids_list) const
{
float tmp_delta_max_z = point_curr.z - gnd_grids_list.back().max_height;
float tmp_delta_avg_z = point_curr.z - gnd_grids_list.back().avg_height;
float tmp_delta_radius = pd.radius - gnd_grids_list.back().radius;
float local_slope_ratio = tmp_delta_avg_z / tmp_delta_radius;

if (
abs(local_slope_ratio) < local_slope_max_ratio_ ||
abs(tmp_delta_avg_z) < non_ground_height_threshold_ ||
abs(tmp_delta_max_z) < non_ground_height_threshold_) {
const auto & grid_ref = gnd_grids_list.back();
const float delta_avg_z = point_curr.z - grid_ref.avg_height;
if (abs(delta_avg_z) < non_ground_height_threshold_) {
pd.point_state = PointLabel::GROUND;
return;
}
const float delta_max_z = point_curr.z - grid_ref.max_height;
if (abs(delta_max_z) < non_ground_height_threshold_) {
pd.point_state = PointLabel::GROUND;
} else if (local_slope_ratio > global_slope_max_ratio_) {
return;
}
const float delta_radius = pd.radius - grid_ref.radius;
const float local_slope_ratio = delta_avg_z / delta_radius;
if (abs(local_slope_ratio) < local_slope_max_ratio_) {
pd.point_state = PointLabel::GROUND;
return;
}
if (local_slope_ratio >= local_slope_max_ratio_) {
pd.point_state = PointLabel::NON_GROUND;
return;
}
}

void ScanGroundFilterComponent::checkBreakGndGrid(
PointData & pd, const pcl::PointXYZ & point_curr,
const std::vector<GridCenter> & gnd_grids_list) const
{
float tmp_delta_avg_z = point_curr.z - gnd_grids_list.back().avg_height;
float tmp_delta_radius = pd.radius - gnd_grids_list.back().radius;
float local_slope_ratio = tmp_delta_avg_z / tmp_delta_radius;
const auto & grid_ref = gnd_grids_list.back();
const float delta_avg_z = point_curr.z - grid_ref.avg_height;
const float delta_radius = pd.radius - grid_ref.radius;
const float local_slope_ratio = delta_avg_z / delta_radius;
if (abs(local_slope_ratio) < global_slope_max_ratio_) {
pd.point_state = PointLabel::GROUND;
} else if (local_slope_ratio > global_slope_max_ratio_) {
return;
}
if (local_slope_ratio >= global_slope_max_ratio_) {
pd.point_state = PointLabel::NON_GROUND;
return;
}
}

void ScanGroundFilterComponent::recheckGroundCluster(
const PointsCentroid & gnd_cluster, const float non_ground_threshold, const bool use_lowest_point,
pcl::PointIndices & non_ground_indices) const
{
float reference_height =
const float reference_height =
use_lowest_point ? gnd_cluster.getMinHeight() : gnd_cluster.getAverageHeight();
const pcl::PointIndices & gnd_indices = gnd_cluster.getIndicesRef();
const std::vector<float> & height_list = gnd_cluster.getHeightListRef();
Expand Down Expand Up @@ -379,62 +421,70 @@ void ScanGroundFilterComponent::classifyPointCloudGridScan(
}

bool initialized_first_gnd_grid = false;
bool prev_list_init = false;

PointData pd_curr, pd_prev;
pcl::PointXYZ point_curr, point_prev;

// initialize the previous point
pd_curr = in_radial_ordered_clouds[i][0];
{
pd_curr = in_radial_ordered_clouds[i][0];
const size_t data_index = in_cloud->point_step * pd_curr.orig_index;
get_point_from_data_index(in_cloud, data_index, point_curr);
}

// iterate over the points in the ray
for (const auto & point : in_radial_ordered_clouds[i]) {
// set the previous point
point_prev = point_curr;
pd_prev = pd_curr;
point_prev = point_curr;

// set the current point
pd_curr = point;
const size_t data_index = in_cloud->point_step * pd_curr.orig_index;
get_point_from_data_index(in_cloud, data_index, point_curr);

// set the thresholds
const float global_slope_ratio_p = point_curr.z / pd_curr.radius;
float non_ground_height_threshold_local = non_ground_height_threshold_;
if (point_curr.x < low_priority_region_x_) {
non_ground_height_threshold_local =
non_ground_height_threshold_ * abs(point_curr.x / low_priority_region_x_);
}
// determine if the current point is in new grid
const bool is_curr_in_next_grid = pd_curr.grid_id > pd_prev.grid_id;

// initialization process for the first grid and interpolate the previous grids
if (!initialized_first_gnd_grid) {
// classify first grid's point cloud
// set the thresholds
const float global_slope_ratio_p = point_prev.z / pd_prev.radius;
float non_ground_height_threshold_local = non_ground_height_threshold_;
if (point_prev.x < low_priority_region_x_) {
non_ground_height_threshold_local =
non_ground_height_threshold_ * abs(point_prev.x / low_priority_region_x_);
}
// non_ground_height_threshold_local is only for initialization

// prepare centroid_bin for the first grid
if (
// classify previous point
global_slope_ratio_p >= global_slope_max_ratio_ &&
point_curr.z > non_ground_height_threshold_local) {
out_no_ground_indices.indices.push_back(pd_curr.orig_index);
pd_curr.point_state = PointLabel::NON_GROUND;
point_prev.z > non_ground_height_threshold_local) {
out_no_ground_indices.indices.push_back(pd_prev.orig_index);
pd_prev.point_state = PointLabel::NON_GROUND;
} else if (
abs(global_slope_ratio_p) < global_slope_max_ratio_ &&
abs(point_curr.z) < non_ground_height_threshold_local) {
centroid_bin.addPoint(pd_curr.radius, point_curr.z, pd_curr.orig_index);
pd_curr.point_state = PointLabel::GROUND;
// if the gird id is not the initial grid_id, then the first gnd grid is initialized
initialized_first_gnd_grid = static_cast<bool>(pd_curr.grid_id - pd_prev.grid_id);
abs(point_prev.z) < non_ground_height_threshold_local) {
centroid_bin.addPoint(pd_prev.radius, point_prev.z, pd_prev.orig_index);
pd_prev.point_state = PointLabel::GROUND;
// centroid_bin is filled at least once
// if the current point is in the next gird, it is ready to be initialized
initialized_first_gnd_grid = is_curr_in_next_grid;
}
// else, the point is not classified
continue;
}

// initialize gnd_grids based on the initial centroid_bin
if (!prev_list_init) {
// keep filling the centroid_bin until it is ready to be initialized
if (!initialized_first_gnd_grid) {
continue;
}
// estimate previous grids by linear interpolation
float h = centroid_bin.getAverageHeight();
float r = centroid_bin.getAverageRadius();
initializeFirstGndGrids(h, r, pd_curr.grid_id, gnd_grids);
prev_list_init = true;
initializeFirstGndGrids(h, r, pd_prev.grid_id, gnd_grids);
}

// finalize the current centroid_bin and update the gnd_grids
if (pd_curr.grid_id > pd_prev.grid_id && centroid_bin.getAverageRadius() > 0.0) {
if (is_curr_in_next_grid && centroid_bin.getIndicesRef().indices.size() > 0) {
// check if the prev grid have ground point cloud
if (use_recheck_ground_cluster_) {
recheckGroundCluster(
Expand All @@ -452,8 +502,12 @@ void ScanGroundFilterComponent::classifyPointCloudGridScan(
centroid_bin.initialize();
}

// 0: set the thresholds
const float global_slope_ratio_p = point_curr.z / pd_curr.radius;
const auto & grid_ref = gnd_grids.back();

// 1: height is out-of-range
if (point_curr.z - gnd_grids.back().avg_height > detection_range_z_max_) {
if (point_curr.z - grid_ref.avg_height > detection_range_z_max_) {
pd_curr.point_state = PointLabel::OUT_OF_RANGE;
continue;
}
Expand All @@ -477,17 +531,17 @@ void ScanGroundFilterComponent::classifyPointCloudGridScan(
continue;
}

uint16_t next_gnd_grid_id_thresh = (gnd_grids.end() - gnd_grid_buffer_size_)->grid_id +
gnd_grid_buffer_size_ + gnd_grid_continual_thresh_;
float curr_grid_size = grid_.getGridSize(pd_curr.radius, pd_curr.grid_id);
const uint16_t next_gnd_grid_id_thresh = (gnd_grids.end() - gnd_grid_buffer_size_)->grid_id +
gnd_grid_buffer_size_ + gnd_grid_continual_thresh_;
const float curr_grid_width = grid_.getGridSize(pd_curr.radius, pd_curr.grid_id);
if (
// 4: the point is continuous with the previous grid
pd_curr.grid_id < next_gnd_grid_id_thresh &&
pd_curr.radius - gnd_grids.back().radius < gnd_grid_continual_thresh_ * curr_grid_size) {
pd_curr.radius - grid_ref.radius < gnd_grid_continual_thresh_ * curr_grid_width) {
checkContinuousGndGrid(pd_curr, point_curr, gnd_grids);
} else if (
// 5: the point is discontinuous with the previous grid
pd_curr.radius - gnd_grids.back().radius < gnd_grid_continual_thresh_ * curr_grid_size) {
pd_curr.radius - grid_ref.radius < gnd_grid_continual_thresh_ * curr_grid_width) {
checkDiscontinuousGndGrid(pd_curr, point_curr, gnd_grids);
} else {
// 6: the point is break the previous grid
Expand Down
Loading

0 comments on commit dd6e677

Please sign in to comment.