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).

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
|