feat(autoware_ground_segmentation): grid data structure revision for efficiency improve #9297

technolojin · 2024-11-12T09:16:16Z

Description

Change data structure of grid for calculation efficiency improvement.

(Performance) Grid and grid cells are defined
(Performance) The grid geometry is modeled to Concentric Zone Model (CZM) based polar grid
(Refactor) The grid process is separated from node.cpp
(Performance) The segmentation process is ran cell-wise (while current process is point-wise).
(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

By replacing existing data structure, sorting is not needed.
Segmentation condition (connected, disconnected, break) is determined grid cell-wise, not point-wise.
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.
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

Introducing CZM polar grid requires cell geometry calculation. If the number of cell grids is large, the calculation can take longer time.
Finding non-empty grid cell take calculation cost. Increase by the number of cell grids.
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

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.

After: grid_size_m: 0.1

🌲 Total Processing Time Tree 🌲
100.00% faster_filter: total 4829.41 [ms], avg. 19.96 [ms], run count: 242
    ├── 98.48% process: total 4755.83 [ms], avg. 19.65 [ms], run count: 242
    │   ├── 8.14% resetCells: total 392.91 [ms], avg. 1.62 [ms], run count: 242
    │   ├── 21.71% convert: total 1048.67 [ms], avg. 4.33 [ms], run count: 242
    │   ├── 10.50% preprocess: total 507.22 [ms], avg. 2.10 [ms], run count: 242
    │   │   ├── 10.49% setGridConnections: total 506.39 [ms], avg. 2.09 [ms], run count: 242
    │   │   └── 0.02% rest: 0.83 [ms]
    │   ├── 4.58% initializeGround: total 221.22 [ms], avg. 0.91 [ms], run count: 242
    │   ├── 53.39% classify: total 2578.31 [ms], avg. 10.65 [ms], run count: 242
    │   └── 0.16% rest: 7.50 [ms]
    ├── 1.17% extractObjectPoints: total 56.59 [ms], avg. 0.23 [ms], run count: 242
    └── 0.35% rest: 16.99 [ms]

After: grid_size_m: 0.2

🌲 Total Processing Time Tree 🌲
100.00% faster_filter: total 3486.55 [ms], avg. 14.35 [ms], run count: 243
    ├── 97.94% process: total 3414.80 [ms], avg. 14.05 [ms], run count: 243
    │   ├── 6.08% resetCells: total 212.07 [ms], avg. 0.87 [ms], run count: 243
    │   ├── 24.60% convert: total 857.85 [ms], avg. 3.53 [ms], run count: 243
    │   ├── 6.70% preprocess: total 233.55 [ms], avg. 0.96 [ms], run count: 243
    │   │   ├── 6.68% setGridConnections: total 232.85 [ms], avg. 0.96 [ms], run count: 243
    │   │   └── 0.02% rest: 0.70 [ms]
    │   ├── 3.73% initializeGround: total 130.04 [ms], avg. 0.54 [ms], run count: 243
    │   ├── 56.65% classify: total 1975.29 [ms], avg. 8.13 [ms], run count: 243
    │   └── 0.17% rest: 6.00 [ms]
    ├── 1.57% extractObjectPoints: total 54.74 [ms], avg. 0.23 [ms], run count: 243
    └── 0.49% rest: 17.01 [ms]

After: grid_size_m: 0.5

🌲 Total Processing Time Tree 🌲
100.00% faster_filter: total 2503.10 [ms], avg. 10.26 [ms], run count: 244
    ├── 97.29% process: total 2435.27 [ms], avg. 9.98 [ms], run count: 244
    │   ├── 3.21% resetCells: total 80.33 [ms], avg. 0.33 [ms], run count: 244
    │   ├── 29.74% convert: total 744.51 [ms], avg. 3.05 [ms], run count: 244
    │   ├── 3.09% preprocess: total 77.26 [ms], avg. 0.32 [ms], run count: 244
    │   │   ├── 3.05% setGridConnections: total 76.42 [ms], avg. 0.31 [ms], run count: 244
    │   │   └── 0.03% rest: 0.84 [ms]
    │   ├── 1.97% initializeGround: total 49.26 [ms], avg. 0.20 [ms], run count: 244
    │   ├── 59.07% classify: total 1478.57 [ms], avg. 6.06 [ms], run count: 244
    │   └── 0.21% rest: 5.35 [ms]
    ├── 1.95% extractObjectPoints: total 48.80 [ms], avg. 0.20 [ms], run count: 244
    └── 0.76% rest: 19.03 [ms]

After: grid_size_m: 1.0

🌲 Total Processing Time Tree 🌲
100.00% faster_filter: total 1989.49 [ms], avg. 8.22 [ms], run count: 242
    ├── 96.41% process: total 1918.08 [ms], avg. 7.93 [ms], run count: 242
    │   ├── 2.04% resetCells: total 40.52 [ms], avg. 0.17 [ms], run count: 242
    │   ├── 34.13% convert: total 679.11 [ms], avg. 2.81 [ms], run count: 242
    │   ├── 1.59% preprocess: total 31.61 [ms], avg. 0.13 [ms], run count: 242
    │   │   ├── 1.56% setGridConnections: total 31.09 [ms], avg. 0.13 [ms], run count: 242
    │   │   └── 0.03% rest: 0.52 [ms]
    │   ├── 1.71% initializeGround: total 34.12 [ms], avg. 0.14 [ms], run count: 242
    │   ├── 56.69% classify: total 1127.92 [ms], avg. 4.66 [ms], run count: 242
    │   └── 0.24% rest: 4.80 [ms]
    ├── 2.55% extractObjectPoints: total 50.80 [ms], avg. 0.21 [ms], run count: 242
    └── 1.04% rest: 20.61 [ms]

Notes for reviewers

None.

Interface changes

None.

Effects on system behavior

None.

github-actions · 2024-11-12T09:16:36Z

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.

Signed-off-by: Taekjin LEE <[email protected]>

…ode.cpp Signed-off-by: Taekjin LEE <[email protected]>

Signed-off-by: Taekjin LEE <[email protected]>

… size refactor: Update Grid class initialization to use radians for azimuth size refactor: Update Grid class initialization to use radians for azimuth size Signed-off-by: Taekjin LEE <[email protected]>

Signed-off-by: Taekjin LEE <[email protected]>

… size Signed-off-by: Taekjin LEE <[email protected]>

Signed-off-by: Taekjin LEE <[email protected]>

…nt a segmentation logic refactor: Update Grid class initialization and reset methods, implement a segmentation logic refactor: replace original methods Signed-off-by: Taekjin LEE <[email protected]>

Signed-off-by: Taekjin LEE <[email protected]>

refactor: remove previous scan ground grid Signed-off-by: Taekjin LEE <[email protected]>

…for radius comparisons Signed-off-by: Taekjin LEE <[email protected]>

Signed-off-by: Taekjin LEE <[email protected]>

fix: ground bin logic fix Signed-off-by: Taekjin LEE <[email protected]>

Signed-off-by: Taekjin LEE <[email protected]>

fix: azimuth grid index converter bug Signed-off-by: Taekjin LEE <[email protected]>

fix: cell connection bug fix Signed-off-by: Taekjin LEE <[email protected]>

Signed-off-by: Taekjin LEE <[email protected]>

codecov · 2024-11-13T06:19:23Z

Codecov Report

Attention: Patch coverage is 84.26724% with 73 lines in your changes missing coverage. Please review.

Project coverage is 13.97%. Comparing base (1ab29fd) to head (88c93a9).
Report is 67 commits behind head on main.

Files with missing lines	Patch %	Lines
...tion/src/scan_ground_filter/grid_ground_filter.cpp	87.69%	11 Missing and 13 partials ⚠️
...round_segmentation/src/scan_ground_filter/grid.hpp	87.84%	6 Missing and 16 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   29.19%   13.97%   -15.22%     
===========================================
  Files        1333       84     -1249     
  Lines      102795     5932    -96863     
  Branches    39870     1386    -38484     
===========================================
- Hits        30008      829    -29179     
+ Misses      69926     4980    -64946     
+ Partials     2861      123     -2738

Flag	Coverage Δ
differential	`13.97% <84.26%> (?)`
total	`?`

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

🚨 Try these New Features:

Flaky Tests Detection - Detect and resolve failed and flaky tests

Signed-off-by: Taekjin LEE <[email protected]>

badai-nguyen · 2024-11-15T02:33:14Z

perception/autoware_ground_segmentation/src/scan_ground_filter/node.cpp

    std::memcpy(
-      &out_object_cloud.data[output_data_size], &in_cloud_ptr->data[i * in_cloud_ptr->point_step],
+      &out_object_cloud.data[output_data_size], &in_cloud_ptr->data[idx],


idx is index of point in pointcloud, is correct to use directly for memcpy?

badai-nguyen · 2024-11-15T02:39:47Z

perception/autoware_ground_segmentation/src/scan_ground_filter/grid_ground_filter.cpp

+    // Get Point
+    pcl::PointXYZ input_point;
+    data_accessor_.getPoint(in_cloud_, data_index, input_point);
+    grid_ptr_->addPoint(input_point.x, input_point.y, data_index);


Q: why did you use y for input of addPoint? as the function definition, it should be height?

Signed-off-by: Taekjin LEE <[email protected]>

github-actions bot added component:perception Advanced sensor data processing and environment understanding. (auto-assigned) tag:require-cuda-build-and-test labels Nov 12, 2024

technolojin force-pushed the feat/ground-sgmt/grid-revision branch from d5420a1 to 6d76680 Compare November 12, 2024 09:22

technolojin self-assigned this Nov 12, 2024

technolojin mentioned this pull request Nov 13, 2024

feat(scan_ground_filter): update grid size for ground segmentation autowarefoundation/autoware_launch#1223

Open

4 tasks

technolojin added 24 commits November 13, 2024 14:52

fix: replace point index to data index

c6c6b0c

Signed-off-by: Taekjin LEE <[email protected]>

feat: Use emplace_back instead of push_back for adding gnd_grids in n…

f4e5fe1

…ode.cpp Signed-off-by: Taekjin LEE <[email protected]>

fix: prep for non-sorted grid process

9680f48

Signed-off-by: Taekjin LEE <[email protected]>

feat: Add Cell class and Grid class for grid-based segmentation

21c968b

Signed-off-by: Taekjin LEE <[email protected]>

refactor: Add Cell and Grid classes for grid-based segmentation

1490563

Signed-off-by: Taekjin LEE <[email protected]>

feat: initialize new grid

c227275

Signed-off-by: Taekjin LEE <[email protected]>

refactor: Update Grid class initialization to use radians for azimuth…

41d1e75

… size refactor: Update Grid class initialization to use radians for azimuth size refactor: Update Grid class initialization to use radians for azimuth size Signed-off-by: Taekjin LEE <[email protected]>

refactor: Fix calculation of azimuth index in Grid class

b66780d

Signed-off-by: Taekjin LEE <[email protected]>

feat: implement grid based segmentation, temporary logic

f6103e0

Signed-off-by: Taekjin LEE <[email protected]>

refactor: idx position convert methods

00cf8b1

Signed-off-by: Taekjin LEE <[email protected]>

refactor: Update Grid class initialization to use radians for azimuth…

71969a7

… size Signed-off-by: Taekjin LEE <[email protected]>

feat: reconnect grids filled

513c71a

Signed-off-by: Taekjin LEE <[email protected]>

feat: grid initialization

f6d300c

Signed-off-by: Taekjin LEE <[email protected]>

refactor: Update Grid class initialization and reset methods, impleme…

0df2a07

…nt a segmentation logic refactor: Update Grid class initialization and reset methods, implement a segmentation logic refactor: replace original methods Signed-off-by: Taekjin LEE <[email protected]>

feat: add time_keeper

43f911a

Signed-off-by: Taekjin LEE <[email protected]>

refactor: add time keeper in grid class

7b468c3

refactor: remove previous scan ground grid Signed-off-by: Taekjin LEE <[email protected]>

refactor: optimize grid boundary calculations and use squared values …

13c3ffa

…for radius comparisons Signed-off-by: Taekjin LEE <[email protected]>

fix: use pointer for prev cell

822f435

Signed-off-by: Taekjin LEE <[email protected]>

refactor: remove time keeper called too many times

43f23c1

Signed-off-by: Taekjin LEE <[email protected]>

fix: radial idx estimation fix

84c0eb7

Signed-off-by: Taekjin LEE <[email protected]>

refactor: optimize ground bin average calculation

8ca47ce

fix: ground bin logic fix Signed-off-by: Taekjin LEE <[email protected]>

refactor: make grid ground filter separate

cb96e5b

Signed-off-by: Taekjin LEE <[email protected]>

refactor: remove unused code

2102a06

fix: azimuth grid index converter bug Signed-off-by: Taekjin LEE <[email protected]>

fix: segmentation logic determination fix

87ac927

fix: cell connection bug fix Signed-off-by: Taekjin LEE <[email protected]>

technolojin added 6 commits November 13, 2024 14:52

refactor: Update grid radial limit to 200.0m

e26f3e7

Signed-off-by: Taekjin LEE <[email protected]>

refactor: Update grid size to 0.5m for improved ground segmentation

1c5b566

Signed-off-by: Taekjin LEE <[email protected]>

refactor: Improve grid ground filter segmentation logic

1a66124

Signed-off-by: Taekjin LEE <[email protected]>

refactor: Optimize grid ground filter segmentation logic

8a29df0

Signed-off-by: Taekjin LEE <[email protected]>

refactor: Update logic order for fast segmentation

dcb0569

Signed-off-by: Taekjin LEE <[email protected]>

fix: resolve cppcheck issue

980e054

Signed-off-by: Taekjin LEE <[email protected]>

technolojin force-pushed the feat/ground-sgmt/grid-revision branch from b63d4f1 to 980e054 Compare November 13, 2024 05:52

technolojin added the tag:run-build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) label Nov 13, 2024

technolojin marked this pull request as ready for review November 13, 2024 06:46

technolojin requested review from amc-nu, badai-nguyen, miursh, YoshiRi and yukkysaito as code owners November 13, 2024 06:46

fix: pseudo atan2 fix for even distribution of azimuth

1b9d696

Signed-off-by: Taekjin LEE <[email protected]>

badai-nguyen reviewed Nov 18, 2024

View reviewed changes

technolojin and others added 5 commits November 18, 2024 18:33

fix: remove unused next_grid_idx_ update

ed7c65e

Signed-off-by: Taekjin LEE <[email protected]>

fix: introduce pseudo tangent to match result of pseudo arc tangent

aadebd7

Signed-off-by: Taekjin LEE <[email protected]>

style(pre-commit): autofix

5a32d59

fix: limit gradient

013415f

Signed-off-by: Taekjin LEE <[email protected]>

fix: bring previous average when the ground bin is empty

6484fb4

Signed-off-by: Taekjin LEE <[email protected]>

technolojin marked this pull request as draft November 19, 2024 08:15

technolojin and others added 7 commits November 19, 2024 18:14

fix: back to constant azimuth interval grid

6523dcd

Signed-off-by: Taekjin LEE <[email protected]>

perf: remove division for efficiency

cc3a44b

Signed-off-by: Taekjin LEE <[email protected]>

perf: remove division for efficiency

3c7c3d4

Signed-off-by: Taekjin LEE <[email protected]>

perf: contain radius and height to avoid double calculation

f9f6388

Signed-off-by: Taekjin LEE <[email protected]>

perf: optimize grid distance calculation for efficiency

85e7577

Signed-off-by: Taekjin LEE <[email protected]>

style(pre-commit): autofix

024abc3

perf: using isEmpty for efficiency

88c93a9

Signed-off-by: Taekjin LEE <[email protected]>

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

feat(autoware_ground_segmentation): grid data structure revision for efficiency improve #9297

feat(autoware_ground_segmentation): grid data structure revision for efficiency improve #9297

technolojin commented Nov 12, 2024 •

edited

Loading

github-actions bot commented Nov 12, 2024 •

edited

Loading

codecov bot commented Nov 13, 2024 •

edited

Loading

badai-nguyen Nov 15, 2024

badai-nguyen Nov 15, 2024 •

edited

Loading

feat(autoware_ground_segmentation): grid data structure revision for efficiency improve #9297

Are you sure you want to change the base?

feat(autoware_ground_segmentation): grid data structure revision for efficiency improve #9297

Conversation

technolojin commented Nov 12, 2024 • edited Loading

Description

How the efficiency is improved

Drawbacks

Drawback mitigation

Related links

How was this PR tested?

Performance test (check degradation)

Processing time

Before

After

Notes for reviewers

Interface changes

Effects on system behavior

github-actions bot commented Nov 12, 2024 • edited Loading

codecov bot commented Nov 13, 2024 • edited Loading

Codecov Report

badai-nguyen Nov 15, 2024

Choose a reason for hiding this comment

badai-nguyen Nov 15, 2024 • edited Loading

Choose a reason for hiding this comment

technolojin commented Nov 12, 2024 •

edited

Loading

github-actions bot commented Nov 12, 2024 •

edited

Loading

codecov bot commented Nov 13, 2024 •

edited

Loading

badai-nguyen Nov 15, 2024 •

edited

Loading