feat(ndt_scan_matcher): add grid_search initial pose estimation

[SakodaShintaro]

Must be merged with autowarefoundation/autoware_launch#952

Description

Context

The current ndt_scan_matcher performs pure random search or TPE(Tree-structured Parzen Estimator) search in initial pose estimation. However, both methods suffer from randomness, which introduces uncertainty in the initial pose when tested using the logging_simulator.

This pull request implements grid search to reduce uncertainty.

visualize initial poses

grid_search_initial_pose_vidualize_initial_poses.webm

visualize result poses

grid_search_initial_pose_estimation.webm

Tests performed

• AWSIM (I tried initial pose estimation from Rviz input about 100 times.)
• logging_simulator (sample-rosbag)
• logging_simulator (AWSIM rosbag)
• logging_simulator (internal 11 rosbags)

Effects on system behavior

The time required for initial pose estimation will increase, but the randomness will decrease.

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.

[yukkysaito]

Good 👍

[meliketanrikulu]

@SakodaShintaro Thanks for your work and explanations. I would like to ask a few questions about your work.

1. How does this change affect the initialization duration? Does it make the initialization time better or how does it affect it?
2. As far as I know, in the currently used method, random particles are generated around a Gaussian distribution by evaluating stddev values for pose initialization. What effect do stddev values have in your application? How do you evaluate the stddev values of the first pose here? Do you use this to determine the dimensions of the grids or how do you set the grid dimensions?

[SakodaShintaro]

@meliketanrikulu
Thank you for your comment.

First, I apologize for not being descriptive about the status of this pull request.
This pull request is under evaluation, and we've discovered instances where initial pose estimation failed.
I don't think it is ready to merge with the current settings, so I will return the pull request to draft. I'm currently trying a different method.

How does this change affect the initialization duration?

There is no change in the NDT usage, so the initial pose estimation time will continue to be proportional to the number of particles. With this setting, we search 3 x 3 in the x, y direction and 80 in the yaw direction, so the total is 3 * 3 * 80 = 720 particles.
Before the change, the default setting is 200 particles with normal distribution, so compared to that, it is slightly less than 4 times. However, in practical situations, the 200 particle number is often modified, and we have learned from internal interviews that around 500 particles is usual.

What effect do stddev values have in your application?

In this pull request change, stddev is completely ignored and the search range is determined by the grid search settings.
It is possible to set it to a constant multiple of stddev (covering 1σ, 2σ, 3σ, ...), but I thought it would be easier to use if the search range could be explicitly determined.

[meliketanrikulu]

In this pull request change, stddev is completely ignored and the search range is determined by the grid search settings.
It is possible to set it to a constant multiple of stddev (covering 1σ, 2σ, 3σ, ...), but I thought it would be easier to use if the search range could be explicitly determined.

Thanks for the quick reply and explanations.
I think the initialization time is already too long. Requiring so many particles will extend this time considerably. Instead, similar to the classic Monte Carlo localization approach, you can initially experiment with 100 particles and create new particles around those with the highest score. I think this way the initialization accuracy will increase.

In addition, stddev is useful information for incoming poses and I think it should be used but I don't know how exactly it can be integrated into this method.

[SakodaShintaro]

@meliketanrikulu

Reply

I think the initialization time is already too long.

I completely agree. 👍

However, I think that the reason why initial position estimation fails is not that it cannot be optimized sufficiently, but rather that the correct pose cannot be covered with about 200 particles.

So, I'm not confident that the method you're pointing out will work.

you can initially experiment with 100 particles and create new particles around those with the highest score. I think this way the initialization accuracy will increase.

What I'm currently trying

I'm currently trying out the method in the following paper.
https://github.com/KOKIAOKI/3d_bbs

For quick trial and error, I'm implementing it in the ndt_omp repository.

tier4/ndt_omp@tier4/main...tier4:ndt_omp:feat/add_initialpose_estimation

If the experiments in the ndt_omp repository go well, we will introduce the technique to autoware.universe as well.

(We plan to use GNSS stddev to determine the search range in this method.)

About GNSS

Also, the current pose_initializer does not use GNSS stddev, but uses the value set in config, so I think that will also need to be modified.

• autoware.universe/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp

  Line 177 in b630838

  pose.pose.covariance = gnss_particle_covariance_;

• autoware.universe/localization/pose_initializer/config/pose_initializer.param.yaml

  Lines 16 to 24 in b630838

  	gnss_particle_covariance:
  	[
  	1.0, 0.0, 0.0, 0.0, 0.0, 0.0,
  	0.0, 1.0, 0.0, 0.0, 0.0, 0.0,
  	0.0, 0.0, 0.01, 0.0, 0.0, 0.0,
  	0.0, 0.0, 0.0, 0.01, 0.0, 0.0,
  	0.0, 0.0, 0.0, 0.0, 0.01, 0.0,
  	0.0, 0.0, 0.0, 0.0, 0.0, 10.0,
  	]

[SakodaShintaro]

We have introduced some improvements to initial pose estimation in another pull request, so we are closing this pull request for now since it now takes a lot of time to estimate the initial pose.