Handling robot being lost during localization mode

[GiladB]

Hi Mathieu,

I'm encountering an issue with my robot working in localization mode using RTAB-Map, and I was hoping you could provide some insights.
We are running without visual odometry. Initially, the robot was able to localize itself effectively and perform loop closures every once in a while. However, it seems that at a certain point, it became disoriented and began rejecting all potential loop closures.

Log snippet:

[rtabmap-6] [34m[REPORT] (2024-03-20 04:40:17.357) Rtabmap.cpp:3078::process() loopClosureVisualInliers: 264, loopClosureVisualInliersRatio: 0.144737, loopClosureVisualMatches: 227
[rtabmap-6] [34m[REPORT] (2024-03-20 04:40:17.357) Rtabmap.cpp:3333::process() maxLinearErrorRatio: 1.260813
[rtabmap-6] [33m[ WARN] (2024-03-20 04:40:17.357) Rtabmap.cpp:3352::process() Rejecting localization (3929 <-> 14) in this iteration because a wrong loop closure has been detected after graph optimization, resulting in a maximum graph error ratio of 1.260813 (edge 3929->14, type=1, abs error=0.137437 m, stddev=0.109007). The maximum error ratio parameter "RGBD/OptimizeMaxError" is 0.500000 of std deviation.
[rtabmap-6] [33m[ WARN] (2024-03-20 04:40:17.357) Rtabmap.cpp:3394::process() Rejecting localization (3929 <-> 14) in this iteration because a wrong loop closure has been detected after graph optimization, resulting in a maximum graph error ratio of 1.316827 (edge 3896->3898, type=0, abs error=7.544861 deg, stddev=0.100000). The maximum error ratio parameter "RGBD/OptimizeMaxError" is 0.500000 of std deviation.
[rtabmap-6] [33m[ WARN] (2024-03-20 04:40:17.358) Rtabmap.cpp:3442::process() Global and loop closures seem not tallying together, try again to optimize without local loop closures...
[rtabmap-6] [34m[REPORT] (2024-03-20 04:40:17.358) Rtabmap.cpp:3476::process() maxLinearErrorRatio: 1.215640
[rtabmap-6] [33m[ WARN] (2024-03-20 04:40:17.358) Rtabmap.cpp:3495::process() Rejecting localization (3929 <-> 14) in this iteration because a wrong loop closure has been detected after graph optimization, resulting in a maximum graph error ratio of 1.215640 (edge 3929->14, type=1, abs error=0.132513 m, stddev=0.109007). The maximum error ratio parameter "RGBD/OptimizeMaxError" is 0.500000 of std deviation.
[rtabmap-6] [33m[ WARN] (2024-03-20 04:40:17.358) Rtabmap.cpp:3537::process() Rejecting localization (3929 <-> 14) in this iteration because a wrong loop closure has been detected after graph optimization, resulting in a maximum graph error ratio of 1.298543 (edge 3896->3898, type=0, abs error=7.440101 deg, stddev=0.100000). The maximum error ratio parameter "RGBD/OptimizeMaxError" is 0.500000 of std deviation.
[rtabmap-6] [34m[REPORT] (2024-03-20 04:40:17.358) Rtabmap.cpp:3766::process() Reject localization

I have a few questions regarding this issue:

1) Is there an indication or specific signal that RTAB-Map provides when the robot becomes lost, aside from the prolonged failure of loop closures?

2) In case of the robot getting lost, is there a method to reset the local map by dropping all nodes and returning to the initial localization state?

3) For our specific use-case, we've set RGBD/OptimizeMaxError to 0.5, which typically yields satisfactory results. Would it make sense to you, to add a feature that will dynamically adjust or relax this constraint when the robot consistently rejects localization over an extended period or distance?


Thank you for your time and assistance.

Best regards,
Gilad

[matlabbe]

Hi,

RGBD/OptimizeMaxError is quite low, I use between 2 and 4 generally. I would set 1 if I know that odometry has very very low drift.

1) In localization mode, there will be a /rtabmap/localization_pose topic published. It has covariance that can be used on your side to know if the robot is "lost" or not. Similarly, we can set parameter "loc_thr" (in meters) to rtabmap node to publish in ros diagnostics if rtabmap is localized or not (if localization error is under loc_thr, ros diagnostic will show rtabmap localized, otherwise it will say it is not).

2) You can call /rtabmap/trigger_new_map service, it will flush the current localization cache.

3) If rtabmap is constantly rejecting localization, it means there was a link added at some point with wrong covariance that makes all future graph optimization failing. The localization cache should be cleared, or the bad link will eventually be removed from the cache after RGBD/MaxOdomCacheSize updates (default 10) anyway.

If you want to dynamically adjust RGBD/OptimizeMaxError, you can can the parameter on rosparam, then call /rtabmap/update_parameters service. On ros2, you may just need to update the parameter of the node, there should be a callback called when the value is changed without having to explicitly call /rtabmap/update_parameters.

cheers,
Mathieu