feat(ndt_scan_matcher): implement tpe sampler in ndt_align_srv #5078

SakodaShintaro · 2023-09-22T10:32:27Z

Description

The ndt service for initial position estimation has been changed from random search to search using TPE.

Summary of Changes

Tests performed

Tested by

logging simulator about the sample rosbag.
AWSIM

Evaluation

In AWSIM (Nishishinjuku), the initial pose estimation was performed for each method 200 times, and the final best values of ndt_align_srv are obtained.

Method	Mean Score	Process Time
Random	5.938 ± 0.911	1.073 ± 0.043 [sec]
TPE	6.233 ± 0.634	1.208 ± 0.045 [sec]

Effects on system behavior

Improves the performance of initial position estimation.
Initial position estimation takes only slightly more time.

Pre-review checklist for the PR author

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

I've confirmed the contribution guidelines.
The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

The PR follows the pull request guidelines.

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.

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

…ing_initial_pose

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

…ing_initial_pose

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

SakodaShintaro · 2023-10-05T09:29:30Z

@KYabuuchi

I added test on TPE. d56c845

You can test as following commands.

colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTING=ON --packages-select ndt_scan_matcher
colcon test --packages-select ndt_scan_matcher
colcon test-result --verbose

The details of the result are output to directories such as autoware/log/test_2023-10-05_18-22-43/ndt_scan_matcher/stdout.log

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

SakodaShintaro · 2023-10-06T01:15:42Z

@Motsu-san

This is a big change. I would like to review this carefully. Could you let us know the design changes with some functional block diagrams or something?

I added "Summary of Changes" figure to pull request description.

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

localization/ndt_scan_matcher/include/ndt_scan_matcher/tree_structured_parzen_estimator.hpp

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

Signed-off-by: Shintaro SAKODA <shintaro.sakoda@tier4.jp>

KYabuuchi

@SakodaShintaro This will be my final question.
Once this is resolved, I will be ready to approve.

localization/ndt_scan_matcher/include/ndt_scan_matcher/tree_structured_parzen_estimator.hpp

KYabuuchi

LGTM

codecov · 2023-10-12T10:11:26Z

Codecov Report

Attention: 98 lines in your changes are missing coverage. Please review.

Comparison is base (84995ec) 14.84% compared to head (e484b9f) 14.87%.
Report is 15 commits behind head on main.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #5078      +/-   ##
==========================================
+ Coverage   14.84%   14.87%   +0.03%     
==========================================
  Files        1649     1652       +3     
  Lines      114166   114328     +162     
  Branches    35323    35389      +66     
==========================================
+ Hits        16943    17008      +65     
- Misses      78102    78155      +53     
- Partials    19121    19165      +44

Flag	Coverage Δ		*Carryforward flag
differential	`7.14% <39.87%> (?)`
total	`14.84% <ø> (+<0.01%)`	⬆️	Carriedforward from 84995ec

*This pull request uses carry forward flags. Click here to find out more.

Files	Coverage Δ
..._scan_matcher/tree_structured_parzen_estimator.hpp	`0.00% <0.00%> (ø)`
localization/ndt_scan_matcher/test/test_tpe.cpp	`54.16% <54.16%> (ø)`
...n_matcher/src/tree_structured_parzen_estimator.cpp	`55.31% <55.31%> (ø)`
...ion/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp	`0.00% <0.00%> (ø)`

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

kminoda · 2023-10-14T04:03:13Z

Just realized that it may be better to change the name of the function align_using_monte_carlo 😅

…arefoundation#5078) * Added output_pose_with_cov_to_log Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Added logging particles Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Added TreeStructuredParzenEstimator Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * FIxed to run TPE Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed initial pose Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed Input type Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed good condition Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed operation if good_num_ == 0 Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed cov Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed parameter Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed initial Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed to use good_threshold_ Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed to count good_num_ Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed stddev Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed parameter Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed parameters Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Removed unused functions Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed to abs Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed parameters Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * FIxed parameter Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * add_trial by result Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * FIxed constructor Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * FIxed to use n_startup_trials_ Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed parameters Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Added kTargetScore Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed constants Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Removed output_pose_with_cov_to_log Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed a comment Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Added a parameter "n_startup_trials" Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * style(pre-commit): autofix * Fixed include guard Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * style(pre-commit): autofix * Fixed hyphen to underscore Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Added sample_from_prior Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed to use log_sum_exp Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed acquisition_function Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed n_startup_trials_ Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed to 99% Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed n_startup_trials_ Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed value of n_startup_trials Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Removed log output Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed default value Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Removed static distributions Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed PRIOR_WEIGHT Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed order Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Added operator* to Input Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed parameters Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Implemented normal tpe Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Added is_loop_variable Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Organized PRIOR_WEIGHT Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed PRIOR_WEIGHT 0.0 Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Added CONVERT_COEFF Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed n_startup_trials Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Renamed acquisition_function to compute_log_likelihood_ratio Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Changed random number generator to static variables Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Added reference Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed base_stddev_ to a const variable Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed to use VALUE_WIDTH Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Removed a comment Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Added `n_startup_trials` to README.md Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Added a test about tpe Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Added the license statement Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Changed default `n_startup_trials` to 20 Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed sqrt(2) and added comments Signed-off-by: Shintaro SAKODA <shintaro.sakoda@tier4.jp> * Added comments Signed-off-by: Shintaro SAKODA <shintaro.sakoda@tier4.jp> * Added comments Signed-off-by: Shintaro SAKODA <shintaro.sakoda@tier4.jp> --------- Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> 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>

SakodaShintaro added 30 commits September 14, 2023 15:09

Added output_pose_with_cov_to_log

e804d8c

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

Merge remote-tracking branch 'origin' into feat/ndt_scan_matcher_logg…

0f2283f

…ing_initial_pose

Added logging particles

a3994cd

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

Merge remote-tracking branch 'origin' into feat/ndt_scan_matcher_logg…

5c11494

…ing_initial_pose

Merge remote-tracking branch 'origin' into feat/ndt_scan_matcher_logg…

f35a15c

…ing_initial_pose

Added TreeStructuredParzenEstimator

3bd6b1e

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

FIxed to run TPE

8f1bbb6

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

Fixed initial pose

58cc516

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

Fixed Input type

b8be222

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

Fixed good condition

06ce0bc

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

Fixed operation if good_num_ == 0

aa87ffb

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

Fixed cov

8702884

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

Fixed parameter

2e660b7

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

Fixed initial

9169277

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

Fixed to use good_threshold_

2973464

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

Fixed to count good_num_

2e1b5a1

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

Fixed stddev

6fadcff

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

Fixed parameter

91fc952

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

Fixed parameters

3ad5207

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

Removed unused functions

9f77070

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

Fixed to abs

8f6a297

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

Fixed parameters

082b5ba

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

FIxed parameter

a1f7ddc

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

add_trial by result

b006294

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

Merge remote-tracking branch 'origin' into feat/ndt_align_service_tpe

d13c9b4

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

FIxed constructor

5765053

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

FIxed to use n_startup_trials_

9cc9ed3

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

Fixed parameters

c71fc36

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

Added kTargetScore

6aa8207

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

Merge remote-tracking branch 'origin' into feat/ndt_align_service_tpe

256b62b

SakodaShintaro added 6 commits October 5, 2023 17:25

Merge branch 'main' into feat/ndt_align_service_tpe

caa6a40

Added reference

3ad0bf6

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

Fixed base_stddev_ to a const variable

8596755

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

Fixed to use VALUE_WIDTH

479a9f2

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

Removed a comment

5ec9dfc

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

Added n_startup_trials to README.md

9887fcf

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

github-actions bot added the type:documentation Creating or refining documentation. (auto-assigned) label Oct 5, 2023

Added a test about tpe

d56c845

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

SakodaShintaro mentioned this pull request Oct 5, 2023

feat(words): add optuna tier4/autoware-spell-check-dict#628

Merged

SakodaShintaro added 2 commits October 5, 2023 18:32

Added the license statement

dcd6568

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

Changed default n_startup_trials to 20

6277588

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

SakodaShintaro marked this pull request as ready for review October 6, 2023 01:15

KYabuuchi reviewed Oct 10, 2023

View reviewed changes

SakodaShintaro added 5 commits October 11, 2023 10:27

Merge branch 'main' into feat/ndt_align_service_tpe

23e5305

Fixed sqrt(2) and added comments

00b0adc

Signed-off-by: Shintaro SAKODA <shintaro.sakoda@tier4.jp>

Added comments

6967850

Signed-off-by: Shintaro SAKODA <shintaro.sakoda@tier4.jp>

Merge branch 'main' into feat/ndt_align_service_tpe

82cecde

Added comments

e484b9f

Signed-off-by: Shintaro SAKODA <shintaro.sakoda@tier4.jp>

KYabuuchi reviewed Oct 12, 2023

View reviewed changes

localization/ndt_scan_matcher/include/ndt_scan_matcher/tree_structured_parzen_estimator.hpp Show resolved Hide resolved

KYabuuchi approved these changes Oct 12, 2023

View reviewed changes

SakodaShintaro added the run:build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) label Oct 12, 2023

SakodaShintaro merged commit 21acc28 into autowarefoundation:main Oct 13, 2023

SakodaShintaro deleted the feat/ndt_align_service_tpe branch October 13, 2023 04:27

tier4-autoware-public-bot bot mentioned this pull request Oct 25, 2023

chore: sync upstream tier4/autoware.universe#979

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

feat(ndt_scan_matcher): implement tpe sampler in ndt_align_srv #5078

feat(ndt_scan_matcher): implement tpe sampler in ndt_align_srv #5078

SakodaShintaro commented Sep 22, 2023 •

edited

Loading

SakodaShintaro commented Oct 5, 2023

SakodaShintaro commented Oct 6, 2023 •

edited

Loading

KYabuuchi left a comment

KYabuuchi left a comment

codecov bot commented Oct 12, 2023 •

edited

Loading

kminoda commented Oct 14, 2023

feat(ndt_scan_matcher): implement tpe sampler in ndt_align_srv #5078

feat(ndt_scan_matcher): implement tpe sampler in ndt_align_srv #5078

Conversation

SakodaShintaro commented Sep 22, 2023 • edited Loading

Description

Related links

Summary of Changes

Tests performed

Evaluation

Effects on system behavior

Pre-review checklist for the PR author

In-review checklist for the PR reviewers

Post-review checklist for the PR author

SakodaShintaro commented Oct 5, 2023

SakodaShintaro commented Oct 6, 2023 • edited Loading

KYabuuchi left a comment

Choose a reason for hiding this comment

KYabuuchi left a comment

Choose a reason for hiding this comment

codecov bot commented Oct 12, 2023 • edited Loading

Codecov Report

kminoda commented Oct 14, 2023

SakodaShintaro commented Sep 22, 2023 •

edited

Loading

SakodaShintaro commented Oct 6, 2023 •

edited

Loading

codecov bot commented Oct 12, 2023 •

edited

Loading