FilteredTracker: decouple per-frame ops from measurement CV with fine-grained locking

[kobejean]

Problem

The lightweight per-frame ops (predictStep / updateVIOCameraPose / getFilteredTransform) and the heavy measurementUpdate previously had to be externally serialized — the lar-swift ObjC bridge ran them all on a single serial dispatch queue — because FilteredTracker had no internal locking and shares mutable state (the filter strategy + VIO poses) between them.

Consequence: every per-frame call stalled for the entire 0.5–2 s localization wall time. AR tracking froze during each localization attempt (raised as a code-review finding on lar-swift PR #15).

Change

Make FilteredTracker internally thread-safe with a single state_mutex_ guarding the filter strategy, VIO pose state, timing and last_transform_result_.

• Per-frame methods lock for microseconds.
• measurementUpdate runs the expensive base_tracker_ localization without the lock:
  1. snapshot the prediction (PnP seed) + capture-time VIO pose under a brief lock,
  2. CV unlocked (the 0.5–2 s work) — per-frame predict/getFilteredTransform run concurrently,
  3. fuse the result under a brief lock.

Filter-timing correctness

Because per-frame predictStep now keeps advancing the filter during the unlocked CV, the measurement (from the capture-time image) would otherwise be fused against a state that has moved on — a spurious innovation equal to the camera motion during CV (→ likely outlier rejection during motion). The measured pose is time-aligned forward by the VIO motion that elapsed during localization before fusing:

T_lar_from_camera(now) = T_lar_from_camera(capture) · vio(capture)⁻¹ · vio(now)

Phase 3 also re-checks isInitialized() under the lock to handle a concurrent reset().

Caller contract

base_tracker_ is intentionally not mutex-guarded (that's the point — CV runs unlocked), so measurementUpdate must not run concurrently with itself or with getBaseTracker() mutations (configureImageSize). Those heavy/rare ops stay serialized on a single background context. Per-frame ops are always safe concurrently.

Validation

• Builds clean (Linux SuperBuild); filtered_tracker.cpp compiles with no new warnings.
• Full test suite passes (111 tests).
• Threading benefit (no per-frame stall) and the time-alignment behavior need on-device validation in the LARScan app — there is no FilteredTracker unit harness in-repo.

Follow-up (lar-swift)

Once merged + submodule bumped, the bridge (LARFilteredTracker.mm) can drop its serial queue: per-frame ops call C++ directly (guarded by state_mutex_), and only measurementUpdate/configureImageSize stay on a background serial queue per the caller contract. This supersedes the interim dispatch_async mitigation.

🤖 Generated with Claude Code