Robust Graph Optimization

• Single Session
  • Introduction
  • Configuration
    • ROS
  • Example
    • Without Vertigo
  • Issues
• Multi Session
  • Without GPS
  • With GPS
    

---

Single Session

Introduction

RTAB-Map >=0.10.6 required (built with g2o or GTSAM)

Even if RTAB-Map has a good Recall/Precision performance for loop closure detection, it is not perfect! It may happens that some objects very similar (even identical) in different locations could cause wrong loop closure detections. Optimizing the graph with these wrong loop closures will add significant errors to the generated map. Hopefully, some approaches [1, 2] can deal with a lot of wrong loop closures. Vertigo has been integrated in RTAB-Map to increase robustness to graph optimization. To use it, you must have RTAB-Map built with g2o or GTSAM support (you can see that in the About dialog or with "WITH_G2O=YES" and "WITH_GTSAM=YES" on cmake). The next video shows a comparison without and with robust graph optimization.

[1] Latif Y, Cadena C and Neira J (2013) Robust loop closing over time for pose graph slam. Int. J. of Robotics Research 32(14): 1611—1626.

[2] Sunderhauf N and Protzel P (2012) Towards a robust back-end for pose graph SLAM. In: Proc. IEEE Int. Conf. on Robotics and Automation. pp. 1254–1261.

Configuration

In the Preferences dialog under RGBD-SLAM panel and "Graph optimization" group, choose either g2o or GTSAM for the optimization algorithm, then click on "Robust graph optimization using Vertigo..." and set "Reject loop closures if..." to 0 (this cannot be used at the same time than Vertigo):

ROS

If you are using ROS, you can set the parameters of the rtabmap node like that in a launch file:

<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap">
   <param name="RGBD/OptimizeStrategy" type="string" value="2"/> <!-- g2o=1, GTSAM=2 -->
   <param name="RGBD/OptimizeRobust" type="string" value="true"/>
   <param name="RGBD/OptimizeMaxError" type="string" value="0"/> <!-- should be 0 if RGBD/OptimizeRobust is true -->
   ...
</node>

Example

The video above can be reproduced with this database: robust_graph_optimization_stereo.db (32 MB).

1. Open RTAB-Map.

2. Open Preferences dialog.

3. Restore default settings (Preferences->General Settings->"Reset all settings").

4. Click on Source panel: set source from database, scroll down and select the database above and click on "Yes" to use odometry saved in database.

5. Click on RTAB-Map settings: set "Detection rate" to 0. The source database is already at 1 Hz, it is just to make sure that all images are processed.

6. Click on Advanced (bottom left) to show up the RGB-D SLAM panel, then click on it. Set the panel as in the screenshot of the previous section (with g2o or GTSAM).

7. Close the Preferences dialog.

8. Show the Graph view: Menu Window->Show view->Graph view.

9. Create a new database (File -> New database) and click "Start".

You should then see mapping like the introduction video. Note: to actually see the black lines as in the video, you should set "Reject loop closures if optimization error is greater than this value (0=disabled)." to 0, then when back to the main window right-click on the Graph View -> "Set link color..." -> "Set outlier threshold..." and set value to 1 m.

Without Vertigo

An option can be also used with TORO (it works for the other algorithms too) to detect wrong loop closures and reject them. Under RGB-D SLAM panel in the Preferences dialog, there is this parameter (RGBD/OptimizeMaxError, default 1):

• "Reject loop closures if optimization error ratio is greater than this value (0=disabled). Ratio is computed as absolute error over standard deviation of each link. This will help to detect when a wrong loop closure is added to the graph. Not compatible with "Optimizer/Robust" if enabled."

When large graph optimization errors are detected after a loop closure, this means that the last loop closure is a wrong loop closure. RTAB-Map can detect this and remove the link from the graph directly.

• Play this database on ROS:

$ roslaunch rtabmap_ros stereo_mapping.launch left_image_topic:=/left/image right_image_topic:=/right/image left_camera_info_topic:=/left/camera_info right_camera_info_topic:=/right/camera_info visual_odometry:=false wait_for_transform:=1.5 rtabmap_args:="--delete_db_on_start"
$ rosrun rtabmap_ros data_player _rate:=1 _database:=/home/mathieu/Documents/RTAB-Map/robust_graph_optimization_stereo.db

Issues

The above example doesn't work when link's covariances are Identity. There are some cases where Vertigo cannot correctly ignore all wrong loop closures. Even one left can cause large errors in the map. With the combination of the parameter explained in the section Without Vertigo, this can give less chance of accepting a wrong loop closure.

If someone has an approach that can solve this demo in any cases (with or without Identity covariance), contact me! Here the g2o outputs:

• g2o format not optimized:
  • without_vertigo_constraints.g2o
  • without_vertigo_constraints_identity_cov.g2o
• vertigo format not optimized:
  • with_vertigo_constaints.g2o
  • with_vertigo_constaints_identity_cov.g2o
• Correctly optimized version after using Vertigo (note that information matrices are not Identity):
  • correctly_optimized.g2o

Examples using g2o_viewer on these files (note that only the third case works):

$ g2o_viewer without_vertigo_constraints.g2o
$ g2o_viewer without_vertigo_constraints_identity_cov.g2o

$ g2o_viewer -typeslib libvertigo-g2o.so with_vertigo_constaints.g2o
$ g2o_viewer -typeslib libvertigo-g2o.so with_vertigo_constaints_identity_cov.g2o

Screenshot of the correct one after optimization:

Multi Session

RTAB-Map >=0.18.0 required

In previous section, two approaches have been presented to be more robust to bad loop closures. We will test them again in a multi session experiment with this database: loop_3it_gps.db. This database was captured with RTAB-Map Tango with GPS enabled. In the picture below, the blue path is the trajectory from Tango and the yellow path is the corresponding GPS trajectory. Note that I walked directly in the middle of the sidewalk, so the GPS recorded seems to have an offset with the map from Google Earth. Manually we could adjust the blue path to fit the sidewalk to see better the GPS error.

The same box has been placed at two different locations (see green and red rectangles above). We oriented the camera at those locations to see only the box, so that images are identical. First, we will see how the robust approaches perform on this new database in a single session, then we will test again by splitting the database into two sessions.

Single session without GPS

Here are some results when processing the database as a single session.

• Without robust approaches (RGBD/OptimizeRobust=false and RGBD/OptimizeMaxError=0), loop closures are found between opposite boxes and between the first and last images:
• With Vertigo (RGBD/OptimizeRobust=true and RGBD/OptimizeMaxError=0), loop closures are ignored between opposite boxes and between the first and last images (note that by tuning covariance of odometry, the loop closure between first and last images wouldn't have been ignored):
• With max optimization error: RGBD/OptimizeRobust=false and RGBD/OptimizeMaxError=3, loop closures are rejected between opposite boxes, but accepted between the first and last images:

For the multi session example, at node 60 (the pink line in the screenshot), we clicked Edit->"Trigger a new map" to split the database into two sessions so that the opposite images of the same box are not in the same session. This would make the robust approaches detecting large graph deformation not working (for loop closures between the sessions).

Multi session without GPS

• Without robust approaches (RGBD/OptimizeRobust=false and RGBD/OptimizeMaxError=0), loop closures are accepted between opposite boxes and between the first and last images:
• With Vertigo (RGBD/OptimizeRobust=true and RGBD/OptimizeMaxError=0), loop closures are accepted between opposite boxes, but ignored between the first and last images:
• With max optimization error: RGBD/OptimizeRobust=false and RGBD/OptimizeMaxError=3, loop closures are found between opposite boxes, but rejected between the first and last images.

Multi session With GPS

We set Rtabmap/WithGPS=true to use GPS values in the database for likelihood computation and RGBD/LocalRadius=20. In GUI, parameter Rtabmap/WithGPS is under Preferences->"Loop Closure detection" panel and parameter RGBD/LocalRadius is under RGBD-SLAM panel. Without or with robust approaches, they give all the same results: loop closures are not found between opposite boxes (outside GPS radius), but accepted between the first and last images. The green dots on right show which locations are used for loop closure detection within local radius of current GPS location: