fix(ndt_scan_matcher): improved tpe #6990
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
Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>
github-actions
bot
added
the
component:localization
4 tasks
SakodaShintaro
added
the
tag:run-build-and-test-differential
Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>
SakodaShintaro
requested review from
anhnv3991,
KYabuuchi,
Motsu-san,
RyuYamamoto,
TaikiYamada4,
YamatoAndo and
kminoda
as code owners
KYabuuchi
reviewed
May 13, 2024
localization/tree_structured_parzen_estimator/test/test_tpe.cpp
Outdated
Show resolved
Hide resolved
|
I tested this PR, and it seems to be improved compared to before. |
Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>
SakodaShintaro
merged commit ac5190b
into
autowarefoundation:main
25 of 26 checks passed
vividf
pushed a commit
to vividf/autoware.universe
that referenced
this pull request
May 16, 2024
* Improved tpe Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Added name in TODO Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed tpe test Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * style(pre-commit): autofix --------- Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Signed-off-by: vividf <yihsiang.fang@tier4.jp>
8 tasks
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.
Description
Context
The current
ndt_scan_matcheruses Tree-Structured Parzen Estimator (TPE) for initial pose estimation. However, this implementation is not very good and often searches far away positions on the square.In the image below, the search is being performed at a position far from the correct position on the upper and left sides.
One of the reasons for this is that TPE is designed as a general-purpose optimization class and is formulated as sampling for a uniform distribution.
ndt_scan_matcherperforms sampling based on normal distribution, so we introduce mutual conversion between uniform distribution and normal distribution, but it seems that this is not working as expected.Changes
First, since TPE is only used in ndt_scan_matcher, we changed it to an implementation specialized for optimization in NDT. For example, we changed the sampling inside the TPE to a normal distribution, and changed the standard deviation value used in Parzen Estimator to a value that corresponds to the convergence ability of NDT.
Next, change the default number of initial random searches in TPE from 20 to 100. The larger this value is, the closer it gets to a complete random search, so in this case it gets closer to a normal distribution.
As a result of these changes, the distribution became more natural and the initial pose estimation performance improved slightly.
Related PR
Tests performed
Regarding the AWSIM data, we gave ndt_scan_matcher a total of 100 initial positions, 5x5 at 1m intervals for x and y, and 4 at 90 degrees each for yaw, from the correct initial position, and experimented to see if it could converge to the correct pose.
The results of 100 trials were plotted in a boxplot using the Transformation Probability (TP) value as an indicator.
Although it still sometimes converges to a bad score, the results are better than completely random.
We also verified the data of about 20 other points that TIER IV holds internally, and found that the data generally improved, and even in other cases, the performance remained almost the same.
Effects on system behavior
The performance of initial pose estimation is slightly improved.
Interface changes
No 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.
After all checkboxes are checked, anyone who has write access can merge the PR.