Global back end (Mapper)

mola_mapper is the global back end of a MOLA system. It runs next to a front end (LiDAR odometry) and its state estimator, and adds a central keyframe map, loop closure detection and a global pose graph that includes GNSS, gravity and gyroscope factors. It does little estimation of its own: the state estimator remains the only mola::NavStateFilter queried by the front end.


Role within the MOLA ecosystem

The modules of a system are independent and composed by a launch file:

  • mola::LidarOdometry (LIO) uses the state estimator (mola::state_estimation_smoother::StateEstimationSmoother) as its only NavStateFilter, and pushes sparse keyframes to the mapper through mola::SharedKeyframeMap.

  • mola::mapper::Mapper receives those keyframes and, as a mola::RawDataConsumer, the GNSS and IMU readings of the dataset.

  • No estimate flows from the mapper into the state estimator. Both may take the same GNSS readings: the state estimator for a live geo-referenced estimate (its own online geo-reference), the mapper for the geo-reference of its global map.

The mapper:

  1. Attaches to each keyframe the nearest GNSS reading, the accelerometer averaged before it (gravity), and the gyroscope rotation and wheel odometry increment since the previous keyframe.

  2. Searches for loop closures in a background thread, with the detector of mola_sm_loop_closure.

  3. Optimizes the pose graph of all keyframes in another background thread.

  4. Publishes the estimated geo-reference and, at the end of the run, writes the optimized trajectory, map and geo-reference.


Running on a dataset

mola-cli-launchs/ has complete systems (LIO, smoother, mapper and the GUI) for each input: lio_smoother_mapper_from_rosbag1.yaml, lio_smoother_mapper_from_rosbag2.yaml, lio_smoother_mapper_from_kitti.yaml and lio_smoother_mapper_from_oxford_spires.yaml.

For the datasets with a profile in mola_lidar_odometry (the ones of its mola-lo-gui-<name> launchers), mola-mapper-gui-<name> runs the same dataset through the mapper, with the profile’s topics, sensor poses and pipeline:

mola-mapper-gui-vbr /path/to/vbr/colosseo_train0/
mola-mapper-gui-kitti 00
# Any profile, e.g. TIERS with its sensor:
TIERS_SENSOR=ouster-os0 mola-mapper-gui-dataset tiers /path/to/sequence.bag

Launchers exist for conslam, fomo, geode, grandtour, hilti2022, kitti, newer-college, ntu-viral, oxford-spires, rosbag1, rosbag2 and vbr. They always run the smoother as the state estimator. Variables set by the caller win, e.g. MOLA_WITH_GUI=false MOLA_QUIT_ON_DATASET_END=true for a headless run, MOLA_MAPPER_OUTPUT_TUM for the outputs, MOLA_GUI_PREVIEW_SENSORS=true for the raw sensor windows (ROS 1 input).


Provided interfaces

Interface

Role

mola::SharedKeyframeMap

requestInsertKeyframe(): receives the keyframes of the front end (source frame, pose and covariance in that frame, observations).

mola::RawDataConsumer

Receives GNSS and IMU readings from the raw_data_source modules.

mola::MapSourceBase

Publishes the estimated geo-reference.

mola::DiagnosticsProvider

Keyframe, loop closure and factor counts, solves and loop closure scans.

mola::ExecutableBase

Module lifecycle. onQuit() runs the end-of-run passes and writes the outputs.


Reference frame conventions

  • {map} (reference_frame_name, default map): the frame of the optimized keyframe poses. It is the one of the front end keyframes: the first keyframe stays at its front end pose, which is also where the state estimator starts.

  • {enu}: the geo-reference T_enu_to_map is a variable of the graph when there are GNSS readings. It is published as a Georeferencing and can be saved to a YAML file.


Global pose graph

The graph has one pose variable per keyframe in {map} and T_enu_to_map. It is solved in batch with Levenberg-Marquardt (GTSAM), and, when it can be geo-referenced, the marginal covariance of T_enu_to_map is computed. Its factors are:

  • Odometry chain: between consecutive keyframes of the same front end source frame. Stiff by default. The sigma is a floor plus terms per square root of meter and per radian turned (odometry_noise_ang_per_rad). Keyframes of different source frames are joined by a weak edge.

  • Loop closures: Huber kernel, with floors on their sigmas (loop_closure_min_sigma_xyz, loop_closure_min_sigma_ang_deg), since the detector covariance is overconfident.

  • GNSS: ENU position factors (readings with a GGA message) with a selectable robust kernel (default Cauchy). Readings with a horizontal sigma above gnss_max_uncertainty_horiz are discarded. If the spread of the readings is too small for their sigmas, they are not used. The geo-reference is published and saved only once observable: at least georef_min_gnss_factors GNSS factors, and an azimuth sigma of T_enu_to_map below georef_max_sigma_yaw_deg.

  • Gravity: roll and pitch from the averaged accelerometer.

  • Gyroscope: relative rotation between consecutive keyframes, with free translation. The sigma combines the noise density, the bias sigma and the scale error.

  • A tight prior on the first keyframe (gauge) and a weak prior on T_enu_to_map.


Configuration parameters

All parameters are loaded from the params: subtree of the module YAML. The defaults are in params/mapper.yaml, where most of them can be overridden with an environment variable (shown in parentheses below).

- name: mapper
  type: mola::mapper::Mapper
  raw_data_source: "dataset_input"   # GNSS and IMU readings
  params: "../params/mapper.yaml"
Frame and sensors attached to the keyframes
reference_frame_name: "${MOLA_TF_MAP|map}"

# GNSS (regex of sensor labels; empty: none)
gnss_sensor_labels_re: "${MOLA_MAPPER_GNSS_LABELS|gps}"
gnss_max_time_offset: 0.5          # [s] (MOLA_MAPPER_GNSS_MAX_TIME_OFFSET)

# Whether to use the IMU (gravity, gyroscope); false leaves it to LIO, e.g. for de-skew
use_imu: true                      # (MOLA_MAPPER_USE_IMU)

# IMU whose averaged accelerometer levels the map (empty: none).
# The default is the label of the IMU topic of the launch files:
imu_sensor_labels_re: "${MOLA_MAPPER_GRAVITY_IMU_LABELS|imu}"
imu_gravity_window: 1.0            # [s] before each keyframe (MOLA_MAPPER_GRAVITY_WINDOW)
imu_gravity_min_samples: 10

# IMU whose gyroscope gives the rotation between keyframes (empty: none)
gyro_sensor_labels_re: "${MOLA_MAPPER_GYRO_LABELS|imu}"
# Only with a bias estimate, e.g. MOLA_NAVSTATE_IMU_BIAS_TRACKING=true in the
# state estimator; false: zero bias, with gyro_bias_sigma
gyro_require_bias_estimate: true   # (MOLA_MAPPER_GYRO_REQUIRE_BIAS)
gyro_noise_density: 3e-4           # [rad/s/sqrt(Hz)] (MOLA_MAPPER_GYRO_NOISE_DENSITY)
gyro_bias_sigma: 0.005             # [rad/s], without a bias source estimate
gyro_scale_sigma: 0.005            # (MOLA_MAPPER_GYRO_SCALE_SIGMA)
gyro_max_gap: 0.1                  # [s] longer gaps between readings: no factor

# Wheel odometry (CObservationOdometry) increments between keyframes (empty: none).
# The default is the label of the wheel odometry topic of the launch files:
wheel_odom_sensor_labels_re: "${MOLA_MAPPER_WHEEL_ODOM_LABELS|odom_wheels}"
wheel_odom_max_gap: 0.5            # [s] longer gaps between readings: no factor

# Camera images (CObservationImage) stored in the keyframes (empty: none)
image_sensor_labels_re: "${MOLA_MAPPER_IMAGE_LABELS|}"
image_max_time_offset: 0.5         # [s] (MOLA_MAPPER_IMAGE_MAX_TIME_OFFSET)
image_format: "jpg"                # jpg, png, inline (MOLA_MAPPER_IMAGE_FORMAT)
image_jpeg_quality: 95             # (MOLA_MAPPER_IMAGE_JPEG_QUALITY)

Images follow the GNSS rule: each one goes to the keyframe nearest in time, and a keyframe keeps the nearest image of each camera, so no image is stored twice. Once no closer image can replace it, an image is written to <output_simplemap>_Images/ and the keyframe keeps a reference to that file (lazy-load); with inline or no output_simplemap, images stay in RAM. They are not used by the graph yet, nor recorded with record_input (replays have no images). Images already stored as files (lazy-load datasets) are kept as they are. In lio_smoother_mapper_from_rosbag2.yaml the camera topics are MOLA_CAMERA_TOPIC, _1, _2 (labels camera, camera_1, camera_2): with MOLA_MAPPER_IMAGE_LABELS='camera.*', each keyframe gets at most one image of each camera, if one is within image_max_time_offset. Live, the images come from the BridgeROS2 image subscriptions (msg_type: Image), as the other raw data.

The gyroscope bias comes from the first mola::ImuBiasSource module with an estimate (e.g. the state estimator with MOLA_NAVSTATE_IMU_BIAS_TRACKING=true). Otherwise it is taken as zero, with sigma gyro_bias_sigma.

Graph: odometry chain
graph:
  odometry_uncertainty_multiplier: 0.0       # front end covariance, 0: ignored (MOLA_MAPPER_ODOM_SIGMA_MULTIPLIER)
  odometry_additional_noise_xyz: 0.002       # [m] (MOLA_MAPPER_ODOM_SIGMA_XYZ)
  odometry_additional_noise_ang_deg: 0.01    # [deg] (MOLA_MAPPER_ODOM_SIGMA_ANG)
  odometry_noise_xyz_per_sqrt_m: 0.01        # (MOLA_MAPPER_ODOM_SIGMA_XYZ_PER_SQRT_M)
  odometry_noise_ang_deg_per_sqrt_m: 0.05    # (MOLA_MAPPER_ODOM_SIGMA_ANG_PER_SQRT_M)
  odometry_noise_ang_per_rad: 0.02           # (MOLA_MAPPER_ODOM_SIGMA_ANG_PER_RAD)
Graph: wheel odometry
graph:
  use_wheel_odometry: true                # (MOLA_MAPPER_USE_WHEEL_ODOM)
  wheel_odom_min_sigma: 0.01              # [m] (MOLA_MAPPER_WHEEL_ODOM_MIN_SIGMA)
  wheel_odom_sigma_per_m: 0.03            # forward (MOLA_MAPPER_WHEEL_ODOM_SIGMA_PER_M)
  wheel_odom_lateral_sigma_per_m: 0.10    # (MOLA_MAPPER_WHEEL_ODOM_LATERAL_SIGMA_PER_M)
  wheel_odom_sigma_per_rad: 0.05          # [m/rad] (MOLA_MAPPER_WHEEL_ODOM_SIGMA_PER_RAD)
  wheel_odom_max_yaw_error_deg: 3.0       # [deg] (MOLA_MAPPER_WHEEL_ODOM_MAX_YAW_ERROR)
  wheel_odom_max_yaw_error_ratio: 0.2
  wheel_odom_min_ratio_to_front_end: 0.95 # (MOLA_MAPPER_WHEEL_ODOM_MIN_RATIO)
  wheel_odom_ratio_floor: 0.02            # [m]
  wheel_odom_robust_kernel: "none"        # (MOLA_MAPPER_WHEEL_ODOM_ROBUST_KERNEL)
  wheel_odom_robust_k: 3.0                # (MOLA_MAPPER_WHEEL_ODOM_ROBUST_K)
  wheel_odom_chain_robust_kernel: "cauchy"  # (MOLA_MAPPER_WHEEL_ODOM_CHAIN_ROBUST_KERNEL)
  wheel_odom_chain_robust_k: 1.0          # (MOLA_MAPPER_WHEEL_ODOM_CHAIN_ROBUST_K)

Each wheel odometry increment constrains the forward and lateral translation between consecutive keyframes, in the frame of the first one; the rotation is left to the gyroscope (skid steer wheel yaw rates are poor). Where there is one, the forward component of the odometry chain edge goes into a factor of its own with wheel_odom_chain_robust_kernel, so a front end that slips along a degenerate direction (LiDAR odometry in long straight streets falls short) gives way to the wheels, while the rest of the edge stays as stiff as before. A robust kernel on the wheel factors instead (wheel_odom_robust_kernel) rejects wheel slip, but it also rejects the wheels where the front end is the one wrong: without GNSS, the graph cannot tell them apart. Increments whose yaw disagrees with the gyroscope (by wheel_odom_max_yaw_error_deg plus wheel_odom_max_yaw_error_ratio of the gyroscope yaw) are not used, nor is the chain edge split there: wheels that lose encoder counts read false turns. Nor are increments shorter than wheel_odom_min_ratio_to_front_end of the front end forward motion (minus wheel_odom_ratio_floor), along its direction of travel (forward or reverse): a degenerate front end falls short of the real motion, never beyond it, while wheels that lose encoder counts read less than it. Wheel spin (more than real) is the case left: it is taken as a front end slip.

Graph: gyroscope, GNSS, gravity and loop closure factors
graph:
  use_gyro: true                  # (MOLA_MAPPER_USE_GYRO)
  gyro_min_sigma_deg: 0.02

  use_gnss: true                  # (MOLA_MAPPER_USE_GNSS)
  gnss_minimum_uncertainty: 0.5   # [m] (MOLA_MAPPER_GNSS_MIN_SIGMA)
  gnss_uncertainty_multiplier: 1.0   # (MOLA_MAPPER_GNSS_SIGMA_MULTIPLIER)
  gnss_max_uncertainty_horiz: 20.0   # [m]
  gnss_robust_kernel: "cauchy"    # huber, cauchy, geman_mcclure, tukey (MOLA_MAPPER_GNSS_ROBUST_KERNEL)
  gnss_robust_k: 1.5              # [sigmas] (MOLA_MAPPER_GNSS_ROBUST_K)
  georef_min_gnss_factors: 3
  georef_max_sigma_yaw_deg: 10.0  # [deg] (MOLA_MAPPER_GEOREF_MAX_SIGMA_YAW)

  use_imu_gravity: true           # (MOLA_MAPPER_USE_GRAVITY)
  imu_gravity_sigma_deg: 3.0      # (MOLA_MAPPER_GRAVITY_SIGMA_DEG)

  loop_closure_robust_huber_k: 1.5      # (MOLA_MAPPER_LC_HUBER_K)
  loop_closure_min_sigma_xyz: 0.05      # [m] (MOLA_MAPPER_LC_MIN_SIGMA_XYZ)
  loop_closure_min_sigma_ang_deg: 0.25  # [deg] (MOLA_MAPPER_LC_MIN_SIGMA_ANG)
Optimizer
enable_optimizer_thread: true
optimizer_min_period: 1.0          # [s] (MOLA_MAPPER_OPTIMIZER_PERIOD)
Loop closure
loop_closure_enabled: true         # (MOLA_MAPPER_LOOP_CLOSURE)
loop_closure_pipeline_file: "$(ros2 pkg prefix mola_mapper)/share/mola_mapper/params/loop-closure-f2f-mapper.yaml"  # (MOLA_MAPPER_LC_PIPELINE)
loop_closure_check_period_sec: 5.0 # [s] (MOLA_MAPPER_LC_PERIOD)
loop_closure_min_new_keyframes: 10
loop_closure_incremental: true
loop_closure_full_scan_every_n: 10
loop_closure_finalize_rounds: 4    # (MOLA_MAPPER_LC_FINALIZE_ROUNDS)
loop_closure_odometry_check: true  # (MOLA_MAPPER_LC_ODOMETRY_CHECK)
loop_closure_odometry_limits:
  max_xyz: 2.0                     # [m]
  max_xyz_per_m: 0.10              # (MOLA_MAPPER_LC_ODOMETRY_MAX_XYZ_PER_M)
  max_ang_deg: 5.0                 # [deg]
  max_ang_deg_per_m: 0.03          # [deg/m]
  max_z: 0                         # [m] vertical limit, 0: off (MOLA_MAPPER_LC_ODOMETRY_MAX_Z)
  max_z_per_m: 0.008               # [m/m] (MOLA_MAPPER_LC_ODOMETRY_MAX_Z_PER_M)
loop_closure_consistency_check: true  # (MOLA_MAPPER_LC_CONSISTENCY_CHECK)
loop_closure_consistency:
  neighborhood: 50.0               # [m] along the odometry path, at both ends
  max_xyz: 1.0                     # [m] (MOLA_MAPPER_LC_CONSISTENCY_MAX_XYZ)
  max_xyz_per_m: 0.04              # [m/m] of odometry path in the cycle (MOLA_MAPPER_LC_CONSISTENCY_MAX_XYZ_PER_M)
  max_ang_deg: 3.0                 # [deg] (MOLA_MAPPER_LC_CONSISTENCY_MAX_ANG)
  max_ang_deg_per_m: 0.03          # [deg/m] (MOLA_MAPPER_LC_CONSISTENCY_MAX_ANG_PER_M)
  min_inconsistent: 2

The detector is the mola_sm_loop_closure frame-to-frame engine, used only to find candidate edges. Scans are incremental (pairs with a new keyframe), with a full one every loop_closure_full_scan_every_n. At the end of the run, up to loop_closure_finalize_rounds passes of full scan plus re-optimization run until no new loop appears.

Each candidate is registered by ICP from a global registration guess (LC_GLOBAL_REGISTRATION): bev by default, a gravity-aligned search that is robust to the accumulated drift and rejects ambiguous matches (repetitive structures); kiss (KISS-Matcher, if built with it) or none (the current graph estimate). bev uses the roll and pitch of the keyframe poses: absolute with LiDAR-inertial odometry; with LiDAR-only odometry (e.g. KITTI) the estimated ones work too.

Each detected loop between keyframes of the same front end chain is compared with the odometry between them: if they disagree by more than a floor plus a fraction of the distance traveled (loop_closure_odometry_limits), the loop is a false match (e.g. repetitive geometry) and is rejected. Over a short distance the odometry drift is small, so this mostly removes short range false loops and loops matched facing the wrong way. Optionally (max_z > 0), a tighter limit applies to the vertical error alone (z of the odometry frame), which rejects matches between stacked floors. Its per meter allowance must stay below the floor height over the loop length, but above the vertical drift of the odometry, so it depends on the scene and the front end.

Over a long loop the odometry drift allows almost anything, so loops are also checked against each other: two loops whose ends are near each other (along the odometry path, neighborhood) close a cycle through the short odometry segments between their ends, which must be close to the identity (loop_closure_consistency limits: a floor plus a fraction of the odometry path in the cycle). A loop inconsistent with most of its neighbors, and with at least min_inconsistent of them (a tie is not), is an outlier: a place matched to the wrong one along a repetitive street. Each group of loops connected through neighbors is decided again from scratch whenever a loop joins it: starting with all of them, the worst outlier is dropped, one at a time, until there is none, and the dropped ones left without any inconsistent accepted neighbor are accepted again. So the result does not depend on the order the loops arrived in, and loops rejected early are accepted again once enough correct loops around them agree. With fewer correct loops than wrong ones that agree with each other, the wrong ones win until more correct loops arrive. Of three visits to a place, loops pass1-pass3 and pass2-pass3 are never compared (their first keyframes are far apart along the path). Votes are not weighted by distance: since the tolerance grows with the path in the cycle, a loop matched a few meters off can look consistent with correct loops far enough away (4.5 m off, with loops over ~45 m away at 4 %).

Note that the default of loop_closure_enabled in the C++ Parameters struct is false; the provided YAML enables it.

Outputs
# Written at the end of the run (empty: not written)
output_tum_trajectory: ""                 # (MOLA_MAPPER_OUTPUT_TUM)
output_simplemap: ""                      # (MOLA_MAPPER_OUTPUT_SIMPLEMAP)
output_georeference: ""                   # (MOLA_MAPPER_OUTPUT_GEOREF)
output_loop_closures: ""                  # (MOLA_MAPPER_OUTPUT_LOOPS)
output_state_estimator_tum: ""            # (MOLA_MAPPER_OUTPUT_SE_TUM)
output_state_estimator_georeference: ""   # (MOLA_MAPPER_OUTPUT_SE_GEOREF)
output_graph_input: ""                    # (MOLA_MAPPER_OUTPUT_GRAPH_INPUT)
  • output_tum_trajectory: optimized keyframe poses in {map}, in TUM format (timestamp tx ty tz qx qy qz qw), compatible with evo.

  • output_simplemap: mrpt::maps::CSimpleMap with the optimized keyframes and all their observations.

  • output_georeference: YAML with the datum, T_enu_to_map and its sigmas.

  • output_loop_closures: one edge per line.

  • output_state_estimator_*: the trajectory and geo-reference of the state estimator, for comparisons (the smoother needs keep_finalized_trajectory to return its trajectory).

  • output_graph_input: file prefix of the graph input, to re-solve it offline with mola-mapper-reoptimize.


GUI

With a visualizer module in the launch (MOLA_WITH_GUI=true), the mapper draws its keyframe graph in {map}, refreshed when it changes:

  • keyframes as small spheres (orange);

  • edges between consecutive keyframes of each front end (blue lines), except between keyframes farther apart than odometry_edge_max_length (no keyframes are created while revisiting mapped areas, and such edges would cross the map);

  • accepted loop closures (green, thicker lines), as bridges over their two keyframes, which overlap once the loop is closed;

  • optionally, rejected loop candidates (red: by the consistency check, gray: by the odometry check).

A mola_mapper window shows the counts of keyframes, loops (accepted, rejected), factors and solves, a button to run a loop closure scan now, and sliders for the sphere radius and line widths. Defaults in the visualization section of params/mapper.yaml:

visualization:
  keyframe_sphere_radius: 0.10   # [m] (0: hidden)    (MOLA_MAPPER_VIZ_KF_RADIUS)
  odometry_edge_width: 2.0       # [px] (0: hidden)   (MOLA_MAPPER_VIZ_ODOM_EDGE_WIDTH)
  odometry_edge_max_length: 15.0 # [m] (0: no limit)  (MOLA_MAPPER_VIZ_ODOM_EDGE_MAX_LENGTH)
  loop_edge_width: 4.0           # [px] (0: hidden)   (MOLA_MAPPER_VIZ_LOOP_EDGE_WIDTH)
  loop_edge_lift: 1.0            # [m] bridge height  (MOLA_MAPPER_VIZ_LOOP_EDGE_LIFT)
  show_rejected_loops: false     # (MOLA_MAPPER_VIZ_SHOW_REJECTED_LOOPS)
  rejected_loop_edge_width: 1.0  # [px]
  update_period: 1.0             # [s]
  align_front_end_frames: true   # (MOLA_MAPPER_VIZ_ALIGN_FRONT_ENDS)
  show_keyframe_map: false       # (MOLA_MAPPER_VIZ_KEYFRAME_MAP)
  keyframe_map_voxel: 0.20       # [m]  (MOLA_MAPPER_VIZ_KEYFRAME_MAP_VOXEL)
  keyframe_map_min_range: 1.5    # [m]  (MOLA_MAPPER_VIZ_KEYFRAME_MAP_MIN_RANGE)
  keyframe_map_max_range: 50.0   # [m]  (MOLA_MAPPER_VIZ_KEYFRAME_MAP_MAX_RANGE)
  keyframe_map_min_spacing: 2.5  # [m]  (MOLA_MAPPER_VIZ_KEYFRAME_MAP_MIN_SPACING)
  keyframe_map_color_min: -2.0   # [m]  (MOLA_MAPPER_VIZ_KEYFRAME_MAP_COLOR_MIN)
  keyframe_map_color_max: 12.0   # [m]  (MOLA_MAPPER_VIZ_KEYFRAME_MAP_COLOR_MAX)
  keyframe_map_point_size: 1.0   # [px] (MOLA_MAPPER_VIZ_KEYFRAME_MAP_POINT_SIZE)
  keyframe_map_glide: 1.5        # [s]  (MOLA_MAPPER_VIZ_KEYFRAME_MAP_GLIDE)
  camera:                        # scripted camera, see below
    enabled: false               # (MOLA_MAPPER_VIZ_CAMERA)
    overview_period: 40.0        # [s]  (MOLA_MAPPER_VIZ_CAMERA_OVERVIEW_PERIOD)
    closeup_period: 15.0         # [s]  (MOLA_MAPPER_VIZ_CAMERA_CLOSEUP_PERIOD)
    transition: 4.0              # [s]  (MOLA_MAPPER_VIZ_CAMERA_TRANSITION)
    closeup_distance: 40.0       # [m]  (MOLA_MAPPER_VIZ_CAMERA_CLOSEUP_DISTANCE)
    final_overview: 60.0         # [s] of data (MOLA_MAPPER_VIZ_CAMERA_FINAL_OVERVIEW)
    shot_min_correction: 0.5     # [m]  (MOLA_MAPPER_VIZ_CAMERA_SHOT_MIN_CORRECTION)
    shot_hold: 6.0               # [s]  (MOLA_MAPPER_VIZ_CAMERA_SHOT_HOLD)
    timeline_file: ""            # (MOLA_MAPPER_VIZ_CAMERA_TIMELINE)
    # ... more in params/mapper.yaml

align_front_end_frames moves the scene frame each front end draws in (for LIO, its publish_reference_frame, whose keyframes come as <frame>_kf) to its optimized pose in {map}, so the LIO local map and vehicle stay on top of the corrected graph after loop closures. Everything a front end draws in that frame moves rigidly with its newest correction, so after a loop closure LIO’s older path and decaying clouds no longer sit on the graph; only the mapper graph shows the corrected past.

show_keyframe_map draws the map itself from the keyframes: the scan of each keyframe (de-skewed with its IMU velocity buffer, range-filtered, voxel decimated) is built once, in the keyframe frame, and drawn at the current optimized pose of its keyframe, so every loop closure visibly corrects the map. Keyframes closer than keyframe_map_min_spacing to one already in the map add no cloud. Each keyframe has its own scene node, kept for the whole run: loop closures only move nodes, without uploading the clouds again, and they glide into place over keyframe_map_glide seconds (0: they jump), so a correction reads as a motion. Points are colored (turbo colormap, fixed range) by their height relative to the keyframe that observed them, which keeps colors meaningful on slopes and on stacked floors. Hide LIO’s own local map meanwhile (MOLA_GUI_SHOW_LOCAL_MAP=false): it does not show the corrections.

camera scripts the GUI camera, e.g. for videos. From its own thread, at rate Hz, it blends two views: an overview of the whole map (top view fitted to the keyframes plus overview_margin, at a fixed overview_azimuth, zooming out as the map grows; the far clip plane is pushed back as needed) and a close-up behind the vehicle (its latest pose advertised by the front end as a localization update, mapped through the optimized pose of the front end frame). They alternate by a schedule (overview_period, closeup_period, with smooth transition s; wall clock time), and only the overview is shown during the last final_overview seconds of data, if the data source tells its time (a dataset player). Every input is low-pass filtered, so loop closure corrections do not make the camera jump. A map correction larger than shot_min_correction meters gets a correction shot: the camera flies to the corrected region (around the keyframe that moved most), keeping its azimuth, the map glides into place once it is there, and after shot_hold seconds it flies back to the schedule (one shot at a time, none during the final overview). Shots need the map shown with keyframe_map_glide > 0. The keyframe graph and the front end view are corrected at once; only the map clouds glide. With timeline_file, the blend weights of the camera (close-up, shot) are written over time with the wall clock (a space separated text file; shots as comment lines), e.g. to speed up a recorded video differently in each part. Turn off the front end camera follow meanwhile (LIO: MOLA_GUI_CAMERA_FOLLOWS_VEHICLE=false). Both the map and the camera can also be toggled in the View tab of the mola_mapper window.


mola-mapper-reoptimize

Offline tool that re-solves a global graph saved with output_graph_input, to try other graph parameters without running the whole system again:

mola-mapper-reoptimize INPUT_PREFIX MAPPER_YAML OUTPUT_PREFIX [--no-loops] [--keep-initial]
  • INPUT_PREFIX: the output_graph_input prefix.

  • MAPPER_YAML: a mapper parameters file; only its params.graph section is used (environment variables apply, as in a launch).

  • OUTPUT_PREFIX: writes OUTPUT_PREFIX.tum and OUTPUT_PREFIX_georef.yaml.

  • --no-loops: ignore the loop closures.

  • --keep-initial: start from the saved initial guesses. By default, the front end increments are chained from the first keyframe.


Live SLAM on ROS 2 topics

mola-cli-launchs/lio_smoother_mapper_ros2.yaml runs the same system on live ROS 2 topics, with mola::BridgeROS2 as the data source instead of a bag. The bridge subscribes to the sensor topics, reads the sensor poses from /tf, and publishes the fused pose of the state estimator (/state_estimation/pose, map -> base_link on /tf), the LIO local map (/lidar_odometry/localmap_points) and /diagnostics. The mapper corrections are fed back to LIO, which rebuilds its local map after each loop closure.

export MOLA_LIDAR_TOPIC=/ouster/points
export MOLA_IMU_TOPIC=/ouster/imu
export MOLA_GNSS_TOPIC=/gps                # optional, sensor_msgs/NavSatFix
export MOLA_ODOM_TOPIC=/odom               # optional, wheel odometry (used by the mapper too)
export MOLA_MAPPER_OUTPUT_TUM=mapper.tum   # outputs are written on exit
mola-cli $(ros2 pkg prefix mola_mapper)/share/mola_mapper/mola-cli-launchs/lio_smoother_mapper_ros2.yaml

It accepts the variables of the rosbag2 launch file below (except the input bag and the camera topics), plus MOLA_TF_MAP, MOLA_TF_BASE_LINK and the ROS 2 publishing options of BridgeROS2. To try it with a recorded bag, play it with ros2 bag play --clock and add ROS_ARGS='["--ros-args", "-p", "use_sim_time:=true"]'.


Running from a rosbag2 with mola-cli

mola-cli-launchs/lio_smoother_mapper_from_rosbag2.yaml (installed in share/mola_mapper/mola-cli-launchs) loads:

  • mola::Rosbag2Dataset: reads the bag and feeds the other modules.

  • mola::LidarOdometry: the front end (pipeline from mola_lidar_odometry).

  • mola::state_estimation_smoother::StateEstimationSmoother: the state estimator queried by LIO.

  • mola::mapper::Mapper: parameters from params/mapper.yaml.

  • Optionally, a GUI module.

export MOLA_INPUT_ROSBAG2=/path/to/bag
export MOLA_LIDAR_TOPIC=/ouster/points
export MOLA_IMU_TOPIC=/ouster/imu
export MOLA_GNSS_TOPIC=/gps/fix          # optional
export MOLA_MAPPER_OUTPUT_TUM=/tmp/mapper.tum
mola-cli $(ros2 pkg prefix mola_mapper)/share/mola_mapper/mola-cli-launchs/lio_smoother_mapper_from_rosbag2.yaml

Environment variables of the launch file:

Variable

Default

Description

MOLA_INPUT_ROSBAG2

(required)

Path to the rosbag2.

MOLA_LIDAR_TOPIC

/ouster/points

LiDAR topic (sensor label lidar).

MOLA_IMU_TOPIC

/imu

IMU topic (sensor label imu), optional.

MOLA_ATTITUDE_IMU_TOPIC, MOLA_ATTITUDE_IMU_NAME

empty, imu_attitude

Optional second IMU topic and its sensor label.

MOLA_GNSS_TOPIC

empty

GNSS topic (sensor label gps), optional.

MOLA_CAMERA_TOPIC, _1, _2

empty

Camera image topics (labels camera, camera_1, camera_2), optional. Stored in the mapper keyframes with MOLA_MAPPER_IMAGE_LABELS='camera.*'.

MOLA_ODOMETRY_TOPIC

empty

Wheel odometry topic (sensor label odom_wheels), optional.

MOLA_TF_BASE_LINK, MOLA_TF_TOPIC, MOLA_TF_STATIC_TOPIC

base_link, /tf, /tf_static

Vehicle frame and TF topics read from the bag.

MOLA_TF_MAP

map

Map frame (reference_frame_name of the mapper).

MOLA_WITH_MAPPER

true

false runs LIO and the state estimator only.

MOLA_WITH_GUI

true

Enable the GUI and the LiDAR preview.

MOLA_TIME_WARP, MOLA_DATASET_START_PAUSED, MOLA_QUIT_ON_DATASET_END

1.0, false, false

Dataset playback controls.

MOLA_ODOMETRY_PIPELINE_YAML

lidar3d-gicp.yaml

LIO pipeline file.

MOLA_STATE_ESTIMATOR_YAML

smoother default

State estimator parameters.

MOLA_MAPPER_YAML

../params/mapper.yaml

Mapper parameters (relative to the launch file).

The mapper settings (MOLA_MAPPER_*) are listed in Configuration parameters. Useful combinations:

  • A live geo-referenced estimate too: MOLA_ESTIMATE_GEO_REF=true (the state estimator geo-references itself on the fly from the same GNSS readings as the mapper).

  • Gravity factors are added whenever the IMU topic is given. Gyroscope factors are added too while the state estimator estimates the gyroscope bias: MOLA_NAVSTATE_IMU_BIAS_TRACKING=true (or MOLA_MAPPER_GYRO_REQUIRE_BIAS=false to use a zero bias instead).

  • The IMU only for LIO (e.g. de-skew), not in the mapper: MOLA_MAPPER_USE_IMU=false.

When the bag ends, stop the application (or set MOLA_QUIT_ON_DATASET_END=true): the mapper then runs its final loop closure passes and writes the outputs.


This launch file loads no ROS 2 bridge module, so it publishes no ROS 2 topics nor /tf: results are read from the output files (see Configuration parameters).