Localize from an unknown position

[yschleif]

Hi @matlabbe ,

I'm trying to add an "unknown" starting point option. for a "kidnapped robot" scenario. looking at RGBD/SavedLocalizationIgnored I see the possible starting configurations for RTABMap are

1. Map origin
2. Map end
3. Last robot position

The issue I currently have is that if use 1-3, the robot may start at the assumed location, and then after a few seconds correct the position, and jump to the actual one. The behavior I'm aiming at is: don't return a pose, until correctly localizing

What I currently do as a hack:

1. clear the optimizedPoses queue
2. a good localization decided if nearestPoses > 0

Do you have a suggestion for how to achieve this behavior? Is my hack something to go by?

Thanks!

[matlabbe]

Is this scenario in mapping or localization mode?

In localization mode, /rtabmap/localization_pose will be published with very large covariance >99 until the first localization. Also added pub_loc_pose_only_when_localizing parameter for rtabmap node if you prefer that rtabmap/localization_pose is only published on localization.

You are refering to RGBD/StartAtOrigin parameter: Param: RGBD/StartAtOrigin = "false" [If true, rtabmap will assume the robot is starting from origin of the map. If false, rtabmap will assume the robot is restarting from the last saved localization pose from previous session (the place where it shut down previously). Used only in localization mode (Mem/IncrementalMemory=false).]