How to deal with huge discrete jumps for the tf /map -> /odom in localization

[Shoi.H]

Hi,
While in localization, sometimes huge jumps will occur at the tf /map -> /odom. It is probably because of the bad environment, but even if the environment is improved, it cannot prevent to happen, so I want to know how to deal with it.

I often makes a map that drives in a rectangular trajectory. When running localization and placing the robot where the position oscillates (near map's edge), even if the loop closure's match id is correct, the position oscillates up to 1~2 meters. When relocalizing in the optimized graph, the tf map -> odom should hardly change. The ideal is to change gradually.

Considering the problem, the following approach may be taken.
 a. Use the previous tf /map -> /odom until an accurate loop closure is detected.
 b. Ignore tf /map -> /odom that has some distance apart within short-term.

So, these are the questions:
 1. How do you think of this problem? Does my approach makes sense? I'm looking at the source code around graphOptimizer, but it's difficult...
 2. How do I know how accurate the tf /map -> /odom is?
 3. Can any global navigation ROS package handle this issue? I haven't used the navigation package yet. Driving with PID control is not stable because of this issue.

Thanks.

[matlabbe]

Hi,

Jumps are caused by either localization estimation error or odometry error (even if localization is perfect, if odometry drifts, the robot will jump). If your odometry doesn't drift a lot, the jumps may come from localization. Localization accuracy depends on the sensors used and the rtabmap parameters used. It is possible to increase thresholds of rtabmap parameters to reject localizations with more error (for example by increasing Vis/MinInliers to 100 instead of default 20). Another approach is to filter the localization jumps with an EKF filter. See for example http://official-rtab-map-forum.206.s1.nabble.com/Filtering-rtabmap-localization-jumps-with-robot-localization-in-2D-td5931.html

I don't think doing your PID control including localization is a good idea. The navigation stack splits the control and localization in two separate things. The control is done in odometry frame with only odometry (continuous pose without jumps). The global planning (current goal to go) is done using localization (which will jump). See definition of the standard frames in ROS: https://www.ros.org/reps/rep-0105.html

Another approach is to correct the map instead of the pose on localization, so that map->odom is always identity (no jumps for the pose but the map will jump). To do so, you can set RGBD/OptimizeFromGraphEnd=true for rtabmap node.

cheers,
Mathieu

[Shoi.H]

Hi, Mathieu
Thanks for your reply.

I'll try your suggestions (increasing Vis/MinInliers and using an EKF filter).
I don't want to /map to be changed after creating the map, so I still use RGBD/OptimizeFromGraphEnd=false.
And also thanks for about the navigation stack. I will try using it after localization goes well.

Regards,
Shoi

[Shoi.H]

Hi, Mathieu
Here is what I tried:

Increasing Vis/MinInliers went better. Jumps still occur, but oscillation is less. This will make increasing accuracy at the cost of less localizations, so it comes with a trade-off. For now I set Vis/MinInliers to 50.

It was not effective to use the EKF filter for the above situation. When stopping a robot where coordinates are off from expected, the position errors grow with time (in spiral). The EKF filter will be effective if more accurate localization occurs after jumping, but in an environment where localization does not occur frequently will affect learning the wrong/past state too well and diverge.
I tried the UKF filter as well, but the situation did not change. I think we have to remove outliers from tf /map -> /odom or localization_pose (or just take handle by the global planning).

Regards,
Shoi