Skip to content

phase-3-validation.md

Latest commit

 

History

History
57 lines (46 loc) · 2.14 KB

phase-3-validation.md

File metadata and controls

57 lines (46 loc) · 2.14 KB
Raw

Phase 3.0 validation — byte-identity vs Phase 1

Phase 3.0 wires KISS-ICP odometry through a GTSAM ISAM2 pose graph with one PriorFactor (key 0, identity) and one BetweenFactor per scan. With only that, the chain composes exactly the way KISS-ICP composes its own poses — ISAM2's optimizer has no over-determination to fight, so the estimate at each keyframe is prev * delta, i.e. KISS-ICP's own output.

The acceptance criterion is byte-identity (within numerical precision) between Phase 1's trajectory.tum and Phase 3.0's trajectory.tum on the same input. Phase 3.1+ adds IMU preintegration / GPS / loop closure and that's where behavior actually changes.

Result — 2026-05-30, moving_short_bag2

$ scripts/validate_phase3_0.sh
── Phase 1 baseline (lidar-only)
  poses=335  elapsed=0.47s
── Phase 3.0 (pose-graph)
  poses=335  elapsed=0.53s
── Trajectory diff
  poses:            335
  max |ts diff|:    0.00e+00 s
  max trans diff:   0.00e+00 m   (threshold 1e-06)
  max quat diff:    0.00e+00     (threshold 1e-06)
[validate_phase3_0] PASS

Literal 0 on all three diffs — the chain composes through ISAM2 without any numerical wobble.

Overhead: +0.06 s of total wall time (0.47 → 0.53 s) on 335 scans, or ~0.2 ms per scan added by the GTSAM update. Plenty of headroom before IMU preintegration (per-IMU-sample, 50 Hz) tightens the loop further.

Reproducing

cmake -S . -B build -DCMAKE_BUILD_TYPE=Release -DROVE_SLAM_USE_GTSAM=ON
cmake --build build -j8
# (one-time) convert a bag:
.venv/bin/python tools/bag2recording.py ~/bags/moving_short_bag2 /tmp/rove_slam_smoke
scripts/validate_phase3_0.sh

First-time GTSAM build is ~6 min on x86_64 with 8 cores.

Phase 3.1+ entry point

Replace the constant odom_noise = diag(1e-3 rad, 1e-2 m) in src/pose_graph.cpp with KISS-ICP's per-frame covariance once the front-end exposes one, and add gtsam::ImuFactor against the VN-300 preintegrated samples. PreintegrationParams::gravity_z = -9.81 (the VN-300 publishes gravity-bearing accel, unlike the M400 stack — see docs/phase-0.md). The Phase 3.0 byte-identity check stops being a useful acceptance gate at that point.