Skip to content

Commit

Permalink
chore(ground_segmentation): add tuning param (autowarefoundation#6753)
Browse files Browse the repository at this point in the history
* chore(ground_segmentation): add tuning param

Signed-off-by: badai-nguyen <[email protected]>

* fix: and param into yaml file

Signed-off-by: badai-nguyen <[email protected]>

* chore: typo

Signed-off-by: badai-nguyen <[email protected]>

* chore: typo

Signed-off-by: badai-nguyen <[email protected]>

* chore: refactor

Signed-off-by: badai-nguyen <[email protected]>

---------

Signed-off-by: badai-nguyen <[email protected]>
  • Loading branch information
badai-nguyen authored and 0x126 committed Apr 19, 2024
1 parent 434cef6 commit 360fd3c
Show file tree
Hide file tree
Showing 6 changed files with 16 additions and 5 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -33,3 +33,4 @@
center_pcl_shift: 0.0
radial_divider_angle_deg: 1.0
use_recheck_ground_cluster: true
use_lowest_point: true
Original file line number Diff line number Diff line change
Expand Up @@ -15,3 +15,4 @@
center_pcl_shift: 0.0
radial_divider_angle_deg: 1.0
use_recheck_ground_cluster: true
use_lowest_point: true
1 change: 1 addition & 0 deletions perception/ground_segmentation/docs/scan-ground-filter.md
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,7 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref
| `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 |
| `use_lowest_point` | bool | true | to select lowest point for reference in recheck ground cluster, otherwise select middle point |

## Assumptions / Known limits

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -194,6 +194,8 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
split_height_distance_; // useful for close points
bool use_virtual_ground_point_;
bool use_recheck_ground_cluster_; // to enable recheck ground cluster
bool use_lowest_point_; // to select lowest point for reference in recheck ground cluster,
// otherwise select middle point
size_t radial_dividers_num_;
VehicleInfo vehicle_info_;

Expand Down Expand Up @@ -258,7 +260,7 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
* @param non_ground_indices Output non-ground PointCloud indices
*/
void recheckGroundCluster(
PointsCentroid & gnd_cluster, const float non_ground_threshold,
PointsCentroid & gnd_cluster, const float non_ground_threshold, const bool use_lowest_point,
pcl::PointIndices & non_ground_indices);
/*!
* Returns the resulting complementary PointCloud, one with the points kept
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -57,6 +57,7 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions &
split_height_distance_ = declare_parameter<float>("split_height_distance");
use_virtual_ground_point_ = declare_parameter<bool>("use_virtual_ground_point");
use_recheck_ground_cluster_ = declare_parameter<bool>("use_recheck_ground_cluster");
use_lowest_point_ = declare_parameter<bool>("use_lowest_point");
radial_dividers_num_ = std::ceil(2.0 * M_PI / radial_divider_angle_rad_);
vehicle_info_ = VehicleInfoUtil(*this).getVehicleInfo();

Expand Down Expand Up @@ -316,14 +317,15 @@ void ScanGroundFilterComponent::checkBreakGndGrid(
}

void ScanGroundFilterComponent::recheckGroundCluster(
PointsCentroid & gnd_cluster, const float non_ground_threshold,
PointsCentroid & gnd_cluster, const float non_ground_threshold, const bool use_lowest_point,
pcl::PointIndices & non_ground_indices)
{
const float min_gnd_height = gnd_cluster.getMinHeight();
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();
for (size_t i = 0; i < height_list.size(); ++i) {
if (height_list.at(i) >= min_gnd_height + non_ground_threshold) {
if (height_list.at(i) >= reference_height + non_ground_threshold) {
non_ground_indices.indices.push_back(gnd_indices.indices.at(i));
}
}
Expand Down Expand Up @@ -403,7 +405,8 @@ void ScanGroundFilterComponent::classifyPointCloudGridScan(
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);
recheckGroundCluster(
ground_cluster, non_ground_height_threshold_, use_lowest_point_, out_no_ground_indices);
}
curr_gnd_grid.radius = ground_cluster.getAverageRadius();
curr_gnd_grid.avg_height = ground_cluster.getAverageHeight();
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -83,6 +83,7 @@ class ScanGroundFilterTest : public ::testing::Test
rclcpp::Parameter("radial_divider_angle_deg", radial_divider_angle_deg_));
parameters.emplace_back(
rclcpp::Parameter("use_recheck_ground_cluster", use_recheck_ground_cluster_));
parameters.emplace_back(rclcpp::Parameter("use_lowest_point", use_lowest_point_));

options.parameter_overrides(parameters);

Expand Down Expand Up @@ -157,6 +158,7 @@ class ScanGroundFilterTest : public ::testing::Test
center_pcl_shift_ = params["center_pcl_shift"].as<float>();
radial_divider_angle_deg_ = params["radial_divider_angle_deg"].as<float>();
use_recheck_ground_cluster_ = params["use_recheck_ground_cluster"].as<bool>();
use_lowest_point_ = params["use_lowest_point"].as<bool>();
}

float global_slope_max_angle_deg_ = 0.0;
Expand All @@ -174,6 +176,7 @@ class ScanGroundFilterTest : public ::testing::Test
float center_pcl_shift_;
float radial_divider_angle_deg_;
bool use_recheck_ground_cluster_;
bool use_lowest_point_;
};

TEST_F(ScanGroundFilterTest, TestCase1)
Expand Down

0 comments on commit 360fd3c

Please sign in to comment.