Best Practices for Quick Automatic Localization

[dschnabel]

What is a good way for an autonomous robot to localize itself, knowing where in the map it is?

Background: Let's say I have a pre-recorded map and my robot starts up rtabmap not knowing where on the map it is located. In most cases it won't be able to localize itself right away. Right now I need to teleoperate the robot and drive around a bit before it will know where on the map it is located.

1. Would it make sense to start with a low Vis/MinInliers (e.g. 10) until localization was successful and then bump it up to something higher (e.g. 50) to get better accuracy? Any other options to play around with to help with quick localization?

2. At what pattern should the robot start driving around to find out where it is? Does a star pattern make sense (move 2m in one direction, then back to starting point and move 1m in a different direction)?

3. How does rtabmap indicate whether or not localization was successful? This would be helpful for the navigation stack to know when it's time for path planning.

[matlabbe]

Hi,

When reloading a database in localization mode (Mem/IncrementalMemory=false):

A) Normally with the default parameters, the robot will think that it is restarting where it has shutdown, proximity detection would work already without having to find a visual loop closure.

B) If we set RGBD/SavedLocalizationIgnored to true, the robot will think it is restarting from the origin of the map (note that older versions, the robot will not be localized at origin with this option, a loop closure is required).

C) If the robot didn't restart at its last position and not from the origin of the map, the robot would localize only with visual loop closures. It is possible to manually give an estimate pose in the map with the "2D Pose Estimate" button in rviz (assuming the button publishes on /rtabmap/initialpose).

For your comments:
1-2. If the robot starts always in front of a visually discriminative area, it would localize on start. They robot could rotate on place slowly or rotate/stop/rotate/stop (to avoid motion blur) on start to see around, assuming that the robot is starting somewhere inside the map. Other approach is to use an apriltag for localization. Apriltags can be mapped and saved in the map, then on localization, the robot will instantaneously localize on.

3. If you look at the /rtabmap/info topic, loopClosureId will be not null when the robot just localized.

As a side note, if you restart in SLAM mode from a previous database and don't want to start a new map before it has been localized on the previous one, we can set Rtabmap/StartNewMapOnLoopClosure to true.

[dschnabel]

The hint with RGBD/SavedLocalizationIgnored and Rtabmap/StartNewMapOnLoopClosure were very helpful.

I agree it makes most sense for the robot to do a rotate/stop/rotate/stop in order to avoid motion blur. This is probably the best I can do now.

Apriltags sound interesting but I can't find much information on the usage of apriltags in rtabmap. Does OpenCV need to be built with apriltags support? I'm using rtabmap in command line mode only so I can't configure apriltags using the UI. Wondering if you can configure apriltags from command line?

Good to know that /rtabmap/info topic is the topic to look out for.

[matlabbe]

Hi,

To use apriltag inside rtabmap, you should build rtabmap with OpenCV >= 3.4.2, then set parameters Marker/Dictionary to 19 (type 36h10) or 20 (type 36h11), Marker/Length and RGBD/MarkerDetection to true. It assumes that all tags have the same size!

It is also possible to use apriltag ros package. This approach will let you customize the patterns and other more specific apriltag parameters. An example in this launch file: https://github.com/introlab/rtabmap_ros/blob/master/launch/tests/test_apriltag_ros.launch

cheers,
Mathieu

[dschnabel]

Thanks matlabbe! I think I got Apriltag detection working inside rtabmap as I'm seeing this log message when running in SLAM mode (usually it's a debug message but I changed it to be a warning):

[ WARN] (2020-09-13 15:06:39.272) MarkerDetector.cpp:202::detect() Marker 16 detected at xyz=0.511764,0.234447,0.014855 rpy=1.511407,-0.029899,-1.682522 (xyz=-0.214911,-0.011936,0.514296 rpy=3.075250,0.121314,-0.035816)

However when I run in Localization mode, a detected marker doesn't seem to do anything. I expect a loop closure upon marker detection but that's not the case:

[ INFO] [1600034878.456732073]: rtabmap (139): Rate=0.50s, Limit=0.000s, RTAB-Map=0.4311s, Maps update=0.0026s pub=0.0000s (local map=106, WM=106)
[ WARN] (2020-09-13 15:07:59.255) MarkerDetector.cpp:202::detect() Marker 16 detected at xyz=0.481575,-0.176148,0.018942 rpy=1.523897,-0.067092,-2.394404 (xyz=0.195343,-0.018307,0.480121 rpy=3.016185,0.830545,-0.108470)
[ WARN] (2020-09-13 15:07:59.433) Rtabmap.cpp:2562::process() Rejected loop closure 42 -> 140: Not enough inliers 0/70 (matches=10) between 42 and 140
[ INFO] [1600034879.496385973]: rtabmap (140): Rate=0.50s, Limit=0.000s, RTAB-Map=0.4409s, Maps update=0.0038s pub=0.0000s (local map=106, WM=106)
[ WARN] (2020-09-13 15:08:00.195) MarkerDetector.cpp:202::detect() Marker 16 detected at xyz=0.482238,-0.176602,0.018946 rpy=1.543665,-0.066101,-2.407760 (xyz=0.195803,-0.018312,0.480780 rpy=3.034993,0.843908,-0.108564)
[ WARN] (2020-09-13 15:08:00.356) Rtabmap.cpp:2562::process() Rejected loop closure 42 -> 141: Not enough inliers 0/70 (matches=8) between 42 and 141
[ INFO] [1600034880.413426270]: rtabmap (141): Rate=0.50s, Limit=0.000s, RTAB-Map=0.3899s, Maps update=0.0035s pub=0.0000s (local map=106, WM=106)

What am I missing? Here is the database that was built with Apriltags. Is there a way to visualize the Apriltags in rtabmap-databaseViewer? This would be helpful for checking if the tags were properly saved to the database.

[dschnabel]

It appears that the robot does localize instantaneously on apriltags but in this case I cannot rely on "loopClosureId" to tell me that the robot just localized. This is why I was confused initially and thought apriltags didn't work.

So with my current understanding there are four possible ways in which rtabmap can localize:
1. proximity detection
2. visual loop closure (loopClosureId != 0)
3. manual pose estimation
4. apriltag detection

Is this correct?

Is there any indication in the /rtabmap/info topic (or any other way) when the robot localized via apriltag detection?

[matlabbe]

Hi,

For your previous message, it is possible to see the landmarks in Graph View and in Constraints view of DatabaseViewer. Here two screenshots:



Landmarks are at the end of the green links. You can also see the number of landmarks in the map at the bottom of the view, here 33 landmark links were added to the map. You may not see the landmarks on your side, it is because your Optimizer/Strategy is 0 (TORO). Landmarks are added to graph only g2o or gtsam optimizers. Build rtabmap with g2o or gtsam support, then set Optimizer/Strategy to 1 or 2. Recently, I added new warnings for the case when landmarks are in the map but the optimizer is TORO.

This would be also the reason why localization is not working, as landmarks are not linked in he graph.

It is also possible to see the landmarks in 3D Map view of rtabmapviz when running:


They may be hard sometimes to see in 3D Map view, I generally prefer the Graph View with the green links.

cheers,
Mathieu

[matlabbe]

Update: I added landmarkId field to /rtabmap/info message for convenience: https://github.com/introlab/rtabmap_ros/commit/58dd3134be6f2ccc81f998df4e258354f0071e44

We can now have the 3 cases for localization:

int32 loopClosureId
int32 proximityDetectionId
int32 landmarkId

For external localization like with the PoseEstimate, you would have to detect when that topic is posted.

[dschnabel]

Yes, after building rtabmap with g2o support and using Optimizer/Strategy = 1 the apriltags show up in Graph View and also under the topic /rtabmap/landmarks. Great that 'landmarkId' has been added to Info.msg. Thanks!

Interestingly, apriltags localization was already working with TORO, I just couldn't see them in database viewer. But on detection my robot was jumping to the rough location on the map even without a loop closure or proximity detection.

And I noticed that localization based on landmark detection is not very precise. In my case, driving toward an apriltag causes the robot to jump around drastically compared to when MarkerDetection is turned off. This might be because my tags are fairly small (5.5x5.5 cm) and I assume localization gets better the bigger the tags are, correct?

In my case apriltags are useful for rough location estimates, e.g. when robot starts in an unknown location. But after the initial estimate I'd rather rely on odometry. As a workaround I modified the code in rtabmap.cpp and memory.cpp to disable marker detection after the first apriltag has been detected or once we have a loop closure. This gives me smoother results.

By the way, I noticed that when I run my database through rtabmap-recovery, then all my landmarks are gone. Is that a known issue?

[matlabbe]

The farther you are from the tag or the smaller it is, the larger the orientation error will be. For example, if the tag is detected 10 meters in from of the robot, an error of 3 deg will make the robot jump 50 cm.

As I remember, I've already thought about adding Marker/MaxRange parameter to ignore detection over a fixed range (to limit the jumps).

To disable loop closure detection and landmark detection dynamically, this can be done through rtabmapviz or by command line:

# disable marker detection
rosparam set /rtabmap/RGBD/MarkerDetection 'false'
# disable loop closure detection
rosparam set /rtabmap/Kp/MaxFeatures -1
rosservice call /rtabmap/update_parameters

Note that I've just found a strange bug when setting parameters from command line, then opening Preferences in rtabmapviz fail. However, rtabmap node should have correctly updated the parameters.

If aprilttag detection inside is used, the database should contain compressed RGB image so that recovery can redetect the tags. If apriltag detection was done outside rtabmap node, the landmark links are indeed lost on recovery (along with the case where images are not saved, we would have to implement the support for landmark links republished in recovery).

cheers,
Mathieu

[dschnabel]

Your explanation makes sense. A "Marker/MaxRange" might make sense, though in my case there were still significant jumps even if very close to the marker. This might be because my depth/color images (Realsense D435) sometimes have noise which could mess up the pose calculation.

I see, I can also disable marker detection by setting the rosparam. Good to know!

Yes, I used apriltag detection inside rtabmap and images are not saved, so that explains why a recovery/reprocess won't preserve landmarks.

I think what I will do is to record a database (master) with images and move that database from my robot to my PC because it will take up too much disk space on my robot. Then I will reprocess the master db and strip all images. The new trimmed down db I will copy back to my robot and use it for navigation purposes. I think this way the landmarks should be preserved.

[matlabbe]

The apriltag detection is done only using the RGB image. The depth would be used for initialization if Marker/Length is 0 (default). To be totally independent of the depth image, set Marker/Length to real width of the marker.

If you are using Apriltag2 dictionary (Marker/Dictionary=19 or Marker/Dictionary=20), set Marker/CornerRefinementMethod to 3 to use apriltag2 refinement approach, which may give better results.

cheers,
Mathieu

[dschnabel]

Got it, thanks Mathieu! This works great.