reports/final.md

Frame-Level Bundle Adjustment — Final Design (2026-04-20)

Author: Claude (autonomous, per user pivot on 2026-04-20) Supersedes: all prior V4 submap-stitch / Sim(3)-PGO designs.

0. Problem statement

All ATE gaps remaining on KITTI/ETH3D/Oxford trace to the chained Sim(3) submap stitch drift: the online stitch averages 10-20% scale/rotation error per junction, and neither overlap (3→6) nor scorer (learned→temporal) helps after a plateau ~137 m on KITTI mean. Pose-graph BA over submap Sim(3) nodes improves TUM (<2 cm) but is blind to outliers and cannot recover from bad edges because submap->submap is a single relative transform.

The user's pivot: replace Sim(3) pose-graph with frame-level bundle adjustment. Per-frame SE(3) poses are refined jointly using dense point correspondences between (a) sequential neighbors and (b) loop-closure pairs selected via the top-5 submap retrieval logged by the scorer.

No backbone fine-tune. A matching head sits on top of frozen backend tokens and produces descriptors (or uses raw L2-normed tokens directly).

1. Goals & Non-Goals

Goals

• Frame-level SE(3) BA over all keyframes inside a run (submap-agnostic).
• Residual = 2-view reprojection of matched 3D points between frame pairs.
• Sources of correspondences:
  1. Sequential edges: every frame ↔ previous frame (dense + cheap).
  2. Loop edges: for each top-5 retrieved submap of the current frame, pick the single best-QV frame as the loop anchor.
• Sparse-first matching (DINO-VO-like, grid-aligned) because:
  • ~1% of tokens carry 95% of reprojection signal (M³ Q-confidence).
  • Dense matching has quadratic memory; sparse is O(N·K).
• Training-free v0 (deliver first). Optional trained head v1 (deferred).

Non-goals

• No backbone gradient (frozen).
• No joint point-and-pose optimization. Points are reconstructed from frozen backbone outputs per frame; only poses move.
• No Sim(3). Keep SE(3) — scale is fixed by the backbone + anchor.
• No online on-the-fly BA during demo_submap. Phase-B offline only for v0.

2. Graph topology

Nodes: one SE(3) pose per keyframe f_i, i=0..N−1 (N frames total). Edges:

• E_seq: (f_i, f_{i−1}) for i ≥ 1 and (f_i, f_{i−2}) for i ≥ 2. The double-skip edge stabilizes against short-window flat degeneracies.
• E_loop: for each frame f_i, read final_retrievals.json[i] → top-5 submaps s_{j_1}..s_{j_5}. For each s_{j_k}, find the frame f* ∈ s_{j_k} whose tokens give max ⟨Q_current, K_that_frame⟩ using the layer-22 VGGT attention proxy (approximated by token cosine sim in v0 — see §3.1). That f* becomes a loop edge (f_i, f*).
• Anchor: pose_0 is fixed to identity with weight 10⁶.

Per-scene N ≈ 300-600 for KITTI/TUM at stride 5. Edge count ≈ 6N = 1.8-3.6k.

3. Matching head (training-free v0)

3.1 Input

For each keyframe f, we already have from slamformer.extract(hidden):

• points (H×W×3, world-frame 3D)
• local_points = (xy·z, z) (H×W×3, camera-frame 3D; xy normalized)
• conf (H×W×C, confidence logits)
• hidden tokens (the backend-updated tokens — shape (N_tokens, D))

We use backend-updated tokens directly as descriptors. These are already 384-D L2-normalizable vectors that have seen cross-frame attention.

3.2 Keypoint selection (sparse)

Following DINO-VO: one candidate per patch cell (14×14 px for 518×518 input ⇒ 37×37 = 1369 candidates per frame). Rank by confidence conf_mean over the cell, keep top K=256 per frame.

3.3 Descriptor

d = normalize(token_cell) — L2-normalize the backend token at that cell.

3.4 Matching

Mutual nearest neighbor across two frames' top-K with cosine similarity threshold τ = 0.6 (conservative). For frame_i with K keypoints and frame_j with K, this is a 256×256 dot-product = 66k ops, trivially fast.

Output per edge: list of (uv_i, uv_j, X_i_world, X_j_world, w) tuples.

• uv_i, uv_j: pixel coordinates (for reprojection residual)
• X_i_world: 3D point predicted by frame_i (from points)
• w: match weight = √(conf_i · conf_j), following M³-SLAM

3.5 Why this might work

The backend cross-frame attention already injects correspondence information into tokens (the same mechanism the scorer exploits at submap level). Raw cosine between tokens was enough for retrieval; the hypothesis under test is that it's also enough for pixel-level matching when restricted to the confidence-top K.

Fallback v1 (not for first submission): train a 2-layer MLP head (DPT_desc → 24-D L2-norm) using InfoNCE from M³-SLAM. Initialize from the point head weights as the paper does.

4. Bundle adjustment formulation

4.1 Variables

• θ ∈ ℝ^{6(N−1)}: se(3) perturbations for f_1..f_{N−1} (f_0 fixed at I).
• Pose reconstruction: T_i = exp(ξ_i) · T_i^{(0)} where T_i^{(0)} is the initial pose from the online stitch (final_traj.txt).

4.2 Residual per match (MASt3R-SLAM style, SE(3) only)

For match m ∈ edge (f_i, f_j), given X_i in camera_i frame (from local_points) and observation uv_j in frame_j:

X_j_cam_pred = (T_j^{-1} · T_i · [X_i; 1])[:3] uv_j_pred = X_j_cam_pred[:2] / X_j_cam_pred[2] # implicit fx=fy=1 r_m = w_m · ρ_huber(uv_j - uv_j_pred, δ=0.1)

The /z projection uses normalized-camera coordinates — SLAMFormer's local_points already factored fx=fy=1, so no intrinsics are needed.

Symmetric residual for (j, i) direction: r_m' with roles swapped. Both are accumulated, giving a 4-D residual per match.

4.3 Loss

L(θ) = Σ_edges Σ_matches r_m² + 10⁶ · ||T_0 - I||²_F

Anchor term keeps f_0 fixed; required because any global SE(3) is unobserved.

4.4 Solver

scipy.optimize.least_squares(method='trf', loss='huber', f_scale=0.1, max_nfev=200, verbose=2). Jacobian computed via finite differences (N ≤ 600 ⇒ state dim ≤ 3600, feasible for TRF). For large N we will switch to analytic Jacobians in a follow-up.

4.5 Initialization

Start from the online stitch trajectory (final_traj.txt). This is already within a small SE(3) of the optimum for TUM (< 5 cm median drift) and provides a warm start; the BA tightens rotations and removes outlier-induced kinks.

5. Outputs

Per run directory:

• frame_matches.jsonl: one line per edge, with match lists.
• frame_ba.log: solver log (cost_start, cost_end, n_edges, n_matches, termination reason).
• final_traj_frame_ba.txt: TUM-format refined per-frame trajectory.

Evaluated with same harness as the Sim(3) variants: added as a 5th column alongside stitch/raw/offline/posegraph.

6. Implementation plan

6.1 New files

Path	Purpose
thirdparty/submap/slam/frame_matcher.py	keypoint selection + mutual-NN matching; match(frame_i_tokens, frame_j_tokens, conf_i, conf_j, top_k=256, tau=0.6) -> List[Match]
thirdparty/submap/slam/frame_ba.py	build residuals + run scipy; returns refined poses dict[frame_id -> 4x4]
run_frame_ba_offline.py	project-root entry: reads frame_dump.jsonl + retrieval log, drives matcher + BA, writes final_traj_frame_ba.txt

6.2 demo_submap.py additions

Add --dump_frame_tokens flag. When set, each extracted frame emits to frame_dump.jsonl:

{"fid": int, "submap_id": int, "pose_w": 4x4,
 "local_points": H*W*3 list (downsampled 4×4 grid),
 "conf": H*W list, "tokens_path": "<npy or mmap ref>"}

Tokens are too big for JSONL (37×37×384 = 526 KB per frame × 600 frames = 316 MB); store them as a single contiguous frame_tokens.npy (float16, shape (N_frames, 37, 37, 384)).

6.3 New sbatch scripts

• run_v4_frame_ba_tum.sh — mirrors run_v4_posegraph_tum.sh; adds a 5th eval variant frame_ba.
• run_v4_frame_ba_kitti.sh — mirrors run_v4_posegraph_kitti.sh.

Both skip inference if final_traj.txt + frame_dump.jsonl already exist (allows iterating on BA without rerunning backbone).

7. Expected outcomes

Dataset	Current best (Sim3 PGO)	MASt3R-SLAM reported	Frame-BA target
TUM fr1 mean ATE	≤0.22 m (V4-B'' ckpt-1)	0.04 m	0.05-0.10 m
KITTI 00/05/07/09 mean ATE	137 m (o3 offline)	n/a	40-80 m
ETH3D median ATE	0.63 m	~0.03 m	0.1-0.3 m

The gap to MASt3R on TUM is mostly dense-vs-sparse and trained-vs-untrained descriptors — we're deliberately accepting 2-5× worse absolute to keep v0 training-free.

8. Risks & mitigations

Risk	Mitigation
Raw backend tokens give weak matches	v1 trained head (M³-style) behind --desc_head trained flag
Top-K=256 is too sparse on low-texture TUM scenes	Conf-weighted, so fewer confident matches > more noisy. Fall back to K=512 if <30 matches/edge median
scipy TRF fails on large N (>600 frames)	Chunk into overlapping 100-frame windows with shared boundary, fix alternate windows (ICEBA-style)
Loop edges from scorer are wrong (top-5 not always in same place)	Reject edge if <10 mutual NN matches survive τ
Anchor locks global frame; if f_0 is an outlier the whole traj shifts	Use first 5 frames as a weighted multi-anchor instead
Double-skip edges create 2-cycles that trivially satisfy residual	Add explicit velocity smoothness term Σ

9. Submission plan

1. Write matcher + BA + driver + demo_submap flag + sbatch scripts.
2. Smoke-test locally on TUM fr1/xyz (smallest scene, 19 frames strided).
3. Submit TUM job (all 9 scenes).
4. Submit KITTI job (00/05/07/09).
5. Monitor both; parse final_traj_frame_ba.txt into the existing v4_posegraph_* summary table.
6. Iterate on matcher τ / top-K / anchor weight based on ATE.

---

10. Addendum: Sim(3) + matcher GT eval pivot (2026-04-21)

The v0 §1 non-goal "No Sim(3). Keep SE(3)" did not survive first contact with the SE(3) results: frame-BA cost dropped 1500× but ATE actually got worse on several TUM scenes. Root cause: the backbone's local-points scale is not consistent across frames, so reprojection residuals pull poses away from the GT minimum. Three changes follow.

10.1 Per-frame Sim(3) (replaces §1 "No Sim(3)")

• State widened to 7 DoF/frame: 6 SE(3) twist + 1 log-scale log_s_k. s_0 = 1 (anchor), so state dim = 7·(N−1).
• Reprojection residual becomes r = uv_obs − π(T_j^{-1} · T_i · s_i · X_i_cam).
• Scale-difference prior added to residuals: ds_k = (log_s_k − log_s_{k−1}) · scale_prior_weight — prevents pathological scale drift where matches are geometrically degenerate.
• Solver unchanged (scipy.least_squares + huber + LSMR + sparse J); --use_sim3 --scale_prior_weight 0.1 --max_nfev 500 CLI flags.
• Smoke on fr1_xyz: scales converged to [0.92, 1.17] (median 1.003), RPE_rot 12.4° (Sim(3)) vs 29.8° (SE(3)). ATE similar.

10.2 Sparse-LSMR Jacobian (perf fix for KITTI)

• The v0 dense Jacobian on KITTI 00's 389-submap problem never returned.
• _jac_sparsity() emits a scipy.sparse.lil_matrix covering reprojection blocks, velocity-prior rows, and (with Sim(3)) scale-prior rows. Density ~0.5% on KITTI.
• tr_solver="lsmr" + jac_sparsity=... in least_squares → KITTI 00 PGO went from "stuck forever" to 5 s solve, and frame-BA to bounded runtime. Applied to both frame_ba.py and submap_pose_graph.py.

10.3 M³-style matcher GT eval

• New script eval_matcher_gt.py. Given GT depth + GT pose + K, a proposed match (token_a in frame i, token_b in frame j) is scored by unprojecting each token's 3D via GT, then checking ‖X_i_gt − X_j_gt‖ < err_thresh_m.
• Loaders: --dataset {tum|scannetpp}; TUM uses rgb.txt/depth.txt timestamps + fr1 K + depth factor 5000; ScanNet++ uses new_scene_metadata.npz (per-frame K + c2w + mm depth).
• rc in dumps are patch indices (0..36 at target_size=518), not pixels. Unproject via rc * 14 + 7 → network pixel → scale to original by orig_{H,W} / new_{H,W} where new_H = round(orig_H * 518 / orig_W / 14) * 14.
• Baseline on fr1_xyz at τ=0.6: precision 8% @ 5 cm, 55% @ 20 cm, 88% @ 50 cm; median 3D error 19 cm. Patch-level descriptors cannot reach sub-cm precision — 14×14 patches integrate across depth jumps.

10.4 Online BA: deferred

See notes/online_frame_ba_design.md. Summary: offline BA is enough for pose-eval; online only matters if we need corrected realtime trajectory. Recommendation — ship offline Sim(3) first.

10.5 Jobs

• 22058142 TUM Sim(3) frame-BA (9 scenes, v4_frame_ba_tum_sim3/; reuses dumps from the SE(3) run).
• 22058145 KITTI 00 Sim(3) frame-BA (v4_frame_ba_kitti_sim3/; reuses dumps from v4_frame_ba_kitti/).
• 22058336 ScanNet++ matcher GT eval (FAILED — mixed aspect ratios DSLR 1035×690 + iPhone 920×690 per scene, kf_detect torch.stack crash).
• 22058916 ScanNet++ matcher GT eval, iPhone-only filter (frame_*.jpg) — COMPLETED.

10.6 ScanNet++ matcher eval results

scene	n_pairs	matches	correct@10cm	prec	med_err
036bce3393	51	1292	75	5.8%	1.99 m
079a326597	41	1319	172	13.0%	0.68 m
07ff1c45bb	53	1286	59	4.6%	1.31 m

TUM baseline (fr1_xyz) at same threshold: 30% precision / 19 cm median. ScanNet++ is worse because STRIDE=5 on iPhone (~30 fps) skips to ~0.17 s gap → keyframes accept ~0.3-0.8 s gap → wider view change than TUM at 30 fps. Pose/depth/intrinsic conventions verified (c2w, 2.4 m median depth, cx/cy match).

10.7 TUM Sim(3) vs SE(3) frame-BA head-to-head (22058142, 9 scenes)

scene	SE3 fba	Sim3 fba	Δ
360	0.164	0.159	-0.005 win
desk	0.699	0.615	-0.085 win
desk2	0.011	0.012	+0.001 tie
floor	0.183	0.178	-0.005 win
plant	--	--	Umeyama degenerate (known TUM coplanar-GT)
room	0.283	0.298	+0.016
rpy	0.044	0.047	+0.003 tie
teddy	0.576	0.685	+0.109
xyz	0.097	0.105	+0.008 tie
mean	0.257	0.262	+0.005

Read: TUM fr1 isn't the dataset where Sim(3) pays off (tight indoor, scale ≈ 1.0, low drift). 3 wins, 2 losses, 3 ties. The losses on room/teddy suggest scale_prior_weight=0.1 is too permissive for indoor where over-flexible scale slightly degrades rigid alignment. The real test case is KITTI (SE(3) had 172 m ATE on seq 00 due to per-frame backbone scale inconsistency).

10.8 KITTI Sim(3) vs SE(3) frame-BA (22058145, seq 00)

variant	ATE_Sim3 (m)
stitch	173.052
raw	192.944
offline	173.175
posegraph	172.624 (best)
frame_ba	173.193

SE(3) frame-BA baseline was 172.6 m. Sim(3) frame-BA arrived at 173.2 m — marginally worse. Solver ran 500 iters (max), cost dropped 20× (8.0e5 → 4.0e4). Per-frame log-scale stats: median=0.9999, range [0.83, 1.02] — the scale DoF was essentially unused.

Verdict: hypothesis falsified. Per-frame scale drift is NOT the ATE ceiling on KITTI. The backbone's monocular depth is scale-consistent (no per-frame drift to absorb) but systematically biased in absolute scale. On the 784 m Karlsruhe loop, a ~22% uniform depth-bias × trajectory length explains ~170 m of ATE — exactly what we see. The Sim(3) DoF is the wrong lever for this failure mode.

Next direction (not this sprint): global depth rescaling (one Umeyama-fitted scalar over all frames' depth predictions) or backbone retrain with real KITTI + L_overlap. Adding more DoF to BA won't help when the input observations (depth maps) are uniformly biased.

10.9 Pixel-accurate matching: reprojection filter + subpixel refinement

Motivation. §10.3's ScanNet++ matcher eval showed precision of only ~13% at 10 cm on DSLR/iPhone wide-baseline pairs — the primary reason frame-BA lagged on TUM and KITTI. Raw patch matching at 14 × 14 px granularity on DINOv3-like ViT features is too coarse for BA to Huber-down the outliers (saturates around 85-95% outlier rate). Two interventions were added; one is the lever, one is window dressing.

(a) Geometric reprojection filter — the lever. filter_by_reprojection (thirdparty/submap/slam/frame_matcher.py:303) uses the SLAM frontend's initial per-frame c2w poses as a geometric prior. For every surviving (i, j) match, it unprojects frame-i's patch center to camera-j via T_ji = T_j⁻¹·T_i, computes the reprojection error against frame-j's observed normalized-camera pixel, and rejects pairs with ‖uv_proj − uv_obs‖₂ > max_err_norm. At TUM f ≈ 517 px, max_err_norm=0.10 corresponds to 50 px image displacement (7% of width) — loose enough to tolerate VO drift, tight enough to kill mismatches that would torpedo BA's Huber loss.

dataset	matches pre-filter	matches post-filter	precision @ 10cm
ScanNet++ 079a...	6.1k	1.1k (18% survive)	13% → 76.6%
ScanNet++ median err	67.7 cm	2.7 cm	—
TUM fr1/xyz	(sequential pairs)	(no rejection)	15.6% → 14.5%

ScanNet++ has wide-baseline pairs (DSLR + iPhone, STRIDE=5 iPhone keyframes) where the initial pose prior is still geometrically tight enough to discriminate. TUM's sequential-pair matching is already decent; the filter has nothing large to reject, so the lift is small. The filter is the actual fix for §10.3's reliability crisis.

(b) Subpixel quadratic refinement — negligible empirical lift. _subpixel_quadratic (thirdparty/submap/slam/frame_matcher.py:156) fits a 2D parabola to the 3×3 cosine-similarity neighborhood around the winning patch (r₀, c₀) in frame j:

• ar = ½(f(r+1) − f(r−1)); br = ½(f(r+1) + f(r−1) − 2·f₀)
• ac = ½(f(c+1) − f(c−1)); bc = ½(f(c+1) + f(c−1) − 2·f₀)
• peak at (dr, dc) = (−ar/2br, −ac/2bc), clipped to [−0.5, 0.5] patch units

Requires concave peak (br, bc < 0) and finite offset; else falls back to patch center. The sub-patch offset (dr, dc) is then fed to _interp_uv_norm_at which bilinearly interpolates uv_norm across the 4 surrounding patches — giving a pixel-accurate observation instead of one quantized to 14 px.

Data wiring:

MatchEdge {
    uv_j_refined: (M, 2)  # normalized cam, pixel-accurate
    subpx_offset: (M, 2)  # diagnostic, patch units
}

BAEdge.from_match (thirdparty/submap/slam/frame_ba.py) prefers uv_j_refined over kp_j.uv_norm[idx_j] when populated, so BA residuals are evaluated at the refined pixel.

Empirical result on TUM xyz (60% refinement rate, median |offset| = 4.5 px):

config	precision @ 5 cm	median err
baseline	5.75%	18.79 cm
+ subpixel refinement	5.83%	18.75 cm

Negligible. The dominant error mode is inter-surface mismatches (tens of cm — wrong object entirely), not patch quantization (~7 px ceiling, ~1.4 cm at TUM depth 1-2 m). Fitting a parabola to the cos-sim surface doesn't fix "matched to the wrong chair leg"; only (a) can.

Limitations of the subpixel path.

1. Coverage: top-K=256 out of 37×37 = 1369 patches means only 19% of the grid has descriptors dumped; 40% of candidate peaks hit an incomplete 3×3 neighborhood and fall back. Full-grid dumping would push refinement rate to ~100%, but the offset is still bounded at ±7 px per axis — the ceiling doesn't move.
2. No cure for inter-surface outliers: subpixel helps only when the match is on the right surface. The big error bars on TUM are from mismatches that (a) already catches cheaply and (b) leaves untouched.
3. BA residuals vs. eval: eval_matcher_gt.py was updated to re-unproject frame-b at the refined pixel (applies subpx_offset before _unproject_tokens) so precision measurements reflect what BA actually sees.

Verdict. Ship the reprojection filter (the lever). Keep the subpixel path as infrastructure — it's < 200 LOC, the coverage ceiling is the real bottleneck, and if we later dump full-grid descriptors for loop-closure verification the subpixel path is ready to use it. The precision crisis on ScanNet++ is resolved; TUM/KITTI ATE gains still hinge on backbone depth bias (§10.8), not matcher precision.