feat(ndt_scan_matcher): estimate NDT covariance in real-time with limited initial positions

[KOKIAOKI]

Description

Calculate 2D covariance (xx, xy, yx, yy) in real time using the convergence of NDT.
I limit the arrangement of multiple initial positions based on the Hessian matrix of the NDT score function.

Related links

original paper

Tests performed

I evaluated it In a tunnel evaluation (already shared in weekly meeting).
The real-time covariance estimation obtained the same convergence tendency as offline convergence evaluation.

Notes for reviewers

Related PRs:

• feat: output Hessian matrix tier4/ndt_omp#22
  Output Hessian matrix
• feat(ndt_scan_matcher): add parameters of real-time covariance estimation autoware_launch#643
  Parameters for this covariance estaimtion

Interface changes

• topic
  /localization/pose_estimator/multi_ndt_pose
  ->NDT convergence positions
  /localization/pose_estimator/multi_initial_pose
  ->NDT initial positions

• yaml setting (ndt_scan_matcher.param.yaml)
  Category: 2D Real-time covariance estimation (output_pose_covariance is the minimum value)
  Default use_covariance_estimation is false.

Effects on system behavior

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].
• 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.
  [contribution guidelines]: https://autowarefoundation.github.io/autoware-documentation/main/contributing/
  [pull request guidelines]: https://autowarefoundation.github.io/autoware-documentation/main/contributing/pull-request-guidelines/

[KYabuuchi]

Thank you for creating the PR. I found a risky code in a brief review. Please check.

[kminoda]

Hi, thank you for the PR 🎉
Left some minor comments so please check 🙏

[kminoda]

Please do not use the default parameter and instead do as follow

Suggested change

	use_cov_estimation_ = this->declare_parameter("use_covariance_estimation", use_cov_estimation_);
	use_cov_estimation_ = this->declare_parameter<bool>("use_covariance_estimation");

[kminoda]

I believe this part is relatively computationally heavy, so I would recommend to move this right after the following part:

  // perform ndt scan matching
  (*state_ptr_)["state"] = "Aligning";
  const Eigen::Matrix4f initial_pose_matrix =
    pose_to_matrix4f(interpolator.get_current_pose().pose.pose);
  auto output_cloud = std::make_shared<pcl::PointCloud<PointSource>>();
  ndt_ptr_->align(*output_cloud, initial_pose_matrix);
  const pclomp::NdtResult ndt_result = ndt_ptr_->getResult();
  (*state_ptr_)["state"] = "Sleeping

This will also enable users to monitor the total execution time with exe_time as well.

[YamatoAndo]

@KOKIAOKI Thank you for your contribution!
Could you write an explanation in the readme?
I'd also like you to include a link of your paper in the readme.

[KOKIAOKI]

Thank you for checking my implementation.
Please let me test again after fixing these fixes.

[KOKIAOKI]

I tested the fixed source code. It worked fine in my environment.

Here are the corrections

• I have corrected all the issues pointed out by reviewers.
  • Moved is_converged processing and processing time calculation.
• Added "const" to all immutable variables.
• Removed unnecessary comments

I will resolve the confliction later

[KYabuuchi]

I added some comments. Please check it out 🙏

[KOKIAOKI]

Sorry for the delay in reporting.
I have reflected all the reviews made by @Motsu-san and @KYabuuchi into the source code.

• Changed the calculation of covariance to a method that uses smaller variables.
• There were many variable names that caused misunderstandings, so I made them easier to understand.
• Changed and added notes and explanations in README

Thank you for further reviewing!

[codecov]

Codecov Report

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

Comparison is base (4e00bf2) 14.84% compared to head (3e31b93) 15.09%.
Report is 2 commits behind head on main.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #5338      +/-   ##
==========================================
+ Coverage   14.84%   15.09%   +0.24%     
==========================================
  Files        1665     1661       -4     
  Lines      116231   115372     -859     
  Branches    36256    35646     -610     
==========================================
+ Hits        17259    17419     +160     
+ Misses      79473    78436    -1037     
- Partials    19499    19517      +18     

Flag	Coverage Δ		*Carryforward flag
differential	0.00% <0.00%> (?)
total	15.10% <ø> (+0.25%)	⬆️	Carriedforward from 5eda6c3

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

Files	Coverage Δ
...ion/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp	0.00% <0.00%> (ø)

... and 64 files with indirect coverage changes

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

[KOKIAOKI]

@YamatoAndo @KYabuuchi
Thank you for further checks.
I created the function of eigenvector calculation and added README empty line.

[KOKIAOKI]

@YamatoAndo @KYabuuchi @SakodaShintaro
Thank you for further code checks.
Main changes are as below:

• @SakodaShintaro Like your advice, I added parameter warning and std::exeption about Eigen::Success.
• @SakodaShintaro Although review about util.cpp was closed, I moved two functions from util.cpp to ndt_scan_matcher.cpp.