fix(ground_segmentation): add omp parallel for

perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp

@@ -219,10 +219,10 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
   void checkBreakGndGrid(PointRef & p, const std::vector<GridCenter> & gnd_grids_list);
   void classifyPointCloud(
     std::vector<PointCloudRefVector> & in_radial_ordered_clouds,
-    pcl::PointIndices & out_no_ground_indices);
+    std::vector<pcl::PointIndices> & out_no_ground_indices);
   void classifyPointCloudGridScan(
     std::vector<PointCloudRefVector> & in_radial_ordered_clouds,
-    pcl::PointIndices & out_no_ground_indices);
+    std::vector<pcl::PointIndices> & out_no_ground_indices);
   /*!
    * Re-classifies point of ground cluster based on their height
    * @param gnd_cluster Input ground cluster for re-checking
@@ -240,7 +240,8 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
    * @param out_object_cloud_ptr Resulting PointCloud with the indices kept
    */
   void extractObjectPoints(
-    const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr, const pcl::PointIndices & in_indices,
+    const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr,
+    const std::vector<pcl::PointIndices> & in_indices,
     pcl::PointCloud<pcl::PointXYZ>::Ptr out_object_cloud_ptr);
 
   /** \brief Parameter service callback result : needed to be hold */

perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp

@@ -1,4 +1,4 @@
 // Copyright 2021 Tier IV, Inc. All rights reserved.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -19,6 +19,8 @@
 #include <tier4_autoware_utils/math/unit_conversion.hpp>
 #include <vehicle_info_util/vehicle_info_util.hpp>
 
+#include <omp.h>
+
 #include <memory>
 #include <string>
 #include <vector>
@@ -283,126 +285,129 @@
 }
 void ScanGroundFilterComponent::classifyPointCloudGridScan(
   std::vector<PointCloudRefVector> & in_radial_ordered_clouds,
-  pcl::PointIndices & out_no_ground_indices)
+  std::vector<pcl::PointIndices> & out_no_ground_indices)
 {
-  out_no_ground_indices.indices.clear();
+  out_no_ground_indices.resize(in_radial_ordered_clouds.size());
+#pragma omp parallel for
   for (size_t i = 0; i < in_radial_ordered_clouds.size(); ++i) {
+    auto & radial_no_ground_indices = out_no_ground_indices[i];
+    radial_no_ground_indices.indices.clear();
     PointsCentroid ground_cluster;
     ground_cluster.initialize();
     std::vector<GridCenter> gnd_grids;
     GridCenter curr_gnd_grid;
 
     // check empty ray
     if (in_radial_ordered_clouds[i].size() == 0) {
       continue;
     }
 
-    // check the first point in ray
-    auto * p = &in_radial_ordered_clouds[i][0];
-    PointRef * prev_p;
-    prev_p = &in_radial_ordered_clouds[i][0];  // for checking the distance to prev point
+    // // check the first point in ray
+    auto p = &in_radial_ordered_clouds[i][0];
+    auto prev_p = &in_radial_ordered_clouds[i][0];  // for checking the distance to prev point
 
     bool initialized_first_gnd_grid = false;
     bool prev_list_init = false;
 
     for (size_t j = 0; j < in_radial_ordered_clouds[i].size(); ++j) {
       p = &in_radial_ordered_clouds[i][j];
       float global_slope_p = std::atan(p->orig_point->z / p->radius);
       float non_ground_height_threshold_local = non_ground_height_threshold_;
       if (p->orig_point->x < low_priority_region_x_) {
         non_ground_height_threshold_local =
           non_ground_height_threshold_ * abs(p->orig_point->x / low_priority_region_x_);
       }
       // classify first grid's point cloud
       if (
         !initialized_first_gnd_grid && global_slope_p >= global_slope_max_angle_rad_ &&
         p->orig_point->z > non_ground_height_threshold_local) {
-        out_no_ground_indices.indices.push_back(p->orig_index);
+        radial_no_ground_indices.indices.push_back(p->orig_index);
         p->point_state = PointLabel::NON_GROUND;
         prev_p = p;
         continue;
       }
 
       if (
         !initialized_first_gnd_grid && abs(global_slope_p) < global_slope_max_angle_rad_ &&
         abs(p->orig_point->z) < non_ground_height_threshold_local) {
         ground_cluster.addPoint(p->radius, p->orig_point->z, p->orig_index);
         p->point_state = PointLabel::GROUND;
         initialized_first_gnd_grid = static_cast<bool>(p->grid_id - prev_p->grid_id);
         prev_p = p;
         continue;
       }
 
       if (!initialized_first_gnd_grid) {
         prev_p = p;
         continue;
       }
 
       // initialize lists of previous gnd grids
       if (prev_list_init == false && initialized_first_gnd_grid == true) {
         float h = ground_cluster.getAverageHeight();
         float r = ground_cluster.getAverageRadius();
         initializeFirstGndGrids(h, r, p->grid_id, gnd_grids);
         prev_list_init = true;
       }
 
       if (prev_list_init == false && initialized_first_gnd_grid == false) {
         // assume first gnd grid is zero
         initializeFirstGndGrids(0.0f, p->radius, p->grid_id, gnd_grids);
         prev_list_init = true;
       }
 
       // move to new grid
       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_, radial_no_ground_indices);
         }
         curr_gnd_grid.radius = ground_cluster.getAverageRadius();
         curr_gnd_grid.avg_height = ground_cluster.getAverageHeight();
         curr_gnd_grid.max_height = ground_cluster.getMaxHeight();
         curr_gnd_grid.grid_id = prev_p->grid_id;
         gnd_grids.push_back(curr_gnd_grid);
         ground_cluster.initialize();
       }
       // classify
       if (p->orig_point->z - gnd_grids.back().avg_height > detection_range_z_max_) {
         p->point_state = PointLabel::OUT_OF_RANGE;
         prev_p = p;
         continue;
       }
       float points_xy_distance = std::hypot(
         p->orig_point->x - prev_p->orig_point->x, p->orig_point->y - prev_p->orig_point->y);
       if (
         prev_p->point_state == PointLabel::NON_GROUND &&
         points_xy_distance < split_points_distance_tolerance_ &&
         p->orig_point->z > prev_p->orig_point->z) {
         p->point_state = PointLabel::NON_GROUND;
-        out_no_ground_indices.indices.push_back(p->orig_index);
+        radial_no_ground_indices.indices.push_back(p->orig_index);
         prev_p = p;
         continue;
       }
 
       if (global_slope_p > global_slope_max_angle_rad_) {
-        out_no_ground_indices.indices.push_back(p->orig_index);
+        radial_no_ground_indices.indices.push_back(p->orig_index);
         prev_p = p;
         continue;
       }
       // gnd grid is continuous, the last gnd grid is close
       uint16_t next_gnd_grid_id_thresh = (gnd_grids.end() - gnd_grid_buffer_size_)->grid_id +
                                          gnd_grid_buffer_size_ + gnd_grid_continual_thresh_;
       if (
         p->grid_id < next_gnd_grid_id_thresh &&
         p->radius - gnd_grids.back().radius < gnd_grid_continual_thresh_ * p->grid_size) {
         checkContinuousGndGrid(*p, gnd_grids);
 
       } else if (p->radius - gnd_grids.back().radius < gnd_grid_continual_thresh_ * p->grid_size) {
         checkDiscontinuousGndGrid(*p, gnd_grids);
       } else {
         checkBreakGndGrid(*p, gnd_grids);
       }
       if (p->point_state == PointLabel::NON_GROUND) {
-        out_no_ground_indices.indices.push_back(p->orig_index);
+        radial_no_ground_indices.indices.push_back(p->orig_index);
       } else if (p->point_state == PointLabel::GROUND) {
         ground_cluster.addPoint(p->radius, p->orig_point->z, p->orig_index);
       }
@@ -413,17 +418,20 @@
 
 void ScanGroundFilterComponent::classifyPointCloud(
   std::vector<PointCloudRefVector> & in_radial_ordered_clouds,
-  pcl::PointIndices & out_no_ground_indices)
+  std::vector<pcl::PointIndices> & out_no_ground_indices)
 {
-  out_no_ground_indices.indices.clear();
+  out_no_ground_indices.resize(in_radial_ordered_clouds.size());
 
   const pcl::PointXYZ init_ground_point(0, 0, 0);
   pcl::PointXYZ virtual_ground_point(0, 0, 0);
   calcVirtualGroundOrigin(virtual_ground_point);
 
-  // point classification algorithm
-  // sweep through each radial division
+// point classification algorithm
+// sweep through each radial division
+#pragma omp parallel for
   for (size_t i = 0; i < in_radial_ordered_clouds.size(); ++i) {
+    auto & radial_no_ground_indices = out_no_ground_indices[i];
+    radial_no_ground_indices.indices.clear();
     float prev_gnd_radius = 0.0f;
     float prev_gnd_slope = 0.0f;
     float points_distance = 0.0f;
@@ -498,12 +506,12 @@
         non_ground_cluster.initialize();
       }
       if (p->point_state == PointLabel::NON_GROUND) {
-        out_no_ground_indices.indices.push_back(p->orig_index);
+        radial_no_ground_indices.indices.push_back(p->orig_index);
       } else if (  // NOLINT
         (prev_point_label == PointLabel::NON_GROUND) &&
         (p->point_state == PointLabel::POINT_FOLLOW)) {
         p->point_state = PointLabel::NON_GROUND;
-        out_no_ground_indices.indices.push_back(p->orig_index);
+        radial_no_ground_indices.indices.push_back(p->orig_index);
       } else if (  // NOLINT
         (prev_point_label == PointLabel::GROUND) && (p->point_state == PointLabel::POINT_FOLLOW)) {
         p->point_state = PointLabel::GROUND;
@@ -527,11 +535,14 @@
 }
 
 void ScanGroundFilterComponent::extractObjectPoints(
-  const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr, const pcl::PointIndices & in_indices,
+  const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr,
+  const std::vector<pcl::PointIndices> & in_indices,
   pcl::PointCloud<pcl::PointXYZ>::Ptr out_object_cloud_ptr)
 {
-  for (const auto & i : in_indices.indices) {
-    out_object_cloud_ptr->points.emplace_back(in_cloud_ptr->points[i]);
+  for (const auto & indices : in_indices) {
+    for (const auto & i : indices.indices) {
+      out_object_cloud_ptr->points.emplace_back(in_cloud_ptr->points[i]);
+    }
   }
 }
 
@@ -546,7 +557,7 @@
 
   std::vector<PointCloudRefVector> radial_ordered_points;
 
-  pcl::PointIndices no_ground_indices;
+  std::vector<pcl::PointIndices> no_ground_indices;
   pcl::PointCloud<pcl::PointXYZ>::Ptr no_ground_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
   no_ground_cloud_ptr->points.reserve(current_sensor_cloud_ptr->points.size());