Pose initialization orientation sometimes (randomly) comes incorrect

[meliketanrikulu]

Checklist

• [X] I've read the contribution guidelines.
• [X] I've searched other issues and no duplicate issues were found.
• [X] I'm convinced that this is not my fault but a bug.

Description

After the first position comes from Gnss, it is localized in the correct position, but its rotation is wrong. We encounter this error randomly, not on every run. The same issue occurs in both cases where the use_autoware_orientation parameter is true and false.

Expected behavior

After the first position comes from gnss, it is expected to initialize with the correct orientation by finding the most correct orientation using the monte carlo method.

Actual behavior

After the first position comes from Gnss, it is localized in the correct position, but its rotation is wrong.

Steps to reproduce

1. Bag File and Map are here
2. Individual params package is here
3. sensor_kit_launch is here

Versions

No response

Possible causes

To debug, here I found that when I print the scores for each particle, all or nearly all of the scores are calculated as 0.0 if they are not initialized.

Additional context

No response

[VRichardJP]

To debug, here I found that when I print the scores for each particle, all or nearly all of the scores are calculated as 0.0 if they are not initialized.

So autoware choses the "best" localization, but all of them are bad.

How far is the GNSS position from the actual position? x, y, yaw? What about z? I had an issue once where the vehicle was initially placed deep underground and ndt would then have nothing to match (https://github.com/autowarefoundation/autoware.universe/issues/2048).

[YamatoAndo]

I apologize, but the current implementation of the initial position estimation cannot guarantee its success, so it may fail randomly.

The fact that the score is 0.0 indicates that the map point cloud and LiDAR point cloud did not match at all. As VRichardJP mentioned, there may be an issue with the estimation of the z-coordinate.

By the way, which source code is the parameter use_autoware_orientation referring to?

[meliketanrikulu]

I apologize, but the current implementation of the initial position estimation cannot guarantee its success, so it may fail randomly.

The fact that the score is 0.0 indicates that the map point cloud and LiDAR point cloud did not match at all. As VRichardJP mentioned, there may be an issue with the estimation of the z-coordinate.

By the way, which source code is the parameter use_autoware_orientation referring to?

Hello thank you for your reply @YamatoAndo @VRichardJP . I actually added skip for score calculation is 0.0 status. If the number of attempts is 100, when more than 50 comes to 0.0, I return the initialization success status to false and it continues to try again. that way it usually initialized correctly on the second try. what do you think about adding it autoware.

[VRichardJP]

I'm not a big fan of endless initialization loop. Usually if localization fails after 50~100 tries, then most likely something is wrong (bad initial guess, config, map, etc). If the problem is really the pose initializer logic, it might be worth spending some time to make it more robust/reliable.

[meliketanrikulu]

I'm not a big fan of endless initialization loop. Usually if localization fails after 50~100 tries, then most likely something is wrong (bad initial guess, config, map, etc). If the problem is really the pose initializer logic, it might be worth spending some time to make it more robust/reliable.

The first position correct that comes from gnss . There is some error with height it gets for the pose initialization but I don't think that's the problem as it was able to find it on the second try. I will also share the sensor kit with you so that you can reproduce it. I would be very happy if you could check it out. I couldn't find the exact error source. maybe there are problems with timing too

[meliketanrikulu]

I'm not a big fan of endless initialization loop. Usually if localization fails after 50~100 tries, then most likely something is wrong (bad initial guess, config, map, etc). If the problem is really the pose initializer logic, it might be worth spending some time to make it more robust/reliable.

The first position correct that comes from gnss . There is some error with height it gets for the pose initialization but I don't think that's the problem as it was able to find it on the second try. I will also share the sensor kit with you so that you can reproduce it. I would be very happy if you could check it out. I couldn't find the exact error source. maybe there are problems with timing too

I added sensor_kit_launch here, individual_params is here. I also shared my map and bag files on "Steps to reproduce" part. For initializing while testing, you can print and view the scores (ndt score in the monte carlo localization part) on each attempt. I print after this line

[meliketanrikulu]

Maybe we can add re-initialization button on rviz. When you want to try again, try to initialize again using the gnss pose. @YamatoAndo @VRichardJP

[meliketanrikulu]

I created discussion here for rviz re-initialization