LO/LIO 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 GICP 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.

Summary of ICP pipelines

The table below summarizes the optimization algorithm and local-map types used by the ICP pipelines in MOLA-LO. When saving a map to a *.mm file, the corresponding C++ class will match the “Local map type” column. Remember that the accompanying .simplemap file will always contain a keyframe-map representation independent of the local map type used for ICP.

To use a prepared .mm map for localization, ensure it contains a layer named localmap with the exact C++ class listed below.

Pipeline config file	Local map type and layer name	ICP algorithm
`lidar3d-gicp.yaml` (Default)	Keyframe-based 3D point clouds (layer: `localmap`) (class mola::KeyframePointCloudMap) Optionally, an incremental k-d tree point cloud (`mola::IncrementalPointCloud`), selected via `MOLA_LOCALMAP_CLASS`: see Selecting the local map class.	Generalized ICP (“cov-to-cov”)
`lidar3d-icp.yaml`	Voxel-based 3D point clouds (layer: `localmap`) (class mola::HashedVoxelPointCloud)	Standard ICP (point-to-point)
`lidar3d-ndt.yaml`	Voxel-based 3D NDT map (layer: `localmap`) (class mola::NDT)	Standard ICP (point-to-plane, point-to- point)
`lidar2d.yaml`	Voxel-based 2D occupancy map (layer: `localmap`) (`mrpt::maps::CVoxelMap`)	Standard ICP (point-to-occupied-voxel)

1. Generalized ICP (GICP) pipeline for 3D LiDAR (`lidar3d-gicp.yaml`)

As of Oct 2025, this is the default and recommended configuration for most common sensor setups and environments.

A peer-reviewed paper describing this pipeline is in preparation, but very briefly, this pipeline defines a local map based on key-frames of point clouds, and exploits the Generalized ICP algorithm [SHT09].

https://mrpt.github.io/imgs/MOLA_LIO_Oxford_Spires_stairs.gif

Note

This pipeline can be LO (default) or LIO. To actually employ IMU data, you must enable the ROS 2 launch file argument:

ros2 launch [...] \
  mola_deskew_method:=MotionCompensationMethod::IMU \
  [...]

or, if using MOLA from the CLI:

MOLA_DESKEW_METHOD=MotionCompensationMethod::IMU \
mola-lo-gui-rosbag2 [...]

Note

See: Configuring pipelines via environment variables

Selecting the local map class

This pipeline can build its local map with either of two classes. Both implement the mp2p_icp::NearestPointWithCovCapable interface required by the Matcher_Cov2Cov matcher, and the choice applies to both the localmap layer and the per-scan observation layer, since ICP pairs the two:

`MOLA_LOCALMAP_CLASS`	Description
`mola::KeyframePointCloudMap`	Default. Keyframe-based: points are stored in per-keyframe local frames, so the map survives a loop-closure re-map. Required for SLAM (`mola_sm_loop_closure`) and for any mapping session where the trajectory may be corrected after the fact.
`mola::IncrementalPointCloud`	Single global frame, holding one incremental, self-balancing k-d tree that is updated in place instead of being rebuilt on every scan. Odometry only: a global SE(3) re-map would force a full rebuild, so it must not be used together with loop closure.

To try the incremental map from the CLI:

MOLA_LOCALMAP_CLASS=mola::IncrementalPointCloud \
MOLA_ODOMETRY_PIPELINE_YAML=$(ros2 pkg prefix mola_lidar_odometry)/share/mola_lidar_odometry/pipelines/lidar3d-gicp.yaml \
mola-lo-gui-rosbag2 /path/to/dataset.mcap

or from a ROS 2 launch file:

MOLA_LOCALMAP_CLASS=mola::IncrementalPointCloud \
ros2 launch mola_lidar_odometry ros2-lidar-odometry.launch.py [...]

Note that lidar3d-gicp.yaml is already the default pipeline, so MOLA_ODOMETRY_PIPELINE_YAML only needs to be set for the dataset wrappers that default to another one (e.g. mola-lo-gui-oxford-spires).

Note

mola::IncrementalPointCloud requires mola_metric_maps to have been built against nanoflann >= 1.10.0, the release that introduced the incremental k-d tree index. On ROS distributions shipping an older nanoflann the class still exists and is registered, but instantiating it throws an explanatory error, so selecting it fails with a clear message rather than silently falling back.

Tuning is done through the MOLA_INCREMENTAL_MAP_* variables, see Configuring pipelines via environment variables.

YAML listing

File: mola_lidar_odometry/pipelines/lidar3d-gicp.yaml

# =====================================================================================
# Pipeline: GICP (Generalized ICP) with cov-to-cov pairings
#
# For paper references, see https://github.com/MOLAorg/mola_lidar_odometry/
#
# 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:
# =====================================================================================

# TODO: Once mola_yaml >=3.0.0 for all ros2 distros, simplify all yaml with $import and common files

params:
  pipeline_name: "GICP (Generalized ICP) with cov-to-cov pairings" # For display/debug only

  # These sensor labels will be handled as LIDAR observations:
  # Can be overridden 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 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]

  # Whether a scan arriving while the worker is still busy replaces the one
  # already queued (real-time behavior) or waits for it (lossless). Set to false
  # for offline batch runs, where every scan must be processed.
  drop_stale_scans: ${MOLA_DROP_STALE_SCANS|true}

  # Parameters for max sensor range automatic estimation:
  observation_radius_filter_coefficient: 0.95
  absolute_minimum_observation_radius: ${MOLA_ABS_MIN_SENSOR_RANGE|5.0}
  # Quantile of the per-point norms taken as ESTIMATED_OBSERVATION_RADIUS.
  # 1.0 = the bounding-box max-norm, i.e. one far return sets the scene
  # scale for the six parameters derived from it. Default unchanged;
  # exposed so the robust alternative can be measured.
  observation_radius_quantile: ${MOLA_OBSERVATION_RADIUS_QUANTILE|1.0}

  # If enabled, vehicle twist will be optimized during ICP
  # enabling better and more robust odometry in high dynamics motion without an IMU.
  # NOTE: Disabled for more efficient deskew in the GICP pipeline with IMU.
  # Enabling it costs about 25% more time per scan. On KITTI, which has no IMU
  # and so no other source of velocity, it still bought nothing measurable, so
  # do not assume it helps just because a platform lacks an IMU: measure it.
  optimize_twist: ${MOLA_OPTIMIZE_TWIST|false}

  # IMU accelerometer-based verticality correction:
  # Uses averaged accelerometer readings to constrain ICP pitch/roll.
  imu_gravity_correction:
    enabled: ${MOLA_IMU_GRAVITY_CORRECTION|true}
    # Yaw-free, rank-2 verticality constraint solved by mp2p_icp. Set to false
    # only to reproduce results from the legacy path, which folded tilt into
    # the SE(3) pose prior.
    use_rank2_prior: ${MOLA_IMU_GRAVITY_RANK2|true}
    # Widen sigma_deg by the measured dispersion of the accelerometer
    # directions, so the constraint stands down when the readings are not
    # actually gravity (braking, cornering, vibration).
    adaptive_sigma: ${MOLA_IMU_GRAVITY_ADAPTIVE_SIGMA|true}
    sigma_deg: ${MOLA_IMU_GRAVITY_SIGMA_DEG|2.0}
    averaging_samples: ${MOLA_IMU_GRAVITY_AVG_SAMPLES|20}
    max_age_seconds: ${MOLA_IMU_GRAVITY_MAX_AGE|2.0}
    # The one-shot map-origin verticality capture defines the map's vertical
    # for the whole run, so it is deferred to a later scan while the buffered
    # accelerometer directions disagree by more than this (i.e. the reading is
    # motion, not gravity), and taken as-is after the timeout.
    map_origin_max_dispersion_deg: ${MOLA_IMU_GRAVITY_MAP_ORIGIN_MAX_DISPERSION|1.0}
    map_origin_capture_timeout: ${MOLA_IMU_GRAVITY_MAP_ORIGIN_TIMEOUT|3.0}
    # NOTE: averaging_samples must fit inside max_age_seconds at the actual IMU
    # rate, or estimatedPitchRoll() returns nothing and the constraint is
    # silently inactive (at 400 Hz, 2.0 s holds only ~800 samples).
    #
    # Take the verticality reading from an odometry source's ABSOLUTE attitude
    # instead of the accelerometer. Only the "up" axis is used: the source's
    # frame differs from the map frame by an unknown yaw and translation, and
    # neither moves the vertical, so that one direction transfers exactly while
    # the position and heading do not.
    #
    # An accelerometer only measures gravity while quasi-static, so on a
    # platform that accelerates continuously `adaptive_sigma` correctly stands
    # the constraint down nearly always, leaving the vertical unconstrained. A
    # kinematic-inertial estimator on the platform has no such limitation.
    # The map-origin reference is captured from the same source, so the
    # constant offset between the two verticals stays a gauge instead of
    # becoming a growing tilt.
    odometry_attitude:
      enabled: ${MOLA_ODOM_VERTICALITY|false}
      # The VALUE here must equal the label the odometry observation is
      # published under, which the shipped launch files set through their own
      # MOLA_ODOM_SENSOR_LABEL variable; the two variables are separate on
      # purpose, since the verticality source need not be the fused one. That
      # observation must also be a CObservationRobotPose: planar odometry has
      # no pitch or roll to offer.
      sensor_label: "${MOLA_ODOM_VERTICALITY_LABEL|odom_wheels}"
      sigma_deg: ${MOLA_ODOM_VERTICALITY_SIGMA_DEG|1.0}
      max_age_seconds: ${MOLA_ODOM_VERTICALITY_MAX_AGE|0.5}

    # Estimate the map-frame vertical online instead of freezing it from one
    # accelerometer average at the first keyframe. See
    # mola::imu::MapGravityEstimator: it solves for gravity in the map frame
    # (plus IMU biases) from preintegrated IMU and this odometry's own relative
    # attitudes/velocities, so platform acceleration cancels and no
    # quasi-static window is required.
    map_gravity:
      enabled: ${MOLA_IMU_MAP_GRAVITY|false}
      # Compute and log the estimate without letting it affect the verticality
      # reference, so it can be scored against ground truth on a new dataset
      # without its own feedback contaminating the map frame it is estimating.
      log_only: ${MOLA_IMU_MAP_GRAVITY_LOG_ONLY|false}
      solve_every_n: ${MOLA_IMU_MAP_GRAVITY_SOLVE_EVERY_N|5}
      # Rotate the MAP FRAME itself, once, so it becomes gravity-aligned,
      # instead of only feeding the per-scan verticality prior. The map frame is
      # the initial body frame, so a platform that starts tilted produces a map
      # that leans by that tilt forever: measured on a handheld dataset, a
      # median of 8.7 deg (max 20.7). This is a gauge change - local map,
      # simplemap, trajectory, state estimator and the published `odom` frame
      # all rotate together about the map origin - so no relative quantity
      # moves. OFF by default: it changes the frame every product of the run is
      # expressed in.
      relevel_map_frame: ${MOLA_IMU_MAP_GRAVITY_RELEVEL|false}
      # Readiness for the re-level, as an interval count. Note this is NOT
      # min_intervals_for_convergence below: that one gates the per-scan prior,
      # where the later, more settled estimate is what matters. For leveling the
      # map once, the estimate is at its BEST at the first solve (measured 0.72
      # deg at ~5.6 s) and slowly degrades from there, so waiting costs accuracy.
      # 5 intervals is the first solve under solve_every_n above.
      relevel_min_intervals: ${MOLA_IMU_MAP_GRAVITY_RELEVEL_MIN_INTERVALS|5}
      # Magnitude gate: only re-level when there is enough tilt that removing it
      # beats the error of the estimate doing the removing. Measured at the
      # firing point on a handheld dataset, that error is 0.63 deg median,
      # 1.43 deg p90, 1.75 deg worst. Note the gate tests the ESTIMATE while the
      # quantity that must be large is the TRUE tilt, and the two differ by
      # exactly that error, so the threshold is ~2x the p90 rather than 1x:
      # gating at 2 deg let one near-level sequence through (estimate 2.8 deg
      # against a true 1.1 deg) and made it 0.7 deg worse. Below the threshold
      # the re-level stands down permanently (and says so in the log) instead of
      # retrying, which is what keeps an already-level dataset untouched.
      relevel_min_tilt_deg: ${MOLA_IMU_MAP_GRAVITY_RELEVEL_MIN_TILT|3.0}
      # NOTE: there is deliberately no ACCURACY threshold here. The estimator
      # reports every usable estimate with its earned pitch/roll sigma, which is
      # added in quadrature to the gravity prior's sigma.
      #
      # CAUTION: "a weak estimate silences itself" is NOT established. A later
      # measurement on a handheld dataset put the error/sigma ratio at 8.4
      # median and 19.0 worst case, i.e. the reported sigma is roughly an order
      # of magnitude tighter than the estimate is accurate, and no better
      # calibrated after the interval gate below than before it. Do not use
      # those sigmas as a confidence gate until that is understood; the
      # re-level trigger above deliberately does not.
      #
      # There IS a data-quantity precondition, which is a different thing. Until
      # the window holds enough intervals the solution is pulled by the |g|
      # constraint and the bias priors, and that pull is a BIAS the linearized
      # covariance cannot see, so the estimate is not merely uncertain, it is
      # confidently wrong. Until then the accelerometer capture is the better
      # reference, and this keeps LO on it. This gates the PER-SCAN PRIOR only:
      # the one-off map-frame re-level above has the opposite need (its estimate
      # is best at the first solve) and uses its own, much shorter, gate.
      min_intervals_for_convergence: ${MOLA_IMU_MAP_GRAVITY_MIN_INTERVALS|130}
      # Sliding-window length. The whole point is that verticality information
      # ACCUMULATES, so this wants to be much longer than the estimator default.
      window_size: ${MOLA_IMU_MAP_GRAVITY_WINDOW|200}
      min_interval_seconds: ${MOLA_IMU_MAP_GRAVITY_MIN_INTERVAL|1.0}
  # When publishing pose updates, the reference frame for both, estimated robot poses, and the local map.
  publish_reference_frame: "${MOLA_LO_PUBLISH_REF_FRAME|odom}"

  # When publishing pose updates, the vehicle frame name.
  publish_vehicle_frame: "${MOLA_LO_PUBLISH_VEHICLE_FRAME|base_link}"

  # If enabled, deskewed scans will be published (so, they will be available as ROS2 messages), mostly for visualization.
  # This may slow-down the system, so it is disabled by default.
  publish_deskewed_scans: "${MOLA_LO_PUBLISH_DESKEWED_SCANS|false}"

  # How often to update the local map model:
  local_map_updates:
    enabled: "${MOLA_MAPPING_ENABLED|true}"
    load_existing_local_map: ${MOLA_LOAD_MM|""}
    load_map_after_gui_init: ${MOLA_LO_LOAD_MAP_AFTER_GUI|false}
    save_final_local_map: ${MOLA_SAVE_MM|""} # If not empty, saves the final local metric map to a ".mm" file

    # 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_OBSERVATION_RADIUS}" # [m]
    min_rotation_between_keyframes: "${MOLA_MIN_ROT_BETWEEN_MAP_UPDATES|(15 + sqrt(wx^2+wy^2+wz^2)*5 )}" # [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_OBSERVATION_RADIUS)}" # [m]
    check_for_removal_every_n: 100
    min_nearby_poses_occupied: ${MOLA_MIN_NEARBY_POSES_OCCUPIED|1}
    publish_map_updates_every_n: ${MOLA_PUBLISH_LOCAL_MAP_UPDATES_EVERY_N|40}

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

  # If defined, ".icplog" files will be saved if ICP quality drops below the given threshold.
  # Useful to debug mapping issues. If empty, only the env var MP2P_ICP_GENERATE_DEBUG_FILES and the icp.params setting will define when to save logs.
  write_debug_icp_log_if_quality_under: ${MOLA_WRITE_DEBUG_ICP_LOG_IF_QUALITY_UNDER|""}

  # Adaptive threshold:
  adaptive_threshold:
    enabled: true
    initial_sigma: ${MOLA_SIGMA_INITIAL|0.50} # [m]
    min_motion: ${MOLA_SIGMA_MIN_MOTION|0.5} # [m]
    maximum_sigma: ${MOLA_SIGMA_MAX_MOTION|2.00} # [m]
    max_sigma_step: ${MOLA_SIGMA_MAX_STEP|0.05} # [m]
    icp_quality_controller_setpoint: ${MOLA_SIGMA_CONTROLLER_QUALITY_SETPOINT|0.85}
    kp: ${MOLA_SIGMA_CONTROLLER_GAIN|2.0}
    alpha: ${MOLA_ADAPT_THRESHOLD_ALPHA|0.90}
    # Sustained-failure recovery: if enabled, sigma is grown multiplicatively
    # after a streak of bad ICPs, capped at maximum_sigma, so the matcher
    # window can re-open and ICP can recover. Enabled by default: without it,
    # once sigma is driven down near min_motion by a run of easy/near-static
    # scans (e.g. goodness consistently above icp_quality_controller_setpoint),
    # a single larger inter-scan motion (a turn, a bump, or just ordinary
    # scan-to-scan variability once sigma is already pinned at its floor) can
    # push ICP into failure, and with sigma frozen the correspondence search
    # window never reopens, so the pipeline stalls indefinitely
    # (estimated_trajectory never grows again). Set to false to restore the
    # old behavior. `maximum_sigma` must be set strictly above
    # `initial_sigma`, or this mechanism is a no-op: sigma always starts
    # each run AT initial_sigma, so a bad ICP on frame 1 (e.g. a slightly
    # imprecise localization-only seed pose against a prebuilt map) has no
    # room to grow into and can never recover (observed in testing as ICP
    # goodness stuck just under min_icp_goodness for an entire run).
    #
    # recover_after_n_bad/recover_growth_factor default to a fast reaction
    # (2 bad frames, x2.0 growth) rather than a slow one (5 bad frames,
    # x1.5): every frame spent stuck is a frame of real, untracked vehicle
    # motion accumulating; the slower defaults let that gap grow to the
    # point where, once the search window finally reopens, ICP can lock onto
    # a self-consistent but WRONG registration (observed in testing as a
    # sudden ~30-40 deg yaw error that then persisted for the rest of a run)
    # instead of recovering the true pose.
    recover_on_sustained_failure: ${MOLA_ADAPT_THRESHOLD_RECOVER|true}
    recover_after_n_bad: ${MOLA_ADAPT_THRESHOLD_RECOVER_AFTER_N_BAD|2}
    recover_growth_factor: ${MOLA_ADAPT_THRESHOLD_RECOVER_GROWTH_FACTOR|2.0}

  # REP-107 diagnostics thresholds published on /diagnostics via mola_bridge_ros2.
  # Any of these can be omitted to keep its built-in default.
  diagnostics:
    icp_quality_warn: 0.30 # [0-1] WARN if icp_quality below this
    icp_quality_error: 0.10 # [0-1] ERROR if icp_quality below this
    input_stale_sec: 3.0 # [s]   STALE if no observation for this long
    input_error_sec: 5.0 # [s]   ERROR if no observation for this long
    dropped_ratio_warn: 0.20 # [0-1] WARN if dropped-frames ratio exceeds
    dropped_ratio_error: 0.50 # [0-1] ERROR if dropped-frames ratio exceeds
    timing_utilization_warn: 0.80 # [0-1] WARN if avg process time / sensor period exceeds

  # 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 overridden with CLI flag --output-simplemap
    load_existing_simple_map: ${MOLA_LOAD_SM|""}

    save_final_map_to_file: ${MOLA_SIMPLEMAP_OUTPUT|'final_map.simplemap'}

    # NOTE: unlike local_map_updates above, these thresholds are NOT scaled by
    # angular velocity: this is what feeds the SharedKeyframeMap sink (e.g.
    # mola_mapper), and the old w-scaled formula (rotation threshold growing
    # by 500 deg per rad/s) made it create close to NO keyframes while
    # smoothly turning, starving loop closure of the keyframes it needs.
    min_translation_between_keyframes: "${MOLA_SIMPLEMAP_MIN_XYZ|(1.0e-2 + sqrt(wx^2+wy^2+wz^2)*0.1)*ESTIMATED_OBSERVATION_RADIUS}" # [m]
    min_rotation_between_keyframes: "${MOLA_SIMPLEMAP_MIN_ROT|(15 + sqrt(wx^2+wy^2+wz^2)*5 )}" # [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.
    min_nearby_poses_occupied: ${MOLA_SIMPLEMAP_MIN_NEARBY_POSES|1}
    # Revisiting an already-mapped area creates NO keyframes with the purely
    # spatial criterion above, which starves loop closure of the second endpoint
    # of the loop. Set to a positive value [s] so only keyframes newer than that
    # take part in the "is there one here already?" test.
    nearby_keyframe_time_window: ${MOLA_SIMPLEMAP_KF_TIME_WINDOW|0} # [s], 0=disabled
    save_gnss_max_age: 1.0 # [s] max age of GNSS observations to keep in the keyframe

    # If enabled, this will store deskewed scans into the keyframes of simplemaps.
    save_deskewed_scans: ${MOLA_SAVE_DESKEWED_SCANS|false}

  # 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: ${MOLA_GUI_MAP_UPDATE_DECIMATION|10}
    show_trajectory: ${MOLA_GUI_SHOW_TRAJECTORY|true}
    trajectory_rgba: [0.1, 0.1, 0.1, 1.0]

    # Camera tracks the vehicle. Exposed as an env hook so an integrator (e.g.
    # mola_mapper, where the mapper module owns the camera) can disable it here
    # without editing this pipeline. Default true (standalone LIO behavior).
    camera_follows_vehicle: ${MOLA_GUI_CAMERA_FOLLOWS_VEHICLE|true}

    # Enable or disable each of the three GUI panels:
    show_tab_status: ${MOLA_GUI_SHOW_TAB_STATUS|true}
    show_tab_control: ${MOLA_GUI_SHOW_TAB_CONTROL|true}
    show_tab_view: ${MOLA_GUI_SHOW_TAB_VIEW|true}

    show_current_observation: ${MOLA_GUI_SHOW_CURRENT_OBS|false} # shows "live deskewed" LiDAR points
    show_last_deskewed_observations_decay: ${MOLA_GUI_SHOW_DESKEWED_DECAY|true} # shows "live deskewed" LiDAR points over time
    last_deskewed_observations_point_size: ${MOLA_GUI_LAST_CLOUDS_POINT_SIZE|1.0}
    last_deskewed_observations_colormap: ${MOLA_GUI_LAST_CLOUDS_COLORMAP|cmJET} # mrpt::img::TColormap
    last_deskewed_observations_color_by_field: ${MOLA_GUI_LAST_CLOUDS_COLOR_FIELD|intensity}
    observations_initial_alpha: 0.10
    observations_decay_seconds: ${MOLA_GUI_CLOUDS_DECAY_SECS|10.0} # For deskewed clouds above
    current_observation_point_size: ${MOLA_GUI_CURRENT_CLOUD_POINT_SIZE|2.0}
    current_observation_colormap: ${MOLA_GUI_CURRENT_CLOUD_COLORMAP|cmHOT} # mrpt::img::TColormap
    current_observation_color_by_field: ${MOLA_GUI_CURRENT_CLOUD_COLOR_FIELD|intensity}
    current_observation_alpha: 0.20
    show_gravity_align_vector: ${MOLA_GUI_SHOW_ESTIMATED_GRAVITY_VECTOR|false}

    background_color_gray_level: "${MOLA_GUI_BACKGROUND_GRAY_LEVEL|0.3}"

    show_localmap: ${MOLA_GUI_SHOW_LOCAL_MAP|true}
    local_map_point_size: 1

    show_ground_grid: ${MOLA_GUI_SHOW_GROUND_GRID|true}
    ground_grid_spacing: 5.0 # [m]
    current_pose_corner_size: ${MOLA_LO_CURRENT_POSE_CORNER_SIZE|1.5} # [m]
    #sensor_poses_corner_size: 0.5  # XYZ corner for each LiDAR sensor pose; 0 to disable
    show_current_pose_corner: ${MOLA_LO_SHOW_CURRENT_POSE_CORNER|true} # Set to false to hide the current-pose XYZ corner (e.g. for a first-person camera)

    # Robot /tf tree (e.g. a legged robot's joints). Requires the data source
    # to implement mola::TransformTreeSource (rosbag1/rosbag2 inputs and the
    # ROS 2 bridge do). All of these can also be toggled at runtime, in the
    # GUI's "View" tab.
    show_tf_tree: ${MOLA_LO_SHOW_TF_TREE|false}
    tf_tree_root_frame: ${MOLA_LO_TF_TREE_ROOT|''} # empty: use the data source's base_link frame
    tf_tree_corner_size: ${MOLA_LO_TF_TREE_CORNER_SIZE|0.1} # [m]
    tf_tree_show_links: ${MOLA_LO_TF_TREE_SHOW_LINKS|true}
    tf_tree_link_radius: ${MOLA_LO_TF_TREE_LINK_RADIUS|0.02} # [m], cylinder radius for tf links
    tf_tree_show_names: ${MOLA_LO_TF_TREE_SHOW_NAMES|false}
    tf_tree_exclude_frames: ${MOLA_LO_TF_TREE_EXCLUDE|''} # comma-separated; drops each frame and its subtree

    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}"

  # [m^-2] Minimum motion model covariance (in X,Y,Z) to consider a state estimation as valid as prior for ICP.
  min_motion_model_xyz_cov_inv: 1.0

  # 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}"

# re-localization method to use at start up:
initial_localization:
  method: "${MOLA_LO_INITIAL_LOCALIZATION_METHOD|InitLocalization::FixedPose}"
  # Format: x(m) y(m) z(m) yaw(deg) pitch(deg) roll(deg)
  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}"]
  # Seconds of accelerometer data to average for the initial pitch & roll.
  # A duration, not a sample count: what limits accuracy is platform motion
  # during the window, so the same setting must mean the same averaging time
  # on a 100 Hz and on a 400 Hz IMU.
  imu_initial_calibration_window_seconds: "${MOLA_LO_INITIAL_IMU_WINDOW|1.0}"
  # Sanity floor only, to reject a degenerate handful of samples:
  imu_initial_calibration_min_samples: "${MOLA_LO_INITIAL_IMU_MIN_SAMPLES|20}"
  # Defer initialization while the accelerometer directions in the window
  # disagree by more than this: such a window is measuring platform motion,
  # not gravity. After the timeout it is accepted anyway, so a start that is
  # never still still initializes.
  imu_initial_calibration_max_dispersion_deg: "${MOLA_LO_INITIAL_IMU_MAX_DISPERSION|1.5}"
  imu_initial_calibration_dispersion_timeout: "${MOLA_LO_INITIAL_IMU_DISPERSION_TIMEOUT|5.0}"
  use_imu_orientation: "${MOLA_LO_INITIAL_IMU_USE_ORIENTATION|true}"
  # Right after a re-localization (or initial localization), also hold off
  # local map / simplemap updates for this many timesteps (shares the same
  # recovery-window counter as additional_uncertainty_after_reloc_how_many_
  # timesteps above). 0 (default) disables this and is a no-op: this fires
  # on every initial-localization convergence too, not just explicit
  # relocalizations, so a nonzero default here would drop early keyframes
  # on any dataset where the vehicle is already moving at t=0.
  additional_map_freeze_after_reloc_how_many_timesteps: "${MOLA_LO_INIT_MAP_FREEZE_STEPS|0}"

  # Parameters for InitLocalization::FromStateEstimator:
  from_state_estimator_max_position_sigma: "${MOLA_LO_INIT_SE_MAX_POS_SIGMA|0.5}"
  from_state_estimator_max_orientation_sigma_deg: "${MOLA_LO_INIT_SE_MAX_ORI_SIGMA|3.0}"
  from_state_estimator_timeout: "${MOLA_LO_INIT_SE_TIMEOUT|60.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: ${MOLA_MAX_ICP_ITERATIONS|25}
    minAbsStep_trans: 1e-3
    minAbsStep_rot: 1e-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}

    # Post-optimization SE(3) covariance estimation (see mp2p_icp::covariance).
    # Censi3D is the realistic sandwich estimator for cov2cov pipelines;
    # the per-axis floor absorbs unmodelled errors and keeps downstream
    # filters numerically stable.
    covariance:
      method: ${MOLA_ICP_COVARIANCE_METHOD|Censi3D}
      defaultPointSigma: ${MOLA_ICP_COV_DEFAULT_POINT_SIGMA|0.01} # [m]
      floor_sigma_xyz: ${MOLA_ICP_COV_FLOOR_XYZ|0.001} # [m]
      floor_sigma_angles_deg: ${MOLA_ICP_COV_FLOOR_ANGLES_DEG|0.1} # [deg]

  solvers:
    - class: mp2p_icp::Solver_GaussNewton
      params:
        # Inner Gauss-Newton iterations per ICP iteration. Exposed so an
        # ablation can compare this solver against the annealed multi-iteration
        # one used by the point-to-point pipeline without editing YAML.
        maxIterations: ${MOLA_LO_SOLVER_MAX_ITERS|1}
        robustKernel: "${MOLA_LO_ROBUST_KERNEL|RobustKernel::GemanMcClure}"
        robustKernelParam: "${MOLA_LO_ROBUST_KERNEL_PARAM|6.0}" # In GICP, errors are normalized by covariance
        # Blend [0,1] for the robust kernel residual reference toward the prior
        # mean pose (0=current iterate only, 1=prior mean only). See mp2p_icp.
        robustKernelPriorRefBlend: "${MOLA_LO_ROBUST_KERNEL_PRIOR_REF_BLEND|0.0}"
        #innerLoopVerbose: true

        # Extra per-pairing weight by the geometry class of the map surface.
        # Disabled by default, and bit-exactly inert while the weights are all
        # ones: these numbers do NOT transfer between scene classes, so they
        # belong in a dataset profile and never in this file's defaults. Read
        # mp2p_icp's GeometryClassWeights docs before enabling; in particular
        # 'verticality' is gravity-referenced and needs a gravity source.
        geometry_class_weights:
          enabled: ${MOLA_GCW_ENABLED|false}
          variable: "${MOLA_GCW_VARIABLE|incidence}"
          gravity_source: "${MOLA_GCW_GRAVITY_SOURCE|map_frame}"
          softness: ${MOLA_GCW_SOFTNESS|10.0}
          # Block sequences, not flow ones: the YAML is parsed before the ${}
          # substitution runs, and a ${..|..} inside [ ] is a parse error.
          breakpoints:
            - ${MOLA_GCW_BREAKPOINT|60.0}
          weights:
            - ${MOLA_GCW_W_LOW|1.0}
            - ${MOLA_GCW_W_HIGH|1.0}

        # Sequence of one or more pairs (class, params) defining mp2p_icp::Matcher
        # instances to pair geometric entities between pointclouds.
  matchers:
    - class: mp2p_icp::Matcher_Cov2Cov
      params:
        threshold: "2.0*ADAPTIVE_THRESHOLD_SIGMA"
        # Optional range-adaptive matching distance. thresholdFar: 0 (the
        # default) keeps the flat threshold above, i.e. today's behaviour
        # exactly. When set, the matching distance ramps from `threshold` near
        # the sensor to `thresholdFar` beyond `thresholdKneeRange`.
        #
        # Why the axis exists: a pose-prediction error `dtheta` displaces a
        # point at range `r` by `dtheta*r`, so a flat threshold rejects far
        # returns first, precisely when the prediction is worst. Measured: at a
        # flat 0.34 m, 1 deg of yaw error takes beyond-25 m acceptance from 82%
        # to 4.6% while r<10 m stays at 99%; a 1.0 m far window keeps 94%. Those
        # far returns carry most of the rotation and along-track leverage.
        #
        # Note this is a no-op while ADAPTIVE_THRESHOLD_SIGMA sits at its
        # initial 0.5, where the near value already equals a 1.0 m far value:
        # it interacts with adaptive_threshold.initial_sigma and .min_motion and
        # cannot be swept independently of them.
        thresholdFar: "${MOLA_MATCH_THRESHOLD_FAR|0}"
        thresholdKneeRange: "${MOLA_MATCH_THRESHOLD_KNEE|15.0}"
        thresholdTransitionWidth: "${MOLA_MATCH_THRESHOLD_WIDTH|5.0}"
        pairingsPerPoint: 1
        allowMatchAlreadyMatchedGlobalPoints: true # faster
        # Both layers here must be of the very same cov-capable map class, the
        # one selected below by ${MOLA_LOCALMAP_CLASS}.
        layerMatches:
          - { global: "${MOLA_LOCALMAP_LAYER_NAME|localmap}", local: "observation" }

  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: "${MOLA_LOCALMAP_LAYER_NAME|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/stable/group_mrpt_maps_grp.html
        #
        # Two local-map classes are supported here, both implementing
        # mp2p_icp::NearestPointWithCovCapable (required by Matcher_Cov2Cov):
        #
        #  - mola::KeyframePointCloudMap (default): keyframe-based, points kept
        #    in per-KF local frames, so it survives loop-closure re-mapping.
        #    Required for loop-closure-capable SLAM.
        #
        #  - mola::IncrementalPointCloud: single global frame, one incremental
        #    self-balancing k-d tree updated in place instead of rebuilt on
        #    every scan. ODOMETRY ONLY: a global SE(3) re-map would force a full
        #    rebuild, so do not combine it with loop closure. Needs
        #    mola_metric_maps built against nanoflann >= 1.10.
        #
        #    To try it (the class applies to the 'observation' layer below too,
        #    since Matcher_Cov2Cov pairs the two):
        #
        #      MOLA_LOCALMAP_CLASS=mola::IncrementalPointCloud \
        #      ros2 launch mola_lidar_odometry ros2-lidar-odometry.launch.py [...]
        #
        #    Tuned by the MOLA_INCREMENTAL_MAP_* variables below. See
        #    https://docs.mola-slam.org/latest/mola_lo_pipelines.html
        #
        # Option keys not understood by the selected class are simply ignored,
        # so both sets can coexist below.
        class: ${MOLA_LOCALMAP_CLASS|mola::KeyframePointCloudMap}
        plugin: "libmola_metric_maps.so" # Import additional custom user-defined map classes (search in LD_LIBRARY_PATH)
        creationOpts:
          # --- common to both classes ---
          k_correspondences_for_cov: ${MOLA_LOCALMAP_K_CORRESPONDENCES_FOR_COV|20}
          min_correspondences_for_cov: ${MOLA_LOCALMAP_MIN_CORRESPONDENCES_FOR_COV|5}
          max_distance_for_cov: ${MOLA_LOCALMAP_MAX_DISTANCE_FOR_COV|2.0}
          # 0 disables the planarity test, which is the shipped behavior.
          # See mola::KeyframePointCloudMap's docs before enabling it.
          max_plane_deviation_for_cov: ${MOLA_LOCALMAP_MAX_PLANE_DEV_FOR_COV|0}
          plane_regularization_lambda: ${MOLA_LOCALMAP_PLANE_REG_LAMBDA|0.001}
          # --- mola::IncrementalPointCloud only ---
          # Half side [m] of the cube of points kept around the robot on each
          # insertion (Chebyshev distance, as in HashedVoxelPointCloud's
          # remove_voxels_farther_than). Unlike the keyframe map, EVERY point
          # inside this cube is kept in a single tree, so this is
          # a much tighter budget than 'remove_frames_farther_than' below
          remove_points_farther_than: "${MOLA_INCREMENTAL_MAP_MAX_SIZE|$f{max(100.0, 1.5*ESTIMATED_OBSERVATION_RADIUS)}}" # [m]
          # Run the k-d tree balancing rebuilds on a background thread, so the
          # mapping thread never pays for them. Measured on Oxford Spires, this
          # takes local-map insertion from mean 28 ms / max 371 ms (with 9 calls
          # over 200 ms) down to mean 10 ms / max 38 ms and no spike at all:
          async_rebuild: ${MOLA_INCREMENTAL_MAP_ASYNC_REBUILD|true}
          alpha_balance: ${MOLA_INCREMENTAL_MAP_ALPHA_BALANCE|0.75}
          alpha_deleted: ${MOLA_INCREMENTAL_MAP_ALPHA_DELETED|0.5}
          # Pre-allocated point storage. With async_rebuild this also keeps the
          # mapping thread from ever having to wait for the background worker,
          # which it must do before the point buffers can be reallocated:
          reserve_points: ${MOLA_INCREMENTAL_MAP_RESERVE_POINTS|2000000}
          # --- mola::KeyframePointCloudMap only ---
          max_search_keyframes: ${MOLA_LOCALMAP_MAX_SEARCH_KEYFRAMES|3}
          use_view_direction_filter: ${MOLA_LOCALMAP_USE_VIEW_DIRECTION_FILTER|true}
          max_view_angle_deg: ${MOLA_LOCALMAP_VIEW_DIRECTION_FILTER_ANGLE_DEG|120}
          rotation_distance_weight: 2.0
          num_diverse_keyframes: ${MOLA_LOCALMAP_DIVERSE_KEYFRAMES|1} # Must be < max_search_keyframes
          # If true, cov-to-cov ICP matching (Matcher_Cov2Cov) queries each active
          # keyframe's own cached KD-tree + covariances directly, instead of building
          # a merged submap from the active KF set. Faster (skips the merge/KD-tree
          # rebuild) at the cost of a slightly less accurate covariance estimate
          # (computed only from neighbors within the same source keyframe).
          approximate_cov: ${MOLA_LOCALMAP_APPROXIMATE_COV|false}
        insertOpts:
          remove_frames_farther_than: "${MOLA_LOCAL_MAP_MAX_SIZE|$f{max(100.0, 1.50*ESTIMATED_OBSERVATION_RADIUS)}}" # [m]
        likelihoodOpts: ~ # none required
        renderOpts:
          color.A: 0.25 # [0,1] Use this alpha value for points, RGB from colormap
          colormap: "cmHOT" # cmJET, cmHOT, cmGRAYSCALE
          recolorByPointField: ${MOLA_GUI_LOCAL_MAP_COLOR_BY_COORDINATE|intensity} # x,y,z,ring, intensity, ambient, etc.
          max_points_per_kf: ${MOLA_LOCALMAP_VIZ_MAX_POINTS_PER_KF|10000} # Max number of points to render per keyframe
          max_overall_points: ${MOLA_LOCALMAP_VIZ_MAX_POINTS_OVERALL|500000} # Max number of points to render overall (e.g. to avoid FoxGlove WS overflow)
          #point_size: 1.0  # superseded by visualization.local_map_point_size above

          # ---------------------------------------------------------------------------------
          # 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:
      name: "Generator (raw)"
      target_layer: "raw"
      throw_on_unhandled_observation_class: true
      process_class_names_regex: ".*"
      process_sensor_labels_regex: ".*"

  # This just creates an empty layer of the local map class: Matcher_Cov2Cov
  # pairs it against the local map, so both must be of the same class.
  - class_name: mp2p_icp_filters::Generator
    params:
      name: "Generator (obs)"
      target_layer: "observation"
      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/stable/group_mrpt_maps_grp.html
        class: ${MOLA_LOCALMAP_CLASS|mola::KeyframePointCloudMap}
        plugin: "libmola_metric_maps.so" # Import additional custom user-defined map classes (search in LD_LIBRARY_PATH)
        # The per-point covariances of the SCAN side of a cov-to-cov pairing are
        # built here, and they are half of the pairing weight
        # `(C_global + C_local)^-1`. Leaving this empty used the class defaults
        # (k=20, min=5, max_distance=1.0 m), which do NOT match what the local
        # map above is configured with, so the two sides of the same pairing
        # were estimated over different neighborhood scales.
        #
        # A scan is much sparser than the accumulated map, so a radius that is
        # ample for the map can leave a scan point with fewer than
        # `min_correspondences_for_cov` neighbors. That case does not drop the
        # pairing: it falls back to an isotropic covariance, and the pairing
        # stays in the sum as an almost-isotropic point-to-point constraint
        # instead of the plane constraint the cov-to-cov form is meant to give.
        #
        # `max_distance_for_cov` defaults to 2.0 here, NOT to the map class's
        # own 1.0: measured over 115M pairings on Oxford Spires, 32.3% of
        # pairings could not find `min_correspondences_for_cov` neighbors
        # inside 1.0 m and fell back to an isotropic covariance, which leaves
        # the pairing in the sum as a point-to-point constraint rather than a
        # plane one. Full-corpus A/B at 2.0 m: pooled APE x0.785 over 46
        # sequences, better on six of seven datasets (oxford x0.538,
        # conslam x0.622, botanic x0.696, kitti x0.912, grand-tour x0.930,
        # tiers x0.992) and no new failures.
        #
        # citrus-farm is the exception at x1.201, and its profile overrides
        # this back to 1.0. Working hypothesis: dense close-range orchard
        # foliage has no consistent surface, so a wider neighborhood averages
        # the normal over vegetation instead of structure.
        creationOpts:
          # Required by the map class loader; the class default, and inert for a
          # single-keyframe observation layer.
          max_search_keyframes: 3
          k_correspondences_for_cov: ${MOLA_OBSLAYER_K_CORRESPONDENCES_FOR_COV|20}
          min_correspondences_for_cov: ${MOLA_OBSLAYER_MIN_CORRESPONDENCES_FOR_COV|5}
          max_distance_for_cov: ${MOLA_OBSLAYER_MAX_DISTANCE_FOR_COV|2.0}
          max_plane_deviation_for_cov: ${MOLA_OBSLAYER_MAX_PLANE_DEV_FOR_COV|0}
          plane_regularization_lambda: ${MOLA_OBSLAYER_PLANE_REG_LAMBDA|0.001}
        insertOpts: ~
        likelihoodOpts: ~ # none required
        renderOpts: ~

# 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: "${MOLA_SCAN_POINT_STAMPS_ADJUST_METHOD|TimestampAdjustMethod::MiddleIsZero}"

# Path to an (optional) user-customizable pipeline definition file. Default: empty = none.
observations_prefilter_file: ${MOLA_LO_OBS_PREFILTER_PIPELINE_FILE|""}

# Early deskew of the full raw cloud.
# Runs once; result is reused for decimation, viz, publish, simplemap.
# Skipped at runtime when !hasIMU && optimize_twist (fallback to 2nd-pass deskew).
observations_deskew_pass:
  # Drop near returns to remove "noise" from the vehicle body, a person
  # standing next to the robot (e.g. with a joystick), etc., and clamp
  # very far returns where small angular errors blow up into large
  # translational errors. metric_l_infinity → axis-aligned cube; center
  # is unset → anchored at base_link (the vehicle origin), *not* at the
  # sensor — this is intentional so the deadzone covers the whole
  # vehicle footprint regardless of where the sensor is mounted on it.
  # If you also need a sensor-anchored cut (e.g. the sensor's blind
  # sphere when it is mounted significantly off base_link), add an
  # extra filter via observations_prefilter_file.
  - class_name: mp2p_icp_filters::FilterByRange
    params:
      input_pointcloud_layer: "raw"
      output_layer_between: "raw_range_filtered"
      range_min: "${MOLA_MINIMUM_RANGE_FILTER|max(1.0, 0.03*ESTIMATED_OBSERVATION_RADIUS)}"
      range_max: "${MOLA_MAXIMUM_RANGE_FILTER|1.2*ESTIMATED_OBSERVATION_RADIUS}"
      metric_l_infinity: true

  - class_name: mp2p_icp_filters::FilterDeskew
    params:
      name: "FilterDeskew (early)"
      input_pointcloud_layer: "raw_range_filtered"
      output_pointcloud_layer: "deskewed"
      method: "${MOLA_DESKEW_METHOD|MotionCompensationMethod::Linear}"
      ignore_accelerometer: "${MOLA_DESKEW_IGNORE_ACCELEROMETER|false}"
      silently_ignore_no_timestamps: ${MOLA_IGNORE_NO_POINT_STAMPS|true} # To handle more dataset types
      output_layer_class: "mrpt::maps::CGenericPointsMap" # 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::FilterDeleteLayer
    params:
      pointcloud_layer_to_remove: ["raw_range_filtered"]

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::FilterDecimateAdaptive
    params:
      name: "FilterDecimateAdaptive (map)"
      input_pointcloud_layer: "deskewed"
      output_pointcloud_layer: "decimated_for_map"
      # Per stage since 2026-08-15. The two stages want different cell sizes:
      # a coarser map voxel trades a little short-range precision for markedly
      # less drift accumulation, and no single value expresses both.
      # NOTE: this replaces MOLA_CLOUD_DECIMATION_VOXEL_SIZE, which set both
      # stages at once. Setting the old name now has no effect: to reproduce a
      # run recorded against it, set both variables below to that value.
      voxel_size: "${MOLA_CLOUD_DECIMATION_VOXEL_SIZE_MAP|0.15}" # [m]
      # A floor, not the target: the point count that matters is relative to
      # the number of occupied voxels, which maximum_voxel_stride bounds.
      desired_output_point_count: "${MOLA_DECIMATED_POINTS_MAP|10000}"
      maximum_voxel_stride: "${MOLA_VOXEL_STRIDE_MAP|0}" # 0=off; 1=visit every occupied voxel
      decimate_method: "${MOLA_DECIMATE_METHOD|DecimateMethod::FirstPoint}"
      parallelization_grain_size: 8000 # When TBB is enabled, the grainsize for splitting the input clouds into threads

  - class_name: mp2p_icp_filters::FilterDecimateAdaptive
    params:
      name: "FilterDecimateAdaptive (icp)"
      input_pointcloud_layer: "decimated_for_map"
      output_pointcloud_layer: "decimated_for_icp"
      voxel_size: "${MOLA_CLOUD_DECIMATION_VOXEL_SIZE_ICP|0.10}" # [m]
      desired_output_point_count: "${MOLA_DECIMATED_POINTS_ICP|3000}"
      maximum_voxel_stride: "${MOLA_VOXEL_STRIDE_ICP|0}" # 0=off; 2=visit one occupied voxel in two
      decimate_method: "${MOLA_DECIMATE_METHOD|DecimateMethod::FirstPoint}"
      parallelization_grain_size: 8000 # When TBB is enabled, the grainsize for splitting the input clouds into threads

# 2nd pass:
observations_filter_2nd_pass:
  # The merge below appends to "observation", so this pass has to start from an
  # empty layer: with optimize_twist enabled the whole pass re-runs after a
  # twist correction, and a second append leaves the cov-capable observation
  # map holding two keyframes, which the cov-to-cov nearest-neighbor search
  # rejects outright.
  - class_name: mp2p_icp_filters::FilterClear
    params:
      target_layer: "observation"

  - class_name: mp2p_icp_filters::FilterMerge
    params:
      name: "FilterMerge (icp into obs)"
      input_pointcloud_layer: "decimated_for_icp"
      target_layer: "observation"

# 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", "deskewed"]

# 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:
      name: "FilterMerge (map into localmap)"
      input_pointcloud_layer: "decimated_for_map"
      target_layer: "${MOLA_LOCALMAP_LAYER_NAME|localmap}"
      input_layer_in_local_coordinates: true
      robot_pose: [robot_x, robot_y, robot_z, robot_yaw, robot_pitch, robot_roll]

# Pipeline starting from raw observations (passed through "Generator"), to be
# sent out for visualization of the sliding-window of recent clouds as a dense local map
# This is ONLY run when not using "use_early_deskew", that is: not using IMU, and enabling "optimize_twist"
observations_filter_deskew_for_visualization:
  - class_name: mp2p_icp_filters::FilterByRange
    params:
      input_pointcloud_layer: "raw"
      output_layer_between: "raw_filtered"
      range_min: "${MOLA_MINIMUM_RANGE_FILTER|max(1.0, 0.03*ESTIMATED_OBSERVATION_RADIUS)}"
      range_max: 1.2*ESTIMATED_OBSERVATION_RADIUS
      metric_l_infinity: true

  - class_name: mp2p_icp_filters::FilterDeskew
    params:
      name: "FilterDeskew (viz)"
      input_pointcloud_layer: "raw_filtered"
      output_pointcloud_layer: "viz"
      method: "${MOLA_DESKEW_METHOD|MotionCompensationMethod::Linear}"
      ignore_accelerometer: "${MOLA_DESKEW_IGNORE_ACCELEROMETER|false}"
      silently_ignore_no_timestamps: ${MOLA_IGNORE_NO_POINT_STAMPS|true} # To handle more dataset types
      output_layer_class: "mrpt::maps::CGenericPointsMap" # Keep intensity only

      # 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]

  # Remove raw layer so it's not visible
  - class_name: mp2p_icp_filters::FilterDeleteLayer
    params:
      pointcloud_layer_to_remove: ["raw", "raw_filtered"]

2. ICP pipeline for 3D LiDAR (`lidar3d-icp.yaml`)

This was the reference configuration used for most examples in the MOLA-LO paper [BC25], and should work great out of the box for most common situations; although as of Oct 2025, the newer pipeline lidar3d-gicp.yaml is now the default and recommended in general.

As described in the paper [BC25], this pipeline 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

# =====================================================================================
# Pipeline: GICP (Generalized ICP) with cov-to-cov pairings
#
# For paper references, see https://github.com/MOLAorg/mola_lidar_odometry/
#
# 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:
# =====================================================================================

# TODO: Once mola_yaml >=3.0.0 for all ros2 distros, simplify all yaml with $import and common files

params:
  pipeline_name: "GICP (Generalized ICP) with cov-to-cov pairings" # For display/debug only

  # These sensor labels will be handled as LIDAR observations:
  # Can be overridden 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 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]

  # Whether a scan arriving while the worker is still busy replaces the one
  # already queued (real-time behavior) or waits for it (lossless). Set to false
  # for offline batch runs, where every scan must be processed.
  drop_stale_scans: ${MOLA_DROP_STALE_SCANS|true}

  # Parameters for max sensor range automatic estimation:
  observation_radius_filter_coefficient: 0.95
  absolute_minimum_observation_radius: ${MOLA_ABS_MIN_SENSOR_RANGE|5.0}
  # Quantile of the per-point norms taken as ESTIMATED_OBSERVATION_RADIUS.
  # 1.0 = the bounding-box max-norm, i.e. one far return sets the scene
  # scale for the six parameters derived from it. Default unchanged;
  # exposed so the robust alternative can be measured.
  observation_radius_quantile: ${MOLA_OBSERVATION_RADIUS_QUANTILE|1.0}

  # If enabled, vehicle twist will be optimized during ICP
  # enabling better and more robust odometry in high dynamics motion without an IMU.
  # NOTE: Disabled for more efficient deskew in the GICP pipeline with IMU.
  # Enabling it costs about 25% more time per scan. On KITTI, which has no IMU
  # and so no other source of velocity, it still bought nothing measurable, so
  # do not assume it helps just because a platform lacks an IMU: measure it.
  optimize_twist: ${MOLA_OPTIMIZE_TWIST|false}

  # IMU accelerometer-based verticality correction:
  # Uses averaged accelerometer readings to constrain ICP pitch/roll.
  imu_gravity_correction:
    enabled: ${MOLA_IMU_GRAVITY_CORRECTION|true}
    # Yaw-free, rank-2 verticality constraint solved by mp2p_icp. Set to false
    # only to reproduce results from the legacy path, which folded tilt into
    # the SE(3) pose prior.
    use_rank2_prior: ${MOLA_IMU_GRAVITY_RANK2|true}
    # Widen sigma_deg by the measured dispersion of the accelerometer
    # directions, so the constraint stands down when the readings are not
    # actually gravity (braking, cornering, vibration).
    adaptive_sigma: ${MOLA_IMU_GRAVITY_ADAPTIVE_SIGMA|true}
    sigma_deg: ${MOLA_IMU_GRAVITY_SIGMA_DEG|2.0}
    averaging_samples: ${MOLA_IMU_GRAVITY_AVG_SAMPLES|20}
    max_age_seconds: ${MOLA_IMU_GRAVITY_MAX_AGE|2.0}
    # The one-shot map-origin verticality capture defines the map's vertical
    # for the whole run, so it is deferred to a later scan while the buffered
    # accelerometer directions disagree by more than this (i.e. the reading is
    # motion, not gravity), and taken as-is after the timeout.
    map_origin_max_dispersion_deg: ${MOLA_IMU_GRAVITY_MAP_ORIGIN_MAX_DISPERSION|1.0}
    map_origin_capture_timeout: ${MOLA_IMU_GRAVITY_MAP_ORIGIN_TIMEOUT|3.0}
    # NOTE: averaging_samples must fit inside max_age_seconds at the actual IMU
    # rate, or estimatedPitchRoll() returns nothing and the constraint is
    # silently inactive (at 400 Hz, 2.0 s holds only ~800 samples).
    #
    # Take the verticality reading from an odometry source's ABSOLUTE attitude
    # instead of the accelerometer. Only the "up" axis is used: the source's
    # frame differs from the map frame by an unknown yaw and translation, and
    # neither moves the vertical, so that one direction transfers exactly while
    # the position and heading do not.
    #
    # An accelerometer only measures gravity while quasi-static, so on a
    # platform that accelerates continuously `adaptive_sigma` correctly stands
    # the constraint down nearly always, leaving the vertical unconstrained. A
    # kinematic-inertial estimator on the platform has no such limitation.
    # The map-origin reference is captured from the same source, so the
    # constant offset between the two verticals stays a gauge instead of
    # becoming a growing tilt.
    odometry_attitude:
      enabled: ${MOLA_ODOM_VERTICALITY|false}
      # The VALUE here must equal the label the odometry observation is
      # published under, which the shipped launch files set through their own
      # MOLA_ODOM_SENSOR_LABEL variable; the two variables are separate on
      # purpose, since the verticality source need not be the fused one. That
      # observation must also be a CObservationRobotPose: planar odometry has
      # no pitch or roll to offer.
      sensor_label: "${MOLA_ODOM_VERTICALITY_LABEL|odom_wheels}"
      sigma_deg: ${MOLA_ODOM_VERTICALITY_SIGMA_DEG|1.0}
      max_age_seconds: ${MOLA_ODOM_VERTICALITY_MAX_AGE|0.5}

    # Estimate the map-frame vertical online instead of freezing it from one
    # accelerometer average at the first keyframe. See
    # mola::imu::MapGravityEstimator: it solves for gravity in the map frame
    # (plus IMU biases) from preintegrated IMU and this odometry's own relative
    # attitudes/velocities, so platform acceleration cancels and no
    # quasi-static window is required.
    map_gravity:
      enabled: ${MOLA_IMU_MAP_GRAVITY|false}
      # Compute and log the estimate without letting it affect the verticality
      # reference, so it can be scored against ground truth on a new dataset
      # without its own feedback contaminating the map frame it is estimating.
      log_only: ${MOLA_IMU_MAP_GRAVITY_LOG_ONLY|false}
      solve_every_n: ${MOLA_IMU_MAP_GRAVITY_SOLVE_EVERY_N|5}
      # Rotate the MAP FRAME itself, once, so it becomes gravity-aligned,
      # instead of only feeding the per-scan verticality prior. The map frame is
      # the initial body frame, so a platform that starts tilted produces a map
      # that leans by that tilt forever: measured on a handheld dataset, a
      # median of 8.7 deg (max 20.7). This is a gauge change - local map,
      # simplemap, trajectory, state estimator and the published `odom` frame
      # all rotate together about the map origin - so no relative quantity
      # moves. OFF by default: it changes the frame every product of the run is
      # expressed in.
      relevel_map_frame: ${MOLA_IMU_MAP_GRAVITY_RELEVEL|false}
      # Readiness for the re-level, as an interval count. Note this is NOT
      # min_intervals_for_convergence below: that one gates the per-scan prior,
      # where the later, more settled estimate is what matters. For leveling the
      # map once, the estimate is at its BEST at the first solve (measured 0.72
      # deg at ~5.6 s) and slowly degrades from there, so waiting costs accuracy.
      # 5 intervals is the first solve under solve_every_n above.
      relevel_min_intervals: ${MOLA_IMU_MAP_GRAVITY_RELEVEL_MIN_INTERVALS|5}
      # Magnitude gate: only re-level when there is enough tilt that removing it
      # beats the error of the estimate doing the removing. Measured at the
      # firing point on a handheld dataset, that error is 0.63 deg median,
      # 1.43 deg p90, 1.75 deg worst. Note the gate tests the ESTIMATE while the
      # quantity that must be large is the TRUE tilt, and the two differ by
      # exactly that error, so the threshold is ~2x the p90 rather than 1x:
      # gating at 2 deg let one near-level sequence through (estimate 2.8 deg
      # against a true 1.1 deg) and made it 0.7 deg worse. Below the threshold
      # the re-level stands down permanently (and says so in the log) instead of
      # retrying, which is what keeps an already-level dataset untouched.
      relevel_min_tilt_deg: ${MOLA_IMU_MAP_GRAVITY_RELEVEL_MIN_TILT|3.0}
      # NOTE: there is deliberately no ACCURACY threshold here. The estimator
      # reports every usable estimate with its earned pitch/roll sigma, which is
      # added in quadrature to the gravity prior's sigma.
      #
      # CAUTION: "a weak estimate silences itself" is NOT established. A later
      # measurement on a handheld dataset put the error/sigma ratio at 8.4
      # median and 19.0 worst case, i.e. the reported sigma is roughly an order
      # of magnitude tighter than the estimate is accurate, and no better
      # calibrated after the interval gate below than before it. Do not use
      # those sigmas as a confidence gate until that is understood; the
      # re-level trigger above deliberately does not.
      #
      # There IS a data-quantity precondition, which is a different thing. Until
      # the window holds enough intervals the solution is pulled by the |g|
      # constraint and the bias priors, and that pull is a BIAS the linearized
      # covariance cannot see, so the estimate is not merely uncertain, it is
      # confidently wrong. Until then the accelerometer capture is the better
      # reference, and this keeps LO on it. This gates the PER-SCAN PRIOR only:
      # the one-off map-frame re-level above has the opposite need (its estimate
      # is best at the first solve) and uses its own, much shorter, gate.
      min_intervals_for_convergence: ${MOLA_IMU_MAP_GRAVITY_MIN_INTERVALS|130}
      # Sliding-window length. The whole point is that verticality information
      # ACCUMULATES, so this wants to be much longer than the estimator default.
      window_size: ${MOLA_IMU_MAP_GRAVITY_WINDOW|200}
      min_interval_seconds: ${MOLA_IMU_MAP_GRAVITY_MIN_INTERVAL|1.0}
  # When publishing pose updates, the reference frame for both, estimated robot poses, and the local map.
  publish_reference_frame: "${MOLA_LO_PUBLISH_REF_FRAME|odom}"

  # When publishing pose updates, the vehicle frame name.
  publish_vehicle_frame: "${MOLA_LO_PUBLISH_VEHICLE_FRAME|base_link}"

  # If enabled, deskewed scans will be published (so, they will be available as ROS2 messages), mostly for visualization.
  # This may slow-down the system, so it is disabled by default.
  publish_deskewed_scans: "${MOLA_LO_PUBLISH_DESKEWED_SCANS|false}"

  # How often to update the local map model:
  local_map_updates:
    enabled: "${MOLA_MAPPING_ENABLED|true}"
    load_existing_local_map: ${MOLA_LOAD_MM|""}
    load_map_after_gui_init: ${MOLA_LO_LOAD_MAP_AFTER_GUI|false}
    save_final_local_map: ${MOLA_SAVE_MM|""} # If not empty, saves the final local metric map to a ".mm" file

    # 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_OBSERVATION_RADIUS}" # [m]
    min_rotation_between_keyframes: "${MOLA_MIN_ROT_BETWEEN_MAP_UPDATES|(15 + sqrt(wx^2+wy^2+wz^2)*5 )}" # [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_OBSERVATION_RADIUS)}" # [m]
    check_for_removal_every_n: 100
    min_nearby_poses_occupied: ${MOLA_MIN_NEARBY_POSES_OCCUPIED|1}
    publish_map_updates_every_n: ${MOLA_PUBLISH_LOCAL_MAP_UPDATES_EVERY_N|40}

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

  # If defined, ".icplog" files will be saved if ICP quality drops below the given threshold.
  # Useful to debug mapping issues. If empty, only the env var MP2P_ICP_GENERATE_DEBUG_FILES and the icp.params setting will define when to save logs.
  write_debug_icp_log_if_quality_under: ${MOLA_WRITE_DEBUG_ICP_LOG_IF_QUALITY_UNDER|""}

  # Adaptive threshold:
  adaptive_threshold:
    enabled: true
    initial_sigma: ${MOLA_SIGMA_INITIAL|0.50} # [m]
    min_motion: ${MOLA_SIGMA_MIN_MOTION|0.5} # [m]
    maximum_sigma: ${MOLA_SIGMA_MAX_MOTION|2.00} # [m]
    max_sigma_step: ${MOLA_SIGMA_MAX_STEP|0.05} # [m]
    icp_quality_controller_setpoint: ${MOLA_SIGMA_CONTROLLER_QUALITY_SETPOINT|0.85}
    kp: ${MOLA_SIGMA_CONTROLLER_GAIN|2.0}
    alpha: ${MOLA_ADAPT_THRESHOLD_ALPHA|0.90}
    # Sustained-failure recovery: if enabled, sigma is grown multiplicatively
    # after a streak of bad ICPs, capped at maximum_sigma, so the matcher
    # window can re-open and ICP can recover. Enabled by default: without it,
    # once sigma is driven down near min_motion by a run of easy/near-static
    # scans (e.g. goodness consistently above icp_quality_controller_setpoint),
    # a single larger inter-scan motion (a turn, a bump, or just ordinary
    # scan-to-scan variability once sigma is already pinned at its floor) can
    # push ICP into failure, and with sigma frozen the correspondence search
    # window never reopens, so the pipeline stalls indefinitely
    # (estimated_trajectory never grows again). Set to false to restore the
    # old behavior. `maximum_sigma` must be set strictly above
    # `initial_sigma`, or this mechanism is a no-op: sigma always starts
    # each run AT initial_sigma, so a bad ICP on frame 1 (e.g. a slightly
    # imprecise localization-only seed pose against a prebuilt map) has no
    # room to grow into and can never recover (observed in testing as ICP
    # goodness stuck just under min_icp_goodness for an entire run).
    #
    # recover_after_n_bad/recover_growth_factor default to a fast reaction
    # (2 bad frames, x2.0 growth) rather than a slow one (5 bad frames,
    # x1.5): every frame spent stuck is a frame of real, untracked vehicle
    # motion accumulating; the slower defaults let that gap grow to the
    # point where, once the search window finally reopens, ICP can lock onto
    # a self-consistent but WRONG registration (observed in testing as a
    # sudden ~30-40 deg yaw error that then persisted for the rest of a run)
    # instead of recovering the true pose.
    recover_on_sustained_failure: ${MOLA_ADAPT_THRESHOLD_RECOVER|true}
    recover_after_n_bad: ${MOLA_ADAPT_THRESHOLD_RECOVER_AFTER_N_BAD|2}
    recover_growth_factor: ${MOLA_ADAPT_THRESHOLD_RECOVER_GROWTH_FACTOR|2.0}

  # REP-107 diagnostics thresholds published on /diagnostics via mola_bridge_ros2.
  # Any of these can be omitted to keep its built-in default.
  diagnostics:
    icp_quality_warn: 0.30 # [0-1] WARN if icp_quality below this
    icp_quality_error: 0.10 # [0-1] ERROR if icp_quality below this
    input_stale_sec: 3.0 # [s]   STALE if no observation for this long
    input_error_sec: 5.0 # [s]   ERROR if no observation for this long
    dropped_ratio_warn: 0.20 # [0-1] WARN if dropped-frames ratio exceeds
    dropped_ratio_error: 0.50 # [0-1] ERROR if dropped-frames ratio exceeds
    timing_utilization_warn: 0.80 # [0-1] WARN if avg process time / sensor period exceeds

  # 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 overridden with CLI flag --output-simplemap
    load_existing_simple_map: ${MOLA_LOAD_SM|""}

    save_final_map_to_file: ${MOLA_SIMPLEMAP_OUTPUT|'final_map.simplemap'}

    # NOTE: unlike local_map_updates above, these thresholds are NOT scaled by
    # angular velocity: this is what feeds the SharedKeyframeMap sink (e.g.
    # mola_mapper), and the old w-scaled formula (rotation threshold growing
    # by 500 deg per rad/s) made it create close to NO keyframes while
    # smoothly turning, starving loop closure of the keyframes it needs.
    min_translation_between_keyframes: "${MOLA_SIMPLEMAP_MIN_XYZ|(1.0e-2 + sqrt(wx^2+wy^2+wz^2)*0.1)*ESTIMATED_OBSERVATION_RADIUS}" # [m]
    min_rotation_between_keyframes: "${MOLA_SIMPLEMAP_MIN_ROT|(15 + sqrt(wx^2+wy^2+wz^2)*5 )}" # [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.
    min_nearby_poses_occupied: ${MOLA_SIMPLEMAP_MIN_NEARBY_POSES|1}
    # Revisiting an already-mapped area creates NO keyframes with the purely
    # spatial criterion above, which starves loop closure of the second endpoint
    # of the loop. Set to a positive value [s] so only keyframes newer than that
    # take part in the "is there one here already?" test.
    nearby_keyframe_time_window: ${MOLA_SIMPLEMAP_KF_TIME_WINDOW|0} # [s], 0=disabled
    save_gnss_max_age: 1.0 # [s] max age of GNSS observations to keep in the keyframe

    # If enabled, this will store deskewed scans into the keyframes of simplemaps.
    save_deskewed_scans: ${MOLA_SAVE_DESKEWED_SCANS|false}

  # 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: ${MOLA_GUI_MAP_UPDATE_DECIMATION|10}
    show_trajectory: ${MOLA_GUI_SHOW_TRAJECTORY|true}
    trajectory_rgba: [0.1, 0.1, 0.1, 1.0]

    # Camera tracks the vehicle. Exposed as an env hook so an integrator (e.g.
    # mola_mapper, where the mapper module owns the camera) can disable it here
    # without editing this pipeline. Default true (standalone LIO behavior).
    camera_follows_vehicle: ${MOLA_GUI_CAMERA_FOLLOWS_VEHICLE|true}

    # Enable or disable each of the three GUI panels:
    show_tab_status: ${MOLA_GUI_SHOW_TAB_STATUS|true}
    show_tab_control: ${MOLA_GUI_SHOW_TAB_CONTROL|true}
    show_tab_view: ${MOLA_GUI_SHOW_TAB_VIEW|true}

    show_current_observation: ${MOLA_GUI_SHOW_CURRENT_OBS|false} # shows "live deskewed" LiDAR points
    show_last_deskewed_observations_decay: ${MOLA_GUI_SHOW_DESKEWED_DECAY|true} # shows "live deskewed" LiDAR points over time
    last_deskewed_observations_point_size: ${MOLA_GUI_LAST_CLOUDS_POINT_SIZE|1.0}
    last_deskewed_observations_colormap: ${MOLA_GUI_LAST_CLOUDS_COLORMAP|cmJET} # mrpt::img::TColormap
    last_deskewed_observations_color_by_field: ${MOLA_GUI_LAST_CLOUDS_COLOR_FIELD|intensity}
    observations_initial_alpha: 0.10
    observations_decay_seconds: ${MOLA_GUI_CLOUDS_DECAY_SECS|10.0} # For deskewed clouds above
    current_observation_point_size: ${MOLA_GUI_CURRENT_CLOUD_POINT_SIZE|2.0}
    current_observation_colormap: ${MOLA_GUI_CURRENT_CLOUD_COLORMAP|cmHOT} # mrpt::img::TColormap
    current_observation_color_by_field: ${MOLA_GUI_CURRENT_CLOUD_COLOR_FIELD|intensity}
    current_observation_alpha: 0.20
    show_gravity_align_vector: ${MOLA_GUI_SHOW_ESTIMATED_GRAVITY_VECTOR|false}

    background_color_gray_level: "${MOLA_GUI_BACKGROUND_GRAY_LEVEL|0.3}"

    show_localmap: ${MOLA_GUI_SHOW_LOCAL_MAP|true}
    local_map_point_size: 1

    show_ground_grid: ${MOLA_GUI_SHOW_GROUND_GRID|true}
    ground_grid_spacing: 5.0 # [m]
    current_pose_corner_size: ${MOLA_LO_CURRENT_POSE_CORNER_SIZE|1.5} # [m]
    #sensor_poses_corner_size: 0.5  # XYZ corner for each LiDAR sensor pose; 0 to disable
    show_current_pose_corner: ${MOLA_LO_SHOW_CURRENT_POSE_CORNER|true} # Set to false to hide the current-pose XYZ corner (e.g. for a first-person camera)

    # Robot /tf tree (e.g. a legged robot's joints). Requires the data source
    # to implement mola::TransformTreeSource (rosbag1/rosbag2 inputs and the
    # ROS 2 bridge do). All of these can also be toggled at runtime, in the
    # GUI's "View" tab.
    show_tf_tree: ${MOLA_LO_SHOW_TF_TREE|false}
    tf_tree_root_frame: ${MOLA_LO_TF_TREE_ROOT|''} # empty: use the data source's base_link frame
    tf_tree_corner_size: ${MOLA_LO_TF_TREE_CORNER_SIZE|0.1} # [m]
    tf_tree_show_links: ${MOLA_LO_TF_TREE_SHOW_LINKS|true}
    tf_tree_link_radius: ${MOLA_LO_TF_TREE_LINK_RADIUS|0.02} # [m], cylinder radius for tf links
    tf_tree_show_names: ${MOLA_LO_TF_TREE_SHOW_NAMES|false}
    tf_tree_exclude_frames: ${MOLA_LO_TF_TREE_EXCLUDE|''} # comma-separated; drops each frame and its subtree

    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}"

  # [m^-2] Minimum motion model covariance (in X,Y,Z) to consider a state estimation as valid as prior for ICP.
  min_motion_model_xyz_cov_inv: 1.0

  # 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}"

# re-localization method to use at start up:
initial_localization:
  method: "${MOLA_LO_INITIAL_LOCALIZATION_METHOD|InitLocalization::FixedPose}"
  # Format: x(m) y(m) z(m) yaw(deg) pitch(deg) roll(deg)
  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}"]
  # Seconds of accelerometer data to average for the initial pitch & roll.
  # A duration, not a sample count: what limits accuracy is platform motion
  # during the window, so the same setting must mean the same averaging time
  # on a 100 Hz and on a 400 Hz IMU.
  imu_initial_calibration_window_seconds: "${MOLA_LO_INITIAL_IMU_WINDOW|1.0}"
  # Sanity floor only, to reject a degenerate handful of samples:
  imu_initial_calibration_min_samples: "${MOLA_LO_INITIAL_IMU_MIN_SAMPLES|20}"
  # Defer initialization while the accelerometer directions in the window
  # disagree by more than this: such a window is measuring platform motion,
  # not gravity. After the timeout it is accepted anyway, so a start that is
  # never still still initializes.
  imu_initial_calibration_max_dispersion_deg: "${MOLA_LO_INITIAL_IMU_MAX_DISPERSION|1.5}"
  imu_initial_calibration_dispersion_timeout: "${MOLA_LO_INITIAL_IMU_DISPERSION_TIMEOUT|5.0}"
  use_imu_orientation: "${MOLA_LO_INITIAL_IMU_USE_ORIENTATION|true}"
  # Right after a re-localization (or initial localization), also hold off
  # local map / simplemap updates for this many timesteps (shares the same
  # recovery-window counter as additional_uncertainty_after_reloc_how_many_
  # timesteps above). 0 (default) disables this and is a no-op: this fires
  # on every initial-localization convergence too, not just explicit
  # relocalizations, so a nonzero default here would drop early keyframes
  # on any dataset where the vehicle is already moving at t=0.
  additional_map_freeze_after_reloc_how_many_timesteps: "${MOLA_LO_INIT_MAP_FREEZE_STEPS|0}"

  # Parameters for InitLocalization::FromStateEstimator:
  from_state_estimator_max_position_sigma: "${MOLA_LO_INIT_SE_MAX_POS_SIGMA|0.5}"
  from_state_estimator_max_orientation_sigma_deg: "${MOLA_LO_INIT_SE_MAX_ORI_SIGMA|3.0}"
  from_state_estimator_timeout: "${MOLA_LO_INIT_SE_TIMEOUT|60.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: ${MOLA_MAX_ICP_ITERATIONS|25}
    minAbsStep_trans: 1e-3
    minAbsStep_rot: 1e-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}

    # Post-optimization SE(3) covariance estimation (see mp2p_icp::covariance).
    # Censi3D is the realistic sandwich estimator for cov2cov pipelines;
    # the per-axis floor absorbs unmodelled errors and keeps downstream
    # filters numerically stable.
    covariance:
      method: ${MOLA_ICP_COVARIANCE_METHOD|Censi3D}
      defaultPointSigma: ${MOLA_ICP_COV_DEFAULT_POINT_SIGMA|0.01} # [m]
      floor_sigma_xyz: ${MOLA_ICP_COV_FLOOR_XYZ|0.001} # [m]
      floor_sigma_angles_deg: ${MOLA_ICP_COV_FLOOR_ANGLES_DEG|0.1} # [deg]

  solvers:
    - class: mp2p_icp::Solver_GaussNewton
      params:
        # Inner Gauss-Newton iterations per ICP iteration. Exposed so an
        # ablation can compare this solver against the annealed multi-iteration
        # one used by the point-to-point pipeline without editing YAML.
        maxIterations: ${MOLA_LO_SOLVER_MAX_ITERS|1}
        robustKernel: "${MOLA_LO_ROBUST_KERNEL|RobustKernel::GemanMcClure}"
        robustKernelParam: "${MOLA_LO_ROBUST_KERNEL_PARAM|6.0}" # In GICP, errors are normalized by covariance
        # Blend [0,1] for the robust kernel residual reference toward the prior
        # mean pose (0=current iterate only, 1=prior mean only). See mp2p_icp.
        robustKernelPriorRefBlend: "${MOLA_LO_ROBUST_KERNEL_PRIOR_REF_BLEND|0.0}"
        #innerLoopVerbose: true

        # Extra per-pairing weight by the geometry class of the map surface.
        # Disabled by default, and bit-exactly inert while the weights are all
        # ones: these numbers do NOT transfer between scene classes, so they
        # belong in a dataset profile and never in this file's defaults. Read
        # mp2p_icp's GeometryClassWeights docs before enabling; in particular
        # 'verticality' is gravity-referenced and needs a gravity source.
        geometry_class_weights:
          enabled: ${MOLA_GCW_ENABLED|false}
          variable: "${MOLA_GCW_VARIABLE|incidence}"
          gravity_source: "${MOLA_GCW_GRAVITY_SOURCE|map_frame}"
          softness: ${MOLA_GCW_SOFTNESS|10.0}
          # Block sequences, not flow ones: the YAML is parsed before the ${}
          # substitution runs, and a ${..|..} inside [ ] is a parse error.
          breakpoints:
            - ${MOLA_GCW_BREAKPOINT|60.0}
          weights:
            - ${MOLA_GCW_W_LOW|1.0}
            - ${MOLA_GCW_W_HIGH|1.0}

        # Sequence of one or more pairs (class, params) defining mp2p_icp::Matcher
        # instances to pair geometric entities between pointclouds.
  matchers:
    - class: mp2p_icp::Matcher_Cov2Cov
      params:
        threshold: "2.0*ADAPTIVE_THRESHOLD_SIGMA"
        # Optional range-adaptive matching distance. thresholdFar: 0 (the
        # default) keeps the flat threshold above, i.e. today's behaviour
        # exactly. When set, the matching distance ramps from `threshold` near
        # the sensor to `thresholdFar` beyond `thresholdKneeRange`.
        #
        # Why the axis exists: a pose-prediction error `dtheta` displaces a
        # point at range `r` by `dtheta*r`, so a flat threshold rejects far
        # returns first, precisely when the prediction is worst. Measured: at a
        # flat 0.34 m, 1 deg of yaw error takes beyond-25 m acceptance from 82%
        # to 4.6% while r<10 m stays at 99%; a 1.0 m far window keeps 94%. Those
        # far returns carry most of the rotation and along-track leverage.
        #
        # Note this is a no-op while ADAPTIVE_THRESHOLD_SIGMA sits at its
        # initial 0.5, where the near value already equals a 1.0 m far value:
        # it interacts with adaptive_threshold.initial_sigma and .min_motion and
        # cannot be swept independently of them.
        thresholdFar: "${MOLA_MATCH_THRESHOLD_FAR|0}"
        thresholdKneeRange: "${MOLA_MATCH_THRESHOLD_KNEE|15.0}"
        thresholdTransitionWidth: "${MOLA_MATCH_THRESHOLD_WIDTH|5.0}"
        pairingsPerPoint: 1
        allowMatchAlreadyMatchedGlobalPoints: true # faster
        # Both layers here must be of the very same cov-capable map class, the
        # one selected below by ${MOLA_LOCALMAP_CLASS}.
        layerMatches:
          - { global: "${MOLA_LOCALMAP_LAYER_NAME|localmap}", local: "observation" }

  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: "${MOLA_LOCALMAP_LAYER_NAME|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/stable/group_mrpt_maps_grp.html
        #
        # Two local-map classes are supported here, both implementing
        # mp2p_icp::NearestPointWithCovCapable (required by Matcher_Cov2Cov):
        #
        #  - mola::KeyframePointCloudMap (default): keyframe-based, points kept
        #    in per-KF local frames, so it survives loop-closure re-mapping.
        #    Required for loop-closure-capable SLAM.
        #
        #  - mola::IncrementalPointCloud: single global frame, one incremental
        #    self-balancing k-d tree updated in place instead of rebuilt on
        #    every scan. ODOMETRY ONLY: a global SE(3) re-map would force a full
        #    rebuild, so do not combine it with loop closure. Needs
        #    mola_metric_maps built against nanoflann >= 1.10.
        #
        #    To try it (the class applies to the 'observation' layer below too,
        #    since Matcher_Cov2Cov pairs the two):
        #
        #      MOLA_LOCALMAP_CLASS=mola::IncrementalPointCloud \
        #      ros2 launch mola_lidar_odometry ros2-lidar-odometry.launch.py [...]
        #
        #    Tuned by the MOLA_INCREMENTAL_MAP_* variables below. See
        #    https://docs.mola-slam.org/latest/mola_lo_pipelines.html
        #
        # Option keys not understood by the selected class are simply ignored,
        # so both sets can coexist below.
        class: ${MOLA_LOCALMAP_CLASS|mola::KeyframePointCloudMap}
        plugin: "libmola_metric_maps.so" # Import additional custom user-defined map classes (search in LD_LIBRARY_PATH)
        creationOpts:
          # --- common to both classes ---
          k_correspondences_for_cov: ${MOLA_LOCALMAP_K_CORRESPONDENCES_FOR_COV|20}
          min_correspondences_for_cov: ${MOLA_LOCALMAP_MIN_CORRESPONDENCES_FOR_COV|5}
          max_distance_for_cov: ${MOLA_LOCALMAP_MAX_DISTANCE_FOR_COV|2.0}
          # 0 disables the planarity test, which is the shipped behavior.
          # See mola::KeyframePointCloudMap's docs before enabling it.
          max_plane_deviation_for_cov: ${MOLA_LOCALMAP_MAX_PLANE_DEV_FOR_COV|0}
          plane_regularization_lambda: ${MOLA_LOCALMAP_PLANE_REG_LAMBDA|0.001}
          # --- mola::IncrementalPointCloud only ---
          # Half side [m] of the cube of points kept around the robot on each
          # insertion (Chebyshev distance, as in HashedVoxelPointCloud's
          # remove_voxels_farther_than). Unlike the keyframe map, EVERY point
          # inside this cube is kept in a single tree, so this is
          # a much tighter budget than 'remove_frames_farther_than' below
          remove_points_farther_than: "${MOLA_INCREMENTAL_MAP_MAX_SIZE|$f{max(100.0, 1.5*ESTIMATED_OBSERVATION_RADIUS)}}" # [m]
          # Run the k-d tree balancing rebuilds on a background thread, so the
          # mapping thread never pays for them. Measured on Oxford Spires, this
          # takes local-map insertion from mean 28 ms / max 371 ms (with 9 calls
          # over 200 ms) down to mean 10 ms / max 38 ms and no spike at all:
          async_rebuild: ${MOLA_INCREMENTAL_MAP_ASYNC_REBUILD|true}
          alpha_balance: ${MOLA_INCREMENTAL_MAP_ALPHA_BALANCE|0.75}
          alpha_deleted: ${MOLA_INCREMENTAL_MAP_ALPHA_DELETED|0.5}
          # Pre-allocated point storage. With async_rebuild this also keeps the
          # mapping thread from ever having to wait for the background worker,
          # which it must do before the point buffers can be reallocated:
          reserve_points: ${MOLA_INCREMENTAL_MAP_RESERVE_POINTS|2000000}
          # --- mola::KeyframePointCloudMap only ---
          max_search_keyframes: ${MOLA_LOCALMAP_MAX_SEARCH_KEYFRAMES|3}
          use_view_direction_filter: ${MOLA_LOCALMAP_USE_VIEW_DIRECTION_FILTER|true}
          max_view_angle_deg: ${MOLA_LOCALMAP_VIEW_DIRECTION_FILTER_ANGLE_DEG|120}
          rotation_distance_weight: 2.0
          num_diverse_keyframes: ${MOLA_LOCALMAP_DIVERSE_KEYFRAMES|1} # Must be < max_search_keyframes
          # If true, cov-to-cov ICP matching (Matcher_Cov2Cov) queries each active
          # keyframe's own cached KD-tree + covariances directly, instead of building
          # a merged submap from the active KF set. Faster (skips the merge/KD-tree
          # rebuild) at the cost of a slightly less accurate covariance estimate
          # (computed only from neighbors within the same source keyframe).
          approximate_cov: ${MOLA_LOCALMAP_APPROXIMATE_COV|false}
        insertOpts:
          remove_frames_farther_than: "${MOLA_LOCAL_MAP_MAX_SIZE|$f{max(100.0, 1.50*ESTIMATED_OBSERVATION_RADIUS)}}" # [m]
        likelihoodOpts: ~ # none required
        renderOpts:
          color.A: 0.25 # [0,1] Use this alpha value for points, RGB from colormap
          colormap: "cmHOT" # cmJET, cmHOT, cmGRAYSCALE
          recolorByPointField: ${MOLA_GUI_LOCAL_MAP_COLOR_BY_COORDINATE|intensity} # x,y,z,ring, intensity, ambient, etc.
          max_points_per_kf: ${MOLA_LOCALMAP_VIZ_MAX_POINTS_PER_KF|10000} # Max number of points to render per keyframe
          max_overall_points: ${MOLA_LOCALMAP_VIZ_MAX_POINTS_OVERALL|500000} # Max number of points to render overall (e.g. to avoid FoxGlove WS overflow)
          #point_size: 1.0  # superseded by visualization.local_map_point_size above

          # ---------------------------------------------------------------------------------
          # 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:
      name: "Generator (raw)"
      target_layer: "raw"
      throw_on_unhandled_observation_class: true
      process_class_names_regex: ".*"
      process_sensor_labels_regex: ".*"

  # This just creates an empty layer of the local map class: Matcher_Cov2Cov
  # pairs it against the local map, so both must be of the same class.
  - class_name: mp2p_icp_filters::Generator
    params:
      name: "Generator (obs)"
      target_layer: "observation"
      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/stable/group_mrpt_maps_grp.html
        class: ${MOLA_LOCALMAP_CLASS|mola::KeyframePointCloudMap}
        plugin: "libmola_metric_maps.so" # Import additional custom user-defined map classes (search in LD_LIBRARY_PATH)
        # The per-point covariances of the SCAN side of a cov-to-cov pairing are
        # built here, and they are half of the pairing weight
        # `(C_global + C_local)^-1`. Leaving this empty used the class defaults
        # (k=20, min=5, max_distance=1.0 m), which do NOT match what the local
        # map above is configured with, so the two sides of the same pairing
        # were estimated over different neighborhood scales.
        #
        # A scan is much sparser than the accumulated map, so a radius that is
        # ample for the map can leave a scan point with fewer than
        # `min_correspondences_for_cov` neighbors. That case does not drop the
        # pairing: it falls back to an isotropic covariance, and the pairing
        # stays in the sum as an almost-isotropic point-to-point constraint
        # instead of the plane constraint the cov-to-cov form is meant to give.
        #
        # `max_distance_for_cov` defaults to 2.0 here, NOT to the map class's
        # own 1.0: measured over 115M pairings on Oxford Spires, 32.3% of
        # pairings could not find `min_correspondences_for_cov` neighbors
        # inside 1.0 m and fell back to an isotropic covariance, which leaves
        # the pairing in the sum as a point-to-point constraint rather than a
        # plane one. Full-corpus A/B at 2.0 m: pooled APE x0.785 over 46
        # sequences, better on six of seven datasets (oxford x0.538,
        # conslam x0.622, botanic x0.696, kitti x0.912, grand-tour x0.930,
        # tiers x0.992) and no new failures.
        #
        # citrus-farm is the exception at x1.201, and its profile overrides
        # this back to 1.0. Working hypothesis: dense close-range orchard
        # foliage has no consistent surface, so a wider neighborhood averages
        # the normal over vegetation instead of structure.
        creationOpts:
          # Required by the map class loader; the class default, and inert for a
          # single-keyframe observation layer.
          max_search_keyframes: 3
          k_correspondences_for_cov: ${MOLA_OBSLAYER_K_CORRESPONDENCES_FOR_COV|20}
          min_correspondences_for_cov: ${MOLA_OBSLAYER_MIN_CORRESPONDENCES_FOR_COV|5}
          max_distance_for_cov: ${MOLA_OBSLAYER_MAX_DISTANCE_FOR_COV|2.0}
          max_plane_deviation_for_cov: ${MOLA_OBSLAYER_MAX_PLANE_DEV_FOR_COV|0}
          plane_regularization_lambda: ${MOLA_OBSLAYER_PLANE_REG_LAMBDA|0.001}
        insertOpts: ~
        likelihoodOpts: ~ # none required
        renderOpts: ~

# 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: "${MOLA_SCAN_POINT_STAMPS_ADJUST_METHOD|TimestampAdjustMethod::MiddleIsZero}"

# Path to an (optional) user-customizable pipeline definition file. Default: empty = none.
observations_prefilter_file: ${MOLA_LO_OBS_PREFILTER_PIPELINE_FILE|""}

# Early deskew of the full raw cloud.
# Runs once; result is reused for decimation, viz, publish, simplemap.
# Skipped at runtime when !hasIMU && optimize_twist (fallback to 2nd-pass deskew).
observations_deskew_pass:
  # Drop near returns to remove "noise" from the vehicle body, a person
  # standing next to the robot (e.g. with a joystick), etc., and clamp
  # very far returns where small angular errors blow up into large
  # translational errors. metric_l_infinity → axis-aligned cube; center
  # is unset → anchored at base_link (the vehicle origin), *not* at the
  # sensor — this is intentional so the deadzone covers the whole
  # vehicle footprint regardless of where the sensor is mounted on it.
  # If you also need a sensor-anchored cut (e.g. the sensor's blind
  # sphere when it is mounted significantly off base_link), add an
  # extra filter via observations_prefilter_file.
  - class_name: mp2p_icp_filters::FilterByRange
    params:
      input_pointcloud_layer: "raw"
      output_layer_between: "raw_range_filtered"
      range_min: "${MOLA_MINIMUM_RANGE_FILTER|max(1.0, 0.03*ESTIMATED_OBSERVATION_RADIUS)}"
      range_max: "${MOLA_MAXIMUM_RANGE_FILTER|1.2*ESTIMATED_OBSERVATION_RADIUS}"
      metric_l_infinity: true

  - class_name: mp2p_icp_filters::FilterDeskew
    params:
      name: "FilterDeskew (early)"
      input_pointcloud_layer: "raw_range_filtered"
      output_pointcloud_layer: "deskewed"
      method: "${MOLA_DESKEW_METHOD|MotionCompensationMethod::Linear}"
      ignore_accelerometer: "${MOLA_DESKEW_IGNORE_ACCELEROMETER|false}"
      silently_ignore_no_timestamps: ${MOLA_IGNORE_NO_POINT_STAMPS|true} # To handle more dataset types
      output_layer_class: "mrpt::maps::CGenericPointsMap" # 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::FilterDeleteLayer
    params:
      pointcloud_layer_to_remove: ["raw_range_filtered"]

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::FilterDecimateAdaptive
    params:
      name: "FilterDecimateAdaptive (map)"
      input_pointcloud_layer: "deskewed"
      output_pointcloud_layer: "decimated_for_map"
      # Per stage since 2026-08-15. The two stages want different cell sizes:
      # a coarser map voxel trades a little short-range precision for markedly
      # less drift accumulation, and no single value expresses both.
      # NOTE: this replaces MOLA_CLOUD_DECIMATION_VOXEL_SIZE, which set both
      # stages at once. Setting the old name now has no effect: to reproduce a
      # run recorded against it, set both variables below to that value.
      voxel_size: "${MOLA_CLOUD_DECIMATION_VOXEL_SIZE_MAP|0.15}" # [m]
      # A floor, not the target: the point count that matters is relative to
      # the number of occupied voxels, which maximum_voxel_stride bounds.
      desired_output_point_count: "${MOLA_DECIMATED_POINTS_MAP|10000}"
      maximum_voxel_stride: "${MOLA_VOXEL_STRIDE_MAP|0}" # 0=off; 1=visit every occupied voxel
      decimate_method: "${MOLA_DECIMATE_METHOD|DecimateMethod::FirstPoint}"
      parallelization_grain_size: 8000 # When TBB is enabled, the grainsize for splitting the input clouds into threads

  - class_name: mp2p_icp_filters::FilterDecimateAdaptive
    params:
      name: "FilterDecimateAdaptive (icp)"
      input_pointcloud_layer: "decimated_for_map"
      output_pointcloud_layer: "decimated_for_icp"
      voxel_size: "${MOLA_CLOUD_DECIMATION_VOXEL_SIZE_ICP|0.10}" # [m]
      desired_output_point_count: "${MOLA_DECIMATED_POINTS_ICP|3000}"
      maximum_voxel_stride: "${MOLA_VOXEL_STRIDE_ICP|0}" # 0=off; 2=visit one occupied voxel in two
      decimate_method: "${MOLA_DECIMATE_METHOD|DecimateMethod::FirstPoint}"
      parallelization_grain_size: 8000 # When TBB is enabled, the grainsize for splitting the input clouds into threads

# 2nd pass:
observations_filter_2nd_pass:
  # The merge below appends to "observation", so this pass has to start from an
  # empty layer: with optimize_twist enabled the whole pass re-runs after a
  # twist correction, and a second append leaves the cov-capable observation
  # map holding two keyframes, which the cov-to-cov nearest-neighbor search
  # rejects outright.
  - class_name: mp2p_icp_filters::FilterClear
    params:
      target_layer: "observation"

  - class_name: mp2p_icp_filters::FilterMerge
    params:
      name: "FilterMerge (icp into obs)"
      input_pointcloud_layer: "decimated_for_icp"
      target_layer: "observation"

# 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", "deskewed"]

# 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:
      name: "FilterMerge (map into localmap)"
      input_pointcloud_layer: "decimated_for_map"
      target_layer: "${MOLA_LOCALMAP_LAYER_NAME|localmap}"
      input_layer_in_local_coordinates: true
      robot_pose: [robot_x, robot_y, robot_z, robot_yaw, robot_pitch, robot_roll]

# Pipeline starting from raw observations (passed through "Generator"), to be
# sent out for visualization of the sliding-window of recent clouds as a dense local map
# This is ONLY run when not using "use_early_deskew", that is: not using IMU, and enabling "optimize_twist"
observations_filter_deskew_for_visualization:
  - class_name: mp2p_icp_filters::FilterByRange
    params:
      input_pointcloud_layer: "raw"
      output_layer_between: "raw_filtered"
      range_min: "${MOLA_MINIMUM_RANGE_FILTER|max(1.0, 0.03*ESTIMATED_OBSERVATION_RADIUS)}"
      range_max: 1.2*ESTIMATED_OBSERVATION_RADIUS
      metric_l_infinity: true

  - class_name: mp2p_icp_filters::FilterDeskew
    params:
      name: "FilterDeskew (viz)"
      input_pointcloud_layer: "raw_filtered"
      output_pointcloud_layer: "viz"
      method: "${MOLA_DESKEW_METHOD|MotionCompensationMethod::Linear}"
      ignore_accelerometer: "${MOLA_DESKEW_IGNORE_ACCELEROMETER|false}"
      silently_ignore_no_timestamps: ${MOLA_IGNORE_NO_POINT_STAMPS|true} # To handle more dataset types
      output_layer_class: "mrpt::maps::CGenericPointsMap" # Keep intensity only

      # 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]

  # Remove raw layer so it's not visible
  - class_name: mp2p_icp_filters::FilterDeleteLayer
    params:
      pointcloud_layer_to_remove: ["raw", "raw_filtered"]

3. 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 [BC25], 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

# =====================================================================================
# Pipeline: point-to-{point,plane} 3D ICP with 3D-NDT-like maps
#
# For paper references, see https://github.com/MOLAorg/mola_lidar_odometry/
#
# 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.
# =====================================================================================

params:
  pipeline_name: "point-to-{point,plane} 3D ICP with 3D-NDT-like maps" # For display/debug only

  # These sensor labels will be handled as LIDAR observations:
  # Can be overridden 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 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]

  # Whether a scan arriving while the worker is still busy replaces the one
  # already queued (real-time behavior) or waits for it (lossless). Set to false
  # for offline batch runs, where every scan must be processed.
  drop_stale_scans: ${MOLA_DROP_STALE_SCANS|true}

  # Parameters for max sensor range automatic estimation:
  observation_radius_filter_coefficient: 0.95
  absolute_minimum_observation_radius: ${MOLA_ABS_MIN_SENSOR_RANGE|5.0}
  # Quantile of the per-point norms taken as ESTIMATED_OBSERVATION_RADIUS.
  # 1.0 = the bounding-box max-norm, i.e. one far return sets the scene
  # scale for the six parameters derived from it. Default unchanged;
  # exposed so the robust alternative can be measured.
  observation_radius_quantile: ${MOLA_OBSERVATION_RADIUS_QUANTILE|1.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]

  # When publishing pose updates, the reference frame for both, estimated robot poses, and the local map.
  publish_reference_frame: "${MOLA_LO_PUBLISH_REF_FRAME|odom}"

  # When publishing pose updates, the vehicle frame name.
  publish_vehicle_frame: "${MOLA_LO_PUBLISH_VEHICLE_FRAME|base_link}"

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

    # 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_OBSERVATION_RADIUS}" # [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_OBSERVATION_RADIUS)}" # [m]
    check_for_removal_every_n: 100
    min_nearby_poses_occupied: ${MOLA_MIN_NEARBY_POSES_OCCUPIED|1}

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

  # Adaptive threshold:
  adaptive_threshold:
    enabled: true
    initial_sigma: ${MOLA_SIGMA_INITIAL|0.50} # [m]
    min_motion: ${MOLA_SIGMA_MIN_MOTION|0.5} # [m]
    maximum_sigma: ${MOLA_SIGMA_MAX_MOTION|2.00} # [m]
    max_sigma_step: ${MOLA_SIGMA_MAX_STEP|0.05} # [m]
    icp_quality_controller_setpoint: ${MOLA_SIGMA_CONTROLLER_QUALITY_SETPOINT|0.85}
    kp: ${MOLA_SIGMA_CONTROLLER_GAIN|2.0}
    alpha: ${MOLA_ADAPT_THRESHOLD_ALPHA|0.90}
    # Sustained-failure recovery: if enabled, sigma is grown multiplicatively
    # after a streak of bad ICPs, capped at maximum_sigma, so the matcher
    # window can re-open and ICP can recover. Enabled by default: without it,
    # once sigma is driven down near min_motion by a run of easy/near-static
    # scans (e.g. goodness consistently above icp_quality_controller_setpoint),
    # a single larger inter-scan motion (a turn, a bump, or just ordinary
    # scan-to-scan variability once sigma is already pinned at its floor) can
    # push ICP into failure, and with sigma frozen the correspondence search
    # window never reopens, so the pipeline stalls indefinitely
    # (estimated_trajectory never grows again). Set to false to restore the
    # old behavior. `maximum_sigma` must be set strictly above
    # `initial_sigma`, or this mechanism is a no-op: sigma always starts
    # each run AT initial_sigma, so a bad ICP on frame 1 (e.g. a slightly
    # imprecise localization-only seed pose against a prebuilt map) has no
    # room to grow into and can never recover (observed in testing as ICP
    # goodness stuck just under min_icp_goodness for an entire run).
    #
    # recover_after_n_bad/recover_growth_factor default to a fast reaction
    # (2 bad frames, x2.0 growth) rather than a slow one (5 bad frames,
    # x1.5): every frame spent stuck is a frame of real, untracked vehicle
    # motion accumulating; the slower defaults let that gap grow to the
    # point where, once the search window finally reopens, ICP can lock onto
    # a self-consistent but WRONG registration (observed in testing as a
    # sudden ~30-40 deg yaw error that then persisted for the rest of a run)
    # instead of recovering the true pose.
    recover_on_sustained_failure: ${MOLA_ADAPT_THRESHOLD_RECOVER|true}
    recover_after_n_bad: ${MOLA_ADAPT_THRESHOLD_RECOVER_AFTER_N_BAD|2}
    recover_growth_factor: ${MOLA_ADAPT_THRESHOLD_RECOVER_GROWTH_FACTOR|2.0}

  # 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 overridden with CLI flag --output-simplemap
    load_existing_simple_map: ${MOLA_LOAD_SM|""}

    save_final_map_to_file: ${MOLA_SIMPLEMAP_OUTPUT|'final_map.simplemap'}

    # NOTE: unlike local_map_updates above, these thresholds are NOT scaled by
    # angular velocity: this is what feeds the SharedKeyframeMap sink (e.g.
    # mola_mapper), and the old w-scaled formula (rotation threshold growing
    # by 500 deg per rad/s) made it create close to NO keyframes while
    # smoothly turning, starving loop closure of the keyframes it needs.
    min_translation_between_keyframes: ${MOLA_SIMPLEMAP_MIN_XYZ|1.0e-2*ESTIMATED_OBSERVATION_RADIUS} # m
    min_rotation_between_keyframes: ${MOLA_SIMPLEMAP_MIN_ROT|15} # 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.
    min_nearby_poses_occupied: ${MOLA_SIMPLEMAP_MIN_NEARBY_POSES|1}
    # Revisiting an already-mapped area creates NO keyframes with the purely
    # spatial criterion above, which starves loop closure of the second endpoint
    # of the loop. Set to a positive value [s] so only keyframes newer than that
    # take part in the "is there one here already?" test.
    nearby_keyframe_time_window: ${MOLA_SIMPLEMAP_KF_TIME_WINDOW|0} # [s], 0=disabled
    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: ${MOLA_GUI_MAP_UPDATE_DECIMATION|10}
    show_trajectory: true
    show_current_observation: false # shows "live raw" LiDAR points
    show_ground_grid: true
    ground_grid_spacing: 5.0 # [m]
    #current_pose_corner_size: 1.5
    #sensor_poses_corner_size: 0.5  # XYZ corner for each LiDAR sensor pose; 0 to disable
    show_current_pose_corner: ${MOLA_LO_SHOW_CURRENT_POSE_CORNER|true} # Set to false to hide the current-pose XYZ corner (e.g. for a first-person camera)
    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}"

# re-localization method to use at start up:
initial_localization:
  method: "InitLocalization::FixedPose"
  # Format: x(m) y(m) z(m) yaw(deg) pitch(deg) roll(deg)
  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)"
        # Blend [0,1] for the robust kernel residual reference toward the prior
        # mean pose (0=current iterate only, 1=prior mean only). See mp2p_icp.
        robustKernelPriorRefBlend: "${MOLA_LO_ROBUST_KERNEL_PRIOR_REF_BLEND|0.0}"
        #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/stable/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_OBSERVATION_RADIUS))}}" # [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_OBSERVATION_RADIUS)}}" # [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'

# Path to an (optional) user-customizable pipeline definition file. Default: empty = none.
observations_prefilter_file: ${MOLA_LO_OBS_PREFILTER_PIPELINE_FILE|""}

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_OBSERVATION_RADIUS) # [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_OBSERVATION_RADIUS)}"
      range_max: 1.2*ESTIMATED_OBSERVATION_RADIUS

  # 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_OBSERVATION_RADIUS, -0.20*INSTANTANEOUS_OBSERVATION_RADIUS, 0.01*INSTANTANEOUS_OBSERVATION_RADIUS]
      bounding_box_max: [0.20*INSTANTANEOUS_OBSERVATION_RADIUS, 0.20*INSTANTANEOUS_OBSERVATION_RADIUS, 0.10*INSTANTANEOUS_OBSERVATION_RADIUS]

  - 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_OBSERVATION_RADIUS) # [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"
      method: "${MOLA_DESKEW_METHOD|MotionCompensationMethod::Linear}"
      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"
      method: "${MOLA_DESKEW_METHOD|MotionCompensationMethod::Linear}"
      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]

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

# =====================================================================================
# Pipeline: point-to-gridmap 2D ICP
#
# For paper references, see https://github.com/MOLAorg/mola_lidar_odometry/
#
# 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.
# =====================================================================================

params:
  pipeline_name: "point-to-gridmap 2D ICP" # For display/debug only

  # These sensor labels will be handled as LIDAR observations:
  # Can be overridden 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 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]

  # Whether a scan arriving while the worker is still busy replaces the one
  # already queued (real-time behavior) or waits for it (lossless). Set to false
  # for offline batch runs, where every scan must be processed.
  drop_stale_scans: ${MOLA_DROP_STALE_SCANS|true}

  # Parameters for max sensor range automatic estimation:
  observation_radius_filter_coefficient: 0.999
  absolute_minimum_observation_radius: 20.0

  # When publishing pose updates, the reference frame for both, estimated robot poses, and the local map.
  publish_reference_frame: "${MOLA_LO_PUBLISH_REF_FRAME|odom}"

  # When publishing pose updates, the vehicle frame name.
  publish_vehicle_frame: "${MOLA_LO_PUBLISH_VEHICLE_FRAME|base_link}"

  # How often to update the local map model:
  local_map_updates:
    enabled: "${MOLA_MAPPING_ENABLED|true}"
    load_existing_local_map: ${MOLA_LOAD_MM|""}
    load_map_after_gui_init: ${MOLA_LO_LOAD_MAP_AFTER_GUI|false}
    min_translation_between_keyframes: "${MOLA_MIN_XYZ_BETWEEN_MAP_UPDATES|0.02*ESTIMATED_OBSERVATION_RADIUS}" # [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_OBSERVATION_RADIUS)" # [m]
    check_for_removal_every_n: 100
    min_nearby_poses_occupied: ${MOLA_MIN_NEARBY_POSES_OCCUPIED|1}

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

  # Adaptive threshold:
  adaptive_threshold:
    enabled: true
    initial_sigma: ${MOLA_SIGMA_INITIAL|0.50} # [m]
    min_motion: ${MOLA_SIGMA_MIN_MOTION|0.04} # [m]
    maximum_sigma: ${MOLA_SIGMA_MAX_MOTION|2.00} # [m]
    max_sigma_step: ${MOLA_SIGMA_MAX_STEP|0.05} # [m]
    icp_quality_controller_setpoint: ${MOLA_SIGMA_CONTROLLER_QUALITY_SETPOINT|0.85}
    kp: ${MOLA_SIGMA_CONTROLLER_GAIN|2.0}
    alpha: ${MOLA_ADAPT_THRESHOLD_ALPHA|0.90}
    # Sustained-failure recovery: if enabled, sigma is grown multiplicatively
    # after a streak of bad ICPs, capped at maximum_sigma, so the matcher
    # window can re-open and ICP can recover. Enabled by default: without it,
    # once sigma is driven down near min_motion by a run of easy/near-static
    # scans (e.g. goodness consistently above icp_quality_controller_setpoint),
    # a single larger inter-scan motion (a turn, a bump, or just ordinary
    # scan-to-scan variability once sigma is already pinned at its floor) can
    # push ICP into failure, and with sigma frozen the correspondence search
    # window never reopens, so the pipeline stalls indefinitely
    # (estimated_trajectory never grows again). Set to false to restore the
    # old behavior. `maximum_sigma` must be set strictly above
    # `initial_sigma`, or this mechanism is a no-op: sigma always starts
    # each run AT initial_sigma, so a bad ICP on frame 1 (e.g. a slightly
    # imprecise localization-only seed pose against a prebuilt map) has no
    # room to grow into and can never recover (observed in testing as ICP
    # goodness stuck just under min_icp_goodness for an entire run).
    #
    # recover_after_n_bad/recover_growth_factor default to a fast reaction
    # (2 bad frames, x2.0 growth) rather than a slow one (5 bad frames,
    # x1.5): every frame spent stuck is a frame of real, untracked vehicle
    # motion accumulating; the slower defaults let that gap grow to the
    # point where, once the search window finally reopens, ICP can lock onto
    # a self-consistent but WRONG registration (observed in testing as a
    # sudden ~30-40 deg yaw error that then persisted for the rest of a run)
    # instead of recovering the true pose.
    recover_on_sustained_failure: ${MOLA_ADAPT_THRESHOLD_RECOVER|true}
    recover_after_n_bad: ${MOLA_ADAPT_THRESHOLD_RECOVER_AFTER_N_BAD|2}
    recover_growth_factor: ${MOLA_ADAPT_THRESHOLD_RECOVER_GROWTH_FACTOR|2.0}

  # 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 overridden 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

    # Revisiting an already-mapped area creates NO keyframes with the purely
    # spatial criterion above, which starves loop closure of the second endpoint
    # of the loop. Set to a positive value [s] so only keyframes newer than that
    # take part in the "is there one here already?" test.
    nearby_keyframe_time_window: ${MOLA_SIMPLEMAP_KF_TIME_WINDOW|0} # [s], 0=disabled
    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: ${MOLA_GUI_MAP_UPDATE_DECIMATION|40}
    show_trajectory: true
    show_current_observation: true # shows "live raw" LiDAR points
    #current_pose_corner_size: 1.5
    #sensor_poses_corner_size: 0.5  # XYZ corner for each LiDAR sensor pose; 0 to disable
    show_current_pose_corner: ${MOLA_LO_SHOW_CURRENT_POSE_CORNER|true} # Set to false to hide the current-pose XYZ corner (e.g. for a first-person camera)
    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]

# 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)
        # Blend [0,1] for the robust kernel residual reference toward the prior
        # mean pose (0=current iterate only, 1=prior mean only). See mp2p_icp.
        robustKernelPriorRefBlend: "${MOLA_LO_ROBUST_KERNEL_PRIOR_REF_BLEND|0.0}"
        #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/stable/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_OBSERVATION_RADIUS))}' # [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: ".*"

# Path to an (optional) user-customizable pipeline definition file. Default: empty = none.
observations_prefilter_file: ${MOLA_LO_OBS_PREFILTER_PIPELINE_FILE|""}

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_OBSERVATION_RADIUS)
      range_max: 1.25*ESTIMATED_OBSERVATION_RADIUS

  # 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, so set this to your ROS 2 topic name for the LiDAR, 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 observation radius (see ESTIMATED_OBSERVATION_RADIUS).
MOLA_MINIMUM_RANGE_FILTER (Default: 3% of the estimated observation radius): Minimum range (L-infinity cube around base_link) for 3D points used by ICP; intended to cut out points coming from the robot/vehicle body itself or a person standing right next to it.

Sensor inputs: IMU (optional)

MOLA_IMU_NAME (Default: imu): Sensor label (or regex) of the observations with IMU data, if it exists. This is used to estimate the vehicle’s pose and velocity, and to deskew point clouds. For most dataset sources, the default imu 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, so set this to your ROS 2 topic name for the IMU, 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_DESKEW_IGNORE_ACCELEROMETER (Default: false): Enable if a noisy IMU sensor causes shaky motion in the estimation, but you still want to use gyroscope for precise deskewing in LIO.

IMU gravity correction (pitch/roll)

When an IMU is available, MOLA-LO can use the accelerometer readings to continuously estimate the gravity direction and inject it as a pitch/roll constraint into the ICP prior. This prevents vertical drift in the LiDAR odometry output without requiring a full factor-graph smoother.

The feature averages recent accelerometer samples in a circular buffer, rotates the result to the vehicle frame using the IMU sensorPose extrinsics, and derives pitch and roll from the gravity direction.

Note

This feature acts on the ICP prior independently of the StateEstimationSmoother gravity factor. Both can be active simultaneously.

MOLA_IMU_GRAVITY_CORRECTION (Default: true): Set to false to disable accelerometer-based pitch/roll correction of the ICP prior. Safe to leave enabled even without an IMU: when no IMU data is present the correction is silently skipped.
MOLA_IMU_GRAVITY_SIGMA_DEG (Default: 2.0 [deg]): Standard deviation of the gravity-derived pitch/roll prior. Lower values give more trust to the IMU. Typical range: 1-5 deg.
MOLA_IMU_GRAVITY_AVG_SAMPLES (Default: 20): Number of recent accelerometer samples to average for the gravity estimate. The correction activates as soon as 3 samples are available, even if the full window has not filled yet.
MOLA_IMU_GRAVITY_MAX_AGE (Default: 2.0 [s]): Maximum age in seconds for accelerometer samples used in the gravity average. Samples older than this are discarded, ensuring the estimate reflects current orientation rather than stale data. Set to 0 to disable age filtering.

Sensor inputs: Wheels odometry (optional)

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

Sensor inputs: GPS (GNSS) (optional)

MOLA_GPS_NAME (Default: gps): Sensor label (or regex) of GNSS observations inside the pipeline. Used only for storage in simple-maps for post-processing (geo-referencing, etc.).

Note

For ROS 2 live or rosbag sources, the ROS topic name that the bridge subscribes to is controlled separately by MOLA_GNSS_TOPIC (see the sensor-input environment variables). These are two different variables: MOLA_GPS_NAME is the internal sensor label; MOLA_GNSS_TOPIC is the ROS topic.

Scan de-skew options

MOLA_DESKEW_METHOD (Default: MotionCompensationMethod::Linear): Selects the scan de-skew (motion compensation) method.

Note

IMPORTANT: If you do not change this from its default, IMU data will not be used for deskewing. To fully achieve the best accuracy when an IMU is available, set this to MotionCompensationMethod::IMU.
MOLA_IGNORE_NO_POINT_STAMPS (Default: true): If enabled (default), input point clouds without per-point timestamps will be processed without doing any de-skew. If set to false, an exception is triggered in that event, which can be used as a fail-safe check against missing stamps, important in high-velocity scenarios.
MOLA_SCAN_POINT_STAMPS_ADJUST_METHOD (Default: TimestampAdjustMethod::MiddleIsZero): Method for adjusting per-point timestamps so that the scan mid-point is at time zero. Affects how twist is applied during deskewing.

General options

MOLA_OPTIMIZE_TWIST (Default: true for most pipelines, hardcoded false in the GICP pipeline): Whether to optimize vehicle twist (linear and angular velocity vectors) within the ICP loop. Useful for high-dynamics scenarios. Requires incoming point clouds with per-point timestamps. In the GICP pipeline the early-deskew pass takes priority; use lidar3d-gicp-optimize-twist.yaml to re-enable it.
MOLA_MAPPING_ENABLED (Default: true): Whether to update the local map. Can be temporarily disabled, or permanently disabled when using MOLA-LO for localization from a prebuilt map.
MOLA_LOAD_MM (Default: none): Path to a metric map (*.mm) file with a prebuilt metric map to load at startup. Useful for multisession mapping or localization-only mode.
MOLA_SAVE_MM (Default: none): If set to a non-empty path, the final local metric map is saved to a *.mm file at the end of the session.
MOLA_MINIMUM_ICP_QUALITY (Default: 0.50): Minimum quality (from mp2p_icp quality evaluators), in the range [0, 1], to consider an ICP optimization valid.
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).
MOLA_PROFILER (Default: true): Enable pipeline and ICP step-level timing profiler. Disable to reduce overhead in production deployments.
MOLA_LO_PUBLISH_REF_FRAME (Default: odom): Reference frame name used when publishing pose updates.
MOLA_LO_PUBLISH_VEHICLE_FRAME (Default: base_link): Vehicle frame name used when publishing pose updates.
MOLA_LO_PUBLISH_DESKEWED_SCANS (Default: false): If enabled, deskewed scans are published as ROS 2 messages, mostly for visualization. May slow down the system.

Adaptive threshold

The adaptive threshold controls the ICP matching window (sigma), following the approach of KISS-ICP.

MOLA_SIGMA_INITIAL (Default: 0.50 [m]): Initial sigma value at startup.
MOLA_SIGMA_MIN_MOTION (Default: 0.04 [m]): Absolute minimum for sigma.
MOLA_SIGMA_MAX_MOTION (Default: 0.50 [m]; GICP and ICP pipelines): Upper cap for sigma. Named MOLA_SIGMA_MAX in the NDT pipeline with a default of 0.5 [m].
MOLA_ADAPT_THRESHOLD_ALPHA (Default: 0.90): Alpha parameter of the IIR low-pass filter for the adaptive threshold proportional controller (refer to the paper).
MOLA_ADAPT_THRESHOLD_RECOVER (Default: false): If enabled, sigma is grown multiplicatively after a streak of bad ICP results, up to MOLA_SIGMA_MAX_MOTION, allowing the matcher window to re-open and ICP to recover from sustained failure.
MOLA_ADAPT_THRESHOLD_RECOVER_AFTER_N_BAD (Default: 5): Number of consecutive bad ICP results before recovery growth kicks in, when MOLA_ADAPT_THRESHOLD_RECOVER is enabled.
MOLA_ADAPT_THRESHOLD_RECOVER_GROWTH_FACTOR (Default: 1.5): Multiplicative growth factor applied to sigma per bad frame during recovery.

Local map update

MOLA_MIN_XYZ_BETWEEN_MAP_UPDATES (Default: heuristic formula, see YAML file): Minimum distance in meters between updates to the local map.
MOLA_MIN_ROT_BETWEEN_MAP_UPDATES (In degrees. Default: 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 than this distance, measured from the current robot pose, will be removed. This saves memory and avoids inconsistencies before loop-closure (which is handled outside of the LO module).
MOLA_LOCAL_VOXELMAP_RESOLUTION (In meters; default: heuristic formula, see YAML file): Size of voxels for the local map. Not used in the GICP pipeline, which uses a keyframe-based map instead.
MOLA_MIN_NEARBY_POSES_OCCUPIED (Default: 1): Minimum number of nearby local-map poses that must be occupied before a new keyframe is accepted into the map.

Note

Non-repetitive / solid-state LiDARs (e.g. Livox AVIA): a single scan does not cover the full field of view uniformly. Set this to 2 or higher so that multiple frames are accumulated from each location before the robot moves on, producing denser local-map coverage. For spinning LiDARs (Velodyne, Ouster, …) the default of 1 is correct.
MOLA_SIMPLEMAP_MIN_NEARBY_POSES (Default: 1): Same criterion, applied to the simplemap keyframe insertion instead of the local map. Set to 2+ for non-repetitive LiDARs for the same reason as MOLA_MIN_NEARBY_POSES_OCCUPIED above.
MOLA_PUBLISH_LOCAL_MAP_UPDATES_EVERY_N (Default: 40 in GICP, 5 in NDT): Publish local map visualization updates every N ICP iterations.

Observation filter pipeline

MOLA_LO_OBS_PREFILTER_PIPELINE_FILE (Default: none): Path to an optional user-defined mp2p_icp pipeline YAML file applied to raw observations before any built-in filtering. Use this to inject custom filters (e.g. a ring-based ground filter or a sector masking filter) without modifying the main pipeline file.
MOLA_CLOUD_DECIMATION_VOXEL_SIZE (Default: 0.15 [m] for map, 0.10 [m] for ICP; GICP pipeline only): Minimum voxel size for adaptive decimation of point clouds. The GICP pipeline uses two separate decimation stages; this sets the floor voxel size for both. See also MOLA_DECIMATED_POINTS_MAP and MOLA_DECIMATED_POINTS_ICP.
MOLA_DECIMATED_POINTS_MAP (Default: 10000; GICP pipeline only): Target point count for the cloud inserted into the local map after adaptive decimation.
MOLA_DECIMATED_POINTS_ICP (Default: 3000; GICP pipeline only): Target point count for the cloud used in ICP matching after adaptive decimation.

ICP settings

MOLA_MAX_ICP_ITERATIONS (Default: 25): Maximum number of ICP iterations per scan.
MOLA_LOCALMAP_LAYER_NAME (Default: localmap; GICP and ICP pipelines): Name of the metric map layer used as the local map for ICP matching and map insertion.
MOLA_LOCALMAP_CLASS (Default: mola::KeyframePointCloudMap; GICP pipeline only): C++ class used for both the local map and the per-scan observation layer. The alternative is mola::IncrementalPointCloud (odometry only, no loop closure). See Selecting the local map class.

The following only apply when MOLA_LOCALMAP_CLASS=mola::IncrementalPointCloud:

MOLA_INCREMENTAL_MAP_MAX_SIZE (Default: max(25.0, 0.5*ESTIMATED_OBSERVATION_RADIUS) [m]): Half side of the axis-aligned cube of points kept around the robot; everything outside it is evicted on each map update. Note this is a much tighter budget than the keyframe map’s MOLA_LOCAL_MAP_MAX_SIZE, which is a radius over keyframe centers: here every point inside the cube is kept in a single tree and rendered, so setting it too large makes the local map accumulate scans instead of sliding.
MOLA_INCREMENTAL_MAP_ASYNC_REBUILD (Default: true): Run the k-d tree balancing rebuilds on a background thread, so the mapping thread never pays for them. Measured on the Oxford Spires dataset, this takes local map insertion from mean 28 ms / max 371 ms down to mean 10 ms / max 38 ms, removing all latency spikes. Costs roughly twice the index memory plus one worker thread; set to false on core-constrained targets.
MOLA_INCREMENTAL_MAP_RESERVE_POINTS (Default: 2000000): Point storage reserved up front. With ASYNC_REBUILD enabled this also prevents the mapping thread from ever having to wait for the background worker, which it must do before the point buffers can be reallocated.
MOLA_INCREMENTAL_MAP_ALPHA_BALANCE (Default: 0.75): A subtree is rebuilt when its larger child holds more than this fraction of its points. Lower values keep queries faster at the price of more frequent rebuilds.
MOLA_INCREMENTAL_MAP_ALPHA_DELETED (Default: 0.5): A subtree is rebuilt, physically dropping evicted points, once this fraction of it is dead. Lower values reclaim memory more aggressively.
MOLA_LO_ROBUST_KERNEL (Default: RobustKernel::GemanMcClure): Robust kernel type used in the ICP Gauss-Newton solver.
MOLA_LO_ROBUST_KERNEL_PARAM (Default: 6.0): Parameter for the robust kernel (scale; in normalized covariance units for the GICP pipeline).
MOLA_LO_ROBUST_KERNEL_PRIOR_REF_BLEND (Default: 0.0): Blend factor in [0, 1] for the residual reference used by the robust kernel in the Gauss-Newton solver. 0.0 (default) keeps the classic behavior, where each factor is judged only by how much it diverges from the current linearization point. With values >0 the kernel residual is blended toward the value predicted at the prior mean pose (e.g. the motion model / IMU prior), so correspondences inconsistent with the prior are down-weighted even when the current iterate is already corrupted. Has no effect when no prior is supplied to ICP.
MOLA_ICP_COVARIANCE_METHOD (Default: Censi3D; GICP pipeline only): Post-optimization SE(3) covariance estimation method. Censi3D is the sandwich estimator suited for cov-to-cov pipelines.
MOLA_ICP_COV_DEFAULT_POINT_SIGMA (Default: 0.01 [m]; GICP pipeline only): Per-point sigma used in covariance estimation.
MOLA_ICP_COV_FLOOR_XYZ (Default: 0.001 [m]; GICP pipeline only): Floor on the XYZ diagonal of the output covariance, to keep downstream filters numerically stable.
MOLA_ICP_COV_FLOOR_ANGLES_DEG (Default: 0.1 [deg]; GICP pipeline only): Floor on the angular diagonal of the output covariance.

GICP local map parameters

The following variables tune the mola::KeyframePointCloudMap used as local map in the GICP pipeline. They have no effect on the ICP or NDT pipelines.

MOLA_LOCALMAP_MAX_SEARCH_KEYFRAMES (Default: 3): Maximum number of keyframes searched for correspondences per ICP step.
MOLA_LOCALMAP_K_CORRESPONDENCES_FOR_COV (Default: 20): Number of nearest neighbors used to estimate per-point covariance in the local map.
MOLA_LOCALMAP_USE_VIEW_DIRECTION_FILTER (Default: true): Enable filtering of candidate keyframes by view direction, to avoid matching from very different angles.
MOLA_LOCALMAP_VIEW_DIRECTION_FILTER_ANGLE_DEG (Default: 120 [deg]): Maximum angular difference allowed between the current view direction and a candidate keyframe to be considered for matching.
MOLA_LOCALMAP_DIVERSE_KEYFRAMES (Default: 1): Number of diverse keyframes to force into the candidate set regardless of proximity. Must be less than MOLA_LOCALMAP_MAX_SEARCH_KEYFRAMES.
MOLA_LOCALMAP_VIZ_MAX_POINTS_PER_KF (Default: 10000): Maximum points rendered per keyframe in the GUI.
MOLA_LOCALMAP_VIZ_MAX_POINTS_OVERALL (Default: 500000): Maximum total points rendered for the local map in the GUI (e.g. to avoid FoxGlove WebSocket overflow).

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_LOAD_SM (Default: none): If set, loads an existing simple-map file at startup and continues appending keyframes to it.
MOLA_SIMPLEMAP_OUTPUT (Default: final_map.simplemap): Output file name for simple-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_MIN_NEARBY_POSES (Default: 1): Minimum number of nearby poses occupied before a new keyframe is accepted into the simple-map.
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 those maps faster to process later.
MOLA_SIMPLEMAP_ALSO_NON_KEYFRAMES (Default: false): If enabled, all LiDAR observations will generate a KeyFrame in the simple-map, but without raw sensory data if the keyframe does not fulfill the minimum distance criteria. Useful to generate, in post-processing, the full vehicle trajectory without missing any timestep.
MOLA_SAVE_DESKEWED_SCANS (Default: false): If enabled, deskewed (motion-compensated) scans are stored into simple-map keyframes instead of raw scans. Useful when post-processing requires already-compensated clouds.

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): Output file name for the TUM trajectory.

Observation validity filter

MOLA_ENABLE_OBS_VALIDITY_FILTER (Default: false): Enables a pre-filter that discards incoming scans that appear incomplete, e.g. due to faulty network connections or missing UDP packets.
MOLA_OBS_VALIDITY_MIN_POINTS (Default: 1000): Minimum number of points a scan must contain to be accepted when MOLA_ENABLE_OBS_VALIDITY_FILTER is enabled.

Initial localization

MOLA_LO_INITIAL_LOCALIZATION_METHOD (Default: InitLocalization::FixedPose): Strategy used to determine the initial pose at startup. Options:
- InitLocalization::FixedPose: Use a fixed pose defined by MOLA_INITIAL_* below.
- InitLocalization::PitchAndRollFromIMU: Collect IMU samples to determine initial orientation.
- InitLocalization::FromStateEstimator: Wait for an external state estimator to converge.
MOLA_INITIAL_X, MOLA_INITIAL_Y, MOLA_INITIAL_Z (Default: 0.0 [m]): Initial position when using InitLocalization::FixedPose.
MOLA_INITIAL_YAW, MOLA_INITIAL_PITCH, MOLA_INITIAL_ROLL (Default: 0.0 [deg]): Initial orientation when using InitLocalization::FixedPose.
MOLA_LO_INITIAL_IMU_SAMPLES (Default: 400): Number of IMU samples to collect for initial orientation calibration when using InitLocalization::PitchAndRollFromIMU.
MOLA_LO_INITIAL_IMU_USE_ORIENTATION (Default: true): Whether to use the IMU orientation quaternion (if provided by the driver) directly, instead of computing orientation from accelerometer readings.
MOLA_LO_INIT_SE_MAX_POS_SIGMA (Default: 0.5 [m]): Maximum position uncertainty accepted from the state estimator for InitLocalization::FromStateEstimator to be considered converged.
MOLA_LO_INIT_SE_MAX_ORI_SIGMA (Default: 3.0 [deg]): Maximum orientation uncertainty accepted from the state estimator for InitLocalization::FromStateEstimator.
MOLA_LO_INIT_SE_TIMEOUT (Default: 60.0 [s]): Timeout for InitLocalization::FromStateEstimator. If the state estimator does not converge within this time, initialization falls back to FixedPose.

Visualization

Note

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

MOLA_VEHICLE_MODEL_FILE (Default: none): Path to a 3D model file loadable via Assimp (e.g. Collada *.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): Transformation applied to the 3D asset 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_state_estimation_simple 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 the latest vehicle pose to be used as the initial guess instead.
MOLA_NAVSTATE_SIGMA_RANDOM_WALK_LINACC (Default: 1.0 [m/s^2]): Linear acceleration standard deviation.
MOLA_NAVSTATE_SIGMA_RANDOM_WALK_ANGACC (Default: 10.0 [rad/s^2]): 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.
MOLA_WRITE_DEBUG_ICP_LOG_IF_QUALITY_UNDER (Default: none): If set to a value in [0, 1], .icplog debug files are saved whenever ICP quality drops below that threshold, independently of MP2P_ICP_GENERATE_DEBUG_FILES. Useful for targeted debugging of bad frames without enabling full logging.
MOLA_DEBUG_DUMP_ICP_LOG_FROM_TIMESTAMP / MOLA_DEBUG_DUMP_ICP_LOG_TO_TIMESTAMP (Default: 0, disabled): A valid, non-empty window (both set, with TO >= FROM) saves .icplog files for all ICP runs whose timestamp falls within the [FROM, TO] range (in seconds, as Unix epoch or dataset time), independently of MP2P_ICP_GENERATE_DEBUG_FILES. Useful to capture a specific time window without enabling full logging. Each .icplog embeds the full local-map snapshot for that call (tens of MB), so keep the range narrow.

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 hood. 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 increases 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.

LO/LIO pipelines

Summary of ICP pipelines

1. Generalized ICP (GICP) pipeline for 3D LiDAR (lidar3d-gicp.yaml)

Selecting the local map class

2. ICP pipeline for 3D LiDAR (lidar3d-icp.yaml)

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

4. Pipeline for 2D LiDAR (lidar2d.yaml)

Configuring pipelines via environment variables

Sensor inputs: LiDAR

Sensor inputs: IMU (optional)

IMU gravity correction (pitch/roll)

Sensor inputs: Wheels odometry (optional)

Sensor inputs: GPS (GNSS) (optional)

Scan de-skew options

General options

Adaptive threshold

Local map update

Observation filter pipeline

ICP settings

GICP local map parameters

Simple-map generation

Trajectory files generation

Observation validity filter

Initial localization

Visualization

Motion model

ICP log files

Trace debug files

1. Generalized ICP (GICP) pipeline for 3D LiDAR (`lidar3d-gicp.yaml`)

2. ICP pipeline for 3D LiDAR (`lidar3d-icp.yaml`)

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

4. Pipeline for 2D LiDAR (`lidar2d.yaml`)