How to limit teleportation distance in localization mode?

[Divelix]

Hi, Matheu,

I have one L515 in front of robot as main source of data for RTAB-Map and when in stands perpendicular to a wall, it may teleport robot a few meters away to another similar wall in different room. Is there any built-in mechanism to limit localization distance? I've found Robust Graph Optimization and now plan to try it out when I'll have chance to install GTSAM, but for now, I thought maybe there some parameter I'm missing inside rtabmap that would limit localization distance?

From practice, I can say that localization on my map happens once in 1-5 secs and, considering relatively low robot movement speed, it won't move away more than 1-2 meters away between localization events.

Looking forward for you comments on this, thanks in advance

[matlabbe]

Hi,

it depends if it the jump is coming from relocalization or from odometry drift. Like you described, it seems an odometry drift. Which kind of odometry is used? Is it icp_odometry?

cheers,
Mathieu

[Divelix]

Yes, I use wheel odometry (perfect in simulation, as I understand) as guess for icp_odometry node. I doubt that teleportation is caused by icp_odometry (can icp_odometry teleport robot several meters away?). It seems more like rtabmap finds a scene similar to the one seen by the camera in the database and teleports the robot there.

[matlabbe]

If /map -> /odom jumps, this is localization. If /odom_icp -> /odom jumps, it is icp_odometry. If /odom -> /base_link jumps, it is wheel odometry.

icp_odometry can drift a lot if there are not a lot of geometry constraints (e..g, lidar seeing only one line of points).

[Divelix]

/map -> /odom_icp jumps in my case.

Btw, I use depth from L515 as input for icp_odometry, so I don't think ICP can shift point cloud of a wall to another wall few meters away while having quite accurate guess from previous frame.

[matlabbe]

In this case, can you provide a link to download that database?

[Divelix]

Sure, there it is.

For instance, if I move close to one of these similar places (red arrows), rtabmap will accidentally localize to another one. Scenes in both places contain practically identical walls of low res wood texture. I would like to restrict rtabmap from localize more than 1-2 meters away from /icp_odom to prevent such teleportation behavior.

[stevemartinov]

Can we please get an answer to this question? There are tons of people wondering about this feature

[matlabbe]

Hi,

It seems the exact same texture can be seen on the wall at the different locations:




It is indeed the same problem than in this post: https://github.com/introlab/rtabmap/wiki/Robust-Graph-Optimization

Those graph checks are done by default in mapping mode, but for localization mode it was not handled before this year (actually I did an update on December 2021 to fix that problem). You can increase parameter "RGBD/MaxOdomCacheSize" (default 10). Here how it works:

On localization, rtabmap will use the past X odometry poses to check if the localization makes sense based on the previous localizations and the actual odometry computed. If no localizations happened since the last X updates, rtabmap will likely accept the teleportation (re-localization). However, if there was a previous localization inside the last X updates, the odometry path (up to X last poses) will be optimized using the past re-localization constraints. Based on "RGBD/OptimizeMaxError", if the odometry path is too much deformed (bad re-localization), localization will be rejected (which would be your case).

With RGBD/MaxOdomCacheSize set to 10 and Rtabmap/DetectionRate set to 1, it means it will only check the past 10 seconds. You could increase RGBD/MaxOdomCacheSize to 100/200 to reject a new re-localization if it doesn't deform too much the odometry constraints of the past 100/200 seconds.

cheers,
Mathieu