mola_sm_loop_closure

Simplemap loop-closure postprocessing library and CLI tool

Links

README

Distro	Build dev	Build releases	Stable version
ROS 2 Humble (u22.04)		amd64 arm64
ROS 2 Jazzy @ u24.04		amd64 arm64
ROS 2 Kilted @ u24.04		amd64 arm64
ROS 2 Rolling (u24.04)		amd64 arm64

mola_sm_loop_closure

Offline loop-closure engine for MOLA CSimpleMap files. Given an input simplemap produced by any MOLA odometry front-end, the package re-optimises all keyframe poses by detecting and closing loops, then writes the corrected simplemap back to disk.

Two algorithms

Algorithm	Class	Best for
SimplemapLoopClosure	`mola::SimplemapLoopClosure`	Long maps with noticeable drift; groups keyframes into submaps and runs heavy point-cloud ICP between submap pairs
FrameToFrameLoopClosure	`mola::FrameToFrameLoopClosure`	GNSS-augmented datasets or quick re-optimisation; runs frame-to-frame ICP with a GNC graph optimizer

CLI usage

# SimplemapLoopClosure (default algorithm):
mola-sm-lc-cli -i in.simplemap -o out.simplemap \
    -p pipelines/loop-closure-lidar3d-gicp.yaml

# FrameToFrameLoopClosure:
mola-sm-lc-cli -i in.simplemap -o out.simplemap \
    -a mola::FrameToFrameLoopClosure \
    -p pipelines/loop-closure-f2f-lidar3d-gicp.yaml

Available pipelines

YAML file	Sensor type	Algorithm
`loop-closure-lidar3d-gicp.yaml`	3-D LiDAR (GICP)	SimplemapLoopClosure
`loop-closure-lidar3d-icp.yaml`	3-D LiDAR (point-to-point ICP)	SimplemapLoopClosure
`loop-closure-lidar2d.yaml`	2-D LiDAR	SimplemapLoopClosure
`loop-closure-f2f-lidar3d-gicp.yaml`	3-D LiDAR (GICP)	FrameToFrameLoopClosure

Key YAML knobs

SimplemapLoopClosure

submap_max_absolute_length / submap_min_absolute_length : controls submap granularity.
assume_planar_world: true enables annealed soft planar constraints (z, roll, pitch).
- planar_world_initial_sigma_z, planar_world_initial_sigma_ang, planar_world_annealing_rounds : tune the annealing schedule.
- planar_world_hard_flatten: true restores the old hard-flattening behaviour.
use_gnss: true / gnss_add_horizontality: true : GNSS-assisted global alignment.
- gnss_factor_strategy: "submap" (default, scalable) or "per_kf" (sensor-pose-aware, larger graph).
- gnss_max_uncertainty_horiz (default 20.0 m) : reject GPS readings whose horizontal ENU uncertainty (√(σ_E²+σ_N²)) exceeds this threshold.
- gnss_max_uncertainty_vert (default 40.0 m) : reject GPS readings whose vertical ENU uncertainty (σ_U) exceeds this threshold.
- Stats on accepted/rejected GNSS readings are printed at the INFO log level after processing.
use_imu_gravity: true / imu_gravity_sigma_deg : IMU-derived gravity-alignment factors added in stage 1.

FrameToFrameLoopClosure

lc_candidate_strategy : DISTANCE_STRATIFIED (default), PROXIMITY_ONLY, or MULTI_OBJECTIVE.
assume_planar_world: true : planar-world annealing (subset of SM options; IMU-gravity options are not exposed by FrameToFrameLoopClosure::Parameters).
use_gnss: true : per-keyframe GNSS factors (FactorGnssEnu).
- gnss_max_uncertainty_horiz (default 20.0 m) : reject GPS readings whose horizontal ENU uncertainty exceeds this threshold.
- gnss_max_uncertainty_vert (default 40.0 m) : reject GPS readings whose vertical ENU uncertainty exceeds this threshold.
manual_loop_constraints : list of hand-specified loop closure edges (by UNIX timestamp pair). Each entry supports:
- timestamp_i, timestamp_j : UNIX timestamps identifying the two keyframes.
- sigma_xyz : positional uncertainty [m].
- trust_as_inlier (default false): if true, the constraint is added to the GNC set of known inliers and will not be downweighted by the robust optimizer.
use_kiss_matcher: true : use KISS-Matcher global registration to seed the ICP initial guess for each loop-closure candidate (default: false; requires the third_party/kiss-matcher submodule to be populated).
- kiss_matcher_resolution (default 1.0 m) : voxel size for KISS-Matcher feature extraction; controls normal_radius ≈ 3× and fpfh_radius ≈ 5×.
- kiss_matcher_layer (default "points_to_register_points") : name of the mp2p_icp::metric_map_t layer whose points are fed into KISS-Matcher.

See the online tutorial for a step-by-step example.

License

This package is released under the GNU GPL v3 license as open source, with the main intention of being useful for research and evaluation purposes. Commercial licenses available upon request.

Contributions require acceptance of the Contributor License Agreement (CLA).