19
19
#include < tier4_autoware_utils/math/unit_conversion.hpp>
20
20
#include < vehicle_info_util/vehicle_info_util.hpp>
21
21
22
+ #include < omp.h>
23
+
22
24
#include < memory>
23
25
#include < string>
24
26
#include < vector>
25
- #include < omp.h>
26
27
27
28
namespace ground_segmentation
28
29
{
@@ -56,7 +57,6 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions &
56
57
split_height_distance_ = declare_parameter (" split_height_distance" , 0.2 );
57
58
use_virtual_ground_point_ = declare_parameter (" use_virtual_ground_point" , true );
58
59
use_recheck_ground_cluster_ = declare_parameter (" use_recheck_ground_cluster" , true );
59
- omp_num_threads_ = declare_parameter (" omp_num_threads" , 1 );
60
60
radial_dividers_num_ = std::ceil (2.0 * M_PI / radial_divider_angle_rad_);
61
61
vehicle_info_ = VehicleInfoUtil (*this ).getVehicleInfo ();
62
62
@@ -287,15 +287,13 @@ void ScanGroundFilterComponent::recheckGroundCluster(
287
287
}
288
288
void ScanGroundFilterComponent::classifyPointCloudGridScan (
289
289
std::vector<PointCloudRefVector> & in_radial_ordered_clouds,
290
- pcl::PointIndices & out_no_ground_indices)
290
+ std::vector< pcl::PointIndices> & out_no_ground_indices)
291
291
{
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
297
294
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 ();
299
297
PointsCentroid ground_cluster;
300
298
ground_cluster.initialize ();
301
299
std::vector<GridCenter> gnd_grids;
@@ -325,10 +323,7 @@ void ScanGroundFilterComponent::classifyPointCloudGridScan(
325
323
if (
326
324
!initialized_first_gnd_grid && global_slope_p >= global_slope_max_angle_rad_ &&
327
325
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 );
332
327
p->point_state = PointLabel::NON_GROUND;
333
328
prev_p = p;
334
329
continue ;
@@ -367,7 +362,8 @@ void ScanGroundFilterComponent::classifyPointCloudGridScan(
367
362
if (p->grid_id > prev_p->grid_id && ground_cluster.getAverageRadius () > 0.0 ) {
368
363
// check if the prev grid have ground point cloud
369
364
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);
371
367
}
372
368
curr_gnd_grid.radius = ground_cluster.getAverageRadius ();
373
369
curr_gnd_grid.avg_height = ground_cluster.getAverageHeight ();
@@ -389,13 +385,13 @@ void ScanGroundFilterComponent::classifyPointCloudGridScan(
389
385
points_xy_distance < split_points_distance_tolerance_ &&
390
386
p->orig_point ->z > prev_p->orig_point ->z ) {
391
387
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 );
393
389
prev_p = p;
394
390
continue ;
395
391
}
396
392
397
393
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 );
399
395
prev_p = p;
400
396
continue ;
401
397
}
@@ -410,47 +406,34 @@ void ScanGroundFilterComponent::classifyPointCloudGridScan(
410
406
} else if (p->radius - gnd_grids.back ().radius < gnd_grid_continual_thresh_ * p->grid_size ) {
411
407
checkDiscontinuousGndGrid (*p, gnd_grids);
412
408
} 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);
424
410
}
425
411
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 );
427
413
} else if (p->point_state == PointLabel::GROUND) {
428
414
ground_cluster.addPoint (p->radius , p->orig_point ->z , p->orig_index );
429
415
}
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;
437
417
}
438
418
}
439
419
}
440
420
441
421
void ScanGroundFilterComponent::classifyPointCloud (
442
422
std::vector<PointCloudRefVector> & in_radial_ordered_clouds,
443
- pcl::PointIndices & out_no_ground_indices)
423
+ std::vector< pcl::PointIndices> & out_no_ground_indices)
444
424
{
445
- out_no_ground_indices.indices . clear ( );
425
+ out_no_ground_indices.resize (in_radial_ordered_clouds. size () );
446
426
447
427
const pcl::PointXYZ init_ground_point (0 , 0 , 0 );
448
428
pcl::PointXYZ virtual_ground_point (0 , 0 , 0 );
449
429
calcVirtualGroundOrigin (virtual_ground_point);
450
430
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
453
434
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 ();
454
437
float prev_gnd_radius = 0 .0f ;
455
438
float prev_gnd_slope = 0 .0f ;
456
439
float points_distance = 0 .0f ;
@@ -525,12 +508,12 @@ void ScanGroundFilterComponent::classifyPointCloud(
525
508
non_ground_cluster.initialize ();
526
509
}
527
510
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 );
529
512
} else if ( // NOLINT
530
513
(prev_point_label == PointLabel::NON_GROUND) &&
531
514
(p->point_state == PointLabel::POINT_FOLLOW)) {
532
515
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 );
534
517
} else if ( // NOLINT
535
518
(prev_point_label == PointLabel::GROUND) && (p->point_state == PointLabel::POINT_FOLLOW)) {
536
519
p->point_state = PointLabel::GROUND;
@@ -554,11 +537,14 @@ void ScanGroundFilterComponent::classifyPointCloud(
554
537
}
555
538
556
539
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,
558
542
pcl::PointCloud<pcl::PointXYZ>::Ptr out_object_cloud_ptr)
559
543
{
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
+ }
562
548
}
563
549
}
564
550
@@ -573,7 +559,7 @@ void ScanGroundFilterComponent::filter(
573
559
574
560
std::vector<PointCloudRefVector> radial_ordered_points;
575
561
576
- pcl::PointIndices no_ground_indices;
562
+ std::vector< pcl::PointIndices> no_ground_indices;
577
563
pcl::PointCloud<pcl::PointXYZ>::Ptr no_ground_cloud_ptr (new pcl::PointCloud<pcl::PointXYZ>);
578
564
no_ground_cloud_ptr->points .reserve (current_sensor_cloud_ptr->points .size ());
579
565
0 commit comments