Improve GLIM three-LiDAR map quality

[FieldDiTian]

Summary

This aligns the ucb-roar GLIM path with the proven perception-ws
three-LiDAR mapping contract while keeping dataset-specific topics,
calibration, GNSS datum, output paths, and scratch paths injected per run.

The full Laguna Run1 regression identified two separate long-run failures:

• grouping ten scans into one rigid global submap removed the successful
  one-scan mapping cadence;
• the inherited isotropic odometry sigma of 0.001 made the long chain much
  too stiff in both translation and roll/pitch, so position-only GNSS plus
  inlier-only repeated-track loops could deform the map orientation.

This PR keeps one scan per geometric submap, independently batches global
optimization, independently samples loop-registration nodes, uses honest
rotation/translation odometry covariance, adds an opt-in fused-INS body-Z
prior, deterministically bounds loop candidates, gates loop corrections, and
fully converges each loop batch before map-health callbacks.

It also retains the earlier PR15 fixes for three-LiDAR timing, bounded dense
map memory, fused-GNSS factor delivery and health, deterministic final drain,
true-EOF batch exit, and exact ENU-to-ENU map export.

No map, bag, log, generated config, report, PNG, build output, temporary
runner, QoS file, or process file is committed.

Root cause and repaired behavior

The earlier ten-keyframe map had an optimized-trajectory rotation P95 of
13.123 deg and 2,637 frames above 10 degrees. The inherited 0.001
odometry sigma represented 1 mm and 0.057 degrees per step; over the long
multi-lap chain, it overruled the tested 0.5-degree gravity evidence.
Repeated Laguna geometry then allowed high-inlier loop registrations in the
wrong longitudinal phase. A loose 1 m/5-degree loop check was not sufficient,
and a single partially relinearized iSAM2 update could expose a transient
deformation to the GNSS health callback.

The repaired profile uses:

• one scan per geometric submap;
• global optimizer update interval 10;
• loop-registration interval 10;
• odometry sigma 0.01 rad / 0.05 m;
• fused-INS body-Z sigma 0.5 degrees, without a yaw prior;
• closest-one deterministic loop candidate per source;
• 0.3 m / 1.0-degree loop correction admission limits;
• a 30 s loop-detection synchronization bound;
• up to 20 forced-relinearization passes, stopping on convergence;
• dense per-scan point offload/restoration.

The shared GLIM JSON retains backward-compatible behavior: legacy odometry
defaults remain 0.001/0.001, gravity remains disabled, loop correction gates
remain disabled, loop cadence remains every submap, and extra loop updates
remain zero. The production generator injects the validated high-quality
profile explicitly.

File-by-file rationale

• GLIM/glim/config/config_global_mapping_pose_graph.json: documents and
  exposes independent optimizer/loop cadence, odometry rotation/translation
  sigmas, loop candidate/correction/synchronization controls, refinement
  bounds, and dense-point offload with backward-compatible defaults.
• GLIM/glim/include/glim/mapping/global_mapping_pose_graph.hpp: declares the
  new cadence and covariance parameters, loop synchronization state, accepted
  correction maxima, convergence telemetry, and dense-point bookkeeping.
• GLIM/glim/include/glim/util/time_keeper.hpp: exposes a setter for the
  configured point timestamp offset instead of reaching into private state.
• GLIM/glim/src/glim/mapping/global_mapping_pose_graph.cpp: preserves
  per-scan pose-graph nodes while batching iSAM2, retains only a bounded
  loop-registration working set, applies anisotropic odometry noise,
  deterministic candidate selection and correction gates, synchronizes loop
  completion, converges loop batches, restores dense exports, and records
  auditable telemetry.
• GLIM/glim/src/glim/util/export_factors.cpp: registers
  Pose3AttitudeFactor so the opt-in body-Z evidence is preserved when the
  final factor graph is serialized.
• GLIM/glim/src/glim/util/time_keeper.cpp: implements the timestamp-offset
  setter.
• GLIM/glim_ext/README.md: documents the opt-in gravity-direction factor and
  its validated-orientation requirement.
• GLIM/glim_ext/config/config_gnss_global.json: exposes recent-window fit,
  covariance bounds, robust width, optimized-anchor health, persistent
  failure, and disabled-by-default body-Z settings.
• GLIM/glim_ext/modules/mapping/gnss_global/include/glim_ext/gnss_global_module.hpp:
  fits the newest trajectory segment spanning the requested baseline, carries
  GNSS covariance through interpolation, inserts factors atomically, measures
  post-iSAM2 anchor health, rejects only persistent divergence, and optionally
  constrains body Z from a validated fused-INS quaternion without constraining
  yaw.
• GLIM/glim_ros2/include/glim_ros/glim_ros.hpp: exposes aggregate extension
  health and stores the injected IMU input-vector rotation.
• GLIM/glim_ros2/include/glim_ros/lidar_concat.hpp: makes relative-time
  primary clouds wait until each auxiliary stream crosses the primary header
  before selecting the nearest sweep.
• GLIM/glim_ros2/src/glim_pcap_rosbag.cpp: propagates mapping-quality failure
  status from the PCAP-backed runner.
• GLIM/glim_ros2/src/glim_ros/glim_ros.cpp: rotates acceleration and gyro
  into the calibrated IMU frame, throttles against the slowest mapping stage,
  drains extension work before serialization, and exposes extension health.
• GLIM/glim_ros2/src/glim_rosbag.cpp: supports bounded two-pass indexing,
  exact EOF accounting, strict three-LiDAR boundary handling, nonzero quality
  failures, and auto_quit=true so a true EOF drains, saves, and exits without
  an interactive wait.
• GLIM/glim_ros2/test/lidar_concat_point_time_test.cpp: adds the formal
  regression proving that a relative-time primary waits for the future
  auxiliary sweep.
• README.md: documents the mapping contract, every path/topic/parameter
  injection point, independent geometric/optimizer/loop cadence, exact datum
  conversion, root cause, and validated full-run profile.
• scripts/export_glim_dump_to_pcd.py: separates input GNSS datum from output
  map datum and applies the exact WGS84 ENU-to-ENU transform without fitting a
  second alignment.
• scripts/generate_glim_mapping_config.py: creates a self-contained,
  dataset-independent profile with explicit topics, extrinsics, IMU
  correction, GNSS gates, output/config/scratch paths, and validated
  high-quality pose-graph parameters.

Build and focused tests

Base: ucb-roar at
69eb5742ebe7bb60f58e0a8525963f5800809262.

• external targeted builds passed for GLIM core and the GNSS extension;
• the existing lidar_concat_point_time_test passed;
• a 40 s mapping/serialization smoke passed with position plus
  Pose3AttitudeFactor factors preserved in the saved graph;
• Python compile checks passed for the production generator and exporter;
• generated-config validation covered topics, calibration, recent GNSS fit,
  strict absolute scratch/output paths, independent cadence, loop gates,
  odometry sigmas, and gravity prior;
• git diff --check passed;
• final PR15 worktree is clean.

Full Laguna Run1 mapping

Input: decoded three-LiDAR Run1, /novatel_a/imu_body, and fused /gnss.

• process return code 0;
• primary frames received/forwarded: 25,096/25,096;
• strict/IMU/timed/no-plan/incomplete/pending skips: all zero;
• geometric/loop-registration submaps: 25,065/2,507;
• loop proposals/evaluated/completed: 2,008/2,008/2,008;
• loop sanity rejects/accepted/dropped: 713/1,295/0;
• maximum accepted loop correction: 0.299438 m / 0.993978 degree;
• loop synchronization timeouts: 0;
• refinement batches/converged/cap-exhausted: 1,295/1,295/0;
• GNSS fit RMS: 0.107 m;
• position/body-Z factors: 24,838/24,838;
• factors delivered/undelivered: 49,676/0;
• anchor coverage/median/health: 0.991 / 0.000 m / true.

Optimized trajectory versus fused GNSS/INS:

Metric	Earlier 10-keyframe map	Repaired map
position median / P95	0.081 / 0.694 m	0.000961 / 0.008112 m
rotation median / P95	4.900 / 13.123 deg	1.743 / 5.099 deg
rotation P99 / max	20.046 / 30.386 deg	6.946 / 21.779 deg
rotation >10 deg	2,637	41
body-Z P95	12.869 deg	0.583 deg

The final 0.15 m map has 140,235,636 points. Same-frame comparison against the
perception-ws references:

Reference	Median	P95	Within 1 m
32.5M consistent map	0.631 m	3.916 m	64.35%
130.2M full map	0.187 m	1.861 m	88.82%

The earlier ten-keyframe map versus the 32.5M reference was 1.131 m median,
6.667 m P95, and 46% within 1 m.

Full PR14 GICP replay on the repaired map

The existing PR14 small_gicp path replayed all of Run1 at 1.0x:

• source/final-pose debug samples: 25,096/25,095;
• SCAN DEBUG rows: 25,045, all accepted;
• normal/partial: 25,044/1; rejection streak: 0;
• accepted position median/P95/max:
  1.284 / 3.786 / 9.973 m; zero over 20 m;
• accepted rotation median/P95/P99/max:
  1.920 / 5.150 / 7.346 / 19.990 deg;
• accepted rotation frames above 10 degrees: 40;
• trusted perception-ws 32.5M baseline above 10 degrees: 41;
• earlier PR15 map above 10 degrees: 407;
• fused-GNSS wrong-lock resets: 50;
• GICP median/P95/P99: 7.24 / 100.27 / 355.40 ms;
• six complete laps plus two partial segments generated with the generic PR14
  top-down tool.

This passes the requested map/GICP gates and fixes the 10x orientation tail.
The position difference was reviewed and accepted for this result. Reset
frequency and P95 latency remain explicit localization-runtime watchpoints.

All reports, maps, logs, MCAP, and top-down images remain under the Laguna
dataset directories and are not PR content.

[FieldDiTian]

Addressed the requested P2 final-flush issue in c3d83fb7c228aba4b9b8a496631f41891ea832e0.

GlobalMappingPoseGraph::update_optimizer() now invokes Callbacks::on_smoother_update(...) before the empty-work check and returns only when the post-callback values and factors are both empty. This lets asynchronously emitted GNSS tail factors enter iSAM2 during the final pre-save flush instead of remaining undelivered at EOF.

Validation:

• rebuilt glim, glim_ros, and glim_ext, including libgnss_global.so;
• targeted empty-core-batch regression injected a value/factor from the extension callback and confirmed the iSAM2 result callback;
• colcon test: 7 tests, 0 errors, 0 failures, 0 skipped;
• git diff --check passed.