# 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 Σ ||log(T_i^{-1} T_{i+1})||² with tiny weight |

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