costmap_3d: avoid unnecessary grid expansions #726
Merged
Conversation
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This comment has been minimized.
This comment has been minimized.
Codecov Report
@@ Coverage Diff @@
## master #726 +/- ##
==========================================
- Coverage 88.98% 88.62% -0.37%
==========================================
Files 62 62
Lines 4584 4596 +12
==========================================
- Hits 4079 4073 -6
- Misses 505 523 +18
|
This comment has been minimized.
This comment has been minimized.
… avoid-expanding-surrrounded-cells
This comment has been minimized.
This comment has been minimized.
at-wat
reviewed
Nov 1, 2023
| const int gx = pos % msg->info.width; | ||
| const int gy = pos / msg->info.width; | ||
| uint8_t mask = 0; | ||
| if (gx == 0 || (msg->data[pos - 1] >= msg->data[pos])) |
There was a problem hiding this comment.
msg->data[pos] can be stored locally to be reused
There was a problem hiding this comment.
The pointer of the cell is resued.
const int8_t* const ptr = msg->data.data() + pos;
Comment on lines
307
to
323
| if (gx == 0 || (msg->data[pos - 1] >= msg->data[pos])) | ||
| { | ||
| mask += 1; | ||
| } | ||
| if (gx == static_cast<int>(msg->info.width) - 1 || (msg->data[pos + 1] >= msg->data[pos])) | ||
| { | ||
| mask += 2; | ||
| } | ||
| if (gy == 0 || (msg->data[pos - msg->info.width] >= msg->data[pos])) | ||
| { | ||
| mask += 4; | ||
| } | ||
| if (gy == static_cast<int>(msg->info.height) - 1 || (msg->data[pos + msg->info.width] >= msg->data[pos])) | ||
| { | ||
| mask += 8; | ||
| } | ||
| return template_ranges_[mask]; |
There was a problem hiding this comment.
As here are all conditions for template generation to calculate the template index, directly generating Rect here would reduce heap memory access without performance degradation
This comment has been minimized.
This comment has been minimized.
[535] PASSED on noeticAll tests passed |
[536] PASSED on noeticAll tests passed |
[537] PASSED on noeticAll tests passed |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
Assume that there are 3 consecutive occupied grid cells on an
OccupancyGrid, andcostmap_3dexpands these cells.The black cells in the diagram above are the occupied cells, and the yellow, green, and red cells are expanded cells.
The nearest occupied cell from the yellow cells is the left one, the nearest occupied cell from the green cells is the center one, and the nearest occupied cell from the red cell is the right one. Therefore, the expansion process for the left cell should be performed on the yellow cells, the process for the center cell on the green cells, and the process for the right cell on the red cells respectively.
By this PR, the expansion ranges are limited when adjacent cells have the same or larger costs than the expanding cell. Since occupied cells on the occupancy grid map are rarely isolated, this algorithm can speed up the expansion process. Note that this algorithm is effective only when the costmap and the
OccupancyGridhave the same resolution.The calculation time of
Costmap3dLayerFootprint::updateCSpace()on my laptop (Map size: 4.5K x 2.4K x 16, the number of occupied cells: 188K)Master branch: 5.118 seconds
This PR: 0.335 seconds