feat(ba): activate robust kernel infrastructure + add Tukey and GNC-TLS

[qiaozhijian]

Activate robust kernel in BA; add Tukey and GNC-TLS

Summary

VIPE's bundle-adjustment solver already supports per-term robust kernels
(Solver.add_term(term, kernel=...) + ConcreteTermEvalReturn.apply_robust_kernel)
and ships a HuberRobustKernel, but no caller ever passes one — the IRLS
machinery is effectively unused. This PR:

1. Wires ba.robust_kernel / ba.robust_kernel_threshold from the SLAM
   config into GraphBuffer.bundle_adjustment so DenseDepthFlowTerm
   (dense flow and, if enabled, sparse tracks) receives the kernel.
2. Parameterises HuberRobustKernel.threshold (was hard-coded to 1,
   which is too tight for the /8 feature-grid residuals).
3. Adds two kernels for handling dynamic content and long-baseline drift:
   • TukeyRobustKernel(c) — biweight, hard-rejects |r| > c.
   • TLSGncRobustKernel(c_bar, mu_init, mu_step, mu_max) — Graduated
     Non-Convexity with Truncated Least Squares
     (Yang-Antonante-Tzoumas-Carlone, RA-L 2020;
     weights / schedule follow GTSAM's
     GncOptimizer.h).
     The BA loop gains a μ-outer / GN-inner nesting: each μ level runs
     n_iters // gnc_n_mu_steps Gauss-Newton iterations at frozen μ,
     then μ advances multiplicatively toward the hard-TLS limit.

Default robust_kernel: null reproduces the pre-PR L2 behaviour bit-for-bit.

Changes

Two atomic commits:

• 1/2 feat(ba): parameterize HuberRobustKernel and wire into DenseDepthFlowTerm
  — activates the existing kernel infra with Huber only. (+76 / −8)
• 2/2 feat(ba): add Tukey and GNC-TLS robust kernels with mu-outer / GN-inner schedule
  — adds the two kernels and the GNC schedule loop. (+196 / −10)

No changes to solver.py, terms.py, or any consumer outside
GraphBuffer.bundle_adjustment. No new dependencies.

Benchmark

Protocol: 5 datasets × 2 clips × 201 frames @ 10 Hz (20 s each) = 10 clips.
Config: configs/slam/default.yaml defaults (keyframe_depth=metric3d-small,
backend_iters=24), varying only ba.robust_kernel. For GNC-TLS the BA
loop runs gnc_n_mu_steps × gnc_gn_iters_per_mu = 4 × 6 = 24 GN iterations
per call (matches the default backend_iters=24 used here).

Metrics (lower-is-better except mAA):

• ATE_se3: SE(3)-aligned absolute trajectory RMSE normalised by GT path
  length. Alignment leaves scale unchanged, so this exposes monocular
  scale drift.
• RTE / RRE°: median relative translation / rotation error across
  strides {10, 50, 100} frames.
• mAA@τ: AUC of consecutive-frame rotation accuracy under τ°.

Overall (mean over 10 clips)

Kernel	ATE_se3 ↓	RTE ↓	RRE° ↓	mAA@3° ↑	mAA@10° ↑	ms/frame
None (L2)	0.3766	1.5731	3.56	0.833	0.945	390
Huber	0.3713	1.5559	3.49	0.833	0.945	368
Tukey	0.3432	1.4745	3.66	0.836	0.941	373
GNC-TLS	0.3552	1.5083	3.62	0.836	0.943	390

Relative vs L2

Kernel	ATE_se3	RTE	RRE	mAA@3°	mAA@10°
Huber	−1.4 %	−1.1 %	−2.1 %	±0 %	±0 %
Tukey	−8.9 %	−6.3 %	+2.6 %	+0.4 %	−0.4 %
GNC-TLS	−5.7 %	−4.1 %	+1.7 %	+0.3 %	−0.3 %

Per-dataset (2 clips each)

Dataset	Scene	Kernel	ATE_se3 ↓	RTE ↓	RRE° ↓	mAA@3° ↑
KITTI-360	driving	None	0.0337	0.1535	1.05	0.985
KITTI-360	driving	Huber	0.0280	0.1397	1.05	0.985
KITTI-360	driving	Tukey	0.0909	0.1336	1.05	0.981
KITTI-360	driving	GNC-TLS	0.0795	0.1345	1.05	0.983
OpenLORIS	indoor	None	0.0160	0.0864	0.93	0.956
OpenLORIS	indoor	Huber	0.0167	0.0886	0.94	0.956
OpenLORIS	indoor	Tukey	0.0175	0.0901	0.96	0.956
OpenLORIS	indoor	GNC-TLS	0.0172	0.0904	0.94	0.956
OxfordSpires	walking SfM	None	0.7229	2.3828	7.73	0.535
OxfordSpires	walking SfM	Huber	0.7325	2.3631	7.69	0.539
OxfordSpires	walking SfM	Tukey	0.7718	2.7057	8.41	0.520
OxfordSpires	walking SfM	GNC-TLS	0.8011	2.7349	8.23	0.519
GrandTour	wide-FOV	None	0.8881	4.5513	6.49	0.785
GrandTour	wide-FOV	Huber	0.8662	4.5371	6.49	0.785
GrandTour	wide-FOV	Tukey	0.6385	3.8496	6.45	0.785
GrandTour	wide-FOV	GNC-TLS	0.7016	4.0631	6.44	0.786
InCrowd-VI	dense pedestrians	None	0.2223	0.6916	1.62	0.904
InCrowd-VI	dense pedestrians	Huber	0.2130	0.6511	1.28	0.898
InCrowd-VI	dense pedestrians	Tukey	0.1971	0.5935	1.43	0.940
InCrowd-VI	dense pedestrians	GNC-TLS	0.1767	0.5184	1.46	0.935

Takeaways:

• Huber is Pareto-safe: −1 % to −2 % across all aggregate metrics,
  never worse than L2 by more than noise floor on any dataset.
• Tukey / GNC-TLS shine on dynamic / drift-heavy scenes — InCrowd-VI
  ATE_se3 −21 %, GrandTour ATE_se3 −28 %. They hurt on low-parallax
  static outdoor (KITTI ATE_se3 +169 % for Tukey) where aggressive
  outlier rejection discards valid long-tail residuals. Recommended as
  opt-in for domains known to contain dynamic content.

Kernel apply costs < 1 % of BA iteration time; the 0–6 % ms/frame
variation is within per-clip noise.

Backward compatibility

ba.robust_kernel: null (default) → build_robust_kernel returns None
→ Solver.run_inplace skips apply_robust_kernel → identical linear
system as before.

[heiwang1997]

Looks good to me, thanks for the contribution @qiaozhijian.

I will be keeping the robust kernel by default null as what you suggested, since per your benchmark the improvement is mainly on the scenes with a bunch of dynamic objects. We can advise the users to use the GNC-TLS/Tukey kernel when there's dynamic content that cannot be filtered out via the semantic masks. Potentially also related to #75.