LiDAR odometry pipelines

Most parts of the MOLA-LO system are configured dynamically from a YAML file. Basically, the whole design about how many local map layers exist, the pointcloud processing pipelines, ICP matchers and optimizers, etc. can be changed from this YAML file, without the need to touch the code or recompile. Users can design new systems by learning how to modify the provided pipeline files.

The best way to understand the different parts of this file is to browse the YAML file of the default pipeline provided for 3D LiDARs. Most of the times, comments in the YAML are self-explanatory. In case of doubts, do not hesitate in opening an issue to ask.

Note

This page also enumerates all environment variables that can be defined to modify the behavior of the pipelines.

MOLA-specific YAML extensions

MOLA-LO uses the C++ library mola_yaml to parse YAML files, hence all YAML language extensions defined there applies to input YAML files used anywhere in a MOLA-LO system, e.g. ${VAR|default} means “replace by environment variable VAR or, if it does not exist, by default”. Read all about MOLA YAML extensions.

Specifying the pipeline file in MOLA-LO apps

All MOLA-LO GUI applications defaults to using the 3D LiDAR pipeline defined below. To use the alternative 2D pipeline or any other custom pipeline, please set the corresponding environment variable before invoking the GUI application (or derive your own script by copying and modifying the provided ones).

If you use the CLI interface, the pipeline file to use needs to be always explicitly specified, there is none by default.



1. Default pipeline for 3D LiDAR (lidar3d-default.yaml)

This is the reference configuration used for most examples in the MOLA-LO paper, and should work great out of the box for most common situations. As described in the paper [BC24], it defines a voxel-based 3D point-cloud local map, and filtering pipelines to downsample incoming raw LiDAR data.

https://mrpt.github.io/imgs/mola-slam-kitti-demo.gif

Note

See: Configuring pipelines via environment variables

YAML listing

File: mola_lidar_odometry/pipelines/lidar3d-default.yaml

# This file holds parameters for mola::LidarOdometry,
# for use either programmatically calling initialize(), or from a MOLA system
# launch file. See "mola-cli-launchs/*" examples or the main project docs:
# https://github.com/MOLAorg/mola_lidar_odometry/

params:
  # These sensor labels will be handled as LIDAR observations:
  # Can be overriden with cli flag --lidar-sensor-label
  lidar_sensor_labels: ['${MOLA_LIDAR_NAME|lidar}', '/ouster/points']

  multiple_lidars:
    lidar_count: ${MOLA_LIDAR_COUNT|1} # useful only if using several lidar_sensor_labels or regex's.
    max_time_offset: ${MOLA_LIDAR_MAX_TIME_OFFSET|0.1}  # [s]

  # These sensor labels will be handled as IMU observations:
  imu_sensor_label: '${MOLA_IMU_NAME|imu}'

  # These sensor labels will be handled as wheel odometry observation (C++11 regex):
  wheel_odometry_sensor_label: '${MOLA_ODOMETRY_NAME|odometry}'

  # These sensor labels will be handled as GNSS (GPS) (For storage in simplemap only)
  gnss_sensor_label: '${MOLA_GPS_NAME|gps}'

  # Optionally, drop lidar data too close in time:
  min_time_between_scans: 1e-3 # [seconds]

  # Parameters for max sensor range automatic estimation:
  max_sensor_range_filter_coefficient: 0.95
  absolute_minimum_sensor_range: ${MOLA_ABS_MIN_SENSOR_RANGE|5.0}

  # If enabled, vehicle twist will be optimized during ICP
  # enabling better and more robust odometry in high dynamics motion.
  optimize_twist: '${MOLA_OPTIMIZE_TWIST|true}'
  optimize_twist_max_corrections: 8  # (max number of corrections)
  optimize_twist_rerun_min_trans: 0.15 # [m]
  optimize_twist_rerun_min_rot_deg: 0.75 # [deg]

  # How often to update the local map model:
  local_map_updates:
    enabled: '${MOLA_MAPPING_ENABLED|true}'
    load_existing_local_map: ${MOLA_LOAD_MM|""}

    # Idea: don't integrate scans with a high rotational speed since they are probably not correctly deskewed:
    min_translation_between_keyframes: '${MOLA_MIN_XYZ_BETWEEN_MAP_UPDATES|(0.1e-2 + sqrt(wx^2+wy^2+wz^2)*0.1)*ESTIMATED_SENSOR_MAX_RANGE}' # [m]
    min_rotation_between_keyframes: '${MOLA_MIN_ROT_BETWEEN_MAP_UPDATES|(15 + sqrt(wx^2+wy^2+wz^2)*500 )}' # [deg]

    # Should match the "remove farther than" option of the local metric map. "0" means deletion of distant key-frames is disabled
    max_distance_to_keep_keyframes: '${MOLA_LOCAL_MAP_MAX_SIZE|max(100.0, 1.50*ESTIMATED_SENSOR_MAX_RANGE)}' # [m]
    check_for_removal_every_n: 100

  # Minimum ICP quality to insert it into the map:
  min_icp_goodness: ${MOLA_MINIMUM_ICP_QUALITY|0.25}

  # Adaptive threshold, as in the KISS-ICP paper:
  adaptive_threshold:
    enabled: true
    initial_sigma: 2.0  # [m]
    min_motion: ${MOLA_SIGMA_MIN_MOTION|0.10}  # [m]
    maximum_sigma: ${MOLA_SIGMA_MAX_MOTION|3.0}  # [m]
    kp: 2.0
    alpha: ${MOLA_ADAPT_THRESHOLD_ALPHA|0.90}

  # If enabled, a map will be stored in RAM and (if using the CLI) stored
  # to a ".simplemap" file for later use for localization, etc.
  simplemap:
    generate: ${MOLA_GENERATE_SIMPLEMAP|false}   # Can be overriden with CLI flag --output-simplemap
    load_existing_simple_map: ${MOLA_LOAD_SM|""}

    save_final_map_to_file: ${MOLA_SIMPLEMAP_OUTPUT|'final_map.simplemap'}
    
    min_translation_between_keyframes: ${MOLA_SIMPLEMAP_MIN_XYZ|(1.0e-2 + sqrt(wx^2+wy^2+wz^2)*1.0)*ESTIMATED_SENSOR_MAX_RANGE}  # m
    min_rotation_between_keyframes: ${MOLA_SIMPLEMAP_MIN_ROT|15 + sqrt(wx^2+wy^2+wz^2)*500}  # deg

    generate_lazy_load_scan_files: ${MOLA_SIMPLEMAP_GENERATE_LAZY_LOAD|false} # If enabled, a directory will be create alongside the .simplemap and pointclouds will be externally serialized there.
    add_non_keyframes_too: ${MOLA_SIMPLEMAP_ALSO_NON_KEYFRAMES|false}  # If enabled, all frames are stored in the simplemap, but non-keyframes will be without associated observations.
    save_gnss_max_age: 1.0  # [s] max age of GNSS observations to keep in the keyframe

  # Save the final trajectory in TUM format. Disabled by default.
  estimated_trajectory:
    save_to_file: ${MOLA_SAVE_TRAJECTORY|false}
    output_file: ${MOLA_TUM_TRAJECTORY_OUTPUT|'estimated_trajectory.tum'}

  # If run within a mola-cli container, and mola_viz is present, use these options 
  # to show live progress:
  visualization:
    map_update_decimation: 10
    show_trajectory: true
    show_current_observation: true # shows "live raw" LiDAR points
    show_ground_grid: true
    ground_grid_spacing: 5.0 # [m]
    #current_pose_corner_size: 1.5
    local_map_point_size: 3
    model:
      - file: ${MOLA_VEHICLE_MODEL_FILE|""} # Default: none
        tf.x: ${MOLA_VEHICLE_MODEL_X|0.0} # deg
        tf.y: ${MOLA_VEHICLE_MODEL_Y|0.0} # deg
        tf.z: ${MOLA_VEHICLE_MODEL_Z|0.0} # deg
        tf.yaw: ${MOLA_VEHICLE_MODEL_YAW|0.0} # deg
        tf.pitch: ${MOLA_VEHICLE_MODEL_PITCH|0.0} # deg
        tf.roll: ${MOLA_VEHICLE_MODEL_ROLL|90.0} # deg

  # Profile the main steps of the odometry pipeline:
  pipeline_profiler_enabled: ${MOLA_PROFILER|true}
  # Profile the internal steps of the ICP implementation:
  icp_profiler_enabled: ${MOLA_PROFILER|true}
  
  # If set to false, the odometry pipeline will ignore incoming observations
  # until active is set to true (e.g. via the GUI).
  start_active: '${MOLA_START_ACTIVE|true}'

  # Generate CSV with the evolution of internal variables:
  debug_traces:
    save_to_file: '${MOLA_SAVE_DEBUG_TRACES|false}'
    output_file:  '${MOLA_DEBUG_TRACES_FILE|mola-lo-traces.csv}'
  
  # Optional filters to discard incomplete 3D scans from 
  # faulty network, missing UDP packets, etc.
  observation_validity_checks:
    enabled: '${MOLA_ENABLE_OBS_VALIDITY_FILTER|false}'
    check_layer_name: 'raw'
    minimum_point_count: '${MOLA_OBS_VALIDITY_MIN_POINTS|1000}'


# Parameter block for the "navstate_fuse" module in charge of merging and 
# extrapolating pose estimations, odometry, IMU, etc.
navstate_fuse_params:
  # Maximum time since last observation to consider the validity of the velocity model:
  # (KITTI360 test_3 has some blackouts >=1.0 seconds while angular acceleration is high, so 
  #  this threshold must be < 1.0 s for that dataset)
  max_time_to_use_velocity_model: ${MOLA_MAX_TIME_TO_USE_VELOCITY_MODEL|0.75} # [s]
  sliding_window_length: 0.50 # [s]
  sigma_random_walk_acceleration_linear: ${MOLA_NAVSTATE_SIGMA_RANDOM_WALK_LINACC|1.0} # [m/s²]
  sigma_random_walk_acceleration_angular: ${MOLA_NAVSTATE_SIGMA_RANDOM_WALK_ANGACC|10.0} # [rad/s²]
  
  sigma_integrator_position: 1.0 # [m]
  sigma_integrator_orientation:  1.0 # [rad]

  # Optional initial guess for the twist (vx vy vz: m/s, wx wy wz: rad/s):
  initial_twist: ['${MOLA_INITIAL_VX|0.0}', 0.0, 0.0,  0.0, 0.0, 0.0]
  initial_twist_sigma_lin: 20.0 # [m/s]
  initial_twist_sigma_ang: 3.0  # [rad/s]

  robust_param: 0  # 0=no robust disabled
  max_rmse: 2 # max inconsistencies to discard solution


# Whether to use a re-localization method at start up:
initial_localization:
  enabled: ${MOLA_INITIAL_LOCALIZATION_ENABLED|false}
  method: 'InitLocalization::FixedPose'
  fixed_initial_pose: ['${MOLA_INITIAL_X|0.0}', '${MOLA_INITIAL_Y|0.0}', '${MOLA_INITIAL_Z|0.0}',  '${MOLA_INITIAL_YAW|0.0}', '${MOLA_INITIAL_PITCH|0.0}', '${MOLA_INITIAL_ROLL|0.0}']


# If "icp_settings_without_vel" is not defined here, defaults to be the same than 'icp_settings_with_vel'
# ICP settings can be included from an external YAML file if desired, or defined
# in this same YAML for self-completeness:
# Include example:
#icp_settings_with_vel: $include{./icp-pipeline-default.yaml}


# ICP parameters for a regular time step:
icp_settings_with_vel:
  # mp2p_icp ICP pipeline configuration file, for use in ICP 
  # odometry and SLAM packages.
  #
  # YAML configuration file for use with the CLI tool mp2p-icp-run or
  # programmatically from function mp2p_icp::icp_pipeline_from_yaml()
  #
  class_name: mp2p_icp::ICP

  # See: mp2p_icp::Parameter
  params:
    maxIterations: 300
    minAbsStep_trans: 1e-4
    minAbsStep_rot: 5e-5

    #debugPrintIterationProgress: true  # Print iteration progress
    #generateDebugFiles: true  # Can be override with env var "MP2P_ICP_GENERATE_DEBUG_FILES=1"
    saveIterationDetails: ${MP2P_ICP_LOG_FILES_SAVE_DETAILS|false}  # Store partial solutions and pairings for each ICP iteration
    decimationIterationDetails: ${MP2P_ICP_LOG_FILES_SAVE_DETAILS_DECIMATION|3}
    debugFileNameFormat: "icp-logs/icp-run-${SEQ|NO_SEQ}-$UNIQUE_ID-local_$LOCAL_ID$LOCAL_LABEL-to-global_$GLOBAL_ID$GLOBAL_LABEL.icplog"
    decimationDebugFiles: ${MP2P_ICP_LOG_FILES_DECIMATION|10}

  solvers:
    - class: mp2p_icp::Solver_GaussNewton
      params:
        maxIterations: 2
        robustKernel: 'RobustKernel::GemanMcClure'
        #robustKernelParam: '0.5*ADAPTIVE_THRESHOLD_SIGMA'
        robustKernelParam: '0.5*max(ADAPTIVE_THRESHOLD_SIGMA, 2.0*ADAPTIVE_THRESHOLD_SIGMA-(2.0*ADAPTIVE_THRESHOLD_SIGMA-0.5*ADAPTIVE_THRESHOLD_SIGMA)*ICP_ITERATION/30)'
        #innerLoopVerbose: true

  # Sequence of one or more pairs (class, params) defining mp2p_icp::Matcher
  # instances to pair geometric entities between pointclouds.
  matchers:
    - class: mp2p_icp::Matcher_Points_DistanceThreshold
      params:
        threshold: '2.0*max(ADAPTIVE_THRESHOLD_SIGMA, 2.0*ADAPTIVE_THRESHOLD_SIGMA-(2.0*ADAPTIVE_THRESHOLD_SIGMA-0.5*ADAPTIVE_THRESHOLD_SIGMA)*ICP_ITERATION/30)'
        #threshold: '2.0*ADAPTIVE_THRESHOLD_SIGMA'
        thresholdAngularDeg: 0  # deg
        pairingsPerPoint: 1
        allowMatchAlreadyMatchedGlobalPoints: true # faster
        pointLayerMatches:
          - {global: "localmap", local: "decimated_for_icp", weight: 1.0}

  quality:
    - class: mp2p_icp::QualityEvaluator_PairedRatio
      params:
        ~  # none required

# Local map updates:
# Very first observation: Use the mp2p_icp pipeline generator to create the local map:
localmap_generator:
  # Generators:
  #
  # One filter object will be created for each entry, instancing the given class,
  # and with the given parameters. Filters are run in definition order on the
  # incoming raw CObservation objects.
  #
  - class_name: mp2p_icp_filters::Generator
    params:
      target_layer: 'localmap'
      throw_on_unhandled_observation_class: true
      process_class_names_regex: ''  # NONE: don't process observations in the generator.
      #process_sensor_labels_regex: '.*'
      # metric_map_definition_ini_file: '${CURRENT_YAML_FILE_PATH}/localmap_definition_voxelmap.ini'
      
      metric_map_definition:
        # Any class derived from mrpt::maps::CMetricMap https://docs.mrpt.org/reference/latest/group_mrpt_maps_grp.html
        class: mola::HashedVoxelPointCloud
        plugin: 'libmola_metric_maps.so' # Import additional custom user-defined map classes (search in LD_LIBRARY_PATH)
        creationOpts:
          voxel_size: '${MOLA_LOCAL_VOXELMAP_RESOLUTION|$f{max(0.5, min(1.0, 0.015*ESTIMATED_SENSOR_MAX_RANGE))}}' # [m]
        insertOpts:
          max_points_per_voxel: ${MOLA_LOCALMAP_MAX_POINTS_PER_VOXEL|20}
          min_distance_between_points: 0  # [m]
          # if !=0, remove voxels farther (L1) than the current observation insert point
          remove_voxels_farther_than: '${MOLA_LOCAL_MAP_MAX_SIZE|$f{max(100.0, 1.50*ESTIMATED_SENSOR_MAX_RANGE)}}' # [m]
        likelihoodOpts:
          sigma_dist: 1.0    # [m]
          max_corr_distance: 2.0  #[m]
          decimation: 10

# ---------------------------------------------------------------------------------
# LIDAR observations are, first, loaded using a generator
# from "observations_generator".
# then, optionally, filtered before being registered with ICP
# against the local map with filter "observations_filter_1st_pass".
# ---------------------------------------------------------------------------------
observations_generator:
  # Generators:
  #
  # One filter object will be created for each entry, instancing the given class,
  # and with the given parameters. Filters are run in definition order on the
  # incoming raw CObservation objects.
  #
  - class_name: mp2p_icp_filters::Generator
    params:
      target_layer: 'raw'
      throw_on_unhandled_observation_class: true
      process_class_names_regex: '.*'
      process_sensor_labels_regex: '.*'


# this pipeline is required so "SENSOR_TIME_OFFSET" has a different value for each independent LiDAR sensor
# in setups with multiple LiDARs:
observations_filter_adjust_timestamps:
  # If twist estimation within ICP is enabled, this defines 
  # the moment for which twist is estimated:
  - class_name: mp2p_icp_filters::FilterAdjustTimestamps
    params:
      pointcloud_layer: 'raw'
      silently_ignore_no_timestamps: true
      time_offset: 'SENSOR_TIME_OFFSET'
      method: 'TimestampAdjustMethod::MiddleIsZero'
      #method: 'TimestampAdjustMethod::EarliestIsZero'

observations_filter_1st_pass:
  # Filters:
  #
  # One filter object will be created for each entry, instancing the given class,
  # and with the given parameters. Filters are run in definition order on the
  # input metric_map_t object.

  - class_name: mp2p_icp_filters::FilterDecimateVoxels
    params:
      input_pointcloud_layer: 'raw'
      output_pointcloud_layer: 'decimated_for_map_raw'
      voxel_filter_resolution: max(0.20, 0.55*1e-2*ESTIMATED_SENSOR_MAX_RANGE)  # [m]
      minimum_input_points_to_filter: 2000  # don't decimate if smaller than this size
      decimate_method: DecimateMethod::FirstPoint
      #decimate_method: DecimateMethod::ClosestToAverage

  # Remove points too close, to prevent "noise" from the vehicle, 
  # the person next to the robot, etc. Remove too distant points since
  # the tiniest angular error projects to a large translational error.
  - class_name: mp2p_icp_filters::FilterByRange
    params:
      input_pointcloud_layer: 'decimated_for_map_raw'
      output_layer_between: 'decimated_for_map_by_range'
      range_min: '${MOLA_MINIMUM_RANGE_FILTER|max(1.0, 0.03*ESTIMATED_SENSOR_MAX_RANGE)}'
      range_max: 1.2*ESTIMATED_SENSOR_MAX_RANGE

  # Remove close ceilings (problematic in most cases!)
  - class_name: mp2p_icp_filters::FilterBoundingBox
    params:
      input_pointcloud_layer: 'decimated_for_map_by_range'
      outside_pointcloud_layer: 'decimated_for_map_skewed'
      bounding_box_min: [ -0.20*INSTANTANEOUS_SENSOR_MAX_RANGE, -0.20*INSTANTANEOUS_SENSOR_MAX_RANGE, 0.01*INSTANTANEOUS_SENSOR_MAX_RANGE ]
      bounding_box_max: [  0.20*INSTANTANEOUS_SENSOR_MAX_RANGE,  0.20*INSTANTANEOUS_SENSOR_MAX_RANGE, 0.10*INSTANTANEOUS_SENSOR_MAX_RANGE ]

  - class_name: mp2p_icp_filters::FilterDecimateVoxels
    params:
      input_pointcloud_layer: 'decimated_for_map_skewed'
      output_pointcloud_layer: 'decimated_for_icp_skewed'
      voxel_filter_resolution: max(0.60, 1.6*1e-2*ESTIMATED_SENSOR_MAX_RANGE)  # [m]
      minimum_input_points_to_filter: 2000  # don't decimate if smaller than this size
      decimate_method: DecimateMethod::FirstPoint
      #decimate_method: DecimateMethod::ClosestToAverage

# 2nd pass:
observations_filter_2nd_pass:
  - class_name: mp2p_icp_filters::FilterDeleteLayer
    params:
      pointcloud_layer_to_remove: ['decimated_for_map','decimated_for_icp']
      error_on_missing_input_layer: false

  - class_name: mp2p_icp_filters::FilterDeskew
    params:
      input_pointcloud_layer: 'decimated_for_map_skewed'
      output_pointcloud_layer: 'decimated_for_map'
      skip_deskew: ${MOLA_SKIP_DESKEW|false}
      silently_ignore_no_timestamps: ${MOLA_IGNORE_NO_POINT_STAMPS|true} # To handle more dataset types
      #output_layer_class: 'mrpt::maps::CPointsMapXYZIRT'  # Keep intensity, ring, time channels

      # These (vx,...,wz) are variable names that must be defined via the
      # mp2p_icp::Parameterizable API to update them dynamically.
      twist: [vx,vy,vz,wx,wy,wz]

  - class_name: mp2p_icp_filters::FilterDeskew
    params:
      input_pointcloud_layer: 'decimated_for_icp_skewed'
      output_pointcloud_layer: 'decimated_for_icp'
      skip_deskew: ${MOLA_SKIP_DESKEW|false}
      silently_ignore_no_timestamps: ${MOLA_IGNORE_NO_POINT_STAMPS|true} # To handle more dataset types
      #output_layer_class: 'mrpt::maps::CPointsMapXYZIRT'  # Keep intensity, ring, time channels

      # These (vx,...,wz) are variable names that must be defined via the
      # mp2p_icp::Parameterizable API to update them dynamically.
      twist: [vx,vy,vz,wx,wy,wz]


# final pass:
observations_filter_final_pass:
  # Remove layers to save memory and log file storage
  - class_name: mp2p_icp_filters::FilterDeleteLayer
    params:
      pointcloud_layer_to_remove: ['raw','decimated_for_map_skewed', 'decimated_for_icp_skewed', 'decimated_for_map_by_range', 'decimated_for_map_raw']

# To populate the local map, one or more observation layers are merged
# into the local map via this pipeline:
insert_observation_into_local_map:
  - class_name: mp2p_icp_filters::FilterMerge
    params:
      input_pointcloud_layer: 'decimated_for_map'
      target_layer: 'localmap'
      input_layer_in_local_coordinates: true
      robot_pose: [robot_x, robot_y, robot_z, robot_yaw, robot_pitch, robot_roll]


2. 3D mapping pipeline using 3D-NDT (lidar3d-ndt.yaml)

This is an alternative configuration for 3D mapping used in the MOLA-LO paper, and should also work great out of the box for most common situations where, at least, part of the environment has flat surfaces.

As described in the paper [BC24], this pipeline uses an NDT-like [MLD07] local map, based on 3D voxels whose contents switch between bare points and Gaussians depending on how planar and how many points are. This pipeline exploits the point-to-plane pairings.

Note

See: Configuring pipelines via environment variables

YAML listing

File: mola_lidar_odometry/pipelines/lidar3d-ndt.yaml

# This file holds parameters for mola::LidarOdometry,
# for use either programmatically calling initialize(), or from a MOLA system
# launch file. See "mola-cli-launchs/*" examples or the main project docs:
# https://github.com/MOLAorg/mola_lidar_odometry/

params:
  # These sensor labels will be handled as LIDAR observations:
  # Can be overriden with cli flag --lidar-sensor-label
  lidar_sensor_labels: ['${MOLA_LIDAR_NAME|lidar}', '/ouster/points']

  multiple_lidars:
    lidar_count: ${MOLA_LIDAR_COUNT|1} # useful only if using several lidar_sensor_labels or regex's.
    max_time_offset: ${MOLA_LIDAR_MAX_TIME_OFFSET|0.1}  # [s]

  # These sensor labels will be handled as IMU observations:
  imu_sensor_label: '${MOLA_IMU_NAME|imu}'

  # These sensor labels will be handled as wheel odometry observation (C++11 regex):
  wheel_odometry_sensor_label: '${MOLA_ODOMETRY_NAME|odometry}'

  # These sensor labels will be handled as GNSS (GPS) (For storage in simplemap only)
  gnss_sensor_label: '${MOLA_GPS_NAME|gps}'

  # Optionally, drop lidar data too close in time:
  min_time_between_scans: 1e-3 # [seconds]

  # Parameters for max sensor range automatic estimation:
  max_sensor_range_filter_coefficient: 0.95
  absolute_minimum_sensor_range: ${MOLA_ABS_MIN_SENSOR_RANGE|5.0}

  # If enabled, vehicle twist will be optimized during ICP
  # enabling better and more robust odometry in high dynamics motion.
  optimize_twist: '${MOLA_OPTIMIZE_TWIST|true}'
  optimize_twist_max_corrections: 8  # (max number of corrections)
  optimize_twist_rerun_min_trans: 0.15 # [m]
  optimize_twist_rerun_min_rot_deg: 0.75 # [deg]

  # How often to update the local map model:
  local_map_updates:
    enabled: '${MOLA_MAPPING_ENABLED|true}'
    load_existing_local_map: ${MOLA_LOAD_MM|""}

    # Idea: don't integrate scans with a high rotational speed since they are probably not correctly deskewed:
    min_translation_between_keyframes: '${MOLA_MIN_XYZ_BETWEEN_MAP_UPDATES|(0.1e-2 + sqrt(wx^2+wy^2+wz^2)*0.1)*ESTIMATED_SENSOR_MAX_RANGE}' # [m]
    min_rotation_between_keyframes: '${MOLA_MIN_ROT_BETWEEN_MAP_UPDATES|(15 + sqrt(wx^2+wy^2+wz^2)*500 )}' # [deg]

    # Should match the "remove farther than" option of the local metric map. "0" means deletion of distant key-frames is disabled
    max_distance_to_keep_keyframes: '${MOLA_LOCAL_MAP_MAX_SIZE|max(100.0, 1.50*ESTIMATED_SENSOR_MAX_RANGE)}' # [m]
    check_for_removal_every_n: 100

  # Minimum ICP quality to insert it into the map:
  min_icp_goodness: ${MOLA_MINIMUM_ICP_QUALITY|0.25}

  # Adaptive threshold, as in the KISS-ICP paper:
  adaptive_threshold:
    enabled: true
    initial_sigma: 2.0  # [m]
    min_motion: ${MOLA_SIGMA_MIN_MOTION|0.10}  # [m]
    maximum_sigma: ${MOLA_SIGMA_MAX|0.5}  # [m]
    kp: 2.0
    alpha: ${MOLA_ADAPT_THRESHOLD_ALPHA|0.90}

  # If enabled, a map will be stored in RAM and (if using the CLI) stored
  # to a ".simplemap" file for later use for localization, etc.
  simplemap:
    generate: ${MOLA_GENERATE_SIMPLEMAP|false}   # Can be overriden with CLI flag --output-simplemap
    load_existing_simple_map: ${MOLA_LOAD_SM|""}

    save_final_map_to_file: ${MOLA_SIMPLEMAP_OUTPUT|'final_map.simplemap'}
    
    min_translation_between_keyframes: ${MOLA_SIMPLEMAP_MIN_XYZ|(1.0e-2 + sqrt(wx^2+wy^2+wz^2)*1.0)*ESTIMATED_SENSOR_MAX_RANGE}  # m
    min_rotation_between_keyframes: ${MOLA_SIMPLEMAP_MIN_ROT|15 + sqrt(wx^2+wy^2+wz^2)*500}  # deg

    generate_lazy_load_scan_files: ${MOLA_SIMPLEMAP_GENERATE_LAZY_LOAD|false} # If enabled, a directory will be create alongside the .simplemap and pointclouds will be externally serialized there.
    add_non_keyframes_too: ${MOLA_SIMPLEMAP_ALSO_NON_KEYFRAMES|false}  # If enabled, all frames are stored in the simplemap, but non-keyframes will be without associated observations.
    save_gnss_max_age: 1.0  # [s] max age of GNSS observations to keep in the keyframe

  # Save the final trajectory in TUM format. Disabled by default.
  estimated_trajectory:
    save_to_file: ${MOLA_SAVE_TRAJECTORY|false}
    output_file: ${MOLA_TUM_TRAJECTORY_OUTPUT|'estimated_trajectory.tum'}

  # If run within a mola-cli container, and mola_viz is present, use these options 
  # to show live progress:
  visualization:
    map_update_decimation: 10
    show_trajectory: true
    show_current_observation: true # shows "live raw" LiDAR points
    show_ground_grid: true
    ground_grid_spacing: 5.0 # [m]
    #current_pose_corner_size: 1.5
    local_map_point_size: 3
    model:
      - file: ${MOLA_VEHICLE_MODEL_FILE|""} # Default: none
        tf.x: ${MOLA_VEHICLE_MODEL_X|0.0} # deg
        tf.y: ${MOLA_VEHICLE_MODEL_Y|0.0} # deg
        tf.z: ${MOLA_VEHICLE_MODEL_Z|0.0} # deg
        tf.yaw: ${MOLA_VEHICLE_MODEL_YAW|0.0} # deg
        tf.pitch: ${MOLA_VEHICLE_MODEL_PITCH|0.0} # deg
        tf.roll: ${MOLA_VEHICLE_MODEL_ROLL|90.0} # deg

  # Profile the main steps of the odometry pipeline:
  pipeline_profiler_enabled: ${MOLA_PROFILER|true}
  # Profile the internal steps of the ICP implementation:
  icp_profiler_enabled: ${MOLA_PROFILER|true}
  
  # If set to false, the odometry pipeline will ignore incoming observations
  # until active is set to true (e.g. via the GUI).
  start_active: '${MOLA_START_ACTIVE|true}'

  # Generate CSV with the evolution of internal variables:
  debug_traces:
    save_to_file: '${MOLA_SAVE_DEBUG_TRACES|false}'
    output_file:  '${MOLA_DEBUG_TRACES_FILE|mola-lo-traces.csv}'
  
  # Optional filters to discard incomplete 3D scans from 
  # faulty network, missing UDP packets, etc.
  observation_validity_checks:
    enabled: '${MOLA_ENABLE_OBS_VALIDITY_FILTER|false}'
    check_layer_name: 'raw'
    minimum_point_count: '${MOLA_OBS_VALIDITY_MIN_POINTS|1000}'


# Parameter block for the "navstate_fuse" module in charge of merging and 
# extrapolating pose estimations, odometry, IMU, etc.
navstate_fuse_params:
  # Maximum time since last observation to consider the validity of the velocity model:
  # (KITTI360 test_3 has some blackouts >=1.0 seconds while angular acceleration is high, so 
  #  this threshold must be < 1.0 s for that dataset)
  max_time_to_use_velocity_model: ${MOLA_MAX_TIME_TO_USE_VELOCITY_MODEL|0.75} # [s]
  sliding_window_length: 0.50 # [s]
  sigma_random_walk_acceleration_linear: ${MOLA_NAVSTATE_SIGMA_RANDOM_WALK_LINACC|1.0} # [m/s²]
  sigma_random_walk_acceleration_angular: ${MOLA_NAVSTATE_SIGMA_RANDOM_WALK_ANGACC|10.0} # [rad/s²]
  
  sigma_integrator_position: 1.0 # [m]
  sigma_integrator_orientation:  1.0 # [rad]

  # Optional initial guess for the twist (vx vy vz: m/s, wx wy wz: rad/s):
  initial_twist: ['${MOLA_INITIAL_VX|0.0}', 0.0, 0.0,  0.0, 0.0, 0.0]
  initial_twist_sigma_lin: 20.0 # [m/s]
  initial_twist_sigma_ang: 3.0  # [rad/s]

  robust_param: 0  # 0=no robust disabled
  max_rmse: 2 # max inconsistencies to discard solution


# Whether to use a re-localization method at start up:
initial_localization:
  enabled: ${MOLA_INITIAL_LOCALIZATION_ENABLED|false}
  method: 'InitLocalization::FixedPose'
  fixed_initial_pose: ['${MOLA_INITIAL_X|0.0}', '${MOLA_INITIAL_Y|0.0}', '${MOLA_INITIAL_Z|0.0}',  '${MOLA_INITIAL_YAW|0.0}', '${MOLA_INITIAL_PITCH|0.0}', '${MOLA_INITIAL_ROLL|0.0}']


# If "icp_settings_without_vel" is not defined here, defaults to be the same than 'icp_settings_with_vel'
# ICP settings can be included from an external YAML file if desired, or defined
# in this same YAML for self-completeness:
# Include example:
#icp_settings_with_vel: $include{./icp-pipeline-default.yaml}


# ICP parameters for a regular time step:
icp_settings_with_vel:
  # mp2p_icp ICP pipeline configuration file, for use in ICP 
  # odometry and SLAM packages.
  #
  # YAML configuration file for use with the CLI tool mp2p-icp-run or
  # programmatically from function mp2p_icp::icp_pipeline_from_yaml()
  #
  class_name: mp2p_icp::ICP

  # See: mp2p_icp::Parameter
  params:
    maxIterations: 300
    minAbsStep_trans: 5e-4
    minAbsStep_rot: 5e-4

    #debugPrintIterationProgress: true  # Print iteration progress
    #generateDebugFiles: true  # Can be override with env var "MP2P_ICP_GENERATE_DEBUG_FILES=1"
    saveIterationDetails: ${MP2P_ICP_LOG_FILES_SAVE_DETAILS|false}  # Store partial solutions and pairings for each ICP iteration
    decimationIterationDetails: ${MP2P_ICP_LOG_FILES_SAVE_DETAILS_DECIMATION|3}
    debugFileNameFormat: "icp-logs/icp-run-${SEQ|NO_SEQ}-$UNIQUE_ID-local_$LOCAL_ID$LOCAL_LABEL-to-global_$GLOBAL_ID$GLOBAL_LABEL.icplog"
    decimationDebugFiles: ${MP2P_ICP_LOG_FILES_DECIMATION|10}

  solvers:
    - class: mp2p_icp::Solver_GaussNewton
      params:
        maxIterations: 1
        robustKernel: 'RobustKernel::GemanMcClure'
        robustKernelParam: '0.5*max(ADAPTIVE_THRESHOLD_SIGMA, 2.0*ADAPTIVE_THRESHOLD_SIGMA-(2.0*ADAPTIVE_THRESHOLD_SIGMA-0.5*ADAPTIVE_THRESHOLD_SIGMA)*ICP_ITERATION/30)'
        #innerLoopVerbose: true

  # Sequence of one or more pairs (class, params) defining mp2p_icp::Matcher
  # instances to pair geometric entities between pointclouds.
  matchers:
    - class: mp2p_icp::Matcher_Point2Plane
      params:
        distanceThreshold: '1.0*ADAPTIVE_THRESHOLD_SIGMA'
        allowMatchAlreadyMatchedGlobalPoints: true # faster
        pointLayerMatches:
          - {global: "localmap", local: "decimated_for_icp", weight: 1.0}

    - class: mp2p_icp::Matcher_Points_DistanceThreshold
      params:
        threshold: '2.0*max(ADAPTIVE_THRESHOLD_SIGMA, 2.0*ADAPTIVE_THRESHOLD_SIGMA-(2.0*ADAPTIVE_THRESHOLD_SIGMA-0.5*ADAPTIVE_THRESHOLD_SIGMA)*ICP_ITERATION/30)'
        #threshold: '5.0*ADAPTIVE_THRESHOLD_SIGMA'
        thresholdAngularDeg: 0  # deg
        pairingsPerPoint: 1
        allowMatchAlreadyMatchedGlobalPoints: true # faster
        pointLayerMatches:
          - {global: "localmap", local: "decimated_for_icp", weight: 1.0}

  quality:
    - class: mp2p_icp::QualityEvaluator_PairedRatio
      params:
        ~  # none required

# Local map updates:
# Very first observation: Use the mp2p_icp pipeline generator to create the local map:
localmap_generator:
  # Generators:
  #
  # One filter object will be created for each entry, instancing the given class,
  # and with the given parameters. Filters are run in definition order on the
  # incoming raw CObservation objects.
  #
  - class_name: mp2p_icp_filters::Generator
    params:
      target_layer: 'localmap'
      throw_on_unhandled_observation_class: true
      process_class_names_regex: ''  # NONE: don't process observations in the generator.
      #process_sensor_labels_regex: '.*'
      # metric_map_definition_ini_file: '${CURRENT_YAML_FILE_PATH}/localmap_definition_voxelmap.ini'
      
      metric_map_definition:
        # Any class derived from mrpt::maps::CMetricMap https://docs.mrpt.org/reference/latest/group_mrpt_maps_grp.html
        class: mola::NDT
        plugin: 'libmola_metric_maps.so' # Import additional custom user-defined map classes (search in LD_LIBRARY_PATH)
        creationOpts:
          #voxel_size: '${MOLA_LOCAL_VOXELMAP_RESOLUTION|2.0}' # [m]
          voxel_size: '${MOLA_LOCAL_VOXELMAP_RESOLUTION|$f{max(0.5, min(2.5, 0.75*ESTIMATED_SENSOR_MAX_RANGE))}}' # [m]
        insertOpts:
          max_points_per_voxel: ${MOLA_LOCALMAP_MAX_POINTS_PER_VOXEL|0}
          min_distance_between_points: '$f{${MOLA_LOCAL_VOXELMAP_RESOLUTION|2.0}*0.1}' # [m]
          # if !=0, remove voxels farther (L1) than the current observation insert point
          remove_voxels_farther_than: '${MOLA_LOCAL_MAP_MAX_SIZE|$f{max(100.0, 1.50*ESTIMATED_SENSOR_MAX_RANGE)}}' # [m]
          # Max ratio between eigenvalues for considering an NDT a plane:
          max_eigen_ratio_for_planes: 0.05
        likelihoodOpts:
          sigma_dist: 1.0    # [m]
          max_corr_distance: 2.0  #[m]
          decimation: 10
        renderOpts:
          points_visible: true
          planes_visible: true
          normals_visible: true

# ---------------------------------------------------------------------------------
# LIDAR observations are, first, loaded using a generator
# from "observations_generator".
# then, optionally, filtered before being registered with ICP
# against the local map with filter "observations_filter_1st_pass".
# ---------------------------------------------------------------------------------
observations_generator:
  # Generators:
  #
  # One filter object will be created for each entry, instancing the given class,
  # and with the given parameters. Filters are run in definition order on the
  # incoming raw CObservation objects.
  #
  - class_name: mp2p_icp_filters::Generator
    params:
      target_layer: 'raw'
      throw_on_unhandled_observation_class: true
      process_class_names_regex: '.*'
      process_sensor_labels_regex: '.*'


# this pipeline is required so "SENSOR_TIME_OFFSET" has a different value for each independent LiDAR sensor
# in setups with multiple LiDARs:
observations_filter_adjust_timestamps:
  # If twist estimation within ICP is enabled, this defines 
  # the moment for which twist is estimated:
  - class_name: mp2p_icp_filters::FilterAdjustTimestamps
    params:
      pointcloud_layer: 'raw'
      silently_ignore_no_timestamps: true
      time_offset: 'SENSOR_TIME_OFFSET'
      method: 'TimestampAdjustMethod::MiddleIsZero'
      #method: 'TimestampAdjustMethod::EarliestIsZero'

observations_filter_1st_pass:
  # Filters:
  #
  # One filter object will be created for each entry, instancing the given class,
  # and with the given parameters. Filters are run in definition order on the
  # input metric_map_t object.

  - class_name: mp2p_icp_filters::FilterDecimateVoxels
    params:
      input_pointcloud_layer: 'raw'
      output_pointcloud_layer: 'decimated_for_map_raw'
      voxel_filter_resolution: max(0.10, 0.10*1e-2*ESTIMATED_SENSOR_MAX_RANGE)  # [m]
      minimum_input_points_to_filter: 2000  # don't decimate if smaller than this size
      decimate_method: DecimateMethod::FirstPoint
      #decimate_method: DecimateMethod::ClosestToAverage

  # Remove points too close, to prevent "noise" from the vehicle, 
  # the person next to the robot, etc. Remove too distant points since
  # the tiniest angular error projects to a large translational error.
  - class_name: mp2p_icp_filters::FilterByRange
    params:
      input_pointcloud_layer: 'decimated_for_map_raw'
      output_layer_between: 'decimated_for_map_by_range'
      range_min: '${MOLA_MINIMUM_RANGE_FILTER|max(1.0, 0.03*ESTIMATED_SENSOR_MAX_RANGE)}'
      range_max: 1.2*ESTIMATED_SENSOR_MAX_RANGE

  # Remove close ceilings (problematic in most cases!)
  - class_name: mp2p_icp_filters::FilterBoundingBox
    params:
      input_pointcloud_layer: 'decimated_for_map_by_range'
      outside_pointcloud_layer: 'decimated_for_map_skewed'
      bounding_box_min: [ -0.20*INSTANTANEOUS_SENSOR_MAX_RANGE, -0.20*INSTANTANEOUS_SENSOR_MAX_RANGE, 0.01*INSTANTANEOUS_SENSOR_MAX_RANGE ]
      bounding_box_max: [  0.20*INSTANTANEOUS_SENSOR_MAX_RANGE,  0.20*INSTANTANEOUS_SENSOR_MAX_RANGE, 0.10*INSTANTANEOUS_SENSOR_MAX_RANGE ]

  - class_name: mp2p_icp_filters::FilterDecimateVoxels
    params:
      input_pointcloud_layer: 'decimated_for_map_skewed'
      output_pointcloud_layer: 'decimated_for_icp_skewed'
      voxel_filter_resolution: max(0.60, 0.20*1e-2*ESTIMATED_SENSOR_MAX_RANGE)  # [m]
      minimum_input_points_to_filter: 2000  # don't decimate if smaller than this size
      decimate_method: DecimateMethod::FirstPoint
      #decimate_method: DecimateMethod::ClosestToAverage

# 2nd pass:
observations_filter_2nd_pass:
  - class_name: mp2p_icp_filters::FilterDeleteLayer
    params:
      pointcloud_layer_to_remove: ['decimated_for_map','decimated_for_icp']
      error_on_missing_input_layer: false

  - class_name: mp2p_icp_filters::FilterDeskew
    params:
      input_pointcloud_layer: 'decimated_for_map_skewed'
      output_pointcloud_layer: 'decimated_for_map'
      skip_deskew: ${MOLA_SKIP_DESKEW|false}
      silently_ignore_no_timestamps: ${MOLA_IGNORE_NO_POINT_STAMPS|true} # To handle more dataset types
      #output_layer_class: 'mrpt::maps::CPointsMapXYZIRT'  # Keep intensity, ring, time channels

      # These (vx,...,wz) are variable names that must be defined via the
      # mp2p_icp::Parameterizable API to update them dynamically.
      twist: [vx,vy,vz,wx,wy,wz]

  - class_name: mp2p_icp_filters::FilterDeskew
    params:
      input_pointcloud_layer: 'decimated_for_icp_skewed'
      output_pointcloud_layer: 'decimated_for_icp'
      skip_deskew: ${MOLA_SKIP_DESKEW|false}
      silently_ignore_no_timestamps: ${MOLA_IGNORE_NO_POINT_STAMPS|true} # To handle more dataset types
      #output_layer_class: 'mrpt::maps::CPointsMapXYZIRT'  # Keep intensity, ring, time channels

      # These (vx,...,wz) are variable names that must be defined via the
      # mp2p_icp::Parameterizable API to update them dynamically.
      twist: [vx,vy,vz,wx,wy,wz]


# final pass:
observations_filter_final_pass:
  # Remove layers to save memory and log file storage
  - class_name: mp2p_icp_filters::FilterDeleteLayer
    params:
      pointcloud_layer_to_remove: ['raw','decimated_for_map_skewed', 'decimated_for_icp_skewed', 'decimated_for_map_by_range', 'decimated_for_map_raw']

# To populate the local map, one or more observation layers are merged
# into the local map via this pipeline:
insert_observation_into_local_map:
  - class_name: mp2p_icp_filters::FilterMerge
    params:
      input_pointcloud_layer: 'decimated_for_map'
      target_layer: 'localmap'
      input_layer_in_local_coordinates: true
      robot_pose: [robot_x, robot_y, robot_z, robot_yaw, robot_pitch, robot_roll]


3. Pipeline for 2D LiDAR (lidar2d.yaml)

This alternative configuration uses an occupancy voxel map instead of point clouds as local map, and performs ray-tracing to accumulate evidence about the freeness or occupancy of voxels from 2D LiDAR scans. If it recommended to use wheels-based odometry to help the mapping process.

https://mrpt.github.io/imgs/lidar2d-radish-demo.gif

Note

See: Configuring pipelines via environment variables

YAML listing

File: mola_lidar_odometry/pipelines/lidar2d.yaml

# This file holds parameters for mola::LidarOdometry,
# for use either programmatically calling initialize(), or from a MOLA system
# launch file. See "mola-cli-launchs/*" examples or the main project docs:
# https://github.com/MOLAorg/mola_lidar_odometry/

params:
  # These sensor labels will be handled as LIDAR observations:
  # Can be overriden with cli flag --lidar-sensor-label

  lidar_sensor_labels: ['${MOLA_LIDAR_NAME|lidar}']
  multiple_lidars:
    lidar_count: ${MOLA_LIDAR_COUNT|1} # useful only if using several lidar_sensor_labels or regex's.
    max_time_offset: ${MOLA_LIDAR_MAX_TIME_OFFSET|25e-3}  # [s]

  # These sensor labels will be handled as IMU observations:
  imu_sensor_label: 'imu'

  # These sensor labels will be handled as wheel odometry observation (C++11 regex):
  wheel_odometry_sensor_label: '${MOLA_ODOMETRY_NAME|odometry}'

  # These sensor labels will be handled as GNSS (GPS) (For storage in simplemap only)
  gnss_sensor_label: 'gps'

  # Optionally, drop lidar data too close in time:
  min_time_between_scans: 1e-3 # [seconds]

  # Parameters for max sensor range automatic estimation:
  max_sensor_range_filter_coefficient: 0.999
  absolute_minimum_sensor_range: 20.0

  # How often to update the local map model:
  local_map_updates:
    enabled: '${MOLA_MAPPING_ENABLED|true}'
    load_existing_local_map: ${MOLA_LOAD_MM|""}
    min_translation_between_keyframes: '${MOLA_MIN_XYZ_BETWEEN_MAP_UPDATES|0.02*ESTIMATED_SENSOR_MAX_RANGE}' # [m]
    min_rotation_between_keyframes: 15.0 # [deg]
    # Should match the "remove farther than" option of the local metric map. "0" means deletion of distant key-frames is disabled
    max_distance_to_keep_keyframes: 'max(100.0, 1.50*ESTIMATED_SENSOR_MAX_RANGE)' # [m]
    check_for_removal_every_n: 100

  # Minimum ICP quality to insert it into the map:
  min_icp_goodness: 0.25

  # Adaptive threshold, as in the KISS-ICP paper:
  adaptive_threshold:
    enabled: true
    initial_sigma: 0.20
    min_motion: ${MOLA_SIGMA_MIN_MOTION|0.10}
    kp: 2.0
    alpha: 0.99

  # If enabled, a map will be stored in RAM and (if using the CLI) stored
  # to a ".simplemap" file for later use for localization, etc.
  simplemap:
    generate: ${MOLA_GENERATE_SIMPLEMAP|false}   # Can be overriden with CLI flag --output-simplemap
    load_existing_simple_map: ${MOLA_LOAD_SM|""}
    min_translation_between_keyframes: ${MOLA_SIMPLEMAP_MIN_XYZ|0.05}   # m
    min_rotation_between_keyframes: ${MOLA_SIMPLEMAP_MIN_ROT|3.0}  # deg
    save_final_map_to_file: ${MOLA_SIMPLEMAP_OUTPUT|'final_map.simplemap'}
    save_gnss_max_age: 1.0  # [s] max age of GNSS observations to keep in the keyframe

  # Save the final trajectory in TUM format. Disabled by default.
  estimated_trajectory:
    save_to_file: ${MOLA_SAVE_TRAJECTORY|false}
    output_file: ${MOLA_TUM_TRAJECTORY_OUTPUT|'estimated_trajectory.txt'}

  # If run within a mola-cli container, and mola_viz is present, use these options 
  # to show live progress:
  visualization:
    map_update_decimation: 40
    show_trajectory: true
    show_current_observation: true # shows "live raw" LiDAR points
    #current_pose_corner_size: 1.5
    local_map_point_size: 3
    local_map_render_voxelmap_free_space: ${MOLA_RENDER_VOXELMAP_FREESPACE|true}  # for 2D-LIDAR SLAM this may be affordable; for 3D it slows down too much
    model:
      - file: ${MOLA_VEHICLE_MODEL_FILE|""} # Default: none
        tf.roll: 90.0 # deg

  # Profile the main steps of the odometry pipeline:
  pipeline_profiler_enabled: ${MOLA_PROFILER|true}
  # Profile the internal steps of the ICP implementation:
  icp_profiler_enabled: ${MOLA_PROFILER|true}
  
  # If set to false, the odometry pipeline will ignore incoming observations
  # until active is set to true (e.g. via the GUI).
  start_active: '${MOLA_START_ACTIVE|true}'

  # Optional initial guess for the twist (vx vy vz: m/s, wx wy wz: rad/s):
  initial_twist: ['${MOLA_INITIAL_VX|0.0}', 0.0, 0.0,  0.0, 0.0, 0.0]

# Parameter block for the "navstate_fuse" module in charge of merging and 
# extrapolating pose estimations, odometry, IMU, etc.
navstate_fuse_params:
  max_time_to_use_velocity_model: 2.0 # [s]
  sliding_window_length: 0.8 # [s]
  sigma_random_walk_acceleration_linear: ${MOLA_NAVSTATE_SIGMA_RANDOM_WALK_LINACC|2.0} # [m/s²]
  sigma_random_walk_acceleration_angular: ${MOLA_NAVSTATE_SIGMA_RANDOM_WALK_ANGACC|5.0} # [rad/s²]
  
  sigma_integrator_position: 0.10 # [m]
  sigma_integrator_orientation:  0.10 # [rad]

  # Optional initial guess for the twist (vx vy vz: m/s, wx wy wz: rad/s):
  initial_twist: ['${MOLA_INITIAL_VX|0.0}', 0.0, 0.0,  0.0, 0.0, 0.0]
  initial_twist_sigma_lin: 20.0 # [m/s]
  initial_twist_sigma_ang: 3.0  # [rad/s]

  robust_param: 0  # 0=no robust disabled


# If "icp_settings_without_vel" is not defined here, defaults to be the same than 'icp_settings_with_vel'
# ICP settings can be included from an external YAML file if desired, or defined
# in this same YAML for self-completeness:
# Include example:
#icp_settings_with_vel: $include{./icp-pipeline-default.yaml}


# ICP parameters for a regular time step:
icp_settings_with_vel:
  # mp2p_icp ICP pipeline configuration file, for use in ICP 
  # odometry and SLAM packages.
  #
  # YAML configuration file for use with the CLI tool mp2p-icp-run or
  # programmatically from function mp2p_icp::icp_pipeline_from_yaml()
  #
  class_name: mp2p_icp::ICP

  # See: mp2p_icp::Parameter
  params:
    maxIterations: 300
    minAbsStep_trans: 1e-4
    minAbsStep_rot: 5e-5

    #debugPrintIterationProgress: true  # Print iteration progress
    #generateDebugFiles: true  # Can be override with env var "MP2P_ICP_GENERATE_DEBUG_FILES=1"
    saveIterationDetails: false  # Store partial solutions and pairings for each ICP iteration
    decimationIterationDetails: 3
    debugFileNameFormat: "icp-logs/icp-run-${SEQ|NO_SEQ}-$UNIQUE_ID-local_$LOCAL_ID$LOCAL_LABEL-to-global_$GLOBAL_ID$GLOBAL_LABEL.icplog"
    decimationDebugFiles: ${MP2P_ICP_LOG_FILES_DECIMATION|10}

  solvers:
    - class: mp2p_icp::Solver_GaussNewton
      params:
        maxIterations: 2
        robustKernel: 'RobustKernel::GemanMcClure'
        robustKernelParam: '0.50*ADAPTIVE_THRESHOLD_SIGMA'  # [m]  # (adaptive)
        #innerLoopVerbose: true

  # Sequence of one or more pairs (class, params) defining mp2p_icp::Matcher
  # instances to pair geometric entities between pointclouds.
  matchers:
    - class: mp2p_icp::Matcher_Points_DistanceThreshold
      params:
        threshold: '2.0*ADAPTIVE_THRESHOLD_SIGMA'   # [m]
        thresholdAngularDeg: 0  # deg
        pairingsPerPoint: 2
        allowMatchAlreadyMatchedGlobalPoints: true # faster
        pointLayerMatches:
          - {global: "localmap", local: "decimated", weight: 1.0}

  quality:
    - class: mp2p_icp::QualityEvaluator_PairedRatio
      params:
        ~  # none required

# Local map updates:
# Very first observation: Use the mp2p_icp pipeline generator to create the local map:
localmap_generator:
  # Generators:
  #
  # One filter object will be created for each entry, instancing the given class,
  # and with the given parameters. Filters are run in definition order on the
  # incoming raw CObservation objects.
  #
  - class_name: mp2p_icp_filters::Generator
    params:
      target_layer: 'localmap'
      throw_on_unhandled_observation_class: true
      process_class_names_regex: ''  # NONE: don't process observations in the generator.
      #process_sensor_labels_regex: '.*'
      # metric_map_definition_ini_file: '${CURRENT_YAML_FILE_PATH}/localmap_definition_voxelmap.ini'
      
      metric_map_definition:
        # Any class derived from mrpt::maps::CMetricMap https://docs.mrpt.org/reference/latest/group_mrpt_maps_grp.html
        class: mrpt::maps::CVoxelMap
        #plugin: 'libmola_metric_maps.so' # Import additional custom user-defined map classes (search in LD_LIBRARY_PATH)
        creationOpts:
          resolution: 0.05 # '$f{max(0.05, min(1.0, 0.005*ESTIMATED_SENSOR_MAX_RANGE))}' # [m]
        insertOpts:
          prob_miss: 0.30
          prob_hit: 0.70
          clamp_min: 0.05
          clamp_max: 0.95
          ray_trace_free_space: true
          decimation: 1
          remove_voxels_farther_than: 60  # 0: disabled, otherwise, voxels farther away than this distance in meters will be erased.
        likelihoodOpts:
          occupiedThreshold: 0.60

# ---------------------------------------------------------------------------------
# LIDAR observations are, first, loaded using a generator
# from "observations_generator".
# then, optionally, filtered before being registered with ICP
# against the local map with filter "observations_filter_1st_pass".
# ---------------------------------------------------------------------------------
observations_generator:
  # Generators:
  #
  # One filter object will be created for each entry, instancing the given class,
  # and with the given parameters. Filters are run in definition order on the
  # incoming raw CObservation objects.
  #
  - class_name: mp2p_icp_filters::Generator
    params:
      target_layer: 'raw'
      throw_on_unhandled_observation_class: true
      process_class_names_regex: '.*'
      process_sensor_labels_regex: '.*'

observations_filter_1st_pass:
  # Filters:
  #
  # One filter object will be created for each entry, instancing the given class,
  # and with the given parameters. Filters are run in definition order on the
  # input metric_map_t object.
  #
  - class_name: mp2p_icp_filters::FilterDeskew
    params:
      input_pointcloud_layer: 'raw'
      output_pointcloud_layer: 'deskewed'
      silently_ignore_no_timestamps: true # To handle more dataset types
      output_layer_class: 'mrpt::maps::CPointsMapXYZIRT'  # Keep intensity & ring channels
      
      # These (vx,...,wz) are variable names that must be defined via the
      # mp2p_icp::Parameterizable API to update them dynamically.
      twist: [vx,vy,vz,wx,wy,wz]

  - class_name: mp2p_icp_filters::FilterDecimateVoxels
    params:
      input_pointcloud_layer: 'deskewed'
      output_pointcloud_layer: 'decimated_pre'
      voxel_filter_resolution: 0.05 # [m]
      minimum_input_points_to_filter: 2000  # don't decimate if smaller than this size
      decimate_method: DecimateMethod::FirstPoint
      #decimate_method: DecimateMethod::ClosestToAverage

  # Remove points too close, to prevent "noise" from the vehicle,
  # the person next to the robot, etc.
  - class_name: mp2p_icp_filters::FilterByRange
    params:
      input_pointcloud_layer: 'decimated_pre'
      output_layer_between: 'decimated'
      range_min: max(0.10, 0.03*ESTIMATED_SENSOR_MAX_RANGE)
      range_max: 1.25*ESTIMATED_SENSOR_MAX_RANGE

  # Remove layers to save memory and log file storage
  - class_name: mp2p_icp_filters::FilterDeleteLayer
    params:
      pointcloud_layer_to_remove: ['raw','deskewed', 'decimated_pre']

# To populate the local map, one or more observation layers are merged
# into the local map via this pipeline:
insert_observation_into_local_map:
  - class_name: mp2p_icp_filters::FilterMerge
    params:
      input_pointcloud_layer: 'decimated'
      target_layer: 'localmap'
      input_layer_in_local_coordinates: true
      robot_pose: [robot_x, robot_y, robot_z, robot_yaw, robot_pitch, robot_roll]


Configuring pipelines via environment variables

All the following environment variables can be set with export VAR=VALUE before invoking any of the MOLA-LO programs (cli,gui, or ROS node), or directly as prefixes to the invocation line, e.g. VAR1=VALUE1 VAR2=VALUE2 mola-xxx.

Unless said otherwise, all variables are valid for all the pipelines described above.

Note

If using MOLA-LO via mola-cli (which includes the GUI applications or the ROS 2 interface), there are additional environment variables to tune each particular mola-cli launch file. Those variables are documented here.

Sensor inputs: LiDAR

  • MOLA_LIDAR_NAME (Default: ['lidar', '/ouster/points']): A sensor label (maybe including a regular expression) of what observations are to be treated as input LiDAR point clouds. For most dataset sources, the default lidar is enough. For ROS bags or live ROS 2 as sources, the default behavior is assigning sensor labels exactly the same than incoming ROS topic names, but in principle both are different things. Read carefully the contents of the mola-cli launch files and the comments therein to understand the differences.

  • MOLA_LIDAR_COUNT (Default: 1): Useful only if using several lidar_sensor_labels or regex’s. Can be used to work with vehicles with two or more LiDARs.

  • MOLA_LIDAR_MAX_TIME_OFFSET (Default: 0.1 [s]): Maximum delay between different LiDAR observations to handle them together. Note that deskewing takes into account the exact delays between clouds from different LiDARs.

  • MOLA_ABS_MIN_SENSOR_RANGE (Default: 5.0 [m]): Absolute minimum for the otherwise automatically detected maximum sensor range.

  • MOLA_MINIMUM_RANGE_FILTER (Default: 3% of max sensor range). Minimum range for 3D points. This removes points from the robot/vehicle itself.

Sensor inputs: Wheels odometry

  • MOLA_ODOMETRY_NAME (Default: odometry): Sensor label (or regex) of the observations with wheels odometry, if it exists.

Sensor inputs: GPS (GNSS)

  • MOLA_GNSS_TOPIC (Default: /gps): For ROS 2 live node or rosbags, the topic name to be treated as GNSS data. Used only for storage in simple-maps for post-processing (geo-referencing, etc.).

MOLA_USE_FIXED_IMU_POSE

Scan de-skew options

  • MOLA_IGNORE_NO_POINT_STAMPS (Default: true): If enabled (default), input point clouds without per-point timestamps will be just processed without doing any de-skew on them. If this variable is set to false, an exception will be triggered in such event, which can be used as a fail-safe check against missing stamps, important in high velocity scenarios.

  • MOLA_SKIP_DESKEW (Default: false): If enabled, scan de-skew (motion compensation) will be skipped.

General options

  • MOLA_OPTIMIZE_TWIST (Default: true): Whether to enable the optimization of vehicle twist (linear+angular velocity vectors) within the ICP loop. Useful for high-dynamics. Requires incoming point clouds with timestamps.

  • MOLA_MAPPING_ENABLED (Default: true): Whether to update the local map. Might be temporarily disabled if so desired, or permanently disabled if using MOLA-LO for localization from a prebuilt map.

  • MOLA_LOAD_MM (Default: none): An optional path to a metric map (*.mm) file with a prebuilt metric map. Useful for multisession mapping or localization-only mode.

  • MOLA_MINIMUM_ICP_QUALITY (Default: 0.25): Minimum quality (from the mpcp_icp quality evaluators), in the range [0,1], to consider an ICP optimization to be valid.

  • MOLA_SIGMA_MIN_MOTION (Default: 0.10 [m]): Absolute minimum adaptive “sigma” threshold (refer to the paper).

  • MOLA_ADAPT_THRESHOLD_ALPHA (Default: 0.9): Alpha parameter of the IIR low-pass filter for adaptive threshold proportional controller (refer to the paper).

  • MOLA_START_ACTIVE (default: true): If set to false, the odometry pipeline will ignore incoming observations until active is set to true (e.g. via the GUI).

Local map update

  • MOLA_MIN_XYZ_BETWEEN_MAP_UPDATES (Default: a heuristic formula, see YAML file): Minimum distance in meters between updates to the local map.

  • MOLA_MIN_ROT_BETWEEN_MAP_UPDATES (In degrees. Default: a heuristic formula, see YAML file): Minimum angle in degrees between updates to the local map.

  • MOLA_LOCAL_MAP_MAX_SIZE (In meters; default: heuristic formula, see YAML file): Parts of the local metric map farther away then this distance, measured from the current robot pose, will be removed. This is to both, save memory usage, and to avoid inconsistencies before closing loops (which shall be processed outside of the LO module).

  • MOLA_LOCAL_VOXELMAP_RESOLUTION (In meters; default: heuristic formula, see YAML file): Size of voxels for the local map.

Simple-map generation

  • MOLA_GENERATE_SIMPLEMAP (Default: false): If enabled, a simple-map will be saved at the end of the mapping session. This can then be used as input to any of the mp2p_icp applications.

  • MOLA_SIMPLEMAP_OUTPUT (Default: final_map.simplemap): Can be used to change the output file name for maps.

  • MOLA_SIMPLEMAP_MIN_XYZ (in meters), MOLA_SIMPLEMAP_MIN_ROT (in degrees): Minimum distance between simple-map keyframes. Useful to control the density of generated simple-maps. Defaults are heuristic formulas.

  • MOLA_SIMPLEMAP_GENERATE_LAZY_LOAD (Default: false): If enabled, generated simple-map files will be much smaller since all heavy observations will be stored in external files, making much faster to process those maps afterwards.

  • MOLA_SIMPLEMAP_ALSO_NON_KEYFRAMES (Default: false): If enabled, all LiDAR observations will generate a KeyFrame in the simple-map, but without real raw sensory data if the keyframe does not fulfill the minimum distance criteria above. Useful to generate, in post-processing, the full reconstruction of the vehicle trajectory without missing any timestep.

Trajectory files generation

  • MOLA_SAVE_TRAJECTORY (Default: false): If enabled, a TUM file will be saved at the end with the full vehicle trajectory.

  • MOLA_TUM_TRAJECTORY_OUTPUT (Default: estimated_trajectory.tum): Can be used to change the output file name.

Visualization

These settings only have effects if launched via MOLA-LO GUI applications.

  • MOLA_VEHICLE_MODEL_FILE (Default: none): If provided, this is path to any 3D model file loadable via Assimp (e.g. Collada files *.dae) with a representation of the vehicle/robot to show in the GUI.

  • MOLA_VEHICLE_MODEL_X, MOLA_VEHICLE_MODEL_Y, MOLA_VEHICLE_MODEL_Z, MOLA_VEHICLE_MODEL_YAW, MOLA_VEHICLE_MODEL_PITCH, MOLA_VEHICLE_MODEL_ROLL (default: 0): Define a transformation to apply to the 3D asset, if defined in MOLA_VEHICLE_MODEL_FILE. Translations are in meters, rotations in degrees.

Motion model

A constant velocity motion model is used by default, provided by the mola_navstate_fuse module.

  • MOLA_MAX_TIME_TO_USE_VELOCITY_MODEL (Default: 0.75 s): Maximum time between LiDAR frames to use the velocity model. Larger delays will cause using the latest vehicle pose as initial guess.

  • MOLA_NAVSTATE_SIGMA_RANDOM_WALK_LINACC (Default: 1.0 m/s²): Linear acceleration standard deviation.

  • MOLA_NAVSTATE_SIGMA_RANDOM_WALK_ANGACC (Default: 10.0 rad/s²): Angular acceleration standard deviation.

ICP log files

  • MP2P_ICP_GENERATE_DEBUG_FILES (Default: false): If enabled, mp2p_icp::ICP log files will be saved into a subdirectory icp-logs under the current directory. Those logs can be analyzed with the GUI tool: icp-log-viewer.

Note

Enabling ICP log files is the most powerful tool to debug mapping or localization issues or to understand what is going on under the hook. However, it introduces a significant cost in both, CPU running time, and disk space.

If MP2P_ICP_GENERATE_DEBUG_FILES is not enabled, the rest of parameters that follow have no effect:

  • MP2P_ICP_LOG_FILES_DECIMATION (Default: 10): How many ICP runs to drop before saving one to disk.

  • MP2P_ICP_LOG_FILES_SAVE_DETAILS (Default: false): If enabled, results, and pairings of intermediate optimization steps are also stored in the ICP logs. Great to learn how ICP actually works, but will increase the log file sizes.

  • MP2P_ICP_LOG_FILES_SAVE_DETAILS_DECIMATION (Default: 3): If MP2P_ICP_LOG_FILES_SAVE_DETAILS is enabled, how many ICP internal iterations to drop for each saved one.

Trace debug files

“Trace” files are optional CSV files with low-level debugging information, sampled once per time step.

  • MOLA_SAVE_DEBUG_TRACES (Default: false): Whether to generate and save this debug information to a file.

  • MOLA_DEBUG_TRACES_FILE (Default: mola-lo-traces.csv): The name of the file to store trace information, if enabled.