Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

perf(map_height_fitter): more efficient query to find closest lanelet #7020

Conversation

maxime-clem
Copy link
Contributor

Description

Improve performance of the map_height_fitter when getting the ground height.
Currently, it checks ALL lanelets to find the closest one. With this PR, the query is more efficient (thanks to the R-tree used in the lanelet layer).

Tests performed

Not applicable.

Effects on system behavior

Not applicable.

Interface changes

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

  • There are no open discussions or they are tracked via tickets.

After all checkboxes are checked, anyone who has write access can merge the PR.

@github-actions github-actions bot added the component:map Map creation, storage, and loading. (auto-assigned) label May 15, 2024
@maxime-clem maxime-clem added run:build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) and removed component:map Map creation, storage, and loading. (auto-assigned) labels May 15, 2024
@SakodaShintaro
Copy link
Contributor

@maxime-clem
Thank you very much!
Sorry, I implemented it inefficiently due to my ignorance of lanelet modules.

I would like to ask an additional question that is not related to the pull request.
Is it reasonable to use centerline() from a retrieved item to get the z-coordinate?
I don't know what a centerline is, and I'm thinking that if this could be a very long lanelet element along the way, then there might be a slight probability that it is far from the query point.

@maxime-clem
Copy link
Contributor Author

Is it reasonable to use centerline() from a retrieved item to get the z-coordinate?

Thank you for checking the PR. It sounds like it would be better to retrieve the closest point. Which is also possible. I can use nearest(lanelet::BasicPoint2d{x, y}, 1); on the vector_map_->pointLayer instead of the vector_map_->laneletLayer. If that sounds okay I will update the PR.

@SakodaShintaro
Copy link
Contributor

I think that change is fine. 👍
I apologize for the inconvenience, but please make the change.

@YamatoAndo YamatoAndo added the component:map Map creation, storage, and loading. (auto-assigned) label May 15, 2024
Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
@maxime-clem maxime-clem force-pushed the perf/map_height_fitter-rm-query-on-all_lanelets branch from 6e2f697 to 957ac7f Compare May 15, 2024 12:49
@maxime-clem
Copy link
Contributor Author

@SakodaShintaro I committed the change. Are you able to test the new version and confirm it works as expected ?

Copy link
Contributor

@SakodaShintaro SakodaShintaro left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I conducted the following three types of tests.

  • planning_simulator (initializing pose by vectormap)
  • logging_simulator (initializing pose by both pcd and vectormap)
  • AWSIM (initializing pose by both pcd and vectormap)

Everything worked fine. Thank you very much.

@maxime-clem maxime-clem merged commit 017b3fb into autowarefoundation:main May 15, 2024
21 of 22 checks passed
@maxime-clem maxime-clem deleted the perf/map_height_fitter-rm-query-on-all_lanelets branch May 15, 2024 14:44
vividf pushed a commit to vividf/autoware.universe that referenced this pull request May 16, 2024
…oint query (autowarefoundation#7020)

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
Signed-off-by: vividf <yihsiang.fang@tier4.jp>
karishma1911 pushed a commit to Interplai/autoware.universe that referenced this pull request Jun 3, 2024
…oint query (autowarefoundation#7020)

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
component:map Map creation, storage, and loading. (auto-assigned) run:build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci)
Projects
None yet
Development

Successfully merging this pull request may close these issues.

3 participants