Fix GNSS validation and add 1x compressed-map GICP pipeline

[FieldDiTian]

Summary

Follow-up to merged PR #15. This branch addresses Allen's GLIM/adapter review findings and adds the complete dataset-scoped Laguna mapping, consistency-compression, and audited 1.0x GICP pipeline.

Review fixes

• Credit GNSS factor delivery only after the successful iSAM2 result contains the exact factor identities; stale, exceptional, and unconfirmed batches remain undelivered.
• Require both a minimum-size alignment training window and a held-out validation suffix, applying the fit gate to training and prediction RMS.
• Prevent the recent-baseline alignment window from collapsing below the configured training minimum.
• Return nonzero after saving a run that produced zero submaps.
• Make replay audits fail closed on analyzer, input, evidence-coverage, acceptance, rejection-streak, and drop gates.
• Validate GNSS/GT fields, covariance, frames, interpolation gaps, recovery state, FLOAT64 point-time encoding, map provenance, and multi-bag seek alignment.
• Record the active dataset profile and per-stage/total scan timing so the retained evidence is self-describing.

Root cause and GICP fix

The compressed map's frame, scale, bounds, and ENU datum were correct. The low acceptance came from initializing the optimizer outside the compressed map's local convergence basin, followed by unbounded long-tail solves; most candidates were then correctly rejected by the unchanged 5 m Atlas wrong-basin gate.

The final front_quality_replay.yaml profile and localizer changes:

• load the compressed target at 0.25 m and downsample the merged localization scan at 0.30 m within a 100 m crop;
• keep deskew and the deployed INS motion/yaw prior enabled;
• use Atlas translation only as the GICP optimizer's initial translation, capped at 20 m;
• never blend Atlas position into basePose, observer state, or published localization;
• preserve correspondence, jump, and the 5 m Atlas candidate-position gate;
• bound the iterative optimizer to 32 iterations and an 80 ms cooperative optimizer budget, failing closed through the existing prior/recovery path on timeout;
• count nan/inf timeout rows in the scorecard instead of dropping them from the denominator.

Mapping and compression pipeline

• Added dataset-root-aware runners and documentation for three-LiDAR GLIM mapping, per-lap XYZ export, cross-lap consistency filtering, sparse staging coverage, deterministic replay-bag preparation, 1.0x GICP audit, and reproducible top-down plots.
• Laguna Run1 mapping consumed 25,096 front + 25,096 left + 25,096 right scans and produced 25,065 submaps.
• GNSS alignment completed at 0.139 m training RMS and 0.169 m validation RMS; 49,688 factors were delivered with zero undelivered factors or delivery failures.
• The final consistent map contains 32,539,032 XYZ points and is 390,468,562 bytes, an 82.625% reduction from this run's 2,247,349,556-byte dense export.
• Its size is comparable to the existing 389,937,195-byte Laguna reference map (+0.136%). The reference map was not rerun, modified, or replaced.
• Final PCD SHA-256: f24a99c94db61e4e4252a0ac481182516e54e90b6306d135c3abb7cf34e26b4e.

Latest 500 s strict three-LiDAR 1.0x audit

The latest retained audit uses commit 4c86beea3c63c25d2396e97425d3882eace138d5, the Laguna local-ENU consistency map, and the same 500-second source interval for front, right, and left LiDAR.

• Source frames: front 5,000, right 5,000, left 5,000.
• Strict merge: lidar_concat=true, require_all_aux=true; incomplete clouds are skipped, never degraded.
• Registered frames: 4,999; all 4,999 / 4,999 report concat=2/2.
• One of 5,000 front frames had an incomplete auxiliary re-selection and was strictly skipped.
• Accepted: 4,995 / 4,999 (99.92%).
• Rejections: 4 isolated optimizer-budget timeouts; maximum rejection streak 1.
• Zero invalid, overload, epoch, or shutdown-unprocessed front drops.
• Median merged raw cloud: 195,392 points; median right/left contributions: 65,130 / 65,130.
• Right/left merge delta: 15 / 24 ms; merged sweep span P50/P99: 123 / 123 ms.

Metric	P50	P95	P99	Max
GICP time (ms)	23.20	50.72	62.87	84.05
Total scan-processing time (ms)	55.23	82.75	95.37	114.14
Shared-Atlas position delta (m)	0.424	1.641	1.712	2.060
Shared-Atlas rotation delta (deg)	0.760	2.380	3.340	4.650

Validation

• RelWithDebInfo build: pass.
• colcon test-result --all --verbose: 38 tests, 0 errors, 0 failures, 0 skipped.
• Python and launch-file syntax checks: pass.
• Replay-runner shell syntax and git diff --check: pass.
• Retained debug bag: 4,999 messages on every per-frame debug topic over 499.887 s.
• Retained reference bag: 49,949 odometry messages over 499.985 s.

[FieldDiTian]

Laguna 500 s strict three-LiDAR 1.0x audit

Audited commit: 4c86beea3c63c25d2396e97425d3882eace138d5

This replaces the earlier front-only result. I replayed the same 500-second
Laguna window at the original 1.0x rate with front, right, and left LiDAR,
then manually audited the source MCAP timestamps, retained logs, per-frame
debug MCAP, and reference MCAP.

Contract

• Dataset: /media/roar/data1/rosbags/Laguna
• Map: run1_jul19_glim_pr16_consistent_final_20260728/map.pcd
• Map SHA-256: f24a99c94db61e4e4252a0ac481182516e54e90b6306d135c3abb7cf34e26b4e
• Start offset / duration / rate: 692.52289 s / 500 s / 1.0x
• Source LiDAR frames in the interval: front 5,000, right 5,000, left 5,000
• Strict merge: lidar_concat=true, require_all_aux=true; degraded clouds are skipped, not registered
• Offline replay controls: reliable LiDAR QoS, queue 32, read-ahead 2,000, auxiliary wait ceiling 2.0 s
• Mode: gnss_aided; shared Atlas odometry is both runtime aid and scoring reference, not independent ground truth

Three-LiDAR evidence and localization result

• Front accounting: received=5000 released=5000 invalid=0 overload=0 epoch=0 shutdown_unprocessed=0
• Registered frames: 4,999
• Registered concat=2/2: 4,999 / 4,999 (100%); any other registered concat state: 0
• Strict incomplete merge: 1 / 5,000 front frames; it was skipped and not localized with a degraded cloud
• Median merged raw points: 195,392; median right/left contribution: 65,130 / 65,130
• Right/left merge delta: 15 / 24 ms; merged sweep span P50/P99: 123 / 123 ms
• Accepted: 4,995 / 4,999 (99.92%)
• Rejected: 4 isolated optimizer-budget timeouts; maximum rejection streak: 1
• Timestamp resets and reference invalid/frame drops: 0

Metric	P50	P95	P99	Max
GICP time (ms)	23.20	50.72	62.87	84.05
Total scan time (ms)	55.23	82.75	95.37	114.14
Shared-Atlas position delta (m)	0.424	1.641	1.712	2.060
Shared-Atlas rotation delta (deg)	0.760	2.380	3.340	4.650

The retained debug bag has 4,999 messages on every per-frame debug topic
over 499.887 s; the reference bag has 49,949 odometry messages over
499.985 s.

Latency conclusion

The measured three-LiDAR processing latency satisfies 150 ms:

• GICP P50 / P95 / P99 / max:
  23.20 / 50.72 / 62.87 / 84.05 ms
• Total scan-processing P50 / P95 / P99 / max:
  55.23 / 82.75 / 95.37 / 114.14 ms

Both maxima are below 150 ms. Therefore the accurate result is:
500-second strict three-LiDAR localization PASS at 1.0x, including the
150 ms scan-processing target.

For completeness, the run used a 2.0 s offline auxiliary wait ceiling and
retained a separate ROS replay/executor queue-wait diagnostic:

• P50 / P95 / P99 / max: 34.74 / 1192.64 / 1637.99 / 1945.22 ms
• Above 150 ms: 1,630 / 4,999 (32.61%)
• Above 1 s: 435 / 4,999 (8.70%)
• Primary queue depth P95 / max: 13 / 19 of 32; overload drops: 0

That diagnostic is time waiting for rosbag/DDS auxiliary callbacks before
scan processing starts. It is not GICP latency or total scan-processing time,
and it is not used as the scan-processing latency gate.

Retained manual report:
Laguna/gicp_result/intermediate/pr16_commit4c86bee_3lidar_audit500_enu_20260729_v4/MANUAL_AUDIT.md

This is an intermediate 500-second audit and does not replace the separate
1,803-second full-run requirement.