-
Notifications
You must be signed in to change notification settings - Fork 696
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
feat(ndt_scan_matcher): estimate the covariance of ndt scan matching in real time instead of using a predefined value #7596
Conversation
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>
There was a problem hiding this 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.
localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp
Outdated
Show resolved
Hide resolved
localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp
Outdated
Show resolved
Hide resolved
localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json
Outdated
Show resolved
Hide resolved
There was a problem hiding this 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
localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json
Outdated
Show resolved
Hide resolved
localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp
Outdated
Show resolved
Hide resolved
Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>
Signed-off-by: yuhei <yuhei1030.mini.normal@gmail.com>
Could you also update config/ndt_scan_matcher.param.yaml? |
…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>
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>
There was a problem hiding this 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
Codecov ReportAttention: Patch coverage is
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
*This pull request uses carry forward flags. Click here to find out more. ☔ View full report in Codecov by Sentry. |
71fc4c3
into
autowarefoundation:main
…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>
…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>
…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>
…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>
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.
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.

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.
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.