Demo: Graph SLAM from a dataset in g2o plain text format

This demo shows how to launch a 2D or 3D Graph SLAM (pose graph) system reading pose-to-pose constraints from a .g2o text file, which are processed sequentially. By manipulating the execution rate of the G2O publisher (mola::G2ODataset) and that of the SLAM back-end (mola::ASLAM_gtsam) we can move between processing edges one by one, up to optimize the entire dataset at once.

Note that the SLAM solver can be also switched between sequential (iSAM2) and batch (Levenberg-Marquardt); this is independent of how fast new edges are appended to the SLAM problem.

_images/demo-pose-graph-g2o-file.png

Usage

cd mola/demos
mola-cli -c pose_graph_g2o_file.yml

Configuration file, commented

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
# -----------------------------------------------------------------------------
#                        SLAM system definition for MOLA
# This file defines:
# a 3D graph slam  (pose graph) without any real-sensor front-end.
# To be fed with .g2o-like text files
# -----------------------------------------------------------------------------

modules:
# Setup of SLAM back-end =====================================
  - name: backend
    # ASLAM_gtsam for SLAM in absolute coordinates
    # RSLAM_gtsam for SLAM in relative coordinates
    type: mola::ASLAM_gtsam
    # The rate at which the SLAM solution will be recomputed
    execution_rate: 10 # Hz
    params:
      # true: use iSAM2
      # false: use global Levenberg-Marquardt
      use_incremental_solver: true
      # ------
      # Select state space representation for KeyFrame poses:
      # See options in: https://docs.mola-slam.org/latest/concept-state-vector.html
      state_vector: SE3
      # --- constant velocity model params (see papers) ---
      const_vel_model_std_pos:   # [m]
      const_vel_model_std_vel: 0.1  # [m/s]
      # ------
      # Save trajectory to file at end? useful for plots, error evaluation,...
      #save_trajectory_file_prefix: pose_graph_slam_result
      # ------------------------
      # ------------------------
# Setup of SLAM front-ends ====================================
  - name: g2o_dataset_fe
    type: mola::G2ODataset
    # The execution rate for this module is the rate at which observations
    # will be sent to the SLAM back-end. Change to modify the speed at which
    # the map is being built.
    execution_rate: 20 # Hz
    params:
      # Dataset file to load:
      g2o_file: $(mola-dir mola-test-datasets)/g2o/parking-garage.g2o
      # ----------
# Setup of World Model (the map) =====================================
  - name: map
    type: mola::WorldModel
    verbosity_level: INFO
    params:
      age_to_unload_keyframes: 1500000.0