Skip to content

Robust Graph Optimization

Jump to bottom
matlabbe edited this page Nov 2, 2016 · 64 revisions

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.

video

[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 m):

  • "Reject loop closures if optimization error is greater than this value (0=disabled)."

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:

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:

Clone this wiki locally