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

feat(ndt_scan_matcher): estimate the covariance of ndt scan matching in real time instead of using a predefined value #7596

Merged

Conversation

SaltUhey
Copy link
Contributor

@SaltUhey SaltUhey commented Jun 20, 2024

Description

Now, Autoware localizer uses Extended Kalman Filter (EKF) based on estimated pose with covariance and estimated velocity.
Moreover, the covariance of estimated pose is empirically defined in advance and remains constant regardless of location.
However, I think that real-time covariance estimation can make some contribution to addressing the following two LiDAR degeneracy environments.

  • Tunnel
  • Open spaces such as fields, rice paddies, and wilderness

By properly estimating the covariance at these locations, where ndt_pose are prone to error, the incorrect ndt_pose information is less likely to be integrated into the ekf_pose.
I propose to implement three methods as sub-functions by using this.

Related links

autoware_launch

Tests performed

Tests were conducted in tunnels and open spaces to verify that the covariance can be estimated in real time.

Notes for reviewers

You need to add the parameter autoware_launch, which is an external repository as shown in the image below.
image

Interface changes

There are no changes to the interface other than the addition of sub-functions.

Effects on system behavior

NDT covariance estimation will not start unless the arguments are changed. In other words, it does not affect the default Autoware behavior.

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.

  • The PR follows the pull request guidelines.
  • The PR has been properly tested.
  • The PR has been reviewed by the code owners.

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.
  • The PR is ready for merge.

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

SaltUhey and others added 7 commits June 6, 2024 15:15
Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>
Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>
Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>
Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>
Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>
@github-actions github-actions bot added type:documentation Creating or refining documentation. (auto-assigned) component:localization Vehicle's position determination in its environment. (auto-assigned) labels Jun 20, 2024
@SaltUhey SaltUhey closed this Jun 20, 2024
@SaltUhey SaltUhey reopened this Jun 20, 2024
@SaltUhey SaltUhey marked this pull request as draft June 20, 2024 05:25
@SaltUhey SaltUhey marked this pull request as ready for review June 21, 2024 08:06
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.

Thank you very much 👍
I have added some comments so please check them.

Copy link
Contributor

@TaikiYamada4 TaikiYamada4 left a comment

Choose a reason for hiding this comment

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

Thank you for your PR!

Could you also change the config/ndt_scan_matcher.param.yaml as same as the one in autoware.launch?
autowarefoundation/autoware_launch#1038

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>
SaltUhey and others added 2 commits June 28, 2024 12:07
@SakodaShintaro
Copy link
Contributor

Could you also update config/ndt_scan_matcher.param.yaml?
https://github.com/autowarefoundation/autoware.universe/blob/1635cbeb4fbcf931a563a3006a45282330019493/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml

SaltUhey and others added 9 commits July 4, 2024 14:39
…om the structure in hyper_parameters.hpp

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>
…from initial_pose_offset_model

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>
Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>
Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>
…e return value of estimate_covariance()

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>
Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>
…te_covariance()

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>
SaltUhey and others added 8 commits July 4, 2024 17:54
Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>
Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>
Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>
… hyper_parameters

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>
Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>
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.

Thank you for all the fixes.
I have confirmed that logging_simulator and AWSIM work properly.
Looks Good To Me

@SakodaShintaro SakodaShintaro added the run:build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) label Jul 5, 2024
Copy link

codecov bot commented Jul 5, 2024

Codecov Report

Attention: Patch coverage is 17.94872% with 32 lines in your changes missing coverage. Please review.

Project coverage is 28.67%. Comparing base (ad38061) to head (4991885).
Report is 12 commits behind head on main.

Files Patch % Lines
...ion/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp 0.00% 28 Missing ⚠️
...cher/include/ndt_scan_matcher/hyper_parameters.hpp 63.63% 3 Missing and 1 partial ⚠️
Additional details and impacted files
@@           Coverage Diff           @@
##             main    #7596   +/-   ##
=======================================
  Coverage   28.66%   28.67%           
=======================================
  Files        1588     1588           
  Lines      116382   116392   +10     
  Branches    49700    49701    +1     
=======================================
+ Hits        33364    33378   +14     
+ Misses      73889    73883    -6     
- Partials     9129     9131    +2     
Flag Coverage Δ *Carryforward flag
differential 23.40% <17.94%> (?)
total 28.67% <ø> (+<0.01%) ⬆️ Carriedforward from ad38061

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

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

@SakodaShintaro SakodaShintaro merged commit 71fc4c3 into autowarefoundation:main Jul 5, 2024
38 of 39 checks passed
palas21 pushed a commit to palas21/autoware.universe that referenced this pull request Jul 12, 2024
…in real time instead of using a predefined value (autowarefoundation#7596)

* enable to calculate 2D covariance in real time

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* enable to display offset_pose

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* enable to publish estimated ndt_dev

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* Addition of explanation of 2D real-time covariance estimation

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* fix name of picture

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* style(pre-commit): autofix

* fix covariance_covariance_estimation.json

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* fix covariance_covariance_estimation.json

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* style(pre-commit): autofix

* Commonality using estimated_cov_2d and removal of unnecessary publishers

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* Add a specific explanation for 2D real-time covariance estimation

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* style(pre-commit): autofix

* Remove initial_pose_offset_model_x and initial_pose_offset_model_y from the structure in hyper_parameters.hpp

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* Generate initial_pose_offset_model_x and initial_pose_offset_model_y from initial_pose_offset_model

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* Addition of covariance_estimation_type

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* style(pre-commit): autofix

* Add temperature to parameters

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* Remove covariance_estimation.enable from the parameters and change the return value of estimate_covariance()

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* style(pre-commit): autofix

* Add a temperature description

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* Add processing when the inverse matrix cannot be calculated in estimate_covariance()

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* set default value and range of temperature

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* Simplify the code

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* style(pre-commit): autofix

* Fix explanation of covariance_estimation_type

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* Define initial_pose_offset_model_x and initial_pose_offset_model_y in hyper_parameters

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* style(pre-commit): autofix

* Add explanation of 2D real-time covariance estimation

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

---------

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Signed-off-by: palas21 <palas21@itu.edu.tr>
tby-udel pushed a commit to tby-udel/autoware.universe that referenced this pull request Jul 14, 2024
…in real time instead of using a predefined value (autowarefoundation#7596)

* enable to calculate 2D covariance in real time

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* enable to display offset_pose

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* enable to publish estimated ndt_dev

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* Addition of explanation of 2D real-time covariance estimation

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* fix name of picture

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* style(pre-commit): autofix

* fix covariance_covariance_estimation.json

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* fix covariance_covariance_estimation.json

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* style(pre-commit): autofix

* Commonality using estimated_cov_2d and removal of unnecessary publishers

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* Add a specific explanation for 2D real-time covariance estimation

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* style(pre-commit): autofix

* Remove initial_pose_offset_model_x and initial_pose_offset_model_y from the structure in hyper_parameters.hpp

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* Generate initial_pose_offset_model_x and initial_pose_offset_model_y from initial_pose_offset_model

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* Addition of covariance_estimation_type

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* style(pre-commit): autofix

* Add temperature to parameters

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* Remove covariance_estimation.enable from the parameters and change the return value of estimate_covariance()

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* style(pre-commit): autofix

* Add a temperature description

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* Add processing when the inverse matrix cannot be calculated in estimate_covariance()

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* set default value and range of temperature

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* Simplify the code

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* style(pre-commit): autofix

* Fix explanation of covariance_estimation_type

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* Define initial_pose_offset_model_x and initial_pose_offset_model_y in hyper_parameters

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* style(pre-commit): autofix

* Add explanation of 2D real-time covariance estimation

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

---------

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
KhalilSelyan pushed a commit that referenced this pull request Jul 22, 2024
…in real time instead of using a predefined value (#7596)

* enable to calculate 2D covariance in real time

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* enable to display offset_pose

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* enable to publish estimated ndt_dev

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* Addition of explanation of 2D real-time covariance estimation

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* fix name of picture

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* style(pre-commit): autofix

* fix covariance_covariance_estimation.json

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* fix covariance_covariance_estimation.json

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* style(pre-commit): autofix

* Commonality using estimated_cov_2d and removal of unnecessary publishers

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* Add a specific explanation for 2D real-time covariance estimation

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* style(pre-commit): autofix

* Remove initial_pose_offset_model_x and initial_pose_offset_model_y from the structure in hyper_parameters.hpp

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* Generate initial_pose_offset_model_x and initial_pose_offset_model_y from initial_pose_offset_model

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* Addition of covariance_estimation_type

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* style(pre-commit): autofix

* Add temperature to parameters

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* Remove covariance_estimation.enable from the parameters and change the return value of estimate_covariance()

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* style(pre-commit): autofix

* Add a temperature description

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* Add processing when the inverse matrix cannot be calculated in estimate_covariance()

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* set default value and range of temperature

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* Simplify the code

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* style(pre-commit): autofix

* Fix explanation of covariance_estimation_type

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* Define initial_pose_offset_model_x and initial_pose_offset_model_y in hyper_parameters

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* style(pre-commit): autofix

* Add explanation of 2D real-time covariance estimation

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

---------

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Ariiees pushed a commit to Ariiees/autoware.universe that referenced this pull request Jul 22, 2024
…in real time instead of using a predefined value (autowarefoundation#7596)

* enable to calculate 2D covariance in real time

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* enable to display offset_pose

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* enable to publish estimated ndt_dev

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* Addition of explanation of 2D real-time covariance estimation

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* fix name of picture

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* style(pre-commit): autofix

* fix covariance_covariance_estimation.json

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* fix covariance_covariance_estimation.json

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* style(pre-commit): autofix

* Commonality using estimated_cov_2d and removal of unnecessary publishers

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* Add a specific explanation for 2D real-time covariance estimation

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* style(pre-commit): autofix

* Remove initial_pose_offset_model_x and initial_pose_offset_model_y from the structure in hyper_parameters.hpp

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* Generate initial_pose_offset_model_x and initial_pose_offset_model_y from initial_pose_offset_model

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* Addition of covariance_estimation_type

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* style(pre-commit): autofix

* Add temperature to parameters

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* Remove covariance_estimation.enable from the parameters and change the return value of estimate_covariance()

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* style(pre-commit): autofix

* Add a temperature description

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* Add processing when the inverse matrix cannot be calculated in estimate_covariance()

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* set default value and range of temperature

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* Simplify the code

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* style(pre-commit): autofix

* Fix explanation of covariance_estimation_type

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* Define initial_pose_offset_model_x and initial_pose_offset_model_y in hyper_parameters

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

* style(pre-commit): autofix

* Add explanation of 2D real-time covariance estimation

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>

---------

Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
component:localization Vehicle's position determination in its environment. (auto-assigned) run:build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) type:documentation Creating or refining documentation. (auto-assigned)
Projects
None yet
Development

Successfully merging this pull request may close these issues.

4 participants