Positioning error in wrong zone when NDT starts

[meliketanrikulu]

Checklist

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

Description

We encountered a random error while NDT was running. After the initial pose estimation stages (monte carlo lozalization) were performed correctly, we observed that when starting NDT, its position was set somewhere between the origin point of the map and the correct location of the vehicle, or it was set directly to the origin point of the map.
While receiving these errors, we see that our gnss errors are not high.

Expected behavior

We expect NDT to search in the right area stably.

Actual behavior

We observed that when starting NDT, its position was set somewhere between the origin point of the map and the correct location of the vehicle, or it was set directly to the origin point of the map.
video 1
video 2

Steps to reproduce

bags
maps
We are working with UTM. You have to select lanelet2_map_projector_type parameter to UTM in tier4_map_loader packages launch file and you could set coordinate_system parameter as 4: LocalCartesianUTM in gnss_poser package

Versions

No response

Possible causes

Before starting NDT, particle filter is used when estimating initial pose, and we visualized best_particle in rviz to make sure there is no problem at this stage. We have seen that this stage works correctly and that the vehicle is in its position. After making sure that this stage works correctly, the NDT algorithm is constantly subscribing to a pose published from ekf, and an algorithm is running to use the most appropriate pose from the poses from ekf. When ekf node starts, it generates a pose at 0 0 0 at the central point of the map.When the NDT algorithm started, we think that there may have been a problem in the algorithm trying to choose the right one among the pose values it received from ekf. For this reason, it tries to produce poses by referencing old poses (map center) from ekf or one of the values in between.

Additional context

No response

[YamatoAndo]

Thank you for your report.
You are right, it seems to refer to the NDT node for a pose at 0 0 0.

To fix this problem, I think that the EKF node should not publish the pose until it receives the initialization pose.
I think that way, the NDT node would not receive a pose at 0 0 0.

[meliketanrikulu]

NDT node graph:

[YamatoAndo]

@meliketanrikulu
I would like to put a flag in ekf_localizer not to estimate the position until the initial position is received.
Any opinions on that?

※ Note that this is a temporary fix and the same problem may occur when re-initializing.

[meliketanrikulu]

@meliketanrikulu
I would like to put a flag in ekf_localizer not to estimate the position until the initial position is received.
Any opinions on that?

※ Note that this is a temporary fix and the same problem may occur when re-initializing.

Hello @YamatoAndo . We solved this problem temporarily by publishing the initial pose for a second.Maybe it is not necessary to keep this period for so long.
Both of these methods seem to offer temporary fix. We can talk about this again according to the method to be used for reinitialization. If a method such as slowing down or stopping the vehicle is used, we can send the initial pose for a while.

[YamatoAndo]

@meliketanrikulu

We can talk about this again according to the method to be used for reinitialization.

Sure!

[meliketanrikulu]

@meliketanrikulu I would like to put a flag in ekf_localizer not to estimate the position until the initial position is received. Any opinions on that?

※ Note that this is a temporary fix and the same problem may occur when re-initializing.

Is there related PR @YamatoAndo

[xmfcx]

Fixes:

• Publish the initial pose multiple times.
  • Can cause unwanted consequences.
• Ekf doesn't publish pose until initialized.

[mitsudome-r]

I think new API for initialization is being discussed in this PR #1431.

[IshitaTakeshi]

By the way now I'm refactoring the EKF code and trying to find and resolve these problems but it takes a while

[IshitaTakeshi]

I think that the easiest way to resolve the problem of publishing the identity pose is

1. Create a flag that manages the initialization state
2. Make the flag true when receiving the initial pose
3. Update the pose using the messages from NDT and publish the estimated pose only if the flag is true

[IshitaTakeshi]

I opened a draft PR to realize the above

[mitsudome-r]

@meliketanrikulu Could you confirm that the issue is fixed with the PR ?