Demo: Velodyne dataset in Rawlog format, 3D-LiDAR SLAM

This demo shows how to launch a 3D LiDAR SLAM system reading Velodyne scans from an MRPT .rawlog file.

To run this demo, download the example dataset: sample_rawlog1 (part of sample_dataset).

_images/demo-rawlog-velodyne-slam.png

Usage

cd mola/demos
MOLA_INPUT_RAWLOG=map1_test1.rawlog  mola-cli -c rawlog_odom_and_lidar.yml -p

Configuration file, explained

  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
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
# -----------------------------------------------------------------------------
#                        SLAM system definition for MOLA
# This file defines:
# a 3D LiDAR-SLAM problem, fed from an MRPT Rawlog dataset.
# -----------------------------------------------------------------------------

modules:
# Setup of SLAM back-end =====================================
  - type: mola::ASLAM_gtsam
    name: backend
    execution_rate: 1 # Rate to update the SLAM solution [Hz]
    #verbosity_level: INFO
    params:
      # true: use iSAM2
      # false: use global Levenberg-Marquardt
      use_incremental_solver: true
      # iSAM2 additional update() steps. Set >0 to fasten convergence, at
      # the cost of more processing time for each timestep:
      isam2_additional_update_steps: 2
      # Refer to iSAM2 C++ docs for these ones:
      isam2_relinearize_threshold: 0.01
      isam2_relinearize_skip: 1
      # ------
      # 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: 1.0  # [m]
      const_vel_model_std_vel: 5.0  # [m/s]
      # ------
      # Save trajectory to file at end? useful for plots, error evaluation,...
      #save_trajectory_file_prefix:
      # ------------------------
# Setup of World Model (the map) =====================================
  - type: mola::WorldModel
    name: map
    #verbosity_level: DEBUG
    params:
      # Age at which map keyframes will be unloaded from memory [s]
      age_to_unload_keyframes: 25.0
# Setup of SLAM front-ends ====================================
    # ------------------
    # Wheel odometry
    # ------------------
  - type: mola::WheelOdometry
    name: wheel_odometry_fe
    launch_ignore: true
    raw_data_source: rawlog_input
    raw_sensor_label: ODOMETRY
    params:
      # Minimum Euclidean distance (x,y,z) between keyframes inserted into
      # the map
      min_dist_xyz_between_keyframes: 2   # [meters]
      # Minimum rotation (in 3D space, yaw, pitch,roll, altogether) between
      # keyframes inserted into  the map:
      min_rotation_between_keyframes: 30.0  # [deg]
    # ------------------
    # Lidar odometry
    # ------------------
  - type: mola::LidarOdometry
    name: lidar_fe
    launch_ignore: false
    raw_data_source: rawlog_input
    raw_sensor_label: SICK_FRONT_OBS
    params:
      # Minimum time (seconds) between scans for being attempted to be
      # aligned. Scans faster than this rate will be just silently ignored.
      min_time_between_scans: 0.01    # [seconds]
      # Minimum Euclidean distance (x,y,z) between keyframes inserted into
      # the map
      min_dist_xyz_between_keyframes: 3.0   # [meters]
      # Minimum rotation (in 3D space, yaw, pitch,roll, altogether) between
      # keyframes inserted into  the map:
      min_rotation_between_keyframes: 30.0  # [deg]
      #
      # Minimum ICP "goodness" (in the range [0,1]) for a new KeyFrame to be accepted
      min_icp_goodness: 0.50        # range [0,1]
      # ditto, for loop-closures attempts to align
      min_icp_goodness_lc: 0.70     # range [0,1]
      #
      # ---------------------------------------------------------
      # Instantiate one of this point cloud filters:
      pointcloud_filter_class: mola::lidar_segmentation::FilterEdgesPlanes
      # And its params:
      pointcloud_filter_params:
        # Size (resolution) of the voxel filter
        voxel_filter_resolution: 1.0  # [meters]
        # decimate as we filter by voxels (1=no decimation)
        full_pointcloud_decimation: 10
        voxel_filter_decimation: 10
        voxel_filter_max_e2_e0: 30
        voxel_filter_max_e1_e0: 30
        voxel_filter_min_e2_e0: 80
        voxel_filter_min_e1_e0: 80
      # ---------------------------------------------------------
      # min/max distance to check for match/loop closure between KFs:
      min_dist_to_matching: 5.0       # [m]
      max_dist_to_matching: 20.0      # [m]
      max_dist_to_loop_closure: 30.0  # [m]
      max_nearby_align_checks: 5
      min_topo_dist_to_consider_loopclosure: 30
      # ---------------------------------------------------------
      # Instantiate one of the ICP algorithms.
      # See available methods with:
      #  mola-cli --rtti-children-of mp2p_icp::ICP_Base
      #
      icp_class: mp2p_icp::ICP_Horn_MultiCloud
      #icp_class: mp2p_icp::ICP_OLAE
      #
      # And its params:
      # Case: WITH a good twist (velocity) model:
      icp_params_with_vel.maxIterations:  10    90
      icp_params_with_vel.thresholdDist:   1.0   0.50   # [meters]
      icp_params_with_vel.thresholdAng:    2.0   0.01   # [deg]
      # Case: WITHOUT a good twist (velocity) model:
      icp_params_without_vel.maxIterations:   5     100
      icp_params_without_vel.thresholdDist:   3.0   0.50  # [meters]
      icp_params_without_vel.thresholdAng:    2.0   0.01  # [deg]
      # Case: Loop closure
      loop_closure_montecarlo_samples: 10
      icp_params_loopclosure.maxIterations: 10    40    90
      icp_params_loopclosure.thresholdDist: 30.0  5.0  0.50  # [meters]
      icp_params_loopclosure.thresholdAng:   2.0  0.5  0.01  # [deg]
      # visualization:
      viz_decor_decimation: 5
      viz_decor_pointsize: 2.0
# Offline or online sensory data sources =====================
  - name: rawlog_input
    type: mola::RawlogDataset
    execution_rate: 50 # Hz
    verbosity_level: INFO
    gui_preview_sensors:
      - raw_sensor_label: SICK_FRONT_OBS
        decimation: 1
        win_pos: 5 5 400 400
    params:
      rawlog_filename: $(mola-dir mola-test-datasets)/ual_dataset_2014_05_23_2dlaser_odo.rawlog
      time_warp_scale: 1.0