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 onlyNavStateFilter, and pushes sparse keyframes to the mapper throughmola::SharedKeyframeMap.mola::mapper::Mapperreceives those keyframes and, as amola::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:
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.
Searches for loop closures in a background thread, with the detector of
mola_sm_loop_closure.Optimizes the pose graph of all keyframes in another background thread.
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 |
|---|---|
|
|
|
Receives GNSS and IMU readings from the |
|
Publishes the estimated geo-reference. |
|
Keyframe, loop closure and factor counts, solves and loop closure scans. |
|
Module lifecycle. |
Reference frame conventions¶
{map} (
reference_frame_name, defaultmap): 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_mapis a variable of the graph when there are GNSS readings. It is published as aGeoreferencingand 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_horizare 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 leastgeoref_min_gnss_factorsGNSS factors, and an azimuth sigma ofT_enu_to_mapbelowgeoref_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::CSimpleMapwith the optimized keyframes and all their observations.output_georeference: YAML with the datum,T_enu_to_mapand 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 needskeep_finalized_trajectoryto 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: theoutput_graph_inputprefix.MAPPER_YAML: a mapper parameters file; only itsparams.graphsection is used (environment variables apply, as in a launch).OUTPUT_PREFIX: writesOUTPUT_PREFIX.tumandOUTPUT_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 frommola_lidar_odometry).mola::state_estimation_smoother::StateEstimationSmoother: the state estimator queried by LIO.mola::mapper::Mapper: parameters fromparams/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 |
|---|---|---|
|
(required) |
Path to the rosbag2. |
|
|
LiDAR topic (sensor label |
|
|
IMU topic (sensor label |
|
empty, |
Optional second IMU topic and its sensor label. |
|
empty |
GNSS topic (sensor label |
|
empty |
Camera image topics (labels |
|
empty |
Wheel odometry topic (sensor label |
|
|
Vehicle frame and TF topics read from the bag. |
|
|
Map frame ( |
|
|
|
|
|
Enable the GUI and the LiDAR preview. |
|
|
Dataset playback controls. |
|
|
LIO pipeline file. |
|
smoother default |
State estimator parameters. |
|
|
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(orMOLA_MAPPER_GYRO_REQUIRE_BIAS=falseto 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).