Skip to content

Commit e2d148b

Browse files
badai-nguyen0x126
authored andcommitted
chore(ground_segmentation): add tuning param (autowarefoundation#6753)
* chore(ground_segmentation): add tuning param Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix: and param into yaml file Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * chore: typo Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * chore: typo Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * chore: refactor Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> --------- Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
1 parent a60b580 commit e2d148b

File tree

6 files changed

+16
-5
lines changed

6 files changed

+16
-5
lines changed

perception/ground_segmentation/config/ground_segmentation.param.yaml

+1
Original file line numberDiff line numberDiff line change
@@ -33,3 +33,4 @@
3333
center_pcl_shift: 0.0
3434
radial_divider_angle_deg: 1.0
3535
use_recheck_ground_cluster: true
36+
use_lowest_point: true

perception/ground_segmentation/config/scan_ground_filter.param.yaml

+1
Original file line numberDiff line numberDiff line change
@@ -15,3 +15,4 @@
1515
center_pcl_shift: 0.0
1616
radial_divider_angle_deg: 1.0
1717
use_recheck_ground_cluster: true
18+
use_lowest_point: true

perception/ground_segmentation/docs/scan-ground-filter.md

+1
Original file line numberDiff line numberDiff line change
@@ -50,6 +50,7 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref
5050
| `low_priority_region_x` | float | -20.0 | The non-zero x threshold in back side from which small objects detection is low priority [m] |
5151
| `elevation_grid_mode` | bool | true | Elevation grid scan mode option |
5252
| `use_recheck_ground_cluster` | bool | true | Enable recheck ground cluster |
53+
| `use_lowest_point` | bool | true | to select lowest point for reference in recheck ground cluster, otherwise select middle point |
5354

5455
## Assumptions / Known limits
5556

perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp

+3-1
Original file line numberDiff line numberDiff line change
@@ -194,6 +194,8 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
194194
split_height_distance_; // useful for close points
195195
bool use_virtual_ground_point_;
196196
bool use_recheck_ground_cluster_; // to enable recheck ground cluster
197+
bool use_lowest_point_; // to select lowest point for reference in recheck ground cluster,
198+
// otherwise select middle point
197199
size_t radial_dividers_num_;
198200
VehicleInfo vehicle_info_;
199201

@@ -258,7 +260,7 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
258260
* @param non_ground_indices Output non-ground PointCloud indices
259261
*/
260262
void recheckGroundCluster(
261-
PointsCentroid & gnd_cluster, const float non_ground_threshold,
263+
PointsCentroid & gnd_cluster, const float non_ground_threshold, const bool use_lowest_point,
262264
pcl::PointIndices & non_ground_indices);
263265
/*!
264266
* Returns the resulting complementary PointCloud, one with the points kept

perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp

+7-4
Original file line numberDiff line numberDiff line change
@@ -57,6 +57,7 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions &
5757
split_height_distance_ = declare_parameter<float>("split_height_distance");
5858
use_virtual_ground_point_ = declare_parameter<bool>("use_virtual_ground_point");
5959
use_recheck_ground_cluster_ = declare_parameter<bool>("use_recheck_ground_cluster");
60+
use_lowest_point_ = declare_parameter<bool>("use_lowest_point");
6061
radial_dividers_num_ = std::ceil(2.0 * M_PI / radial_divider_angle_rad_);
6162
vehicle_info_ = VehicleInfoUtil(*this).getVehicleInfo();
6263

@@ -316,14 +317,15 @@ void ScanGroundFilterComponent::checkBreakGndGrid(
316317
}
317318

318319
void ScanGroundFilterComponent::recheckGroundCluster(
319-
PointsCentroid & gnd_cluster, const float non_ground_threshold,
320+
PointsCentroid & gnd_cluster, const float non_ground_threshold, const bool use_lowest_point,
320321
pcl::PointIndices & non_ground_indices)
321322
{
322-
const float min_gnd_height = gnd_cluster.getMinHeight();
323+
float reference_height =
324+
use_lowest_point ? gnd_cluster.getMinHeight() : gnd_cluster.getAverageHeight();
323325
const pcl::PointIndices & gnd_indices = gnd_cluster.getIndicesRef();
324326
const std::vector<float> & height_list = gnd_cluster.getHeightListRef();
325327
for (size_t i = 0; i < height_list.size(); ++i) {
326-
if (height_list.at(i) >= min_gnd_height + non_ground_threshold) {
328+
if (height_list.at(i) >= reference_height + non_ground_threshold) {
327329
non_ground_indices.indices.push_back(gnd_indices.indices.at(i));
328330
}
329331
}
@@ -403,7 +405,8 @@ void ScanGroundFilterComponent::classifyPointCloudGridScan(
403405
if (p->grid_id > prev_p->grid_id && ground_cluster.getAverageRadius() > 0.0) {
404406
// check if the prev grid have ground point cloud
405407
if (use_recheck_ground_cluster_) {
406-
recheckGroundCluster(ground_cluster, non_ground_height_threshold_, out_no_ground_indices);
408+
recheckGroundCluster(
409+
ground_cluster, non_ground_height_threshold_, use_lowest_point_, out_no_ground_indices);
407410
}
408411
curr_gnd_grid.radius = ground_cluster.getAverageRadius();
409412
curr_gnd_grid.avg_height = ground_cluster.getAverageHeight();

perception/ground_segmentation/test/test_scan_ground_filter.cpp

+3
Original file line numberDiff line numberDiff line change
@@ -83,6 +83,7 @@ class ScanGroundFilterTest : public ::testing::Test
8383
rclcpp::Parameter("radial_divider_angle_deg", radial_divider_angle_deg_));
8484
parameters.emplace_back(
8585
rclcpp::Parameter("use_recheck_ground_cluster", use_recheck_ground_cluster_));
86+
parameters.emplace_back(rclcpp::Parameter("use_lowest_point", use_lowest_point_));
8687

8788
options.parameter_overrides(parameters);
8889

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

162164
float global_slope_max_angle_deg_ = 0.0;
@@ -174,6 +176,7 @@ class ScanGroundFilterTest : public ::testing::Test
174176
float center_pcl_shift_;
175177
float radial_divider_angle_deg_;
176178
bool use_recheck_ground_cluster_;
179+
bool use_lowest_point_;
177180
};
178181

179182
TEST_F(ScanGroundFilterTest, TestCase1)

0 commit comments

Comments
 (0)