-
Notifications
You must be signed in to change notification settings - Fork 788
Robust Graph Optimization
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.
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):
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>
The video above can be reproduced with this database: robust_graph_optimization_stereo.db (32 MB).
-
Open RTAB-Map.
-
Open Preferences dialog.
-
Restore default settings (Preferences->General Settings->"Reset all settings").
-
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.
-
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.
-
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).
-
Close the Preferences dialog.
-
Show the Graph view: Menu Window->Show view->Graph view.
-
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.
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
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: