feat(autoware_ground_segmentation): grid data structure revision for efficiency improvement

[technolojin]

Description

Change data structure of grid for calculation efficiency improvement.

1. (Performance) Grid and grid cells are defined
2. (Performance) The grid geometry is modeled to Concentric Zone Model (CZM) based polar grid
3. (Refactor) The grid process is separated from node.cpp
4. (Performance) The segmentation process is ran cell-wise (while current process is point-wise).
5. (Performance) Replace arc tangent (atan2) to a pseudo arc tangent function for better performance (with accuracy drop, which can be acceptable)

This PR do not change main logic of the scan_ground_filter.
This performance improvement only will be implemented when the option elevation_grid_mode is true.

How the efficiency is improved

1. By replacing existing data structure, sorting is not needed.
2. Segmentation condition (connected, disconnected, break) is determined grid cell-wise, not point-wise.
3. Concentric Zone Model (CZM) based polar grid allows to make various grid cell density. The grid is configured to have lower number of the grids close to the ego vehicle. Less grid cells means lower cell-related calculation.
4. The arc tangent is used for grid allocation on each points. The grid cell allocation do not require high accuracy, therefore, it can be replaced by approximation.

Drawbacks

1. Introducing CZM polar grid requires cell geometry calculation. If the number of cell grids is large, the calculation can take longer time.
2. Finding non-empty grid cell take calculation cost. Increase by the number of cell grids.
3. If the grid cells contains few points, performance improvement will be limited.

Drawback mitigation

set the parameter grid_size_m to 0.5
PR autowarefoundation/autoware_launch#1223

Related links

Parent Issue:

• Link

How was this PR tested?

Performance test (check degradation)

TIER IV INTERNAL
Tested with grid_size_m of 0.5 and there was no degradation confirmed.

Processing time

Before

Computation time difference by grid size is negligible.

• Before: grid_size_m: 0.1

🌲 Total Processing Time Tree 🌲
100.00% faster_filter: total 3118.96 [ms], avg. 12.89 [ms], run count: 242
    ├── 74.77% convertPointcloudGridScan: total 2332.12 [ms], avg. 9.64 [ms], run count: 242
    │   ├── 52.53% azimuth_angle_grouping: total 1638.31 [ms], avg. 6.77 [ms], run count: 242
    │   ├── 22.12% sort: total 690.03 [ms], avg. 2.85 [ms], run count: 242
    │   └── 0.12% rest: 3.79 [ms]
    ├── 22.85% classifyPointCloudGridScan: total 712.62 [ms], avg. 2.94 [ms], run count: 242
    ├── 1.53% extractObjectPoints: total 47.75 [ms], avg. 0.20 [ms], run count: 242
    └── 0.85% rest: 26.46 [ms]

• Before: grid_size_m: 0.2

🌲 Total Processing Time Tree 🌲
100.00% faster_filter: total 3129.68 [ms], avg. 12.88 [ms], run count: 243
    ├── 76.50% convertPointcloudGridScan: total 2394.23 [ms], avg. 9.85 [ms], run count: 243
    │   ├── 54.11% azimuth_angle_grouping: total 1693.44 [ms], avg. 6.97 [ms], run count: 243
    │   ├── 22.28% sort: total 697.30 [ms], avg. 2.87 [ms], run count: 243
    │   └── 0.11% rest: 3.49 [ms]
    ├── 21.30% classifyPointCloudGridScan: total 666.66 [ms], avg. 2.74 [ms], run count: 243
    ├── 1.34% extractObjectPoints: total 41.92 [ms], avg. 0.17 [ms], run count: 243
    └── 0.86% rest: 26.87 [ms]

• Before: grid_size_m: 0.5

🌲 Total Processing Time Tree 🌲
100.00% faster_filter: total 3012.68 [ms], avg. 12.66 [ms], run count: 238
    ├── 78.02% convertPointcloudGridScan: total 2350.36 [ms], avg. 9.88 [ms], run count: 238
    │   ├── 55.51% azimuth_angle_grouping: total 1672.34 [ms], avg. 7.03 [ms], run count: 238
    │   ├── 22.39% sort: total 674.44 [ms], avg. 2.83 [ms], run count: 238
    │   └── 0.12% rest: 3.58 [ms]
    ├── 19.86% classifyPointCloudGridScan: total 598.35 [ms], avg. 2.51 [ms], run count: 238
    ├── 1.28% extractObjectPoints: total 38.42 [ms], avg. 0.16 [ms], run count: 238
    └── 0.85% rest: 25.56 [ms]

• Before: grid_size_m: 1.0

🌲 Total Processing Time Tree 🌲
100.00% faster_filter: total 3021.20 [ms], avg. 12.54 [ms], run count: 241
    ├── 78.97% convertPointcloudGridScan: total 2385.86 [ms], avg. 9.90 [ms], run count: 241
    │   ├── 55.63% azimuth_angle_grouping: total 1680.76 [ms], avg. 6.97 [ms], run count: 241
    │   ├── 23.22% sort: total 701.58 [ms], avg. 2.91 [ms], run count: 241
    │   └── 0.12% rest: 3.52 [ms]
    ├── 18.84% classifyPointCloudGridScan: total 569.28 [ms], avg. 2.36 [ms], run count: 241
    ├── 1.32% extractObjectPoints: total 39.87 [ms], avg. 0.17 [ms], run count: 241
    └── 0.87% rest: 26.19 [ms]

After

The larger grid size, the lower computation time.
! performance test will be updated

• After: grid_size_m: 0.1

🌲 Total Processing Time Tree 🌲
100.00% faster_filter: total 4530.97 [ms], avg. 18.12 [ms], run count: 250
    ├── 97.99% process: total 4439.72 [ms], avg. 17.76 [ms], run count: 250
    │   ├── 7.48% resetCells: total 339.06 [ms], avg. 1.36 [ms], run count: 250
    │   ├── 23.45% convert: total 1062.35 [ms], avg. 4.25 [ms], run count: 250
    │   ├── 6.99% preprocess: total 316.55 [ms], avg. 1.27 [ms], run count: 250
    │   │   ├── 6.97% setGridConnections: total 315.67 [ms], avg. 1.26 [ms], run count: 250
    │   │   └── 0.02% rest: 0.87 [ms]
    │   ├── 5.41% initializeGround: total 244.98 [ms], avg. 0.98 [ms], run count: 250
    │   ├── 54.50% classify: total 2469.17 [ms], avg. 9.88 [ms], run count: 250
    │   └── 0.17% rest: 7.62 [ms]
    ├── 1.58% extractObjectPoints: total 71.46 [ms], avg. 0.29 [ms], run count: 250
    └── 0.44% rest: 19.80 [ms]

• After: grid_size_m: 0.2

🌲 Total Processing Time Tree 🌲
100.00% faster_filter: total 3375.18 [ms], avg. 13.50 [ms], run count: 250
    ├── 97.10% process: total 3277.24 [ms], avg. 13.11 [ms], run count: 250
    │   ├── 5.15% resetCells: total 173.94 [ms], avg. 0.70 [ms], run count: 250
    │   ├── 24.86% convert: total 839.10 [ms], avg. 3.36 [ms], run count: 250
    │   ├── 5.28% preprocess: total 178.12 [ms], avg. 0.71 [ms], run count: 250
    │   │   ├── 5.25% setGridConnections: total 177.33 [ms], avg. 0.71 [ms], run count: 250
    │   │   └── 0.02% rest: 0.79 [ms]
    │   ├── 3.82% initializeGround: total 128.84 [ms], avg. 0.52 [ms], run count: 250
    │   ├── 57.78% classify: total 1950.29 [ms], avg. 7.80 [ms], run count: 250
    │   └── 0.21% rest: 6.97 [ms]
    ├── 2.29% extractObjectPoints: total 77.25 [ms], avg. 0.31 [ms], run count: 250
    └── 0.61% rest: 20.70 [ms]

• After: grid_size_m: 0.5

🌲 Total Processing Time Tree 🌲
100.00% faster_filter: total 2280.85 [ms], avg. 9.12 [ms], run count: 250
    ├── 95.42% process: total 2176.29 [ms], avg. 8.71 [ms], run count: 250
    │   ├── 2.78% resetCells: total 63.40 [ms], avg. 0.25 [ms], run count: 250
    │   ├── 30.24% convert: total 689.65 [ms], avg. 2.76 [ms], run count: 250
    │   ├── 2.50% preprocess: total 56.95 [ms], avg. 0.23 [ms], run count: 250
    │   │   ├── 2.47% setGridConnections: total 56.33 [ms], avg. 0.23 [ms], run count: 250
    │   │   └── 0.03% rest: 0.61 [ms]
    │   ├── 2.12% initializeGround: total 48.24 [ms], avg. 0.19 [ms], run count: 250
    │   ├── 57.53% classify: total 1312.27 [ms], avg. 5.25 [ms], run count: 250
    │   └── 0.25% rest: 5.78 [ms]
    ├── 3.56% extractObjectPoints: total 81.23 [ms], avg. 0.32 [ms], run count: 250
    └── 1.02% rest: 23.33 [ms]

• After: grid_size_m: 1.0

🌲 Total Processing Time Tree 🌲
100.00% faster_filter: total 1976.66 [ms], avg. 7.91 [ms], run count: 250
    ├── 93.98% process: total 1857.57 [ms], avg. 7.43 [ms], run count: 250
    │   ├── 1.82% resetCells: total 35.97 [ms], avg. 0.14 [ms], run count: 250
    │   ├── 34.23% convert: total 676.56 [ms], avg. 2.71 [ms], run count: 250
    │   ├── 1.50% preprocess: total 29.66 [ms], avg. 0.12 [ms], run count: 250
    │   │   ├── 1.47% setGridConnections: total 29.07 [ms], avg. 0.12 [ms], run count: 250
    │   │   └── 0.03% rest: 0.59 [ms]
    │   ├── 1.65% initializeGround: total 32.57 [ms], avg. 0.13 [ms], run count: 250
    │   ├── 54.50% classify: total 1077.29 [ms], avg. 4.31 [ms], run count: 250
    │   └── 0.28% rest: 5.51 [ms]
    ├── 4.74% extractObjectPoints: total 93.74 [ms], avg. 0.37 [ms], run count: 250
    └── 1.28% rest: 25.35 [ms]

Visualization

• before (grid size 0.1m)

ground-sgmt-old2-01.mp4

• after (grid size 0.5m)

ground-sgmt-new2-05.mp4

Notes for reviewers

None.

Interface changes

None.

Effects on system behavior

None.

[github-actions]

Thank you for contributing to the Autoware project!

🚧 If your pull request is in progress, switch it to draft mode.

Please ensure:

• You've checked our contribution guidelines.
• Your PR follows our pull request guidelines.
• All required CI checks pass before marking the PR ready for review.

[codecov]

Codecov Report

Attention: Patch coverage is 82.08955% with 84 lines in your changes missing coverage. Please review.

Project coverage is 13.86%. Comparing base (248bba7) to head (eaff833).
Report is 20 commits behind head on main.

Files with missing lines	Patch %	Lines
...tion/src/scan_ground_filter/grid_ground_filter.cpp	84.61%	16 Missing and 16 partials ⚠️
...round_segmentation/src/scan_ground_filter/grid.hpp	85.54%	7 Missing and 18 partials ⚠️
...tion/src/scan_ground_filter/grid_ground_filter.hpp	65.95%	11 Missing and 5 partials ⚠️
...round_segmentation/src/scan_ground_filter/node.cpp	72.00%	5 Missing and 2 partials ⚠️
...round_segmentation/src/scan_ground_filter/data.hpp	75.00%	1 Missing and 3 partials ⚠️

Additional details and impacted files

@@             Coverage Diff             @@
##             main    #9297       +/-   ##
===========================================
- Coverage   30.09%   13.86%   -16.23%     
===========================================
  Files        1340       84     -1256     
  Lines      103652     5935    -97717     
  Branches    40362     1394    -38968     
===========================================
- Hits        31190      823    -30367     
+ Misses      69450     4984    -64466     
+ Partials     3012      128     -2884     

Flag	Coverage Δ
differential	13.86% <82.08%> (?)
total	?

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

[YoshiRi]

Thanks for you work. I added some comments. Please check it.

[YoshiRi]

Just a comment.

I prefer adding brief explanation before cpp function definition so that developers can easily get into it.
Here is a sample output I asked chat AI to explain grid_ground_filter.

// Converts the input point cloud into grid cells by assigning each point to its corresponding cell.
void GridGroundFilter::convert();

// Preprocesses the grid by setting up grid connections and removing empty cells.
void GridGroundFilter::preprocess();

// Recursively searches for ground cells up to a specified count and collects their indices.
bool GridGroundFilter::recursiveSearch(const int check_idx, const int search_cnt, std::vector<int> & idx) const;

// Fits a line to the ground cells using least squares to compute slope (a) and intercept (b).
void GridGroundFilter::fitLineFromGndGrid(const std::vector<int> & idx, float & a, float & b) const;

// Initializes ground cells by analyzing points in each cell and marking ground and non-ground points.
void GridGroundFilter::initializeGround(pcl::PointIndices & out_no_ground_indices);

// Segments points in continuous cells into ground and non-ground based on height and slope thresholds.
void GridGroundFilter::SegmentContinuousCell(const Cell & cell, PointsCentroid & ground_bin, pcl::PointIndices & out_no_ground_indices);

// Segments points in discontinuous cells into ground and non-ground categories.
void GridGroundFilter::SegmentDiscontinuousCell(const Cell & cell, PointsCentroid & ground_bin, pcl::PointIndices & out_no_ground_indices);

// Segments points in break cells into ground and non-ground categories.
void GridGroundFilter::SegmentBreakCell(const Cell & cell, PointsCentroid & ground_bin, pcl::PointIndices & out_no_ground_indices);

// Classifies grid cells by segmenting points into ground and non-ground categories.
void GridGroundFilter::classify(pcl::PointIndices & out_no_ground_indices);

// Processes the input point cloud to classify points as ground or non-ground.
void GridGroundFilter::process(const PointCloud2ConstPtr & in_cloud, pcl::PointIndices & out_no_ground_indices);

[technolojin]

Explanation of the methods are added f9034ac

[YoshiRi]

LGTM

[badai-nguyen]

LGTM