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

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

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

Copyright (C) 2018-2026 Jose Luis Blanco jlblanco@ual.es, University of Almeria

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

Changelog for package mola_sm_loop_closure

1.2.0 (2026-05-11)

• f2f pipeline: switch to multi-objective as default strategy

• Merge pull request #15 from MOLAorg/simplify-ci CI: simplify clang-format helpers and use ros: docker images for stable builds

• fix: wrap find -iname predicates in parentheses to scope them to listed dirs Without grouping, the -o operators apply at top level and -print0 only attaches to the last alternative, so files outside the intended directories can be matched.

• CI: simplify clang-format helpers and use ros: docker images for stable builds

  • Replace complex Python-based clang_git_format with a simple scripts/formatter.sh supporting --check
  • Standardize formatter to scripts/formatter.sh (moved from root formatter.sh)
  • Simplify check-clang-format.yml to just apt-install clang-format-14 and run the script
  • Use ros:humble / ros:jazzy pre-built images for stable CI builds (faster, no setup-ros needed)

• fix:as libflann as build_depend as needed for kiss-matcher

• CI: sensible job names

• bump min req cmake version to 3.22

• feat: live gui, show GNSS data points

• update package.xml build deps

• Merge pull request #14 from MOLAorg/improve-lc-hypothesis-coverage Improve lc hypothesis coverage

• fix: deduplicate candidates by block-pair ID before selection in find_loop_candidates Multiple (i,j) frame pairs can round to the same (frameGroup_i, frameGroup_j) block and all pass the alreadyChecked filter (which only holds pairs from previous rounds). All of them survived into the scored candidate list, so max_lc_candidates were returned but the evaluation loop in process() silently skipped all but the first occurrence of each block pair -- causing far fewer ICP evaluations than expected. Fix: after scoring (step 1) and before selection (step 2), deduplicate each candidate container by block-pair ID, keeping only the highest-scored entry per block. This is applied to both the flat candidates vector (PROXIMITY_ONLY, MULTI_OBJECTIVE) and each distance bin (DISTANCE_STRATIFIED), so max_lc_candidates now matches the number of candidates that will actually be evaluated. Co-Authored-By: Claude Sonnet 4.6 <<noreply@anthropic.com>>

• fix: MULTI_OBJECTIVE coverage term now uses spatial midpoints instead of sin formula The old coverage score used abs(sin(pi * topologicalMidpoint / totalFrames)), which only rewards candidates near the center of the frame index range and ignores actual map geometry. This caused clustering in the same physical region on non-linear trajectories (U-shapes, figure-8s, long straights). New approach:

  - Add LoopCandidate::spatialMidpoint ((pose_i + pose_j) / 2, computed once per candidate in find_loop_candidates()).

  - Track selectedMidpoints alongside selectedDistances in the greedy selection loop.

  - Coverage score penalizes candidates whose spatial midpoint is within ~20 m of an already-selected midpoint, forcing the selected set to cover different map areas regardless of trajectory shape.

  - Refactor score_multi_objective() to accept a MultiObjectiveArgs struct instead of a long flat parameter list, making call sites self-documenting. Co-Authored-By: Claude Sonnet 4.6 <<noreply@anthropic.com>>

• fix: MULTI_OBJECTIVE diversity scoring is now actually applied during selection selectedDistances was declared but never populated, so score_multi_objective() always computed diversity against an empty set (score = 1.0 always). The fix replaces the simple sort+truncate step 2 path for MULTI_OBJECTIVE with a greedy selection loop: pick the best candidate, record its distance, re-score the rest with the updated selectedDistances, repeat. This makes the diversity term a real participant in selection instead of a no-op. Co-Authored-By: Claude Sonnet 4.6 <<noreply@anthropic.com>>

• fix: live preview GNC stats no longer reset to zero at each LC round start liveStats was zero-initialized at the top of every lcRound loop, causing the 3Dscene caption to show "0 inliers, 0 outliers" until the first GNC optimization ran in that round. Carry lastGncInliers / lastGncOutliers forward so the caption always reflects the most recent optimizer result. Co-Authored-By: Claude Sonnet 4.6 <<noreply@anthropic.com>>

File truncated at 100 lines see the full file