@@ -57,6 +57,7 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions &
57
57
split_height_distance_ = declare_parameter<float >(" split_height_distance" );
58
58
use_virtual_ground_point_ = declare_parameter<bool >(" use_virtual_ground_point" );
59
59
use_recheck_ground_cluster_ = declare_parameter<bool >(" use_recheck_ground_cluster" );
60
+ use_lowest_point_ = declare_parameter<bool >(" use_lowest_point" );
60
61
radial_dividers_num_ = std::ceil (2.0 * M_PI / radial_divider_angle_rad_);
61
62
vehicle_info_ = VehicleInfoUtil (*this ).getVehicleInfo ();
62
63
@@ -316,14 +317,15 @@ void ScanGroundFilterComponent::checkBreakGndGrid(
316
317
}
317
318
318
319
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,
320
321
pcl::PointIndices & non_ground_indices)
321
322
{
322
- const float min_gnd_height = gnd_cluster.getMinHeight ();
323
+ float reference_height =
324
+ use_lowest_point ? gnd_cluster.getMinHeight () : gnd_cluster.getAverageHeight ();
323
325
const pcl::PointIndices & gnd_indices = gnd_cluster.getIndicesRef ();
324
326
const std::vector<float > & height_list = gnd_cluster.getHeightListRef ();
325
327
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) {
327
329
non_ground_indices.indices .push_back (gnd_indices.indices .at (i));
328
330
}
329
331
}
@@ -403,7 +405,8 @@ void ScanGroundFilterComponent::classifyPointCloudGridScan(
403
405
if (p->grid_id > prev_p->grid_id && ground_cluster.getAverageRadius () > 0.0 ) {
404
406
// check if the prev grid have ground point cloud
405
407
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);
407
410
}
408
411
curr_gnd_grid.radius = ground_cluster.getAverageRadius ();
409
412
curr_gnd_grid.avg_height = ground_cluster.getAverageHeight ();
0 commit comments