How to limit teleportation distance in localization mode?
This post was updated on .
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
Re: How to limit teleportation distance in localization mode?
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.
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.
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.