Skip to content

Commit 5cb8eab

Browse files
committed
chore: refactor
Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
1 parent 627020c commit 5cb8eab

File tree

2 files changed

+35
-49
lines changed

2 files changed

+35
-49
lines changed

perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp

+4-4
Original file line numberDiff line numberDiff line change
@@ -174,7 +174,6 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
174174
bool use_virtual_ground_point_;
175175
bool use_recheck_ground_cluster_; // to enable recheck ground cluster
176176
size_t radial_dividers_num_;
177-
int omp_num_threads_{1};
178177
VehicleInfo vehicle_info_;
179178

180179
/*!
@@ -220,10 +219,10 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
220219
void checkBreakGndGrid(PointRef & p, const std::vector<GridCenter> & gnd_grids_list);
221220
void classifyPointCloud(
222221
std::vector<PointCloudRefVector> & in_radial_ordered_clouds,
223-
pcl::PointIndices & out_no_ground_indices);
222+
std::vector<pcl::PointIndices> & out_no_ground_indices);
224223
void classifyPointCloudGridScan(
225224
std::vector<PointCloudRefVector> & in_radial_ordered_clouds,
226-
pcl::PointIndices & out_no_ground_indices);
225+
std::vector<pcl::PointIndices> & out_no_ground_indices);
227226
/*!
228227
* Re-classifies point of ground cluster based on their height
229228
* @param gnd_cluster Input ground cluster for re-checking
@@ -241,7 +240,8 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
241240
* @param out_object_cloud_ptr Resulting PointCloud with the indices kept
242241
*/
243242
void extractObjectPoints(
244-
const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr, const pcl::PointIndices & in_indices,
243+
const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr,
244+
const std::vector<pcl::PointIndices> & in_indices,
245245
pcl::PointCloud<pcl::PointXYZ>::Ptr out_object_cloud_ptr);
246246

247247
/** \brief Parameter service callback result : needed to be hold */

perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp

+31-45
Original file line numberDiff line numberDiff line change
@@ -19,10 +19,11 @@
1919
#include <tier4_autoware_utils/math/unit_conversion.hpp>
2020
#include <vehicle_info_util/vehicle_info_util.hpp>
2121

22+
#include <omp.h>
23+
2224
#include <memory>
2325
#include <string>
2426
#include <vector>
25-
#include <omp.h>
2627

2728
namespace ground_segmentation
2829
{
@@ -56,7 +57,6 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions &
5657
split_height_distance_ = declare_parameter("split_height_distance", 0.2);
5758
use_virtual_ground_point_ = declare_parameter("use_virtual_ground_point", true);
5859
use_recheck_ground_cluster_ = declare_parameter("use_recheck_ground_cluster", true);
59-
omp_num_threads_ = declare_parameter("omp_num_threads", 1);
6060
radial_dividers_num_ = std::ceil(2.0 * M_PI / radial_divider_angle_rad_);
6161
vehicle_info_ = VehicleInfoUtil(*this).getVehicleInfo();
6262

@@ -287,15 +287,13 @@ void ScanGroundFilterComponent::recheckGroundCluster(
287287
}
288288
void ScanGroundFilterComponent::classifyPointCloudGridScan(
289289
std::vector<PointCloudRefVector> & in_radial_ordered_clouds,
290-
pcl::PointIndices & out_no_ground_indices)
290+
std::vector<pcl::PointIndices> & out_no_ground_indices)
291291
{
292-
out_no_ground_indices.indices.clear();
293-
std::vector<pcl::PointIndices> ground_indices;
294-
ground_indices.resize(in_radial_ordered_clouds.size());
295-
omp_set_num_threads(omp_num_threads_);
296-
#pragma omp parallel for
292+
out_no_ground_indices.resize(in_radial_ordered_clouds.size());
293+
#pragma omp parallel for
297294
for (size_t i = 0; i < in_radial_ordered_clouds.size(); ++i) {
298-
auto & ground_indices_private = ground_indices[i];
295+
auto & radial_no_ground_indices = out_no_ground_indices[i];
296+
radial_no_ground_indices.indices.clear();
299297
PointsCentroid ground_cluster;
300298
ground_cluster.initialize();
301299
std::vector<GridCenter> gnd_grids;
@@ -325,10 +323,7 @@ void ScanGroundFilterComponent::classifyPointCloudGridScan(
325323
if (
326324
!initialized_first_gnd_grid && global_slope_p >= global_slope_max_angle_rad_ &&
327325
p->orig_point->z > non_ground_height_threshold_local) {
328-
// #pragma omp critical
329-
{
330-
ground_indices_private.indices.push_back(p->orig_index);
331-
}
326+
radial_no_ground_indices.indices.push_back(p->orig_index);
332327
p->point_state = PointLabel::NON_GROUND;
333328
prev_p = p;
334329
continue;
@@ -367,7 +362,8 @@ void ScanGroundFilterComponent::classifyPointCloudGridScan(
367362
if (p->grid_id > prev_p->grid_id && ground_cluster.getAverageRadius() > 0.0) {
368363
// check if the prev grid have ground point cloud
369364
if (use_recheck_ground_cluster_) {
370-
recheckGroundCluster(ground_cluster, non_ground_height_threshold_, ground_indices_private);
365+
recheckGroundCluster(
366+
ground_cluster, non_ground_height_threshold_, radial_no_ground_indices);
371367
}
372368
curr_gnd_grid.radius = ground_cluster.getAverageRadius();
373369
curr_gnd_grid.avg_height = ground_cluster.getAverageHeight();
@@ -389,13 +385,13 @@ void ScanGroundFilterComponent::classifyPointCloudGridScan(
389385
points_xy_distance < split_points_distance_tolerance_ &&
390386
p->orig_point->z > prev_p->orig_point->z) {
391387
p->point_state = PointLabel::NON_GROUND;
392-
ground_indices_private.indices.push_back(p->orig_index);
388+
radial_no_ground_indices.indices.push_back(p->orig_index);
393389
prev_p = p;
394390
continue;
395391
}
396392

397393
if (global_slope_p > global_slope_max_angle_rad_) {
398-
ground_indices_private.indices.push_back(p->orig_index);
394+
radial_no_ground_indices.indices.push_back(p->orig_index);
399395
prev_p = p;
400396
continue;
401397
}
@@ -410,47 +406,34 @@ void ScanGroundFilterComponent::classifyPointCloudGridScan(
410406
} else if (p->radius - gnd_grids.back().radius < gnd_grid_continual_thresh_ * p->grid_size) {
411407
checkDiscontinuousGndGrid(*p, gnd_grids);
412408
} else {
413-
// checkBreakGndGrid(*p, gnd_grids);
414-
{
415-
float tmp_delta_avg_z = p->orig_point->z - gnd_grids.back().avg_height;
416-
float tmp_delta_radius = p->radius - gnd_grids.back().radius;
417-
float local_slope = std::atan(tmp_delta_avg_z / tmp_delta_radius);
418-
if (abs(local_slope) < global_slope_max_angle_rad_) {
419-
p->point_state = PointLabel::GROUND;
420-
} else if (local_slope > global_slope_max_angle_rad_) {
421-
p->point_state = PointLabel::NON_GROUND;
422-
}
423-
}
409+
checkBreakGndGrid(*p, gnd_grids);
424410
}
425411
if (p->point_state == PointLabel::NON_GROUND) {
426-
ground_indices_private.indices.push_back(p->orig_index);
412+
radial_no_ground_indices.indices.push_back(p->orig_index);
427413
} else if (p->point_state == PointLabel::GROUND) {
428414
ground_cluster.addPoint(p->radius, p->orig_point->z, p->orig_index);
429415
}
430-
prev_p = p;
431-
}
432-
}
433-
434-
for(size_t i = 0; i < in_radial_ordered_clouds.size();++i){
435-
if(ground_indices.at(i).indices.size() > 0){
436-
out_no_ground_indices.indices.insert(out_no_ground_indices.indices.end(),ground_indices.at(i).indices.begin(),ground_indices.at(i).indices.end());
416+
prev_p = p;
437417
}
438418
}
439419
}
440420

441421
void ScanGroundFilterComponent::classifyPointCloud(
442422
std::vector<PointCloudRefVector> & in_radial_ordered_clouds,
443-
pcl::PointIndices & out_no_ground_indices)
423+
std::vector<pcl::PointIndices> & out_no_ground_indices)
444424
{
445-
out_no_ground_indices.indices.clear();
425+
out_no_ground_indices.resize(in_radial_ordered_clouds.size());
446426

447427
const pcl::PointXYZ init_ground_point(0, 0, 0);
448428
pcl::PointXYZ virtual_ground_point(0, 0, 0);
449429
calcVirtualGroundOrigin(virtual_ground_point);
450430

451-
// point classification algorithm
452-
// sweep through each radial division
431+
// point classification algorithm
432+
// sweep through each radial division
433+
#pragma omp parallel for
453434
for (size_t i = 0; i < in_radial_ordered_clouds.size(); ++i) {
435+
auto & radial_no_ground_indices = out_no_ground_indices[i];
436+
radial_no_ground_indices.indices.clear();
454437
float prev_gnd_radius = 0.0f;
455438
float prev_gnd_slope = 0.0f;
456439
float points_distance = 0.0f;
@@ -525,12 +508,12 @@ void ScanGroundFilterComponent::classifyPointCloud(
525508
non_ground_cluster.initialize();
526509
}
527510
if (p->point_state == PointLabel::NON_GROUND) {
528-
out_no_ground_indices.indices.push_back(p->orig_index);
511+
radial_no_ground_indices.indices.push_back(p->orig_index);
529512
} else if ( // NOLINT
530513
(prev_point_label == PointLabel::NON_GROUND) &&
531514
(p->point_state == PointLabel::POINT_FOLLOW)) {
532515
p->point_state = PointLabel::NON_GROUND;
533-
out_no_ground_indices.indices.push_back(p->orig_index);
516+
radial_no_ground_indices.indices.push_back(p->orig_index);
534517
} else if ( // NOLINT
535518
(prev_point_label == PointLabel::GROUND) && (p->point_state == PointLabel::POINT_FOLLOW)) {
536519
p->point_state = PointLabel::GROUND;
@@ -554,11 +537,14 @@ void ScanGroundFilterComponent::classifyPointCloud(
554537
}
555538

556539
void ScanGroundFilterComponent::extractObjectPoints(
557-
const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr, const pcl::PointIndices & in_indices,
540+
const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr,
541+
const std::vector<pcl::PointIndices> & in_indices,
558542
pcl::PointCloud<pcl::PointXYZ>::Ptr out_object_cloud_ptr)
559543
{
560-
for (const auto & i : in_indices.indices) {
561-
out_object_cloud_ptr->points.emplace_back(in_cloud_ptr->points[i]);
544+
for (const auto & indices : in_indices) {
545+
for (const auto & i : indices.indices) {
546+
out_object_cloud_ptr->points.emplace_back(in_cloud_ptr->points[i]);
547+
}
562548
}
563549
}
564550

@@ -573,7 +559,7 @@ void ScanGroundFilterComponent::filter(
573559

574560
std::vector<PointCloudRefVector> radial_ordered_points;
575561

576-
pcl::PointIndices no_ground_indices;
562+
std::vector<pcl::PointIndices> no_ground_indices;
577563
pcl::PointCloud<pcl::PointXYZ>::Ptr no_ground_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
578564
no_ground_cloud_ptr->points.reserve(current_sensor_cloud_ptr->points.size());
579565

0 commit comments

Comments
 (0)