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 <[email protected]>

…ing_initial_pose

Signed-off-by: Shintaro Sakoda <[email protected]>

…ing_initial_pose

Signed-off-by: Shintaro Sakoda <[email protected]>

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 <[email protected]>

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 <[email protected]>

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

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

0f2283f

…ing_initial_pose

Added logging particles

a3994cd

Signed-off-by: Shintaro Sakoda <[email protected]>

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 <[email protected]>

FIxed to run TPE

8f1bbb6

Signed-off-by: Shintaro Sakoda <[email protected]>

Fixed initial pose

58cc516

Signed-off-by: Shintaro Sakoda <[email protected]>

Fixed Input type

b8be222

Signed-off-by: Shintaro Sakoda <[email protected]>

Fixed good condition

06ce0bc

Signed-off-by: Shintaro Sakoda <[email protected]>

Fixed operation if good_num_ == 0

aa87ffb

Signed-off-by: Shintaro Sakoda <[email protected]>

Fixed cov

8702884

Signed-off-by: Shintaro Sakoda <[email protected]>

Fixed parameter

2e660b7

Signed-off-by: Shintaro Sakoda <[email protected]>

Fixed initial

9169277

Signed-off-by: Shintaro Sakoda <[email protected]>

Fixed to use good_threshold_

2973464

Signed-off-by: Shintaro Sakoda <[email protected]>

Fixed to count good_num_

2e1b5a1

Signed-off-by: Shintaro Sakoda <[email protected]>

Fixed stddev

6fadcff

Signed-off-by: Shintaro Sakoda <[email protected]>

Fixed parameter

91fc952

Signed-off-by: Shintaro Sakoda <[email protected]>

Fixed parameters

3ad5207

Signed-off-by: Shintaro Sakoda <[email protected]>

Removed unused functions

9f77070

Signed-off-by: Shintaro Sakoda <[email protected]>

Fixed to abs

8f6a297

Signed-off-by: Shintaro Sakoda <[email protected]>

Fixed parameters

082b5ba

Signed-off-by: Shintaro Sakoda <[email protected]>

FIxed parameter

a1f7ddc

Signed-off-by: Shintaro Sakoda <[email protected]>

add_trial by result

b006294

Signed-off-by: Shintaro Sakoda <[email protected]>

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

d13c9b4

Signed-off-by: Shintaro Sakoda <[email protected]>

FIxed constructor

5765053

Signed-off-by: Shintaro Sakoda <[email protected]>

FIxed to use n_startup_trials_

9cc9ed3

Signed-off-by: Shintaro Sakoda <[email protected]>

Fixed parameters

c71fc36

Signed-off-by: Shintaro Sakoda <[email protected]>

Added kTargetScore

6aa8207

Signed-off-by: Shintaro Sakoda <[email protected]>

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 <[email protected]>

Fixed base_stddev_ to a const variable

8596755

Signed-off-by: Shintaro Sakoda <[email protected]>

Fixed to use VALUE_WIDTH

479a9f2

Signed-off-by: Shintaro Sakoda <[email protected]>

Removed a comment

5ec9dfc

Signed-off-by: Shintaro Sakoda <[email protected]>

Added n_startup_trials to README.md

9887fcf

Signed-off-by: Shintaro Sakoda <[email protected]>

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 <[email protected]>

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 <[email protected]>

Changed default n_startup_trials to 20

6277588

Signed-off-by: Shintaro Sakoda <[email protected]>

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 <[email protected]>

Added comments

6967850

Signed-off-by: Shintaro SAKODA <[email protected]>

Merge branch 'main' into feat/ndt_align_service_tpe

82cecde

Added comments

e484b9f

Signed-off-by: Shintaro SAKODA <[email protected]>

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