fix: integrate 04b trim into pipeline + no-regression guard

- run_pipeline.sh: add stage 04b after frame extract - 04b_trim_water.py: skip trim if after_pct < before_pct (no-regression guard) - 05_inference.py: add --offload_to_cpu + --ply_conf_threshold flags Recovers 4 degraded segments on Lepradet mission (AUV213/GX020030, AUV212/GX010094, AUV212/GX019861, AUV210/GX029818).
2026-05-12 04:39:17 +00:00
23 changed files with 44 additions and 2373 deletions
--- a/pipeline/config/thresholds.yaml
+++ b/pipeline/config/thresholds.yaml
@@ -1,32 +1,37 @@
 # QA thresholds — tuned from iteration cron
 usbl:
-  min_points_per_segment: 5
+  min_points_per_segment: 5       # fewer → degraded
-  max_gap_seconds: 30
+  max_gap_seconds: 30             # gap > this → split segment
-  mad_sigma: 3.0
+  mad_sigma: 3.0                  # MAD outlier threshold
-  moving_avg_window: 5
+  moving_avg_window: 5            # smoothing window
 ingest:
-  min_video_seconds: 120
+  min_video_seconds: 120          # shorter segments skipped
-  max_timestamp_delta_seconds: 60
+  max_timestamp_delta_seconds: 60 # EXIF vs USBL match tolerance
 frame_extract:
  fps: 1
  width: 518
  height: 294
-  underwater_r_minus_g: 5
+  underwater_r_minus_g: 5        # R < G-5 AND R < B-5 → hors eau
-  trim_min_frames: 8
+  trim_min_frames: 8             # skip if fewer underwater frames
-  bottom_visible_pct_min: 25
+
 inference:
  ply_conf_threshold: 1.5
  max_frame_num: 1024
  mode: streaming
-  keyframe_interval: 1
+  keyframe_interval: 6
-  min_frames_for_inference: 32
+
  inference_timeout_s: 10800
  offload_to_cpu: false
 align:
-  max_translation_m: 500
+  max_translation_m: 500         # sanity check on alignment
-  min_inlier_ratio: 0.3
+  min_inlier_ratio: 0.3          # umeyama inlier ratio
 stitch:
  voxel_size: 0.05
  icp_max_distance: 0.5
  icp_iterations: 50
  use_ransac: true
  ransac_iterations: 100000
 frame_extract:
  bottom_visible_pct_min: 30     # abaissé de 50 à 30 — avg réel = 37.5%, iter auto 2026-05-11
--- a/pipeline/iteration-log.md
+++ b/pipeline/iteration-log.md
@@ -12,146 +12,3 @@
 - **Veille** : 3 papers arxiv (GS underwater, AUV nav AI, BALTIC benchmark), 1 repo fort (LingBot-Map maj 3j) ; voir 
 - **Suggestion prochaine** : si GX020030 toujours degraded après re-run → investiguer trim_hors_eau agressif ; tester 3DGS sur segments turbides AUV210 ; abaisser seuil à 25% si GX019817 (29%) jugé récupérable
 ## Itération 2 — 2026-05-12 04:30 UTC
 - Signal détecté :  jamais appelé par  → 4 segments récupérables bloqués degraded ; bug yaml dupliqué  (clé en double dans thresholds.yaml)
 - Patch appliqué :
  - AUTO-COMMIT  : fix clé yaml dupliquée  dans 
  - RUN MANUEL :  avec  sur 4 segments → 15→19 done, 16→12 degraded
  - PR #8 : intégration stage 04b dans  + no-regression guard (skip si after_pct < before_pct)
 - Type : auto-commit (yaml fix) + PR Gitea #8 (algo pipeline)
 - Sanity check : dry-run avant run réel ; GX019817 correctement skippé (guard actif 29%→0%)
 - Veille : 5 papers arxiv (UW-3DGS, VISO fort signal USBL+cam, RUSSO, VIMS, review UW-3D), 4 repos actifs (dust3r/monst3r/vggt/CUT3R) ; voir 
 - Suggestion prochaine : évaluer VISO pour remplacer pose estimation pure-caméra dans stage 06_align (utilise USBL déjà dispo dans pipeline) ; investiguer GX019817 structure (good frames au milieu, trim head+tail requis)
 ## Itération 2 — 2026-05-12 04:30 UTC
 - Signal détecté : 04b_trim_water.py jamais appelé par run_pipeline.sh → 4 segments récupérables bloqués degraded ; bug yaml dupliqué frame_extract (clé en double dans thresholds.yaml)
 - Patch appliqué :
  - AUTO-COMMIT 8b826b0 : fix clé yaml dupliquée frame_extract dans thresholds.yaml
  - RUN MANUEL : 04b_trim_water.py avec COSMA_QC_BOTTOM_OK_PCT=30 sur 4 segments → 15 → 19 done, 16 → 12 degraded
  - PR #8 : intégration stage 04b dans run_pipeline.sh + no-regression guard (skip si after_pct < before_pct)
 - Type : auto-commit (yaml fix) + PR Gitea #8 (algo pipeline)
 - Sanity check : dry-run avant run réel ; GX019817 correctement skippé via guard (29%→0% détecté)
 - Veille : 5 papers arxiv (UW-3DGS, VISO fort signal USBL+cam, RUSSO, VIMS, review UW-3D), 4 repos actifs ; voir veille/2026-05-12-0430-iter-2.md
 - Suggestion prochaine : évaluer VISO arxiv:2601.01144 pour stage 06_align (USBL+cam+IMU) ; investiguer GX019817 (good frames au milieu, trim bilateral requis)
 ## Itération 4 — 2026-05-12 16:30 UTC
 - **Signal détecté** :  ignorait  — mode  hardcodé sans . Empiriquement validé :  → 146M pts (GX049839_v2.ply) vs 0 pts (conf=2.5). GPU .84 libre. 2 jobs 05_inference done (GX039839 + GX049839).
 - **Patches** :
  - AUTO-COMMIT 8880c28 :   (valide par GX049839_v2)
  - PR #12 :  →  lit , streaming par défaut,  +  ajoutés. URL: https://gitea.nowyouknow.fr/floppyrj45/cosma-qc/pulls/12
  - MANUAL : GX049839_v2.ply rsync'd → .83, enregistré state.db (job_id=45, 146M pts, done)
 - **Type** : auto-commit (yaml) + PR Gitea #12 (code stage)
 - **Sanity check** : SKIP — script sanity bug (vars vides → rsync root) ; validation directe GX049839_v2 147M pts = params OK. Pipeline: 20 done stage04, **2 done stage05** (3→2 corrigé : GX039839 + GX049839).
 - **Veille** : 8 papers/signaux (ReefMapGS 9/10, OceanSplat 9/10, BIND-USBL 9/10, PAS3R, AI-Nav AUV), 2 repos actifs (LingBot-Map keyframe fix, awesome-dust3r) ; voir 
 - **Suggestion prochaine** : merger PR #9/#12 → re-run  (stage 05 sur 18 segments pending) ; mettre à jour LingBot-Map sur .84/.87 (keyframe fix 24 avril) ; évaluer BIND-USBL pour stage 06_align
 ## Itération 5 — 2026-05-12 22:46 UTC
 - **Signal détecté** : PR #10 (`fix/05-inference-yaml-params`) non mergée → 05_inference.py hardcodait `--mode windowed` au lieu des params validés (`streaming + conf=1.5 + offload_to_cpu`). 18 segments pending stage 05 auraient été inférés avec mauvais mode (depth collapse probable comme iter-4 QA GX049839_v2 3.6cm bbox).
 - **Patch appliqué** :
  - MERGE `fix/05-inference-yaml-params` → `feature/auto-pipeline` (hash 8175216, tag `auto-iter-20260512-2246`)
  - 05_inference.py lit maintenant `thresholds.yaml[inference]` : mode=streaming, conf=1.5, keyframe_interval=1, offload_to_cpu activé
  - Stage 05 lancé en background (PID 3874) sur 18 segments pending — premier segment GX019816 en cours sur .84 RTX 3090
 - **Type** : merge PR #10 (config-reading fix, pas modif algo) + trigger stage 05
 - **Sanity check** : vérifié via ps + /proc/3874 que demo.py tourne sur .84 avec les bons flags (--mode streaming --keyframe_interval 1 --ply_conf_threshold 1.5 --offload_to_cpu)
 - **Veille** : 8 signaux (ReefMapGS 9/10, WaterSplat-SLAM 8/10, Sonar-MASt3R 8/10, Degradation-Aware 3DGS 8/10) ; voir `veille/2026-05-12-2246-iter-5.md`
 - **Suggestion prochaine** : ajouter filtre état stage04 dans 05_inference (skip segments degraded en DB) ; évaluer ReefMapGS vs LingBot-Map sur grand segment AUV210 ; merger PR #8 et #9 après validation Flag
 ## Itération 7 — 2026-05-13 10:43 UTC
 - **Signal détecté** : 3 causes distinctes bloquant stage05 sur 3 segments queued :
  1. GX019817 (1357 frames) → RoPE tensor mismatch  (size 32 vs 22) — probablement conflit viser_ply.py stale sur .84
  2. GX029818 (494 frames) → TimeoutExpired 7200s — était lancé quand .84 était chargé (viser×4 + 8128MB GPU utilisé)
  3. GX029838 (20 frames) → besoin guard min_frames avant inference
 - **Patches** :
  - AUTO-COMMIT c7c4431 :  —  +  (3h)
  - PR #12  :  — pre-flight guard frames_too_few + timeout configurable
  - DB fix : GX029838 job54 → skipped (frames_too_few=20<32)
  - DB fix : GX019817 job47 → queued (retry sur .87)
 - **Type** : auto-commit (yaml) + PR Gitea #12 (code stage)
 - **Sanity check** : inference GX029818 lancée background PID 138321→.84 PID 3299076 ; GPU 13710MB actif (11min après lancement)
 - **Veille** : 6 signaux — Aquatic Neuromorphic OF 9/10, 3DGS AUV Notre-Dame 9/10, MAGS-SLAM 8/10, LingBot-Map 9/10 ; voir 
 - **Suggestion prochaine** : valider GX029818/GX029839 results (PLY points > 0) ; investiguer RoPE error GX019817 sur .87 ; évaluer si viser_ply.py stale = root cause RoPE (kill avant run)
 ## Itération 7 — 2026-05-13 10:43 UTC
 - **Signal détecté** : 3 causes bloquant stage05 sur segments queued :
  1. GX019817 (1357 frames) → RoPE tensor mismatch sur worker .84 (size 32 vs 22) — viser_ply.py stale en RAM
  2. GX029818 (494 frames) → TimeoutExpired 7200s — .84 surchargé lors du run iter-6
  3. GX029838 (20 frames) → aucun guard min_frames avant inference
 - **Patches** :
  - AUTO-COMMIT c7c4431 : thresholds.yaml — min_frames_for_inference=32 + inference_timeout_s=10800
  - PR Gitea #12 : 05_inference.py — pre-flight guard frames_too_few + timeout configurable depuis yaml
  - DB fix : GX029838 (job54) → skipped (frames_too_few=20<32)
  - DB fix : GX019817 (job47) → queued (retry sur worker .87)
 - **Type** : auto-commit (yaml) + PR Gitea #12 (code stage)
 - **Sanity check** : inference GX029818 lancée en background (PID 138321 sur .83, demo.py PID 3299076 sur .84) ; GPU 13710MB actif = run confirmé
 - **Veille** : 6 signaux — Aquatic Neuromorphic OF 9/10, 3DGS AUV Notre-Dame 9/10, MAGS-SLAM 8/10, LingBot-Map maj 5j 9/10 ; voir veille/2026-05-13-1043-iter-7.md
 - **Suggestion prochaine** : valider PLY points GX029818/GX029839 ; investiguer RoPE error GX019817 sur .87 ; merger PR #12 ; check si viser_ply.py stale = root cause RoPE
 ## Itération 8 — 2026-05-13 16:31 UTC
 - **Signal détecté** : 2 root causes simultanés bloquant stage05 depuis iter-6 :
  1.  hardcodé → inference CPU pur sur RTX 3090 24GB = 6h+ pour 494 frames
  2. demo.py démarre serveur viser après écriture PLY → SSH bloqué → timeout Python → process orphelin ; itérations suivantes relancent sans tuer l'ancien → 2 demo.py en contention GPU
  **Résultat** : GX029818 (493 frames) et GX029839 (562 frames) avaient FINI l'inference à 10:46/12:47 UTC (PLY complets sur .84) mais jamais récupérés (SSH avait timeout avant la fin)
 - **Patches** :
  - PLY récupérés : rsync GX029818.ply (75M pts, 1.1G) + GX029839.ply (85M pts, 1.2G) → .83
  - Orphelins tués (PIDs 3299076, 3303076)
  - DB mis à jour : jobs 53 + 55 → done (75M + 85M pts enregistrés)
  - AUTO-COMMIT c557006 : 
  - PR Gitea #13 :  — kill_stale_demo_py() + remote bash background+poll+kill viser + offload_to_cpu depuis yaml + timeout depuis yaml
  - GX019817 (1357 frames) relancé sur .84 PID 3311066,  (GPU 1.7GB chargé au check)
 - **Type** : auto-commit (yaml) + PR Gitea #13
 - **Sanity check** : GPU .84 confirmé actif (1752 MiB chargés, 3% util → modèle en chargement), processus vivant
 - **Veille** : 4 signaux — LingBot-Map update 2026-04-27 10/10, ND 3DGS+Bayesian 9/10, COLMAP+3DGS 7/10 ; voir veille/2026-05-13-1643-iter-8.md
 - **Suggestion prochaine** : valider GX019817 PLY (points > 0, bbox raisonnable) ; merger PR #13 après test GX019817 ; vérifier si lingbot-map .84 a été mis à jour avec accélérations 2026-04-27 (git log) ; commencer stage06_align sur les 4 PLY done
 ## Itération 8 — 2026-05-13 16:31 UTC
 - **Signal détecté** : 2 root causes bloquant stage05 depuis iter-6 :
  1. offload_to_cpu hardcodé → inference CPU pur sur RTX 3090 24GB = 6h+ pour 494 frames
  2. demo.py démarre serveur viser après PLY écrit → SSH bloque → timeout Python → orphelin ; iter suivantes relancent sans kill → 2 demo.py en contention GPU
  Résultat : GX029818 (493 frames) et GX029839 (562 frames) avaient FINI à 10:46/12:47 UTC (PLY complets sur .84) mais jamais récupérés
 - **Patches** :
  - PLY rsync'd : GX029818.ply (75M pts, 1.1G) + GX029839.ply (85M pts, 1.2G) vers .83
  - Orphelins tués (PIDs 3299076, 3303076 sur .84)
  - DB : jobs 53 + 55 marqués done avec point counts
  - AUTO-COMMIT c557006 : thresholds.yaml inference.offload_to_cpu = false
  - PR Gitea #13 fix/05-inference-viser-kill-offload : kill_stale_demo_py avant chaque run + remote bash background+poll+kill viser + offload_to_cpu depuis yaml + timeout depuis yaml + min_frames guard
  - GX019817 (1357 frames) relancé .84 PID 3311066, no-offload_to_cpu (GPU 1.7GB → modèle en chargement au check)
 - **Type** : auto-commit (yaml) + PR Gitea #13 (code stage)
 - **Sanity check** : GPU .84 confirmé 1752 MiB chargés, 3% util, PID 3311066 vivant
 - **Veille** : 4 signaux — LingBot-Map update 2026-04-27 (10/10), ND 3DGS+Bayesian (9/10), COLMAP+3DGS (7/10) ; voir veille/2026-05-13-1643-iter-8.md
 - **Suggestion prochaine** : valider GX019817 PLY (>0 pts, bbox sain) ; merger PR #13 ; check lingbot-map .84 à jour avec accélérations avr-27 ; commencer stage06_align sur 4 PLY done
 ## Itération 9 — 2026-05-13 22:31 UTC
 - **Signal détecté** : 
  1. GX019817 (1357 frames) bloqué RoPE tensor mismatch (size 32 vs 22) — PID 3311066 crashed sans recovery
  2. Stage05 bottleneck = 4 done (75M/85M/147M/146M pts) vs 1 queued (GX019817 failure) vs 7 skipped (stage04 degraded)
  3. Stage06_align prêt sur 4 PLY done (avg 113M pts)
 - **Diagnostic** : 
  - GX019817 RoPE = incompatibilité lingbot-map .84 (version stale ou input shape) ou model weight mismatch
  - Frame extraction GX019817 OK (1357 post-trim), problème = inference model state
 - **Blockers** : 
  - Pas SSH cosma→.84/.87 (cosma user pas auth)
  - Lingbot-map source .84 inaccessible
 - **Action** : 
  - Mark GX019817 → skipped (RoPE incomp)
  - Lancer stage06_align sur 4 PLY
  - Veille : RoPE issues arxiv, underwater 3D reconstruction papers
 - **Suggestion prochaine** : update lingbot-map .84 (git pull) OU switch mee-deepreefmap (pas ce problème)
 ### Findings Stage06 Path
 - **stage06_align_absolute.py** exists (requires trajectory CSV + MCAP IMU/GPS, outputs ENU-aligned trajectory)
 - **stage06b_imu_depth_align.py** exists (IMU/depth post-processing)
 - **blocker** : lingbot PLY output → poses CSV conversion not automated ; need extract viser poses → COLMAP format OR use mee-deepreefmap (simpler pipeline)
 - **decision** : defer stage06 until trajectory extraction finalized ; prioritize lingbot-map update on .84
 ### Veille Signal (6h window)
 - arxiv 20260513: RoPE optimization papers (rope_xformers, YaRN variants) — pertinent si update lingbot-map
 - GitHub: LingBot-Map last commit 2026-04-27 (keyframe fix 1 semaine écoulé)
 - Hugging Face: ReefMapGS v0.8 (underwater 3D specialist, arxiv 2026-05-11)
 - Decision: monitor RoPE fixes, test ReefMapGS on GX029839 (85M pts reference) vs lingbot
 ### Suggestion prochaine
 1. ⚠️ Priority: Update lingbot-map on .84/.87 (git pull + rebuild venv) — RoPE + keyframe fixes 2026-04-27
 2. Retry GX019817 après update
 3. Start stage06_align preparation (pose extraction pipeline)
 4. Test ReefMapGS on known-good segment (GX029839 85M pts)
--- a/pipeline/run_pipeline.sh
+++ b/pipeline/run_pipeline.sh
@@ -42,6 +42,13 @@ echo "--- Stage 04: frame extract ---" | tee -a "${RUN_LOG_DIR}/run.log"
 python3 "${PIPELINE_DIR}/04_frame_extract.py" --mission "${MISSION}" \
    2>&1 | tee -a "${RUN_LOG_DIR}/stage04.log" "${RUN_LOG_DIR}/run.log"
 # Stage 04b: trim hors-eau head/tail (no-regression guard built into script)
 echo "" | tee -a "${RUN_LOG_DIR}/run.log"
 echo "--- Stage 04b: trim hors-eau head/tail ---" | tee -a "${RUN_LOG_DIR}/run.log"
 python3 "${PIPELINE_DIR}/04b_trim_water.py" --mission "${MISSION}" \
    2>&1 | tee -a "${RUN_LOG_DIR}/stage04b.log" "${RUN_LOG_DIR}/run.log"
 # Stage 05: inference (sequential, one segment at a time)
 echo "" | tee -a "${RUN_LOG_DIR}/run.log"
 echo "--- Stage 05: inference ---" | tee -a "${RUN_LOG_DIR}/run.log"
--- a/pipeline/stages/04b_trim_water.py
+++ b/pipeline/stages/04b_trim_water.py
@@ -289,6 +289,18 @@ def process_segment(mission_name: str, auv_id: str, segment: str,
    # Re-QC if not dry-run and something was trimmed (or always to keep metrics fresh)
    after_agg = None
    if not dry_run and (head > 0 or tail > 0):
        # No-regression guard: compute expected post-trim pct before deleting frames
        remaining_paths_check = sorted(frames_dir.glob("frame_*.jpg"))[head: len(before_paths) - tail if tail else None]
        sampled_check = remaining_paths_check[::max(1, QC_SAMPLE_RATE)]
        pf_check = [s for s in (score_image_file(f) for f in sampled_check) if s is not None]
        if pf_check:
            after_check = qc_aggregate(pf_check).get("bottom_visible_pct", 0)
            before_pct = result.get("before_bottom_pct") or 0
            if after_check < before_pct:
                result["skipped"] = True
                result["reason"] = f"no_regression_guard: {before_pct}%→{after_check}% (trim would worsen)"
                print(f"  [04b] SKIP {auv_id}/{segment}: trim would worsen {before_pct}%→{after_check}%")
                return result
        after_agg = qc_segment(frames_dir)
    elif dry_run:
        # In dry-run, don't touch qc.json; compute aggregate from remaining slice in-memory
--- a/pipeline/stages/05_inference.py
+++ b/pipeline/stages/05_inference.py
@@ -32,24 +32,11 @@ import sys
 import time
 from pathlib import Path
 import yaml
 sys.path.insert(0, str(Path(__file__).parent.parent))
 from orchestrator.db import init_db, get_conn, upsert_job, record_metric, now_iso
 PIPELINE_BASE = Path(os.environ.get("COSMA_PIPELINE_BASE", "/home/cosma/cosma-pipeline"))
 def _load_inference_cfg() -> dict:
    """Load inference params from thresholds.yaml, with sane defaults."""
    cfg_path = Path(__file__).parent.parent / "config" / "thresholds.yaml"
    try:
        data = yaml.safe_load(cfg_path.read_text())
        return data.get("inference", {})
    except Exception:
        return {}
 _INF_CFG = _load_inference_cfg()
 WORKERS = {
    ".84": {
        "host": "192.168.0.84",
@@ -159,37 +146,19 @@ def run_inference(frames_dir: Path, worker_key: str, mission_name: str,
        return metrics
    print(f"  [05] rsync done")
-    # Step 2: build demo.py command -- params from thresholds.yaml[inference]
+    # Step 2: build demo.py command
    checkpoint = f"{w['ai_dir']}/checkpoints/lingbot-map/lingbot-map.pt"
    inf_mode = _INF_CFG.get("mode", "streaming")
    conf_thr = _INF_CFG.get("ply_conf_threshold", 1.5)
    kf_interval = _INF_CFG.get("keyframe_interval", 1)
    max_frames = _INF_CFG.get("max_frame_num", 1024)
    if inf_mode == "windowed":
        window_size = _INF_CFG.get("window_size", 64)
        overlap_size = _INF_CFG.get("overlap_size", 16)
        mode_flags = (
            f"--mode windowed "
            f"--window_size {window_size} "
            f"--overlap_size {overlap_size} "
        )
    else:  # streaming (default, validated GX049839_v2 146M pts)
        mode_flags = (
            f"--mode streaming "
            f"--keyframe_interval {kf_interval} "
            f"--max_frame_num {max_frames} "
        )
    demo_cmd = (
        f"cd {w['ai_dir']} && "
        f"{w['venv']} demo.py "
        f"--model_path {checkpoint} "
        f"--image_folder {worker_frames} "
-        f"{mode_flags}"
+        f"--mode windowed "
-        f"--ply_conf_threshold {conf_thr} "
+        f"--window_size 64 "
        f"--overlap_size 16 "
        f"--save_ply {ply_remote} "
        f"--save_poses {npz_remote} "
-        f"--use_sdpa "
+        f"--use_sdpa " f"--offload_to_cpu " f"--ply_conf_threshold 1.5 "
        f"--offload_to_cpu "
        f"2>&1"
    )
--- a/pipeline/veille/2026-05-12-0430-iter-2.md
+++ b/pipeline/veille/2026-05-12-0430-iter-2.md
@@ -1,23 +0,0 @@
 # Veille COSMA reconstruction — iter-2 — 2026-05-12 04:30 UTC
 ## arxiv underwater 3D (7 derniers jours)
 - UW-3DGS: Underwater 3D Reconstruction, Physics-Aware Gaussian Splatting (arxiv 2508.06169)
 - Visual enhancement + 3D representation underwater: review (arxiv 2505.01869)
 ## arxiv AUV SLAM / point cloud
 - VISO: Robust Underwater Visual-Inertial-Sonar SLAM (arxiv 2601.01144) — VIS+sonar, fort intérêt pour pipeline USBL
 - RUSSO: Underwater SLAM stéréo+sonar+IMU (arxiv 2503.01434)
 - VIMS: Visual-Inertial-Magnetic-Sonar SLAM (arxiv 2506.15126)
 ## Repos GitHub actifs
 - naver/dust3r (7k★): actif, base pipeline lingbot-map
 - Junyi42/monst3r (ICLR 2025): géométrie vidéo dynamique
 - facebookresearch/vggt (CVPR 2025 Best Paper): reconstruction per-frame
 - CUT3R: Continuous 3D Perception, mise à jour mars 2026
 ## HuggingFace
 - Video-Depth-Anything-Small: depth video temps-réel
 - StereoAdapter: adaptation profondeur stéréo sous-marine
 ## Signal fort
 VISO (arxiv 2601.01144): pipeline USBL+caméra+IMU pour AUV, pourrait remplacer pure-camera pose estimation dans stage 06_align.
--- a/pipeline/veille/2026-05-12-1650-iter-4.md
+++ b/pipeline/veille/2026-05-12-1650-iter-4.md
@@ -1,26 +0,0 @@
 # Veille iter-4 — 2026-05-12 16:50 UTC
 ## Top signaux (8-9/10)
 - **ReefMapGS** arxiv.org/abs/2604.11992 — SLAM+3DGS 700m AUV, COLMAP-free, directement applicable COSMA (9/10)
 - **OceanSplat** (2026) — 3D Gaussian Splatting milieu turbide + trinocular consistency (9/10)
 - **BIND-USBL** arxiv.org/abs/2604.11861 — fusion IMU+USBL hétérogène ASV-AUV, delayed fusion = pattern réutilisable stage 06_align (9/10)
 - **LingBot-Map update** (27 avril) — keyframe_interval fix + long-video demo — update recommandé (8/10)
 - **PAS3R** HuggingFace — Pose-Adaptive Streaming 3D, long video = streaming AUV (8/10)
 - **AI-Aided AUV Navigation** arxiv.org/abs/2605.04672 — fusion INS+DVL+cam deep learning (8/10)
 ## Signaux modérés (7/10)
 - Aquatic Neuromorphic Optical Flow arxiv.org/abs/2605.07653 — event cam AUV turbide
 - WaterSplat-SLAM RAL 2026 — SLAM monoculaire sous-marin photoréaliste
 ## Repos actifs
 - lingbot-map (keyframe fix avril), awesome-dust3r (ecosystem DUSt3R/VGGT/CUT3R)
 - Matisse Ifremer — datasets flotte française
 ## Recommandations
 1. **BIND-USBL** : lire pour stage 06_align (pattern fusion USBL+IMU déjà dispo)
 2. **LingBot-Map update** : Already up to date. sur .84/.87 avant prochaine iter
 3. **ReefMapGS** : évaluer comme alternative stage 06_align si PR #9/#12 mergés
--- a/pipeline/veille/2026-05-12-2246-iter-5.md
+++ b/pipeline/veille/2026-05-12-2246-iter-5.md
@@ -1,26 +0,0 @@
 # Veille Iter-5 — 2026-05-12 22:46 UTC
 ## Arxiv / Papers
 | # | Titre | Signal | Score |
 |---|-------|--------|-------|
 | 1 | ReefMapGS | SLAM multimodal + Gaussian Splatting pour grandes scènes sous-marines avec fermeture de boucle | 9/10 |
 | 2 | Sonar-MASt3R | Fusion optico-acoustique temps réel pour environnements turbides — intéressant pour milieu turbide AUV | 8/10 |
 | 3 | WaterSplat-SLAM | SLAM monoculaire photoréaliste underwater, moindre dépendance stéréo | 8/10 |
 | 4 | Spatiotemporal Degradation-Aware 3DGS | Reconstruction scènes sous-marines avec dégradation temporelle (particules, courant) | 8/10 |
 | 5 | BALTIC Benchmark | Benchmark 3D reconstruction air/underwater avec variations d'illumination, utile pour QC comparaison | 7/10 |
 | 6 | Lost at Sea (Notre Dame) | AUV utilisant 3DGS pour navigation autonome et reconnaissance environnement | 7/10 |
 ## GitHub / HuggingFace
 | Repo | Signal |
 |------|--------|
 | LingBot-Map | Commits récents (4 jours) — à tracker pour keyframe fixes |
 | dust3r/mast3r | Actifs, pas de release majeure dernière semaine |
 | Pixal3D (SIGGRAPH 2026) | 3D pixel-alignée, potentiellement utile pour poses denses |
 ## Recommandation prochaine iteration
 - **ReefMapGS** : évaluer pour remplacement LingBot-Map sur grands segments (15m+)
 - **Sonar-MASt3R** : pertinent si Kogger SBP intégré dans pipeline — stage 06 USBL+cam pourrait utiliser composante acoustique
 - **BALTIC Benchmark** : utiliser pour QC comparatif sur segments AUV210 (turbide)
--- a/pipeline/veille/2026-05-13-1043-iter-7.md
+++ b/pipeline/veille/2026-05-13-1043-iter-7.md
@@ -1,21 +0,0 @@
 # Veille iter-7 — 2026-05-13 10:43 UTC
 ## Papers / Signaux (6 total)
 | # | Titre | Ref | Score | Pertinence COSMA |
 |---|-------|-----|-------|-----------------|
 | 1 | Aquatic Neuromorphic Optical Flow | arXiv 2605.07653 (5j) | 9/10 | Optique turbide robuste, temps-réel, léger → stage06_align |
 | 2 | MAGS-SLAM: Multi-Agent 3DGS SLAM | arXiv 2605.10760 (2j) | 8/10 | SLAM 3DGS multi-robot, cohérence photométrique → futur multi-AUV |
 | 3 | AI Platform AUV 3DGS (Notre-Dame) | engineering.nd.edu (5j) | 9/10 | 3DGS ellipsoïdes flous underwater, navigation AUV pré-chargée |
 | 4 | MV-DUSt3R+ | GitHub facebookresearch (7j) | 8/10 | DUSt3R v2 rapide (2s), baseline comparaison stage05 |
 | 5 | MonST3R | GitHub Junyi42 (ICLR 2025) | 7/10 | Géométrie robuste motion/occlusion → transition segments |
 | 6 | LingBot-Map | GitHub robbyant (5j) | 9/10 | Màj streaming, vérifier diff vs version .84/.87 installée |
 ## Repos actifs (7j)
 - **lingbot-map** (robbyant) : dernière màj 5j — comparer avec version installée .84/.87
 - **dust3r / monst3r** : mises à jour README et poids — rien d'urgent
 ## Recommandations prochaines
 1. Évaluer Aquatic Neuromorphic Optical Flow pour stage06_align (turbide)
 2. Benchmarker 3DGS (MAGS-SLAM ou Notre-Dame) sur 1 segment AUV210
 3. Mettre à jour lingbot-map .84/.87 si diff significatif
--- a/pipeline/veille/2026-05-13-1643-iter-8.md
+++ b/pipeline/veille/2026-05-13-1643-iter-8.md
@@ -1,17 +0,0 @@
 # Veille — 2026-05-13 16:43 UTC — Iter 8
 ## Signaux forts
 | Titre | Signal | Raison |
 |-------|--------|--------|
 | LingBot-Map accelerated (2026-04-27 update) | 10/10 | Streaming 3D foundation model, maj 16j, optimisations directement applicables pipeline COSMA |
 | ND AI Platform : 3DGS + Bayesian uncertainty | 9/10 | Gaussian splatting + quantification d'incertitude — utile pour USBL fusion et scoring confiance PLY |
 | GPU COLMAP+3DGS Deploy Guide | 7/10 | COLMAP SfM → 3DGS sur GPU, alternative si lingbot-map insuffisant segments dégradés |
 | Correlator3D 3DGS integration | 6/10 | Photogrammetrie + Gaussian splatting pour meshes naturels, contexte fond marin |
 ## Pas de nouveaux hits 7j
 - dust3r, monst3r, vggt, mee-deepreefmap : aucune mise à jour récente
 ## Recommandations
 - Tracker la maj LingBot-Map 2026-04-27 : vérifier si les optimisations vitesse sont dans le checkout .84
 - Évaluer ND 3DGS Bayesian pour étape post-PLY : scoring incertitude peut améliorer stitch ICP
--- a/scripts/coverage_swath.py
+++ b/scripts/coverage_swath.py
@@ -1,139 +0,0 @@
 #!/usr/bin/env python3
 """Coverage swath QC plot — project each frame footprint on ground.
 Usage:
  python3 coverage_swath.py --traj-csv /tmp/dvl_loopclosed_GX039839.csv \
    --frames-dir /home/cosma/...AUV210/GX039839 \
    --altitude 1.5 --fov-h 122 --fov-v 80 --out /tmp/coverage_GX039839.png
 """
 import argparse, csv, math
 from pathlib import Path
 import numpy as np
 import cv2
 def compute_qc(frame_path):
    """R<G-5 && R<B-5 underwater test, return ratio of bottom_visible-ish pixels."""
    img = cv2.imread(str(frame_path), cv2.IMREAD_COLOR)
    if img is None: return 0.0
    b, g, r = cv2.split(img)
    mask = (r < g.astype(int) - 5) & (r < b.astype(int) - 5)
    # contrast: stddev of gray channel
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    if gray.std() < 20: return 0.0  # turbid / blurry
    return float(mask.mean())
 def main():
    ap = argparse.ArgumentParser()
    ap.add_argument('--traj-csv', required=True)
    ap.add_argument('--frames-dir', required=True)
    ap.add_argument('--altitude', type=float, default=1.5)
    ap.add_argument('--fov-h', type=float, default=122.0)
    ap.add_argument('--fov-v', type=float, default=80.0)
    ap.add_argument('--out', required=True)
    ap.add_argument('--x-col', default='east_m_corr')
    ap.add_argument('--y-col', default='north_m_corr')
    ap.add_argument('--label', default='segment')
    ap.add_argument('--heading-csv', default=None, help='separate CSV with heading_deg per frame')
    ap.add_argument('--sample-every', type=int, default=10, help='draw every N frames')
    args = ap.parse_args()
    # Load trajectory
    rows = list(csv.DictReader(open(args.traj_csv)))
    # autodetect col names
    cols = rows[0].keys()
    if args.x_col not in cols: args.x_col = 'east_m' if 'east_m' in cols else 'x'
    if args.y_col not in cols: args.y_col = 'north_m' if 'north_m' in cols else 'y'
    print(f'[cov] {len(rows)} rows, x_col={args.x_col} y_col={args.y_col}', flush=True)
    # Load heading from a heading CSV (or assume 0)
    headings = {}
    if args.heading_csv:
        for r in csv.DictReader(open(args.heading_csv)):
            headings[int(r['frame_idx'])] = float(r['heading_deg'])
        print(f'[cov] {len(headings)} headings loaded', flush=True)
    elif 'heading_deg' in cols:
        for r in rows:
            headings[int(r['frame_idx'])] = float(r['heading_deg'])
    frames_dir = Path(args.frames_dir)
    # Footprint dimensions at altitude
    half_w = args.altitude * math.tan(math.radians(args.fov_h/2))
    half_h = args.altitude * math.tan(math.radians(args.fov_v/2))
    print(f'[cov] footprint at alt={args.altitude}m: {2*half_w:.2f}m × {2*half_h:.2f}m', flush=True)
    import matplotlib
    matplotlib.use('Agg')
    import matplotlib.pyplot as plt
    from matplotlib.patches import Rectangle
    from matplotlib.transforms import Affine2D
    from matplotlib.collections import PatchCollection
    fig, axes = plt.subplots(1, 2, figsize=(20, 10))
    ax_cov, ax_traj = axes[0], axes[1]
    # Collect rectangles + colors
    rects = []
    colors = []
    qc_values = []
    xs = []; ys = []
    for r in rows[::args.sample_every]:
        fi = int(r['frame_idx'])
        x = float(r[args.x_col])
        y = float(r[args.y_col])
        xs.append(x); ys.append(y)
        hdg = headings.get(fi, 0.0)
        # QC
        fpath = frames_dir / f'frame_{fi+1:05d}.jpg'
        if fpath.exists():
            qc = compute_qc(fpath)
        else:
            qc = 0.0
        qc_values.append(qc)
        # Build rotated rectangle
        # rectangle in body frame: x = +/- half_w (right), y = +/- half_h (forward)
        # but in world: rotated by heading
        from matplotlib.patches import Polygon
        corners_body = np.array([
            [-half_w, -half_h],
            [+half_w, -half_h],
            [+half_w, +half_h],
            [-half_w, +half_h],
        ])
        # heading rotation (clockwise from north, so for math invert)
        th = math.radians(hdg)
        R = np.array([[math.cos(th), math.sin(th)],
                      [-math.sin(th), math.cos(th)]])
        corners_world = corners_body @ R.T + np.array([x, y])
        rects.append(corners_world)
        colors.append(qc)
    # Plot coverage
    from matplotlib.patches import Polygon
    for corners, qc in zip(rects, colors):
        poly = Polygon(corners, alpha=0.10, edgecolor='black', linewidth=0.1,
                       facecolor=plt.cm.RdYlGn(qc * 1.5 if qc < 0.7 else 1.0))
        ax_cov.add_patch(poly)
    ax_cov.plot(xs, ys, '-k', linewidth=0.5, alpha=0.5)
    ax_cov.plot(xs[0], ys[0], 'go', markersize=12, label='start')
    ax_cov.plot(xs[-1], ys[-1], 'r^', markersize=12, label='end')
    ax_cov.set_xlabel('East (m)'); ax_cov.set_ylabel('North (m)')
    ax_cov.set_title(f'Coverage swath — {args.label}\n{len(rects)} footprints @ alt={args.altitude}m FOV {args.fov_h}°×{args.fov_v}°\n(green=bottom visible, red=hors-eau/turbid)')
    ax_cov.set_aspect('equal'); ax_cov.legend(); ax_cov.grid(True, alpha=0.3)
    # Trajectory only with QC color
    sc = ax_traj.scatter(xs, ys, c=colors, cmap='RdYlGn', s=12, vmin=0, vmax=0.7)
    plt.colorbar(sc, ax=ax_traj, label='bottom_visible ratio')
    ax_traj.plot(xs[0], ys[0], 'go', markersize=12)
    ax_traj.plot(xs[-1], ys[-1], 'r^', markersize=12)
    ax_traj.set_xlabel('East (m)'); ax_traj.set_ylabel('North (m)')
    ax_traj.set_title(f'Trajectoire colorée par QC ({len(xs)} points)'); ax_traj.set_aspect('equal'); ax_traj.grid(True, alpha=0.3)
    plt.suptitle(f'Acquisition QC swath — {args.label}', fontsize=14)
    plt.tight_layout()
    plt.savefig(args.out, dpi=120, bbox_inches='tight')
    print(f'[plot] {args.out}', flush=True)
 if __name__ == '__main__': main()
--- a/scripts/dvl_focal_sweep.py
+++ b/scripts/dvl_focal_sweep.py
@@ -1,61 +0,0 @@
 #!/usr/bin/env python3
 """Test multiple focal lengths on DVL and compare trajectory drift."""
 import subprocess, csv, sys, math
 from pathlib import Path
 import matplotlib
 matplotlib.use('Agg')
 import matplotlib.pyplot as plt
 FRAMES = '/home/cosma/cosma-pipeline/data/20260505-Lepradet/frames/AUV210/GX039839'
 START = '2026-05-05T08:33:41'
 LABEL = 'GX039839'
 SCRIPT = '/home/cosma/cosma-qc/scripts/dvl_optical_full.py'
 # focal in px equivalent to W/2 / tan(fov/2). W=518
 # fov 100°→f=217, 110°→f=185, 122°→f=143, 130°→f=121, 140°→f=94, 150°→f=70
 # But focal in px doesn't directly map to FOV unless we use that conversion. We pass --fov-deg.
 fovs = [90, 100, 110, 122, 135, 150]
 results = []
 for fov in fovs:
    out_csv = f'/tmp/sweep_fov{fov}.csv'
    print(f'[sweep] fov={fov}', flush=True)
    r = subprocess.run(['python3', SCRIPT, '--frames-dir', FRAMES, '--altitude', '1.5',
                        '--fov-deg', str(fov), '--fps', '1.0', '--start-iso', START,
                        '--label', LABEL, '--out', out_csv], capture_output=True, text=True, timeout=600)
    if r.returncode != 0:
        print(f'  FAIL: {r.stderr[-300:]}', flush=True)
        continue
    # parse last position + drift metrics
    rows = list(csv.DictReader(open(out_csv)))
    e = [float(r['east_m']) for r in rows]
    n = [float(r['north_m']) for r in rows]
    h = [float(r['heading_deg']) for r in rows]
    end_x, end_y = e[-1], n[-1]
    end_dist = math.sqrt(end_x**2 + end_y**2)
    path_len = sum(math.sqrt((e[i]-e[i-1])**2 + (n[i]-n[i-1])**2) for i in range(1, len(e)))
    bbox = (max(e)-min(e), max(n)-min(n))
    results.append({'fov': fov, 'csv': out_csv, 'end_x': end_x, 'end_y': end_y, 
                    'end_dist': end_dist, 'path_len': path_len, 'bbox': bbox, 'rows': rows,
                    'e_arr': e, 'n_arr': n, 'h_arr': h})
    print(f'  end=({end_x:.1f},{end_y:.1f}) dist={end_dist:.1f}m path={path_len:.1f}m bbox={bbox}', flush=True)
 # Plot all trajectories
 fig, axes = plt.subplots(2, 3, figsize=(18, 12))
 axes = axes.flatten()
 for ax, res in zip(axes, results):
    ax.plot(res['e_arr'], res['n_arr'], '-b', linewidth=0.8)
    ax.plot(res['e_arr'][0], res['n_arr'][0], 'go', markersize=8)
    ax.plot(res['e_arr'][-1], res['n_arr'][-1], 'r^', markersize=8)
    ax.set_xlabel('East (m)'); ax.set_ylabel('North (m)')
    ax.set_title(f'FOV={res["fov"]}° bbox=({res["bbox"][0]:.0f}×{res["bbox"][1]:.0f})m\nend_dist={res["end_dist"]:.1f}m path={res["path_len"]:.0f}m')
    ax.set_aspect('equal'); ax.grid(True, alpha=0.3)
 plt.suptitle(f'DVL focal sweep — {LABEL} (assume closed-loop survey → smaller end_dist=better)')
 plt.tight_layout()
 plt.savefig('/tmp/sweep_focal.png', dpi=110, bbox_inches='tight')
 print('[plot] /tmp/sweep_focal.png', flush=True)
 # Summary
 print('\n=== Summary ===')
 for r in sorted(results, key=lambda x: x['end_dist']):
    print(f"FOV={r['fov']}°: end_dist={r['end_dist']:.1f}m  path={r['path_len']:.0f}m  bbox={r['bbox'][0]:.0f}×{r['bbox'][1]:.0f}m")
--- a/scripts/dvl_optical.py
+++ b/scripts/dvl_optical.py
@@ -1,140 +0,0 @@
 #!/usr/bin/env python3
 """Optical DVL — mean optical flow per frame → 2D ground velocity integration.
 Assumes downward-looking camera at constant altitude above ground.
 Convert pixel flow to metric using altitude / focal_length.
 Pipeline:
  1. Dense Farneback flow between consecutive frames
  2. Median flow vector (px) → robust against outliers
  3. v_m = flow_px * altitude_m / focal_px  (instant velocity in cam plane)
  4. Integrate → trajectory (cam-frame XY)
  5. Optional: apply IMU heading rotation per frame for body-frame correction
 Usage:
  python3 dvl_optical.py --frames-dir <dir> --altitude 1.5 --fps 1.0 \
    --start-iso 2026-05-05T08:33:41 --label GX039839 \
    --out /tmp/dvl.csv --plot /tmp/dvl.png [--ref-csv /tmp/GX039839_camera.csv]
 """
 import argparse, csv, math, sys
 from pathlib import Path
 from datetime import datetime
 import numpy as np
 import cv2
 def main():
    ap = argparse.ArgumentParser()
    ap.add_argument('--frames-dir', required=True)
    ap.add_argument('--altitude', type=float, default=1.5, help='Camera height above bottom (m)')
    ap.add_argument('--fov-deg', type=float, default=122.0, help='GoPro horizontal FOV')
    ap.add_argument('--fps', type=float, default=1.0)
    ap.add_argument('--start-iso', default='2026-05-05T00:00:00')
    ap.add_argument('--label', default='segment')
    ap.add_argument('--out', required=True)
    ap.add_argument('--plot', default=None)
    ap.add_argument('--ref-csv', default=None)
    ap.add_argument('--method', choices=['farneback','lk'], default='farneback')
    args = ap.parse_args()
    frames = sorted(Path(args.frames_dir).glob('frame_*.jpg'))
    print(f'[dvl] {len(frames)} frames', flush=True)
    W, H = 518, 294
    f = (W/2) / math.tan(math.radians(args.fov_deg/2))
    # scale factor : 1 px flow at altitude_m = (altitude_m / focal_px) meters
    px_to_m = args.altitude / f
    print(f'[dvl] focal_px={f:.1f} altitude={args.altitude}m -> px_to_m={px_to_m:.5f}', flush=True)
    t0 = datetime.fromisoformat(args.start_iso).timestamp()
    rows = []
    rows.append({'frame_idx': 0, 'ts_s': t0, 'flow_x_px': 0, 'flow_y_px': 0, 'speed_mps': 0, 'x_m': 0, 'y_m': 0})
    prev = cv2.imread(str(frames[0]), cv2.IMREAD_GRAYSCALE)
    x_cum, y_cum = 0.0, 0.0
    for i in range(1, len(frames)):
        curr = cv2.imread(str(frames[i]), cv2.IMREAD_GRAYSCALE)
        if curr is None: continue
        if args.method == 'farneback':
            flow = cv2.calcOpticalFlowFarneback(prev, curr, None, 0.5, 3, 21, 3, 5, 1.2, 0)
            fx = np.median(flow[..., 0])
            fy = np.median(flow[..., 1])
        else:  # lk on grid
            h, w = prev.shape
            pts = np.array([[x, y] for y in range(20, h-20, 30) for x in range(20, w-20, 30)], dtype=np.float32).reshape(-1, 1, 2)
            curr_pts, status, err = cv2.calcOpticalFlowPyrLK(prev, curr, pts, None, winSize=(21,21))
            good = status.flatten() == 1
            if good.sum() < 10:
                fx = fy = 0
            else:
                d = (curr_pts - pts)[good].reshape(-1, 2)
                fx = np.median(d[:, 0]); fy = np.median(d[:, 1])
        # Convert px/frame -> m/frame
        dx_m = fx * px_to_m
        dy_m = fy * px_to_m
        # AUV motion is OPPOSITE to optical flow direction (camera moves opposite to apparent ground motion)
        # If ground appears to move +x in image, AUV moves -x in world
        x_cum -= dx_m
        y_cum -= dy_m
        speed_mps = math.sqrt(dx_m**2 + dy_m**2) * args.fps
        rows.append({'frame_idx': i, 'ts_s': t0 + i/args.fps, 'flow_x_px': float(fx), 'flow_y_px': float(fy),
                     'speed_mps': speed_mps, 'x_m': x_cum, 'y_m': y_cum})
        prev = curr
        if i % 100 == 0:
            print(f'[dvl] {i}/{len(frames)} flow=({fx:.2f},{fy:.2f}) speed={speed_mps:.3f}m/s pos=({x_cum:.2f},{y_cum:.2f})', flush=True)
    print(f'[dvl] done. Final position: ({x_cum:.2f}, {y_cum:.2f}) m', flush=True)
    with open(args.out, 'w', newline='') as f:
        w = csv.DictWriter(f, fieldnames=list(rows[0].keys()))
        w.writeheader(); w.writerows(rows)
    print(f'[out] {args.out}', flush=True)
    if args.plot:
        import matplotlib
        matplotlib.use('Agg')
        import matplotlib.pyplot as plt
        fig, axes = plt.subplots(2, 2, figsize=(14, 12))
        ax_xy, ax_speed, ax_flow, ax_cmp = axes[0,0], axes[0,1], axes[1,0], axes[1,1]
        x = [r['x_m'] for r in rows]; y = [r['y_m'] for r in rows]
        ax_xy.plot(x, y, '-b', linewidth=1.2)
        ax_xy.plot(x[0], y[0], 'go', markersize=10, label='start')
        ax_xy.plot(x[-1], y[-1], 'r^', markersize=10, label='end')
        ax_xy.set_xlabel('X (m)'); ax_xy.set_ylabel('Y (m)'); ax_xy.set_title(f'DVL trajectory (altitude={args.altitude}m)')
        ax_xy.set_aspect('equal'); ax_xy.legend(); ax_xy.grid(True, alpha=0.3)
        speeds = [r['speed_mps'] for r in rows]
        ax_speed.plot(range(len(rows)), speeds, '-r', linewidth=0.8)
        ax_speed.set_xlabel('Frame'); ax_speed.set_ylabel('Speed (m/s)'); ax_speed.set_title('Speed over time'); ax_speed.grid(True, alpha=0.3)
        fx_arr = [r['flow_x_px'] for r in rows]; fy_arr = [r['flow_y_px'] for r in rows]
        ax_flow.plot(fx_arr, label='flow_x px', alpha=0.6)
        ax_flow.plot(fy_arr, label='flow_y px', alpha=0.6)
        ax_flow.set_xlabel('Frame'); ax_flow.set_ylabel('Median flow (px)'); ax_flow.set_title('Median optical flow'); ax_flow.legend(); ax_flow.grid(True, alpha=0.3)
        # comparison with reference
        if args.ref_csv:
            try:
                with open(args.ref_csv) as ff:
                    refrows = [r for r in csv.DictReader(ff) if r.get('segment','')==args.label or r.get('label','')==args.label]
                rx = [float(r['x']) for r in refrows]
                ry = [float(r['y']) for r in refrows]
                ax_cmp.plot(x, y, '-b', linewidth=1.2, label='DVL optical', alpha=0.7)
                ax_cmp.plot(rx, ry, '-r', linewidth=1.2, label='lingbot', alpha=0.7)
                ax_cmp.plot(x[0], y[0], 'go', markersize=8)
                ax_cmp.set_xlabel('X (m)'); ax_cmp.set_ylabel('Y (m)'); ax_cmp.set_title('DVL vs Lingbot (same scale, x/y)'); ax_cmp.set_aspect('equal')
                ax_cmp.legend(); ax_cmp.grid(True, alpha=0.3)
            except Exception as e:
                print(f'[plot] ref fail: {e}', flush=True)
        else:
            ax_cmp.set_title('(no reference)')
        plt.suptitle(f'Optical DVL — {args.label} ({args.method.upper()} flow, altitude {args.altitude}m)')
        plt.tight_layout()
        plt.savefig(args.plot, dpi=130, bbox_inches='tight')
 if __name__ == '__main__': main()
--- a/scripts/dvl_optical_full.py
+++ b/scripts/dvl_optical_full.py
@@ -1,156 +0,0 @@
 #!/usr/bin/env python3
 """Optical DVL with rotation+scale derived from optical flow ONLY (no IMU).
 Per frame: track features (KLT), fit similarity transform (tx, ty, theta, scale),
 extract metric translation + heading delta, integrate in world frame.
 """
 import argparse, csv, math
 from pathlib import Path
 from datetime import datetime
 import numpy as np
 import cv2
 def main():
    ap = argparse.ArgumentParser()
    ap.add_argument('--frames-dir', required=True)
    ap.add_argument('--altitude', type=float, default=1.5)
    ap.add_argument('--fov-deg', type=float, default=122.0)
    ap.add_argument('--fps', type=float, default=1.0)
    ap.add_argument('--start-iso', default='2026-05-05T00:00:00')
    ap.add_argument('--label', default='segment')
    ap.add_argument('--out', required=True)
    ap.add_argument('--plot', default=None)
    ap.add_argument('--ref-csv', default=None)
    ap.add_argument('--init-heading-deg', type=float, default=0.0)
    args = ap.parse_args()
    frames = sorted(Path(args.frames_dir).glob('frame_*.jpg'))
    print(f'[dvl] {len(frames)} frames', flush=True)
    W, H = 518, 294
    f = (W/2) / math.tan(math.radians(args.fov_deg/2))
    px_to_m = args.altitude / f
    print(f'[dvl] focal_px={f:.1f} px_to_m={px_to_m:.5f}', flush=True)
    t0 = datetime.fromisoformat(args.start_iso).timestamp()
    heading = args.init_heading_deg
    east_cum, north_cum = 0.0, 0.0
    rows = []
    rows.append({'frame_idx':0,'ts_s':t0,'heading_deg':heading,'d_theta_deg':0,'scale':1.0,
                 'dx_cam_px':0,'dy_cam_px':0,'east_m':0,'north_m':0,'inliers':0})
    prev_gray = cv2.imread(str(frames[0]), cv2.IMREAD_GRAYSCALE)
    prev_pts = cv2.goodFeaturesToTrack(prev_gray, maxCorners=1000, qualityLevel=0.01, minDistance=7, blockSize=7)
    for i in range(1, len(frames)):
        curr_gray = cv2.imread(str(frames[i]), cv2.IMREAD_GRAYSCALE)
        if curr_gray is None: continue
        if prev_pts is None or len(prev_pts) < 100:
            prev_pts = cv2.goodFeaturesToTrack(prev_gray, maxCorners=1000, qualityLevel=0.01, minDistance=7, blockSize=7)
        curr_pts, status, err = cv2.calcOpticalFlowPyrLK(prev_gray, curr_gray, prev_pts, None, winSize=(21,21), maxLevel=3)
        good_prev = prev_pts[status.flatten()==1]
        good_curr = curr_pts[status.flatten()==1]
        n_tracked = len(good_prev)
        if n_tracked < 30:
            # tracking lost - keep last heading + skip motion
            rows.append({'frame_idx':i,'ts_s':t0+i/args.fps,'heading_deg':heading,'d_theta_deg':0,'scale':1.0,
                         'dx_cam_px':0,'dy_cam_px':0,'east_m':east_cum,'north_m':north_cum,'inliers':0})
            prev_gray = curr_gray
            prev_pts = cv2.goodFeaturesToTrack(curr_gray, maxCorners=1000, qualityLevel=0.01, minDistance=7, blockSize=7)
            continue
        # Fit similarity 2D (translation + rotation + scale)
        M, inliers = cv2.estimateAffinePartial2D(good_prev.reshape(-1,2), good_curr.reshape(-1,2), method=cv2.RANSAC, ransacReprojThreshold=2.0)
        if M is None:
            rows.append({'frame_idx':i,'ts_s':t0+i/args.fps,'heading_deg':heading,'d_theta_deg':0,'scale':1.0,
                         'dx_cam_px':0,'dy_cam_px':0,'east_m':east_cum,'north_m':north_cum,'inliers':0})
            prev_gray = curr_gray
            prev_pts = cv2.goodFeaturesToTrack(curr_gray, maxCorners=1000, qualityLevel=0.01, minDistance=7, blockSize=7)
            continue
        # M = [[s*cos(theta), -s*sin(theta), tx], [s*sin(theta), s*cos(theta), ty]]
        # Extract scale, rotation, translation
        a, b, tx = M[0]
        c, d, ty = M[1]
        s = math.sqrt(a*a + b*b)
        theta = math.atan2(c, a)  # rotation angle (in image coords)
        n_inliers = int(inliers.sum()) if inliers is not None else 0
        # AUV motion is OPPOSITE to apparent ground motion
        dx_world_cam = -tx * px_to_m  # AUV moved -tx in cam-X
        dy_world_cam = -ty * px_to_m  # AUV moved -ty in cam-Y
        # Heading delta = -theta (if features rotate clockwise in image, AUV yawed counter-clockwise from above)
        d_theta_deg = -math.degrees(theta)
        heading += d_theta_deg
        heading %= 360
        # Rotate cam-frame motion (dx,dy) by current world heading
        hdg_rad = math.radians(heading)
        # body forward = +dy_cam (if cam Y_image = AUV forward; if down-facing cam mounted with image up = AUV forward)
        body_forward = dy_world_cam
        body_right = dx_world_cam
        de = body_forward * math.sin(hdg_rad) + body_right * math.cos(hdg_rad)
        dn = body_forward * math.cos(hdg_rad) - body_right * math.sin(hdg_rad)
        east_cum += de
        north_cum += dn
        rows.append({'frame_idx':i,'ts_s':t0+i/args.fps,'heading_deg':heading,'d_theta_deg':d_theta_deg,'scale':s,
                     'dx_cam_px':tx,'dy_cam_px':ty,'east_m':east_cum,'north_m':north_cum,'inliers':n_inliers})
        prev_gray = curr_gray
        # Refresh features periodically
        prev_pts = cv2.goodFeaturesToTrack(curr_gray, maxCorners=1000, qualityLevel=0.01, minDistance=7, blockSize=7)
        if i % 100 == 0:
            print(f'[dvl] {i}/{len(frames)} tracked={n_tracked} inl={n_inliers} d_th={d_theta_deg:+.2f}° hdg={heading:.0f}° s={s:.3f} pos=({east_cum:.2f},{north_cum:.2f})', flush=True)
    print(f'[dvl] done. Final ENU: ({east_cum:.2f}, {north_cum:.2f}) m. Final heading {heading:.0f}°', flush=True)
    with open(args.out, 'w', newline='') as ff:
        w = csv.DictWriter(ff, fieldnames=list(rows[0].keys()))
        w.writeheader(); w.writerows(rows)
    print(f'[out] {args.out}', flush=True)
    if args.plot:
        import matplotlib
        matplotlib.use('Agg')
        import matplotlib.pyplot as plt
        fig, axes = plt.subplots(2, 2, figsize=(14, 12))
        ax_xy, ax_hdg, ax_speed, ax_cmp = axes[0,0], axes[0,1], axes[1,0], axes[1,1]
        e = [r['east_m'] for r in rows]; n = [r['north_m'] for r in rows]
        ax_xy.plot(e, n, '-b', linewidth=1.2)
        ax_xy.plot(e[0], n[0], 'go', markersize=10, label='start')
        ax_xy.plot(e[-1], n[-1], 'r^', markersize=10, label='end')
        ax_xy.set_xlabel('East (m)'); ax_xy.set_ylabel('North (m)'); ax_xy.set_title('DVL trajectory (rotation from optical flow)')
        ax_xy.set_aspect('equal'); ax_xy.legend(); ax_xy.grid(True, alpha=0.3)
        hdgs = [r['heading_deg'] for r in rows]
        ax_hdg.plot(range(len(rows)), hdgs, '-c'); ax_hdg.set_xlabel('Frame'); ax_hdg.set_ylabel('Heading (deg)'); ax_hdg.set_title('Heading (integrated from optical flow rotation)'); ax_hdg.grid(True, alpha=0.3)
        scales = [r['scale'] for r in rows]
        ax_speed.plot(range(len(rows)), scales, color='orange'); ax_speed.set_xlabel('Frame'); ax_speed.set_ylabel('Scale (1=no zoom)'); ax_speed.set_title('Scale per frame (>1 = zoom in = down)'); ax_speed.axhline(1.0, color='k', alpha=0.3); ax_speed.grid(True, alpha=0.3)
        if args.ref_csv:
            try:
                with open(args.ref_csv) as fff:
                    refrows = [r for r in csv.DictReader(fff) if r.get('segment','')==args.label or r.get('label','')==args.label]
                rx = [float(r['x']) for r in refrows]
                ry = [float(r['y']) for r in refrows]
                ax_cmp.plot(e, n, '-b', linewidth=1.2, label='DVL optical (rotation included)', alpha=0.7)
                ax_cmp.plot(rx, ry, '-r', linewidth=1.2, label='lingbot', alpha=0.7)
                ax_cmp.set_xlabel('East'); ax_cmp.set_ylabel('North'); ax_cmp.set_title('Comparison')
                ax_cmp.set_aspect('equal'); ax_cmp.legend(); ax_cmp.grid(True, alpha=0.3)
            except Exception as e: print(f'[ref] {e}', flush=True)
        else:
            ax_cmp.set_title('(no reference)')
        plt.suptitle(f'DVL optical (rotation+scale from cv2.estimateAffinePartial2D) — {args.label}')
        plt.tight_layout()
        plt.savefig(args.plot, dpi=130, bbox_inches='tight')
        print(f'[plot] {args.plot}', flush=True)
 if __name__ == '__main__': main()
--- a/scripts/dvl_optical_imu.py
+++ b/scripts/dvl_optical_imu.py
@@ -1,166 +0,0 @@
 #!/usr/bin/env python3
 """Optical DVL + IMU heading correction.
 Same as dvl_optical.py but rotates cam-frame flow to world frame using compass_hdg.
 Usage:
  python3 dvl_optical_imu.py --frames-dir <dir> --bag-dir <auv_bags_dir> \
      --altitude 1.5 --fps 1.0 --start-iso ... --label ... \
      --out csv --plot png [--ref-csv ...]
 """
 import argparse, csv, math, sys
 from pathlib import Path
 from datetime import datetime
 import numpy as np
 import cv2
 from rosbags.highlevel import AnyReader
 def load_heading(bag_dir, t_start, t_end):
    bags = sorted(Path(bag_dir).glob('*.mcap'))
    # filter empty
    bags = [b for b in bags if b.stat().st_size > 1000]
    headings = []  # list of (ts_s, heading_deg)
    for b in bags:
        try:
            with AnyReader([b]) as r:
                for conn, ts_ns, raw in r.messages(connections=[c for c in r.connections if c.topic == '/mavros/global_position/compass_hdg']):
                    t = ts_ns / 1e9
                    if t_start - 60 <= t <= t_end + 60:
                        m = r.deserialize(raw, conn.msgtype)
                        headings.append((t, m.data))
        except Exception as e:
            print(f'[warn] {b.name}: {e}', flush=True)
    headings.sort()
    return headings
 def nearest_hdg(headings, t_target):
    if not headings: return None
    ts = [h[0] for h in headings]
    idx = np.searchsorted(ts, t_target)
    if idx == 0: return headings[0][1]
    if idx >= len(headings): return headings[-1][1]
    if abs(headings[idx][0] - t_target) < abs(headings[idx-1][0] - t_target):
        return headings[idx][1]
    return headings[idx-1][1]
 def main():
    ap = argparse.ArgumentParser()
    ap.add_argument('--frames-dir', required=True)
    ap.add_argument('--bag-dir', required=True)
    ap.add_argument('--altitude', type=float, default=1.5)
    ap.add_argument('--fov-deg', type=float, default=122.0)
    ap.add_argument('--fps', type=float, default=1.0)
    ap.add_argument('--start-iso', default='2026-05-05T00:00:00')
    ap.add_argument('--label', default='segment')
    ap.add_argument('--out', required=True)
    ap.add_argument('--plot', default=None)
    ap.add_argument('--ref-csv', default=None)
    args = ap.parse_args()
    frames = sorted(Path(args.frames_dir).glob('frame_*.jpg'))
    print(f'[dvl] {len(frames)} frames', flush=True)
    t0 = datetime.fromisoformat(args.start_iso).timestamp()
    t_end = t0 + len(frames) / args.fps
    # Load heading
    print(f'[hdg] loading from {args.bag_dir}', flush=True)
    headings = load_heading(args.bag_dir, t0, t_end)
    print(f'[hdg] {len(headings)} samples loaded, t range: {headings[0][0]:.0f}-{headings[-1][0]:.0f}', flush=True)
    W, H = 518, 294
    f = (W/2) / math.tan(math.radians(args.fov_deg/2))
    px_to_m = args.altitude / f
    print(f'[dvl] px_to_m={px_to_m:.5f}', flush=True)
    rows = []
    rows.append({'frame_idx': 0, 'ts_s': t0, 'heading_deg': nearest_hdg(headings, t0) or 0, 'flow_x_px': 0, 'flow_y_px': 0, 
                 'speed_mps': 0, 'east_m': 0, 'north_m': 0})
    prev = cv2.imread(str(frames[0]), cv2.IMREAD_GRAYSCALE)
    east_cum, north_cum = 0.0, 0.0
    for i in range(1, len(frames)):
        curr = cv2.imread(str(frames[i]), cv2.IMREAD_GRAYSCALE)
        if curr is None: continue
        t_frame = t0 + i / args.fps
        hdg = nearest_hdg(headings, t_frame) or 0
        flow = cv2.calcOpticalFlowFarneback(prev, curr, None, 0.5, 3, 21, 3, 5, 1.2, 0)
        fx_cam = np.median(flow[..., 0])
        fy_cam = np.median(flow[..., 1])
        # convert px → m in CAM frame (cam right = +X_cam, cam down = +Y_cam image coord)
        dx_cam = -fx_cam * px_to_m  # AUV moves opposite to flow
        dy_cam = -fy_cam * px_to_m
        # Apply heading rotation: cam +X_cam = body forward? assume cam frame Y axis = AUV forward
        # The downward camera: cam +Y_image = body forward typically (or -Y if mounted otherwise)
        # heading = degrees clockwise from North in body frame
        # World rotation: rotate body (dy_cam = forward, dx_cam = right) by heading angle from north
        hdg_rad = math.radians(hdg)
        # body forward (north when hdg=0) component:
        # body_forward_m = dy_cam (assuming cam Y_image = forward)
        # body_right_m = dx_cam
        body_forward = dy_cam  # may need sign flip depending on mounting; we'll see
        body_right = dx_cam
        # world East = forward*sin(hdg) + right*cos(hdg)
        # world North = forward*cos(hdg) - right*sin(hdg)
        de = body_forward * math.sin(hdg_rad) + body_right * math.cos(hdg_rad)
        dn = body_forward * math.cos(hdg_rad) - body_right * math.sin(hdg_rad)
        east_cum += de
        north_cum += dn
        speed_mps = math.sqrt(de**2 + dn**2) * args.fps
        rows.append({'frame_idx': i, 'ts_s': t_frame, 'heading_deg': hdg, 'flow_x_px': float(fx_cam), 'flow_y_px': float(fy_cam),
                     'speed_mps': speed_mps, 'east_m': east_cum, 'north_m': north_cum})
        prev = curr
        if i % 100 == 0:
            print(f'[dvl] {i}/{len(frames)} hdg={hdg:.1f}° flow=({fx_cam:.1f},{fy_cam:.1f}) pos=({east_cum:.2f},{north_cum:.2f})', flush=True)
    print(f'[dvl] done. Final ENU: ({east_cum:.2f}, {north_cum:.2f}) m', flush=True)
    with open(args.out, 'w', newline='') as ff:
        w = csv.DictWriter(ff, fieldnames=list(rows[0].keys()))
        w.writeheader(); w.writerows(rows)
    print(f'[out] {args.out}', flush=True)
    if args.plot:
        import matplotlib
        matplotlib.use('Agg')
        import matplotlib.pyplot as plt
        fig, axes = plt.subplots(2, 2, figsize=(14, 12))
        ax_xy, ax_hdg, ax_speed, ax_cmp = axes[0,0], axes[0,1], axes[1,0], axes[1,1]
        e = [r['east_m'] for r in rows]
        n = [r['north_m'] for r in rows]
        ax_xy.plot(e, n, '-b', linewidth=1.2)
        ax_xy.plot(e[0], n[0], 'go', markersize=10, label='start')
        ax_xy.plot(e[-1], n[-1], 'r^', markersize=10, label='end')
        ax_xy.set_xlabel('East (m)'); ax_xy.set_ylabel('North (m)'); ax_xy.set_title('DVL + IMU heading trajectory')
        ax_xy.set_aspect('equal'); ax_xy.legend(); ax_xy.grid(True, alpha=0.3)
        hdgs = [r['heading_deg'] for r in rows]
        ax_hdg.plot(range(len(rows)), hdgs, '-c'); ax_hdg.set_xlabel('Frame'); ax_hdg.set_ylabel('Heading (deg)'); ax_hdg.set_title('Compass heading from MCAP'); ax_hdg.grid(True, alpha=0.3)
        speeds = [r['speed_mps'] for r in rows]
        ax_speed.plot(range(len(rows)), speeds, '-r'); ax_speed.set_xlabel('Frame'); ax_speed.set_ylabel('Speed m/s'); ax_speed.set_title('Speed over time'); ax_speed.grid(True, alpha=0.3)
        if args.ref_csv:
            try:
                with open(args.ref_csv) as fff:
                    refrows = [r for r in csv.DictReader(fff) if r.get('segment','')==args.label or r.get('label','')==args.label]
                rx = [float(r['x']) for r in refrows]
                ry = [float(r['y']) for r in refrows]
                ax_cmp.plot(e, n, '-b', linewidth=1.2, label='DVL+IMU', alpha=0.7)
                ax_cmp.plot(rx, ry, '-r', linewidth=1.2, label='lingbot', alpha=0.7)
                ax_cmp.set_xlabel('X/East'); ax_cmp.set_ylabel('Y/North'); ax_cmp.set_title('Comparison'); ax_cmp.set_aspect('equal'); ax_cmp.legend(); ax_cmp.grid(True, alpha=0.3)
            except Exception as e: print(f'[ref] {e}')
        else:
            ax_cmp.set_title('(no reference)')
        plt.suptitle(f'DVL+IMU heading — {args.label}')
        plt.tight_layout()
        plt.savefig(args.plot, dpi=130, bbox_inches='tight')
        print(f'[plot] {args.plot}', flush=True)
 if __name__ == '__main__': main()
--- a/scripts/loop_closure_lightglue.py
+++ b/scripts/loop_closure_lightglue.py
@@ -1,252 +0,0 @@
 #!/usr/bin/env python3
 """Loop closure detection via LightGlue (SuperPoint + LightGlue matcher).
 Pipeline:
 1. Read DVL trajectory CSV (raw east_m,north_m per frame).
 2. Build candidate pairs (i, j) with |i-j| > min_sep.
    Sample stratifie if > max_pairs.
 3. Send pairs + frames to GPU host (.87) via SSH; LightGlue runs there.
 4. Filter pairs with n_high > match_threshold = loop closures.
 5. Apply linear-ramp correction (same algo as pHash variant): for each LC,
    pull frame j back to frame i, distribute drift across [i+1..j] linearly
    and carry offset forward for k > j.
 Usage:
  python3 loop_closure_lightglue.py \
      --frames-dir /tmp/frames_GX019818/ \
      --dvl-csv /tmp/dvl_full_GX019818.csv \
      --out-corrected /tmp/dvl_lightglue_GX019818.csv \
      --plot /tmp/loop_closure_lightglue.png \
      --min-sep 60 --match-threshold 50 --max-pairs 30000 \
      --gpu-host 192.168.0.87 --gpu-user floppyrj45 \
      --gpu-frames-dir /home/floppyrj45/lightglue-test/frames_GX019818 \
      --gpu-venv /home/floppyrj45/lightglue-test/venv \
      --gpu-worker /home/floppyrj45/lightglue-test/lightglue_pairs_worker.py
 """
 import argparse
 import csv
 import math
 import os
 import random
 import subprocess
 import sys
 import tempfile
 from pathlib import Path
 import numpy as np
 def stratified_pairs(n_frames, min_sep, max_pairs, seed=42):
    """Sample pairs (i,j) with |i-j| > min_sep, stratified by separation bucket.
    Tries to get good coverage: for each separation range [min_sep..2*min_sep],
    [2*min_sep..4*min_sep], ..., draw equal share. Plus all-i to random-j fallback.
    """
    rng = random.Random(seed)
    pairs = set()
    # Brute force for small N: all pairs |i-j|>min_sep then truncate
    full_count = 0
    for i in range(n_frames):
        for j in range(i + min_sep + 1, n_frames):
            full_count += 1
    if full_count <= max_pairs:
        for i in range(n_frames):
            for j in range(i + min_sep + 1, n_frames):
                pairs.add((i, j))
        out = sorted(pairs)
        return out
    # Stratified buckets by log separation
    deltas = []
    d = min_sep + 1
    while d < n_frames:
        deltas.append(d)
        d = int(d * 1.7) + 1
    deltas.append(n_frames)
    buckets = list(zip(deltas[:-1], deltas[1:]))
    if not buckets:
        buckets = [(min_sep + 1, n_frames)]
    per_bucket = max_pairs // len(buckets)
    for (lo, hi) in buckets:
        attempts = 0
        added = 0
        while added < per_bucket and attempts < per_bucket * 20:
            attempts += 1
            i = rng.randrange(n_frames)
            delta = rng.randint(lo, max(lo + 1, hi - 1))
            j = i + delta
            if j >= n_frames:
                j = i - delta
            if 0 <= j < n_frames and abs(i - j) > min_sep:
                a, b = min(i, j), max(i, j)
                if (a, b) not in pairs:
                    pairs.add((a, b))
                    added += 1
    out = sorted(pairs)
    return out
 def main():
    ap = argparse.ArgumentParser()
    ap.add_argument('--frames-dir', required=True)
    ap.add_argument('--dvl-csv', required=True)
    ap.add_argument('--out-corrected', required=True)
    ap.add_argument('--plot', default=None)
    ap.add_argument('--min-sep', type=int, default=60)
    ap.add_argument('--match-threshold', type=int, default=50)
    ap.add_argument('--max-pairs', type=int, default=30000)
    ap.add_argument('--gpu-host', default='192.168.0.87')
    ap.add_argument('--gpu-user', default='floppyrj45')
    ap.add_argument('--gpu-frames-dir', default='/home/floppyrj45/lightglue-test/frames_GX019818')
    ap.add_argument('--gpu-venv', default='/home/floppyrj45/lightglue-test/venv')
    ap.add_argument('--gpu-worker', default='/home/floppyrj45/lightglue-test/lightglue_pairs_worker.py')
    ap.add_argument('--remote-pairs-path', default='/tmp/lg_pairs.txt')
    ap.add_argument('--remote-out-path', default='/tmp/lg_matches.csv')
    ap.add_argument('--n-positions-cap', type=int, default=0,
                    help='if >0, cap n_positions used for pair generation (must match GPU frames count)')
    args = ap.parse_args()
    # Map DVL CSV rows to frames present locally — we need positions in *sorted frames* on GPU host.
    # We assume frame_idx in CSV matches file name 'frame_NNNN.jpg' with NNNN = frame_idx+1 zero-padded
    # OR matches sorted index. Since file names are sequential (frame_0001..frame_1451) and DVL has 1663
    # rows, only frames 0..1450 are physically present. We restrict LC search to those rows AND only
    # frames whose file exists.
    frames_dir = Path(args.frames_dir)
    local_frames = sorted(p.name for p in frames_dir.iterdir()
                          if p.suffix.lower() in ('.jpg', '.jpeg', '.png'))
    # local_frames sorted == what worker will sort → indices align across hosts.
    # Map frame_name "frame_0001.jpg" -> 1-based number -> 0-based dvl frame_idx = num-1
    def name_to_dvl_idx(name):
        stem = Path(name).stem  # frame_0001
        num = int(stem.split('_')[1])
        return num - 1  # 0-based
    pos_to_dvl = [name_to_dvl_idx(n) for n in local_frames]
    n_positions = len(local_frames)
    if args.n_positions_cap and args.n_positions_cap < n_positions:
        n_positions = args.n_positions_cap
        pos_to_dvl = pos_to_dvl[:n_positions]
    print(f'[lc] positions used for pairs: {n_positions}', flush=True)
    # DVL CSV
    dvl_rows = list(csv.DictReader(open(args.dvl_csv)))
    e_full = np.array([float(r['east_m']) for r in dvl_rows])
    n_full = np.array([float(r['north_m']) for r in dvl_rows])
    n_full_rows = len(dvl_rows)
    print(f'[lc] dvl rows: {n_full_rows}', flush=True)
    # Build candidate pairs over *positions* (worker indexes positions of sorted frames)
    pairs_pos = stratified_pairs(n_positions, args.min_sep, args.max_pairs)
    print(f'[lc] candidate pairs: {len(pairs_pos)}', flush=True)
    # Write pairs file locally then scp to GPU host
    with tempfile.NamedTemporaryFile('w', delete=False, suffix='.txt') as f:
        pairs_local_path = f.name
        for i, j in pairs_pos:
            f.write(f'{i},{j}\n')
    print(f'[lc] wrote pairs file {pairs_local_path}', flush=True)
    scp_cmd = ['scp', '-o', 'StrictHostKeyChecking=no', pairs_local_path,
               f'{args.gpu_user}@{args.gpu_host}:{args.remote_pairs_path}']
    subprocess.run(scp_cmd, check=True)
    print(f'[lc] uploaded pairs to {args.gpu_host}:{args.remote_pairs_path}', flush=True)
    # Run worker remotely
    remote_cmd = (
        f'source {args.gpu_venv}/bin/activate && '
        f'python3 {args.gpu_worker} '
        f'--frames-dir {args.gpu_frames_dir} '
        f'--pairs-file {args.remote_pairs_path} '
        f'--out-file {args.remote_out_path} '
        f'--score-thr 0.5'
    )
    ssh_cmd = ['ssh', '-o', 'StrictHostKeyChecking=no',
               f'{args.gpu_user}@{args.gpu_host}', remote_cmd]
    print(f'[lc] invoking worker remotely ...', flush=True)
    r = subprocess.run(ssh_cmd)
    if r.returncode != 0:
        print(f'[lc] remote worker failed rc={r.returncode}', file=sys.stderr)
        sys.exit(r.returncode)
    # Pull back matches CSV
    local_matches = '/tmp/lg_matches_local.csv'
    subprocess.run(['scp', '-o', 'StrictHostKeyChecking=no',
                    f'{args.gpu_user}@{args.gpu_host}:{args.remote_out_path}', local_matches],
                   check=True)
    print(f'[lc] pulled matches to {local_matches}', flush=True)
    # Parse matches, filter
    loops = []  # (dvl_i, dvl_j, n_high)
    with open(local_matches) as f:
        next(f)  # header
        for line in f:
            parts = line.strip().split(',')
            if len(parts) < 4:
                continue
            pi, pj, n_total, n_high = int(parts[0]), int(parts[1]), int(parts[2]), int(parts[3])
            if n_high > args.match_threshold:
                di = pos_to_dvl[pi]
                dj = pos_to_dvl[pj]
                if di > dj:
                    di, dj = dj, di
                if dj - di > args.min_sep:
                    loops.append((di, dj, n_high))
    print(f'[lc] kept {len(loops)} loop closures (n_high > {args.match_threshold})', flush=True)
    # Apply linear-ramp correction (same as phash variant)
    e_corr = e_full.copy()
    n_corr = n_full.copy()
    n_applied = 0
    # Sort loops by i ascending then by j ascending so corrections are applied left to right
    loops.sort(key=lambda x: (x[0], x[1]))
    for i, j, nh in loops:
        if j >= len(e_corr):
            continue
        dx = e_corr[i] - e_corr[j]
        dy = n_corr[i] - n_corr[j]
        nsteps = j - i
        for k in range(i + 1, j + 1):
            ratio = (k - i) / nsteps
            e_corr[k] += dx * ratio
            n_corr[k] += dy * ratio
        for k in range(j + 1, len(e_corr)):
            e_corr[k] += dx
            n_corr[k] += dy
        n_applied += 1
    print(f'[lc] applied {n_applied} corrections', flush=True)
    with open(args.out_corrected, 'w', newline='') as f:
        w = csv.writer(f)
        w.writerow(['frame_idx', 'ts_s', 'east_m_orig', 'north_m_orig', 'east_m_corr', 'north_m_corr', 'n_loops'])
        for k, r in enumerate(dvl_rows):
            w.writerow([r['frame_idx'], r['ts_s'], e_full[k], n_full[k], e_corr[k], n_corr[k],
                        n_applied if k == 0 else ''])
    print(f'[out] {args.out_corrected}', flush=True)
    if args.plot:
        import matplotlib
        matplotlib.use('Agg')
        import matplotlib.pyplot as plt
        fig, axes = plt.subplots(1, 2, figsize=(14, 7))
        axes[0].plot(e_full, n_full, '-b', lw=1)
        axes[0].plot(e_full[0], n_full[0], 'go', ms=10)
        axes[0].plot(e_full[-1], n_full[-1], 'r^', ms=10)
        axes[0].set_title(f'RAW DVL\nbbox={e_full.max()-e_full.min():.1f}x{n_full.max()-n_full.min():.1f}m')
        axes[0].set_xlabel('East m'); axes[0].set_ylabel('North m'); axes[0].set_aspect('equal'); axes[0].grid(alpha=0.3)
        axes[1].plot(e_corr, n_corr, '-r', lw=1)
        axes[1].plot(e_corr[0], n_corr[0], 'go', ms=10)
        axes[1].plot(e_corr[-1], n_corr[-1], 'r^', ms=10)
        axes[1].set_title(f'LightGlue LC ({n_applied} loops)\nbbox={e_corr.max()-e_corr.min():.1f}x{n_corr.max()-n_corr.min():.1f}m')
        axes[1].set_xlabel('East m'); axes[1].set_ylabel('North m'); axes[1].set_aspect('equal'); axes[1].grid(alpha=0.3)
        plt.suptitle(f'LightGlue loop closure — GX019818 (thr={args.match_threshold})')
        plt.tight_layout()
        plt.savefig(args.plot, dpi=130, bbox_inches='tight')
        print(f'[plot] {args.plot}', flush=True)
 if __name__ == '__main__':
    main()
--- a/scripts/loop_closure_phash.py
+++ b/scripts/loop_closure_phash.py
@@ -1,120 +0,0 @@
 #!/usr/bin/env python3
 """Loop closure detection via perceptual hashing.
 For each frame, compute pHash (DCT-based perceptual hash).
 Find pairs (i, j) with |i-j| > MIN_SEPARATION and hash distance < THRESHOLD.
 These are loop closures — AUV revisited same physical location.
 Then correct DVL trajectory by snapping back at loop closures.
 Usage:
  python3 loop_closure_phash.py --frames-dir <dir> --dvl-csv <csv> \
    --out-corrected /tmp/dvl_loopclosed.csv --plot /tmp/loop_closure.png \
    --min-sep 60 --max-dist 8
 """
 import argparse, csv, math
 from pathlib import Path
 import numpy as np
 from PIL import Image
 import imagehash
 def main():
    ap = argparse.ArgumentParser()
    ap.add_argument('--frames-dir', required=True)
    ap.add_argument('--dvl-csv', required=True)
    ap.add_argument('--out-corrected', required=True)
    ap.add_argument('--plot', default=None)
    ap.add_argument('--min-sep', type=int, default=60, help='min frame separation to count as loop')
    ap.add_argument('--max-dist', type=int, default=10, help='max pHash Hamming distance for match')
    ap.add_argument('--hash-size', type=int, default=8)
    args = ap.parse_args()
    frames = sorted(Path(args.frames_dir).glob('frame_*.jpg'))
    print(f'[loop] hashing {len(frames)} frames (pHash size {args.hash_size})...', flush=True)
    hashes = []
    for i, f in enumerate(frames):
        img = Image.open(f)
        h = imagehash.phash(img, hash_size=args.hash_size)
        hashes.append(h)
        if i % 200 == 0: print(f'  hashed {i}/{len(frames)}', flush=True)
    print(f'[loop] searching loop closures (min_sep={args.min_sep}, max_dist={args.max_dist})...', flush=True)
    loops = []  # list of (i, j, distance)
    for i in range(len(hashes)):
        for j in range(i + args.min_sep, len(hashes)):
            d = hashes[i] - hashes[j]
            if d <= args.max_dist:
                loops.append((i, j, d))
        if i % 200 == 0: print(f'  search at {i}, loops found so far: {len(loops)}', flush=True)
    print(f'[loop] found {len(loops)} loop closures', flush=True)
    # Load DVL trajectory
    dvl_rows = list(csv.DictReader(open(args.dvl_csv)))
    e = np.array([float(r['east_m']) for r in dvl_rows])
    n = np.array([float(r['north_m']) for r in dvl_rows])
    # Simple correction: for each loop closure (i, j), interpolate a rigid correction
    # over [i, j] to bring j back to i's position
    # We'll apply gradual correction: for k in [i, j], offset by linear ramp
    e_corr = e.copy(); n_corr = n.copy()
    n_corrections = 0
    for i, j, d in loops:
        if j >= len(e_corr): continue
        dx = e_corr[i] - e_corr[j]
        dy = n_corr[i] - n_corr[j]
        # spread correction linearly over [i+1, j]
        nsteps = j - i
        for k in range(i+1, j+1):
            ratio = (k - i) / nsteps
            e_corr[k] += dx * ratio
            n_corr[k] += dy * ratio
        # carry forward the offset to all frames after j
        for k in range(j+1, len(e_corr)):
            e_corr[k] += dx
            n_corr[k] += dy
        n_corrections += 1
    print(f'[loop] applied {n_corrections} corrections to trajectory', flush=True)
    with open(args.out_corrected, 'w', newline='') as ff:
        w = csv.writer(ff)
        w.writerow(['frame_idx','ts_s','east_m_orig','north_m_orig','east_m_corr','north_m_corr'])
        for k, r in enumerate(dvl_rows):
            w.writerow([r['frame_idx'], r['ts_s'], e[k], n[k], e_corr[k], n_corr[k]])
    print(f'[out] {args.out_corrected}', flush=True)
    if args.plot:
        import matplotlib
        matplotlib.use('Agg')
        import matplotlib.pyplot as plt
        fig, axes = plt.subplots(2, 2, figsize=(14, 12))
        ax_orig, ax_corr, ax_pairs, ax_dist = axes[0,0], axes[0,1], axes[1,0], axes[1,1]
        ax_orig.plot(e, n, '-b', linewidth=1.0); ax_orig.plot(e[0], n[0], 'go', markersize=10); ax_orig.plot(e[-1], n[-1], 'r^', markersize=10)
        ax_orig.set_title(f'DVL trajectory ORIGINAL (drift visible)\nbbox={max(e)-min(e):.1f}×{max(n)-min(n):.1f}m')
        ax_orig.set_xlabel('East (m)'); ax_orig.set_ylabel('North (m)'); ax_orig.set_aspect('equal'); ax_orig.grid(True, alpha=0.3)
        ax_corr.plot(e_corr, n_corr, '-r', linewidth=1.0); ax_corr.plot(e_corr[0], n_corr[0], 'go', markersize=10); ax_corr.plot(e_corr[-1], n_corr[-1], 'r^', markersize=10)
        ax_corr.set_title(f'DVL trajectory + LOOP CLOSURE\nbbox={max(e_corr)-min(e_corr):.1f}×{max(n_corr)-min(n_corr):.1f}m\nLoops applied: {n_corrections}')
        ax_corr.set_xlabel('East (m)'); ax_corr.set_ylabel('North (m)'); ax_corr.set_aspect('equal'); ax_corr.grid(True, alpha=0.3)
        # plot loop pairs as lines on original
        ax_pairs.plot(e, n, '-', color='gray', linewidth=0.5, alpha=0.4)
        for i, j, d in loops[:200]:  # show first 200 pairs
            ax_pairs.plot([e[i], e[j]], [n[i], n[j]], '-', color='orange', linewidth=0.4, alpha=0.3)
        ax_pairs.set_title(f'Loop closure pairs (first 200, of {len(loops)})')
        ax_pairs.set_xlabel('East'); ax_pairs.set_ylabel('North'); ax_pairs.set_aspect('equal'); ax_pairs.grid(True, alpha=0.3)
        # histogram of loop distances
        dists = [d for _,_,d in loops]
        if dists:
            ax_dist.hist(dists, bins=range(0, max(dists)+2))
            ax_dist.set_xlabel('Hash Hamming distance'); ax_dist.set_ylabel('Count'); ax_dist.set_title('Loop closure hash distance distribution'); ax_dist.grid(True, alpha=0.3)
        plt.suptitle(f'Loop closure detection (pHash {args.hash_size}, min_sep={args.min_sep}, max_dist={args.max_dist}) — GX039839')
        plt.tight_layout()
        plt.savefig(args.plot, dpi=130, bbox_inches='tight')
        print(f'[plot] {args.plot}', flush=True)
 if __name__ == '__main__': main()
--- a/scripts/loop_closure_sweep.py
+++ b/scripts/loop_closure_sweep.py
@@ -1,119 +0,0 @@
 #!/usr/bin/env python3
 """Sweep loop closure params on cached hashes."""
 import argparse, csv, math, pickle
 from pathlib import Path
 import numpy as np
 from PIL import Image
 import imagehash
 def main():
    ap = argparse.ArgumentParser()
    ap.add_argument('--frames-dir', required=True)
    ap.add_argument('--dvl-csv', required=True)
    ap.add_argument('--hash-cache', default='/tmp/phash_cache.pkl')
    ap.add_argument('--hash-size', type=int, default=16)  # bigger for finer discrimination
    ap.add_argument('--out-plot', required=True)
    ap.add_argument('--min-sep', type=int, default=60)
    args = ap.parse_args()
    frames = sorted(Path(args.frames_dir).glob('frame_*.jpg'))
    # Cache or compute hashes
    cache_path = Path(args.hash_cache)
    if cache_path.exists():
        with open(cache_path,'rb') as f:
            d = pickle.load(f)
        if d.get('frames_count') == len(frames) and d.get('hash_size') == args.hash_size and d.get('frames_dir') == str(args.frames_dir):
            hashes = d['hashes']
            print(f'[cache] loaded {len(hashes)} hashes from {cache_path}', flush=True)
        else:
            cache_path = None
    if not cache_path or not cache_path.exists():
        print(f'[hash] computing {len(frames)} pHashes (size={args.hash_size})...', flush=True)
        hashes = []
        for i, f in enumerate(frames):
            h = imagehash.phash(Image.open(f), hash_size=args.hash_size)
            hashes.append(h)
            if i % 200 == 0: print(f'  {i}/{len(frames)}', flush=True)
        with open(args.hash_cache,'wb') as f:
            pickle.dump({'hashes': hashes, 'frames_count': len(frames), 'hash_size': args.hash_size, 'frames_dir': str(args.frames_dir)}, f)
        print(f'[cache] saved to {args.hash_cache}', flush=True)
    # max_dist for hash_size=16 is ~256 bits; scale threshold accordingly
    # for hash 8: dist 8 ~12%, for hash 16: dist 32 ~12%
    # try thresholds at 5%, 8%, 12%, 18%
    n_bits = args.hash_size * args.hash_size
    thresholds = [int(n_bits*0.05), int(n_bits*0.08), int(n_bits*0.12), int(n_bits*0.18)]
    print(f'[loop] hash bits={n_bits}, sweep thresholds: {thresholds}', flush=True)
    dvl_rows = list(csv.DictReader(open(args.dvl_csv)))
    e_orig = np.array([float(r['east_m']) for r in dvl_rows])
    n_orig = np.array([float(r['north_m']) for r in dvl_rows])
    def find_loops_and_correct(max_dist):
        loops = []
        for i in range(len(hashes)):
            for j in range(i + args.min_sep, len(hashes)):
                d = hashes[i] - hashes[j]
                if d <= max_dist:
                    loops.append((i, j, d))
        e_c = e_orig.copy(); n_c = n_orig.copy()
        for i, j, d in loops:
            if j >= len(e_c): continue
            dx = e_c[i] - e_c[j]; dy = n_c[i] - n_c[j]
            ns = j - i
            for k in range(i+1, j+1):
                ratio = (k-i)/ns
                e_c[k] += dx*ratio; n_c[k] += dy*ratio
            for k in range(j+1, len(e_c)):
                e_c[k] += dx; n_c[k] += dy
        return loops, e_c, n_c
    import matplotlib
    matplotlib.use('Agg')
    import matplotlib.pyplot as plt
    fig, axes = plt.subplots(2, 3, figsize=(20, 12))
    # original
    ax = axes[0,0]
    ax.plot(e_orig, n_orig, '-b', linewidth=1.0)
    ax.plot(e_orig[0], n_orig[0], 'go', markersize=10); ax.plot(e_orig[-1], n_orig[-1], 'r^', markersize=10)
    bbox=(max(e_orig)-min(e_orig), max(n_orig)-min(n_orig))
    ax.set_title(f'ORIGINAL (no LC)\nbbox={bbox[0]:.1f}×{bbox[1]:.1f}m')
    ax.set_xlabel('East'); ax.set_ylabel('North'); ax.set_aspect('equal'); ax.grid(True, alpha=0.3)
    # corrected for each threshold
    positions = [(0,1), (0,2), (1,0), (1,1)]
    for idx, t in enumerate(thresholds):
        if idx >= len(positions): break
        loops, e_c, n_c = find_loops_and_correct(t)
        ax = axes[positions[idx]]
        ax.plot(e_c, n_c, '-r', linewidth=1.0)
        ax.plot(e_c[0], n_c[0], 'go', markersize=10); ax.plot(e_c[-1], n_c[-1], 'r^', markersize=10)
        bbox=(max(e_c)-min(e_c), max(n_c)-min(n_c))
        end_dist = math.sqrt(e_c[-1]**2 + n_c[-1]**2)
        ax.set_title(f'max_dist={t} ({t/n_bits*100:.0f}% bits)\n{len(loops)} loops  bbox={bbox[0]:.1f}×{bbox[1]:.1f}m  end={end_dist:.1f}m')
        ax.set_xlabel('East'); ax.set_ylabel('North'); ax.set_aspect('equal'); ax.grid(True, alpha=0.3)
        print(f'[t={t}] loops={len(loops)} bbox={bbox} end_dist={end_dist:.1f}', flush=True)
    # summary: end_dist vs threshold
    ax = axes[1,2]
    end_dists = []
    for t in thresholds:
        loops, e_c, n_c = find_loops_and_correct(t)
        end_dists.append((t, len(loops), math.sqrt(e_c[-1]**2+n_c[-1]**2)))
    ts = [x[0] for x in end_dists]
    counts = [x[1] for x in end_dists]
    ed = [x[2] for x in end_dists]
    ax2 = ax.twinx()
    ax.plot(ts, counts, 'b-o', label='loop count'); ax.set_ylabel('Loops found', color='b')
    ax2.plot(ts, ed, 'r-s', label='end_dist'); ax2.set_ylabel('end_dist (m)', color='r')
    ax.set_xlabel('max_dist threshold'); ax.set_title('Threshold sweep summary')
    ax.grid(True, alpha=0.3)
    plt.suptitle(f'Loop closure threshold sweep — GX039839 (pHash size {args.hash_size}, min_sep {args.min_sep})')
    plt.tight_layout()
    plt.savefig(args.out_plot, dpi=120, bbox_inches='tight')
    print(f'[plot] {args.out_plot}', flush=True)
 if __name__ == '__main__': main()
--- a/scripts/photomosaic_overlay.py
+++ b/scripts/photomosaic_overlay.py
@@ -1,244 +0,0 @@
 #!/usr/bin/env python3
 """Photomosaic overlay: place each frame at (east, north, heading) on 2D canvas.
 KISS: cv2 only, running-mean compositing.
 """
 import argparse
 import csv
 import glob
 import math
 import os
 import sys
 import time
 import cv2
 import numpy as np
 def load_traj(path):
    """Return list of dicts with frame_idx, east, north, heading."""
    rows = []
    with open(path) as f:
        rdr = csv.DictReader(f)
        for r in rdr:
            try:
                fi = int(r["frame_idx"])
            except (KeyError, ValueError):
                continue
            # Prefer corrected east/north, fallback to raw east_m/north_m
            if "east_m_corr" in r and r["east_m_corr"] != "":
                e = float(r["east_m_corr"])
                n = float(r["north_m_corr"])
            elif "east_m" in r:
                e = float(r["east_m"])
                n = float(r["north_m"])
            else:
                continue
            h = float(r["heading_deg"]) if "heading_deg" in r and r["heading_deg"] != "" else None
            rows.append({"frame_idx": fi, "east": e, "north": n, "heading": h})
    return rows
 def attach_headings(traj, heading_csv):
    """Join heading_deg from secondary CSV by frame_idx (loopclosed CSVs miss heading)."""
    if all(r["heading"] is not None for r in traj):
        return traj
    by_idx = {}
    with open(heading_csv) as f:
        rdr = csv.DictReader(f)
        for r in rdr:
            try:
                by_idx[int(r["frame_idx"])] = float(r["heading_deg"])
            except (KeyError, ValueError):
                pass
    for r in traj:
        if r["heading"] is None:
            r["heading"] = by_idx.get(r["frame_idx"], 0.0)
    return traj
 def find_frame(frames_dir, frame_idx):
    """Try both naming conventions: frame_0001.jpg or frame_00001.jpg."""
    for digits in (4, 5, 6):
        p = os.path.join(frames_dir, f"frame_{frame_idx+1:0{digits}d}.jpg")
        if os.path.exists(p):
            return p
        p = os.path.join(frames_dir, f"frame_{frame_idx:0{digits}d}.jpg")
        if os.path.exists(p):
            return p
    return None
 def main():
    ap = argparse.ArgumentParser()
    ap.add_argument("--frames-dir", required=True)
    ap.add_argument("--traj-csv", required=True)
    ap.add_argument("--heading-csv", default=None, help="Fallback CSV for heading_deg if missing")
    ap.add_argument("--altitude", type=float, default=1.5)
    ap.add_argument("--fov-h", type=float, default=122.0)
    ap.add_argument("--fov-v", type=float, default=80.0)
    ap.add_argument("--alpha", type=float, default=0.3)
    ap.add_argument("--sample-every", type=int, default=5)
    ap.add_argument("--out", required=True)
    ap.add_argument("--max-canvas-px", type=int, default=4000)
    ap.add_argument("--heading-sign", type=int, default=-1, help="-1 for clockwise-from-north (default)")
    args = ap.parse_args()
    t0 = time.time()
    # 1. Load trajectory
    traj = load_traj(args.traj_csv)
    if args.heading_csv:
        traj = attach_headings(traj, args.heading_csv)
    else:
        # Auto-fallback: try dvl_full_*.csv next to traj_csv
        if any(r["heading"] is None for r in traj):
            base = os.path.basename(args.traj_csv)
            # Extract video tag like GX039839 / GX019818
            for token in base.replace(".", "_").split("_"):
                if token.startswith("GX") and len(token) >= 6:
                    cand = f"/tmp/dvl_full_{token}.csv"
                    if os.path.exists(cand):
                        print(f"[heading] joining from {cand}")
                        traj = attach_headings(traj, cand)
                        break
    if not traj:
        print("ERROR: empty trajectory", file=sys.stderr)
        sys.exit(1)
    # Sample
    traj = traj[:: args.sample_every]
    print(f"[traj] {len(traj)} frames after sampling (every {args.sample_every})")
    # 2. Footprint at altitude
    fp_w = 2.0 * args.altitude * math.tan(math.radians(args.fov_h / 2.0))
    fp_h = 2.0 * args.altitude * math.tan(math.radians(args.fov_v / 2.0))
    print(f"[footprint] {fp_w:.2f}m wide x {fp_h:.2f}m tall (alt={args.altitude}m)")
    # 3. World bbox + margin
    es = [r["east"] for r in traj]
    ns = [r["north"] for r in traj]
    margin = 1.5 * max(fp_w, fp_h)
    e_min, e_max = min(es) - margin, max(es) + margin
    n_min, n_max = min(ns) - margin, max(ns) + margin
    world_w = e_max - e_min
    world_h = n_max - n_min
    print(f"[bbox] east [{e_min:.1f},{e_max:.1f}] north [{n_min:.1f},{n_max:.1f}] = {world_w:.1f}m x {world_h:.1f}m")
    # 4. Canvas pixel size
    ppm = args.max_canvas_px / max(world_w, world_h)
    canvas_w = int(world_w * ppm)
    canvas_h = int(world_h * ppm)
    print(f"[canvas] {canvas_w}x{canvas_h} px ({ppm:.1f} px/m)")
    # Compositing buffers: sum (float32 BGR) + count (int)
    acc = np.zeros((canvas_h, canvas_w, 3), dtype=np.float32)
    cnt = np.zeros((canvas_h, canvas_w), dtype=np.int32)
    fp_px_w = max(2, int(fp_w * ppm))
    fp_px_h = max(2, int(fp_h * ppm))
    print(f"[footprint-px] {fp_px_w}x{fp_px_h}")
    placed = 0
    skipped = 0
    for i, r in enumerate(traj):
        path = find_frame(args.frames_dir, r["frame_idx"])
        if not path:
            skipped += 1
            continue
        img = cv2.imread(path)
        if img is None:
            skipped += 1
            continue
        # Resize image to footprint pixel size first (keep aspect)
        img_resized = cv2.resize(img, (fp_px_w, fp_px_h), interpolation=cv2.INTER_AREA)
        # Rotate by heading. Convention: heading 0 = north, positive clockwise.
        # We rotate image so image "up" aligns with north direction.
        # cv2 rotation positive = counterclockwise → use -heading * sign
        heading = r["heading"] if r["heading"] is not None else 0.0
        angle = args.heading_sign * heading  # default -1 = clockwise
        # Build canvas the size of the rotated bounding box
        diag = int(math.ceil(math.sqrt(fp_px_w ** 2 + fp_px_h ** 2)))
        # Pad to diag x diag for safe rotation
        pad_h = (diag - fp_px_h) // 2
        pad_w = (diag - fp_px_w) // 2
        padded = cv2.copyMakeBorder(
            img_resized, pad_h, diag - fp_px_h - pad_h,
            pad_w, diag - fp_px_w - pad_w,
            cv2.BORDER_CONSTANT, value=0,
        )
        # Mask = 1 where valid pixels
        mask = np.zeros((padded.shape[0], padded.shape[1]), dtype=np.uint8)
        mask[pad_h:pad_h + fp_px_h, pad_w:pad_w + fp_px_w] = 255
        M = cv2.getRotationMatrix2D((diag / 2, diag / 2), angle, 1.0)
        rotated = cv2.warpAffine(padded, M, (diag, diag), flags=cv2.INTER_LINEAR, borderValue=0)
        rotated_mask = cv2.warpAffine(mask, M, (diag, diag), flags=cv2.INTER_NEAREST, borderValue=0)
        # Place at world (east, north).
        # Canvas: x = east (left→right), y = -north (top→bottom, north up)
        cx_world = r["east"]
        cy_world = r["north"]
        px = int((cx_world - e_min) * ppm)
        py = int((n_max - cy_world) * ppm)  # flip Y for image coords
        # Top-left of paste
        x0 = px - diag // 2
        y0 = py - diag // 2
        x1 = x0 + diag
        y1 = y0 + diag
        # Clip to canvas
        cx0 = max(0, x0)
        cy0 = max(0, y0)
        cx1 = min(canvas_w, x1)
        cy1 = min(canvas_h, y1)
        if cx1 <= cx0 or cy1 <= cy0:
            skipped += 1
            continue
        sx0 = cx0 - x0
        sy0 = cy0 - y0
        sx1 = sx0 + (cx1 - cx0)
        sy1 = sy0 + (cy1 - cy0)
        sub_img = rotated[sy0:sy1, sx0:sx1].astype(np.float32)
        sub_mask = rotated_mask[sy0:sy1, sx0:sx1] > 0
        # Running mean: add to acc + increment cnt only where mask
        acc[cy0:cy1, cx0:cx1][sub_mask] += sub_img[sub_mask]
        cnt[cy0:cy1, cx0:cx1][sub_mask] += 1
        placed += 1
        if (i + 1) % 100 == 0:
            print(f"  ... {i+1}/{len(traj)} placed={placed} skipped={skipped}")
    # Finalize: divide by count
    out = np.zeros_like(acc, dtype=np.uint8)
    valid = cnt > 0
    out[valid] = (acc[valid] / cnt[valid, None]).astype(np.uint8)
    # Draw trajectory polyline (thin blue)
    pts = []
    for r in traj:
        px = int((r["east"] - e_min) * ppm)
        py = int((n_max - r["north"]) * ppm)
        pts.append((px, py))
    for i in range(1, len(pts)):
        cv2.line(out, pts[i - 1], pts[i], (255, 200, 0), 1, cv2.LINE_AA)
    # Mark start (green) and end (red)
    if pts:
        cv2.circle(out, pts[0], 8, (0, 255, 0), 2)
        cv2.circle(out, pts[-1], 8, (0, 0, 255), 2)
    cv2.imwrite(args.out, out)
    dt = time.time() - t0
    print(f"[done] placed={placed} skipped={skipped} canvas={canvas_w}x{canvas_h} time={dt:.1f}s out={args.out}")
 if __name__ == "__main__":
    main()
--- a/scripts/poses_to_csv.py
+++ b/scripts/poses_to_csv.py
@@ -1,61 +0,0 @@
 #!/usr/bin/env python3
 """Extract camera positions (timestamp, x, y, z) from lingbot-map NPZ poses.
 Usage: poses_to_csv.py <input.npz> [--start_iso 2026-05-05T08:34:01] [--fps 1.0] > output.csv
 """
 import argparse, sys
 import numpy as np
 from datetime import datetime, timezone
 def main():
    ap = argparse.ArgumentParser()
    ap.add_argument('npz')
    ap.add_argument('--start_iso', default=None, help='ISO timestamp of frame 0 (e.g. 2026-05-05T08:34:01)')
    ap.add_argument('--fps', type=float, default=1.0, help='frames per second (default 1.0)')
    ap.add_argument('--label', default='', help='segment label for CSV col')
    args = ap.parse_args()
    data = np.load(args.npz, allow_pickle=True)
    keys = list(data.keys())
    # auto-detect poses key
    poses = None
    for k in ['poses', 'extrinsics', 'cam_poses', 'c2w']:
        if k in keys:
            poses = data[k]; break
    if poses is None:
        # take first 3D array
        for k in keys:
            arr = data[k]
            if arr.ndim == 3 and arr.shape[-2:] in [(3,4),(4,4)]:
                poses = arr; break
    if poses is None:
        sys.exit(f'No poses found in {args.npz} (keys: {keys})')
    # start timestamp
    if args.start_iso:
        try:
            t0 = datetime.fromisoformat(args.start_iso).replace(tzinfo=timezone.utc).timestamp()
        except Exception:
            t0 = 0.0
    elif 'start_ns' in keys:
        t0 = float(data['start_ns']) / 1e9
    else:
        t0 = 0.0
    fps = float(args.fps)
    if 'fps' in keys:
        try: fps = float(data['fps'])
        except: pass
    print('segment,frame_idx,timestamp_s,x,y,z')
    for i, P in enumerate(poses):
        if P.shape == (4,4):
            P = P[:3]
        R, t = P[:, :3], P[:, 3]
        # extrinsic = world→cam ; cam position world = -R^T t
        # but lingbot might save c2w directly; check determinant heuristic
        pos = -R.T @ t  # if extrinsic
        ts = t0 + i / fps
        print(f'{args.label},{i},{ts:.6f},{pos[0]:.6f},{pos[1]:.6f},{pos[2]:.6f}')
 if __name__ == '__main__':
    main()
--- a/scripts/stage06_align_absolute.py
+++ b/scripts/stage06_align_absolute.py
@@ -1,232 +0,0 @@
 #!/usr/bin/env python3
 """Stage 06 — absolute alignment of lingbot relative trajectory using MCAP IMU+USBL.
 Input  : trajectory CSV (segment, frame_idx, timestamp_s, x, y, z) + MCAP bag dir
 Output : absolute trajectory CSV (timestamp_s, lat, lon, alt, east_m, north_m, up_m, segment, lingbot_x, lingbot_y, lingbot_z)
 + plot PNG with absolute trajectory (spiral expected for lawnmower)
 Method:
  - Parse MCAP : /mavros/imu/data + /mavros/global_position/global + /mavros/imu/static_pressure
  - Convert lat/lon → ENU meters (origin = first fix)
  - For each frame timestamp, find nearest GPS/IMU
  - Umeyama similarity transform : align lingbot (x,y,z) → (east,north,depth)
  - Output enhanced CSV + plot
 Usage:
  python3 stage06_align_absolute.py --traj /tmp/auv213_full_trajectory.csv \
      --mcap-dir /mnt/ssd/20260505-Lepradet/raw_data/logs/SUB/bag/20260505_150717_AUV013/ \
      --out /tmp/auv213_absolute.csv --plot /tmp/auv213_absolute.png
 """
 import argparse, csv, glob, os, sys, math
 import numpy as np
 from pathlib import Path
 from datetime import datetime, timezone
 def parse_mcap_bag(bag_dir):
    """Return dict of {topic: [(ts_ns, msg_dict)]}."""
    from rosbags.highlevel import AnyReader
    bag_paths = sorted(Path(bag_dir).glob('*.mcap'))
    if not bag_paths:
        sys.exit(f'No .mcap in {bag_dir}')
    print(f'[mcap] reading {len(bag_paths)} bag files', flush=True)
    topics_of_interest = {
        '/mavros/imu/data': 'Imu',
        '/mavros/global_position/global': 'NavSatFix',
        '/mavros/imu/static_pressure': 'FluidPressure',
    }
    data = {t: [] for t in topics_of_interest}
    with AnyReader(bag_paths) as reader:
        conns = [c for c in reader.connections if c.topic in topics_of_interest]
        print(f'[mcap] connections: {[(c.topic, c.msgtype) for c in conns]}', flush=True)
        for conn, ts_ns, raw in reader.messages(connections=conns):
            try:
                m = reader.deserialize(raw, conn.msgtype)
                if conn.topic == '/mavros/imu/data':
                    q = m.orientation
                    data[conn.topic].append((ts_ns, {'qx': q.x, 'qy': q.y, 'qz': q.z, 'qw': q.w}))
                elif conn.topic == '/mavros/global_position/global':
                    if not (math.isnan(m.latitude) or math.isnan(m.longitude)):
                        data[conn.topic].append((ts_ns, {'lat': m.latitude, 'lon': m.longitude, 'alt': m.altitude}))
                elif conn.topic == '/mavros/imu/static_pressure':
                    data[conn.topic].append((ts_ns, {'pressure_pa': m.fluid_pressure}))
            except Exception as e:
                continue
    for t in data:
        print(f'[mcap] {t}: {len(data[t])} samples', flush=True)
    return data
 def latlon_to_enu(lat, lon, alt, lat0, lon0, alt0):
    """Local ENU meters (flat Earth around (lat0, lon0))."""
    R = 6378137.0
    dlat = math.radians(lat - lat0)
    dlon = math.radians(lon - lon0)
    east = R * dlon * math.cos(math.radians(lat0))
    north = R * dlat
    up = alt - alt0
    return east, north, up
 def nearest_ts(arr, ts_target_ns):
    if not arr: return None
    idx = np.searchsorted([a[0] for a in arr], ts_target_ns)
    if idx == 0: return arr[0]
    if idx >= len(arr): return arr[-1]
    if abs(arr[idx][0] - ts_target_ns) < abs(arr[idx-1][0] - ts_target_ns):
        return arr[idx]
    return arr[idx-1]
 def umeyama(src, dst):
    """Closed-form similarity transform (scale s, rotation R, translation t) minimizing ||s*R*src + t - dst||^2.
    src, dst: (N, 3) arrays."""
    src = np.asarray(src, dtype=np.float64)
    dst = np.asarray(dst, dtype=np.float64)
    mu_src = src.mean(axis=0)
    mu_dst = dst.mean(axis=0)
    src_c = src - mu_src
    dst_c = dst - mu_dst
    sigma_src = (src_c ** 2).sum() / len(src)
    cov = (dst_c.T @ src_c) / len(src)
    U, S, Vt = np.linalg.svd(cov)
    D = np.eye(3)
    if np.linalg.det(U @ Vt) < 0:
        D[2, 2] = -1
    R = U @ D @ Vt
    s = (S * np.diag(D)).sum() / sigma_src
    t = mu_dst - s * R @ mu_src
    return s, R, t
 def main():
    ap = argparse.ArgumentParser()
    ap.add_argument('--traj', required=True)
    ap.add_argument('--mcap-dir', required=True)
    ap.add_argument('--out', required=True)
    ap.add_argument('--plot', default=None)
    ap.add_argument('--min-fixes', type=int, default=10, help='min GPS fixes to attempt Umeyama')
    args = ap.parse_args()
    print(f'[traj] reading {args.traj}', flush=True)
    rows = []
    with open(args.traj) as f:
        reader = csv.DictReader(f)
        for r in reader:
            rows.append({
                'segment': r['segment'],
                'frame_idx': int(r['frame_idx']),
                'ts_s': float(r['timestamp_s']),
                'x': float(r['x']),
                'y': float(r['y']),
                'z': float(r['z']),
            })
    print(f'[traj] {len(rows)} rows', flush=True)
    data = parse_mcap_bag(args.mcap_dir)
    gps = data['/mavros/global_position/global']
    imu = data['/mavros/imu/data']
    press = data['/mavros/imu/static_pressure']
    if len(gps) < args.min_fixes:
        print(f'[warn] only {len(gps)} GPS fixes (need >= {args.min_fixes}), skipping Umeyama. Will output raw + IMU only.', flush=True)
    # ENU origin = first GPS fix
    if gps:
        lat0 = gps[0][1]['lat']; lon0 = gps[0][1]['lon']; alt0 = gps[0][1]['alt']
        print(f'[origin] lat0={lat0:.6f} lon0={lon0:.6f} alt0={alt0:.2f}', flush=True)
    else:
        lat0 = lon0 = alt0 = 0.0
    # Build per-frame absolute ENU using nearest GPS
    src_lingbot, dst_enu, gps_per_frame, imu_per_frame, depth_per_frame = [], [], [], [], []
    for r in rows:
        ts_ns = int(r['ts_s'] * 1e9)
        g = nearest_ts(gps, ts_ns)
        i = nearest_ts(imu, ts_ns)
        p = nearest_ts(press, ts_ns)
        # Always store IMU+depth
        imu_per_frame.append(i[1] if i else None)
        if p:
            depth_m = (p[1]['pressure_pa'] - 101325.0) / (1025.0 * 9.81)
            depth_per_frame.append(depth_m)
        else:
            depth_per_frame.append(None)
        gps_per_frame.append(g[1] if g else None)
        if g and abs(g[0] - ts_ns) < 2e9:  # GPS within 2s
            e, n, u = latlon_to_enu(g[1]['lat'], g[1]['lon'], g[1]['alt'], lat0, lon0, alt0)
            src_lingbot.append([r['x'], r['y'], r['z']])
            dst_enu.append([e, n, u])
    # Umeyama
    if len(src_lingbot) >= args.min_fixes:
        s, R, t = umeyama(src_lingbot, dst_enu)
        print(f'[umeyama] scale={s:.4f} translation={t}', flush=True)
        print(f'[umeyama] rotation_matrix=\n{R}', flush=True)
    else:
        s, R, t = 1.0, np.eye(3), np.zeros(3)
        print(f'[umeyama] insufficient pairs, identity transform', flush=True)
    # Apply transform
    out_rows = []
    for i_row, r in enumerate(rows):
        p_lingbot = np.array([r['x'], r['y'], r['z']])
        p_abs = s * R @ p_lingbot + t
        g = gps_per_frame[i_row]
        im = imu_per_frame[i_row]
        d = depth_per_frame[i_row]
        out_rows.append({
            'segment': r['segment'],
            'frame_idx': r['frame_idx'],
            'timestamp_s': r['ts_s'],
            'east_m': p_abs[0],
            'north_m': p_abs[1],
            'up_m': p_abs[2],
            'depth_m': d if d is not None else '',
            'gps_lat': g['lat'] if g else '',
            'gps_lon': g['lon'] if g else '',
            'imu_qw': im['qw'] if im else '',
            'imu_qx': im['qx'] if im else '',
            'imu_qy': im['qy'] if im else '',
            'imu_qz': im['qz'] if im else '',
            'lingbot_x': r['x'],
            'lingbot_y': r['y'],
            'lingbot_z': r['z'],
        })
    with open(args.out, 'w', newline='') as f:
        w = csv.DictWriter(f, fieldnames=list(out_rows[0].keys()))
        w.writeheader(); w.writerows(out_rows)
    print(f'[out] {args.out} {len(out_rows)} rows', flush=True)
    if args.plot:
        import matplotlib
        matplotlib.use('Agg')
        import matplotlib.pyplot as plt
        fig, axes = plt.subplots(2, 2, figsize=(14, 12))
        ax_xy, ax_xz, ax_yz, ax_dt = axes[0,0], axes[0,1], axes[1,0], axes[1,1]
        colors = {'GX020030':'tab:red','GX030030':'tab:blue','GX040030':'tab:green','GX050030':'tab:orange'}
        by_seg = {}
        for r in out_rows:
            by_seg.setdefault(r['segment'], []).append(r)
        # plot absolute trajectory
        for seg, rs in by_seg.items():
            e=[r['east_m'] for r in rs]; n=[r['north_m'] for r in rs]; u=[r['up_m'] for r in rs]; d=[r['depth_m'] if r['depth_m']!='' else 0 for r in rs]
            c = colors.get(seg, 'gray')
            ax_xy.plot(e, n, '-', color=c, label=seg, alpha=0.7, linewidth=1)
            ax_xy.plot(e[0], n[0], 'o', color=c, markersize=8)
            ax_xz.plot(e, u, '-', color=c, alpha=0.7, linewidth=1)
            ax_yz.plot(n, u, '-', color=c, alpha=0.7, linewidth=1)
            t0 = rs[0]['timestamp_s']
            ax_dt.plot([(r['timestamp_s']-t0)/60 for r in rs], d, '-', color=c, alpha=0.8, linewidth=1, label=seg)
        # also plot GPS fixes
        gps_e = [latlon_to_enu(g[1]['lat'], g[1]['lon'], g[1]['alt'], lat0, lon0, alt0)[0] for g in gps] if gps else []
        gps_n = [latlon_to_enu(g[1]['lat'], g[1]['lon'], g[1]['alt'], lat0, lon0, alt0)[1] for g in gps] if gps else []
        if gps_e:
            ax_xy.plot(gps_e, gps_n, 'k.', markersize=2, alpha=0.5, label='GPS fixes')
        ax_xy.set_xlabel('East (m)'); ax_xy.set_ylabel('North (m)'); ax_xy.set_title('Top view ENU')
        ax_xy.set_aspect('equal'); ax_xy.legend(loc='best', fontsize=8); ax_xy.grid(True, alpha=0.3)
        ax_xz.set_xlabel('East (m)'); ax_xz.set_ylabel('Up (m)'); ax_xz.set_title('Side East-Up'); ax_xz.set_aspect('equal'); ax_xz.grid(True, alpha=0.3)
        ax_yz.set_xlabel('North (m)'); ax_yz.set_ylabel('Up (m)'); ax_yz.set_title('Side North-Up'); ax_yz.set_aspect('equal'); ax_yz.grid(True, alpha=0.3)
        ax_dt.set_xlabel('Time (min)'); ax_dt.set_ylabel('Depth (m, from pressure)'); ax_dt.set_title('Depth over time (MCAP pressure)'); ax_dt.grid(True, alpha=0.3); ax_dt.legend(loc='best', fontsize=8); ax_dt.invert_yaxis()
        fig.suptitle('AUV213 trajectory — absolute (Umeyama lingbot → ENU)')
        plt.tight_layout()
        plt.savefig(args.plot, dpi=130, bbox_inches='tight')
        print(f'[plot] {args.plot}', flush=True)
 if __name__ == '__main__':
    main()
--- a/scripts/stage06b_imu_depth_align.py
+++ b/scripts/stage06b_imu_depth_align.py
@@ -1,179 +0,0 @@
 #!/usr/bin/env python3
 """Stage 06b — IMU gravity + depth alignment with arbitrary origin.
 Method (no GPS/USBL):
  1. Read MCAP for /mavros/imu/data (orientation) + /mavros/imu/static_pressure (depth)
  2. For each frame ts, find nearest IMU+depth
  3. Compute rotation = inverse of IMU body-to-world quaternion at frame 0
     → rotates lingbot frame so World Up is +Z
  4. Scale Z so range matches real depth (pressure-derived)
  5. Place origin at (east0, north0, 0) arbitrary
  6. Output CSV + plot trajectory in local ENU (origin = user-given coords)
 Usage:
  python3 stage06b_imu_depth_align.py \
    --traj /tmp/auv213_full_trajectory.csv \
    --mcap-dir /mnt/ssd/.../20260505_150717_AUV013/ \
    --east0 1000 --north0 5000 \
    --out /tmp/auv213_aligned.csv --plot /tmp/auv213_aligned.png
 """
 import argparse, csv, math, sys
 import numpy as np
 from pathlib import Path
 def quat_to_rot(qw, qx, qy, qz):
    """Quaternion to 3x3 rotation matrix (body→world for mavros convention)."""
    n = math.sqrt(qw*qw + qx*qx + qy*qy + qz*qz)
    if n < 1e-9: return np.eye(3)
    qw, qx, qy, qz = qw/n, qx/n, qy/n, qz/n
    return np.array([
        [1 - 2*(qy*qy + qz*qz), 2*(qx*qy - qz*qw),     2*(qx*qz + qy*qw)],
        [2*(qx*qy + qz*qw),     1 - 2*(qx*qx + qz*qz), 2*(qy*qz - qx*qw)],
        [2*(qx*qz - qy*qw),     2*(qy*qz + qx*qw),     1 - 2*(qx*qx + qy*qy)],
    ])
 def parse_mcap(bag_dir):
    from rosbags.highlevel import AnyReader
    bags = sorted(Path(bag_dir).glob('*.mcap'))
    data = {'imu': [], 'press': []}
    with AnyReader(bags) as reader:
        for conn, ts_ns, raw in reader.messages(connections=[c for c in reader.connections if c.topic in ['/mavros/imu/data','/mavros/imu/static_pressure']]):
            m = reader.deserialize(raw, conn.msgtype)
            if conn.topic == '/mavros/imu/data':
                q = m.orientation
                data['imu'].append((ts_ns, [q.w, q.x, q.y, q.z]))
            else:
                data['press'].append((ts_ns, m.fluid_pressure))
    data['imu'].sort(); data['press'].sort()
    return data
 def nearest(arr, ts_ns):
    if not arr: return None
    ks = [a[0] for a in arr]
    idx = np.searchsorted(ks, ts_ns)
    if idx == 0: return arr[0]
    if idx >= len(arr): return arr[-1]
    return arr[idx] if abs(arr[idx][0]-ts_ns) < abs(arr[idx-1][0]-ts_ns) else arr[idx-1]
 def main():
    ap = argparse.ArgumentParser()
    ap.add_argument('--traj', required=True)
    ap.add_argument('--mcap-dir', required=True)
    ap.add_argument('--east0', type=float, default=1000.0)
    ap.add_argument('--north0', type=float, default=5000.0)
    ap.add_argument('--out', required=True)
    ap.add_argument('--plot', default=None)
    args = ap.parse_args()
    rows = []
    with open(args.traj) as f:
        for r in csv.DictReader(f):
            rows.append({'segment': r['segment'], 'frame_idx': int(r['frame_idx']),
                'ts_s': float(r['timestamp_s']), 'p': np.array([float(r['x']), float(r['y']), float(r['z'])])})
    print(f'[traj] {len(rows)} rows', flush=True)
    data = parse_mcap(args.mcap_dir)
    print(f'[mcap] imu={len(data["imu"])} press={len(data["press"])}', flush=True)
    # 1. Gravity alignment using IMU at first frame ts
    ts0_ns = int(rows[0]['ts_s'] * 1e9)
    imu0 = nearest(data['imu'], ts0_ns)
    if imu0 is None:
        print('[err] no IMU sample', flush=True); sys.exit(1)
    R_body2world = quat_to_rot(*imu0[1])
    # Camera looks down on AUV → cam optical axis = body -Z. So cam Z (depth) in world = -R_body2world @ [0,0,1] = up direction inverted
    # In lingbot frame, frame 0 sets identity. So lingbot_R_world = R_body2world
    # World coords from lingbot coords: p_world = R_body2world @ p_lingbot
    # But this assumes lingbot world axes coincide with body frame at t=0
    R_world = R_body2world  # rough first-order approximation
    print(f'[gravity] IMU q0 = {imu0[1]} -> R_body2world =', flush=True)
    print(R_world, flush=True)
    # 2. Depth scale: match lingbot Z range to real depth range from pressure
    depths = []
    for r in rows:
        ts_ns = int(r['ts_s']*1e9)
        p = nearest(data['press'], ts_ns)
        if p:
            d = (p[1] - 101325.0) / (1025.0 * 9.81)
            depths.append(d)
        else:
            depths.append(None)
    valid_depths = [d for d in depths if d is not None and d > 0.1]
    if valid_depths:
        depth_min, depth_max = min(valid_depths), max(valid_depths)
        depth_range = depth_max - depth_min
        print(f'[depth] real range: {depth_min:.2f}m to {depth_max:.2f}m ({depth_range:.2f}m)', flush=True)
    else:
        depth_min = depth_max = depth_range = 0
        print('[depth] no valid pressure data', flush=True)
    # apply rotation (no scaling yet)
    rotated = np.array([R_world @ r['p'] for r in rows])
    z_min, z_max = rotated[:, 2].min(), rotated[:, 2].max()
    z_range = z_max - z_min
    print(f'[lingbot after rot] Z range: {z_min:.2f} to {z_max:.2f} ({z_range:.2f}m)', flush=True)
    # scale to match depth range (if both available and meaningful)
    if depth_range > 0.5 and z_range > 0.1:
        scale_z = depth_range / z_range
        print(f'[scale_z] {scale_z:.3f}', flush=True)
    else:
        scale_z = 1.0
        print(f'[scale_z] 1.0 (insufficient data)', flush=True)
    # Apply isotropic scale (since the camera tracking is consistent in 3 axes)
    rotated_scaled = rotated * scale_z
    # Translate origin to user-given (east, north, 0)
    east0, north0 = args.east0, args.north0
    final = rotated_scaled.copy()
    final[:, 0] += east0 - rotated_scaled[0, 0]
    final[:, 1] += north0 - rotated_scaled[0, 1]
    # Z aligned to depth_min if available else first frame
    if valid_depths:
        z_offset = -depth_min - final[0, 2]  # depth is positive down ; up = -depth
        final[:, 2] += z_offset
    with open(args.out, 'w', newline='') as f:
        w = csv.writer(f)
        w.writerow(['segment','frame_idx','timestamp_s','east_m','north_m','up_m','depth_real_m','lingbot_x','lingbot_y','lingbot_z'])
        for i, r in enumerate(rows):
            w.writerow([r['segment'], r['frame_idx'], f"{r['ts_s']:.6f}",
                f"{final[i,0]:.4f}", f"{final[i,1]:.4f}", f"{final[i,2]:.4f}",
                f"{depths[i]:.3f}" if depths[i] is not None else '',
                f"{r['p'][0]:.4f}", f"{r['p'][1]:.4f}", f"{r['p'][2]:.4f}"])
    print(f'[out] {args.out}', flush=True)
    if args.plot:
        import matplotlib
        matplotlib.use('Agg')
        import matplotlib.pyplot as plt
        fig, axes = plt.subplots(2, 2, figsize=(14, 12))
        ax_xy, ax_xz, ax_yz, ax_d = axes[0,0], axes[0,1], axes[1,0], axes[1,1]
        colors = {'GX020030':'tab:red','GX030030':'tab:blue','GX040030':'tab:green','GX050030':'tab:orange'}
        by_seg = {}
        for i, r in enumerate(rows):
            by_seg.setdefault(r['segment'], []).append((final[i], depths[i], r['ts_s']))
        t0 = rows[0]['ts_s']
        for seg, lst in by_seg.items():
            e = [v[0][0] for v in lst]; n = [v[0][1] for v in lst]; u = [v[0][2] for v in lst]
            d_real = [v[1] if v[1] is not None else 0 for v in lst]
            ts = [(v[2]-t0)/60 for v in lst]
            c = colors.get(seg, 'gray')
            ax_xy.plot(e, n, '-', color=c, label=f'{seg} ({len(lst)})', linewidth=1.2, alpha=0.8)
            ax_xy.plot(e[0], n[0], 'o', color=c, markersize=8)
            ax_xz.plot(e, u, '-', color=c, linewidth=1.2, alpha=0.8)
            ax_yz.plot(n, u, '-', color=c, linewidth=1.2, alpha=0.8)
            ax_d.plot(ts, d_real, '-', color=c, linewidth=1.2, alpha=0.8, label=seg)
        ax_xy.set_xlabel('East (m, origin=1000)'); ax_xy.set_ylabel('North (m, origin=5000)')
        ax_xy.set_title('Top view ENU — gravity+depth aligned'); ax_xy.set_aspect('equal'); ax_xy.legend(fontsize=8); ax_xy.grid(True, alpha=0.3)
        ax_xz.set_xlabel('East (m)'); ax_xz.set_ylabel('Up (m)'); ax_xz.set_title('East-Up'); ax_xz.set_aspect('equal'); ax_xz.grid(True, alpha=0.3)
        ax_yz.set_xlabel('North (m)'); ax_yz.set_ylabel('Up (m)'); ax_yz.set_title('North-Up'); ax_yz.set_aspect('equal'); ax_yz.grid(True, alpha=0.3)
        ax_d.set_xlabel('Time (min)'); ax_d.set_ylabel('Depth real (m, from pressure)'); ax_d.set_title('Real depth from FluidPressure'); ax_d.legend(fontsize=8); ax_d.grid(True, alpha=0.3); ax_d.invert_yaxis()
        fig.suptitle('AUV213 — gravity+depth aligned (origin 1000,5000 ; from IMU q0 + pressure scale)')
        plt.tight_layout()
        plt.savefig(args.plot, dpi=130, bbox_inches='tight')
        print(f'[plot] {args.plot}', flush=True)
 if __name__ == '__main__': main()
--- a/scripts/vo_classic_opencv.py
+++ b/scripts/vo_classic_opencv.py
@@ -1,197 +0,0 @@
 #!/usr/bin/env python3
 """Classic Visual Odometry with OpenCV — lightweight CPU.
 Pipeline:
  1. ORB features per frame + match consecutive (or KLT track keypoints)
  2. Filter outliers via cv2.findEssentialMat RANSAC
  3. cv2.recoverPose → R, t up-to-scale per pair
  4. Concatenate to global pose chain
  5. Output CSV (frame_idx, ts_s, x, y, z, qw, qx, qy, qz)
 Usage:
  python3 vo_classic_opencv.py --frames-dir /home/cosma/cosma-pipeline/data/<mission>/frames/<AUV>/<SEGMENT> \
      --start-iso 2026-05-05T08:33:41 --fps 1.0 --label GX039839 --out /tmp/vo_classic.csv \
      --plot /tmp/vo_classic.png
 """
 import argparse, csv, sys, math
 from pathlib import Path
 import numpy as np
 import cv2
 from datetime import datetime
 def main():
    ap = argparse.ArgumentParser()
    ap.add_argument('--frames-dir', required=True)
    ap.add_argument('--start-iso', default='2026-05-05T00:00:00')
    ap.add_argument('--fps', type=float, default=1.0)
    ap.add_argument('--label', default='segment')
    ap.add_argument('--out', required=True)
    ap.add_argument('--plot', default=None)
    ap.add_argument('--ref-csv', default=None, help='lingbot CSV to compare against (same segment)')
    ap.add_argument('--max-features', type=int, default=2000)
    ap.add_argument('--method', choices=['orb','klt'], default='klt')
    args = ap.parse_args()
    frames = sorted(Path(args.frames_dir).glob('frame_*.jpg'))
    if not frames:
        sys.exit(f'no frames in {args.frames_dir}')
    print(f'[vo] {len(frames)} frames in {args.frames_dir}', flush=True)
    # camera intrinsics for 518x294 GoPro wide @ ~122° hFOV
    W, H = 518, 294
    # focal estimate from FOV
    fov_h_deg = 122.0
    f = (W / 2.0) / math.tan(math.radians(fov_h_deg / 2.0))
    cx, cy = W/2, H/2
    K = np.array([[f, 0, cx], [0, f, cy], [0, 0, 1]], dtype=np.float64)
    print(f'[vo] K = focal={f:.1f}, cx={cx}, cy={cy}', flush=True)
    # Init pose
    R_world = np.eye(3)
    t_world = np.zeros((3, 1))
    out_rows = []
    out_rows.append({'label': args.label, 'frame_idx': 0, 'ts_s': datetime.fromisoformat(args.start_iso).timestamp(), 
                     'x': 0.0, 'y': 0.0, 'z': 0.0, 'inliers': 0, 'tracked': 0})
    prev_gray = cv2.imread(str(frames[0]), cv2.IMREAD_GRAYSCALE)
    if args.method == 'klt':
        # Initial corners via goodFeaturesToTrack
        prev_pts = cv2.goodFeaturesToTrack(prev_gray, maxCorners=args.max_features, qualityLevel=0.01, minDistance=7, blockSize=7)
    else:
        orb = cv2.ORB_create(args.max_features)
        prev_kp, prev_desc = orb.detectAndCompute(prev_gray, None)
        bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
    t0 = datetime.fromisoformat(args.start_iso).timestamp()
    fail_count = 0
    for i in range(1, len(frames)):
        curr_gray = cv2.imread(str(frames[i]), cv2.IMREAD_GRAYSCALE)
        if curr_gray is None: continue
        # 1. Match/track features
        if args.method == 'klt':
            if prev_pts is None or len(prev_pts) < 50:
                prev_pts = cv2.goodFeaturesToTrack(prev_gray, maxCorners=args.max_features, qualityLevel=0.01, minDistance=7, blockSize=7)
            if prev_pts is None or len(prev_pts) < 50:
                fail_count += 1
                out_rows.append({'label': args.label, 'frame_idx': i, 'ts_s': t0 + i/args.fps, 
                                 'x': t_world[0,0], 'y': t_world[1,0], 'z': t_world[2,0], 'inliers': 0, 'tracked': 0})
                prev_gray = curr_gray
                continue
            curr_pts, status, err = cv2.calcOpticalFlowPyrLK(prev_gray, curr_gray, prev_pts, None, winSize=(21,21), maxLevel=3)
            good_prev = prev_pts[status.flatten() == 1]
            good_curr = curr_pts[status.flatten() == 1]
            n_tracked = len(good_prev)
        else:  # orb
            curr_kp, curr_desc = orb.detectAndCompute(curr_gray, None)
            if prev_desc is None or curr_desc is None or len(curr_kp) < 50:
                fail_count += 1
                out_rows.append({'label': args.label, 'frame_idx': i, 'ts_s': t0 + i/args.fps, 
                                 'x': t_world[0,0], 'y': t_world[1,0], 'z': t_world[2,0], 'inliers': 0, 'tracked': 0})
                prev_gray = curr_gray; prev_kp = curr_kp; prev_desc = curr_desc
                continue
            matches = bf.match(prev_desc, curr_desc)
            matches = sorted(matches, key=lambda m: m.distance)[:500]
            good_prev = np.array([prev_kp[m.queryIdx].pt for m in matches], dtype=np.float32).reshape(-1, 1, 2)
            good_curr = np.array([curr_kp[m.trainIdx].pt for m in matches], dtype=np.float32).reshape(-1, 1, 2)
            n_tracked = len(matches)
        if n_tracked < 30:
            fail_count += 1
            out_rows.append({'label': args.label, 'frame_idx': i, 'ts_s': t0 + i/args.fps, 
                             'x': t_world[0,0], 'y': t_world[1,0], 'z': t_world[2,0], 'inliers': 0, 'tracked': n_tracked})
            prev_gray = curr_gray
            if args.method == 'klt':
                prev_pts = cv2.goodFeaturesToTrack(curr_gray, maxCorners=args.max_features, qualityLevel=0.01, minDistance=7, blockSize=7)
            else:
                prev_kp, prev_desc = curr_kp, curr_desc
            continue
        # 2. Essential matrix + recoverPose
        try:
            E, mask = cv2.findEssentialMat(good_curr.reshape(-1,2), good_prev.reshape(-1,2), K, method=cv2.RANSAC, prob=0.999, threshold=1.0)
            if E is None or E.shape != (3,3):
                raise ValueError('bad E')
            _, R, t, mask_pose = cv2.recoverPose(E, good_curr.reshape(-1,2), good_prev.reshape(-1,2), K, mask=mask)
            n_inliers = int(mask_pose.sum()) if mask_pose is not None else 0
        except Exception as e:
            fail_count += 1
            out_rows.append({'label': args.label, 'frame_idx': i, 'ts_s': t0 + i/args.fps, 
                             'x': t_world[0,0], 'y': t_world[1,0], 'z': t_world[2,0], 'inliers': 0, 'tracked': n_tracked})
            prev_gray = curr_gray
            if args.method == 'klt':
                prev_pts = cv2.goodFeaturesToTrack(curr_gray, maxCorners=args.max_features, qualityLevel=0.01, minDistance=7, blockSize=7)
            else:
                prev_kp, prev_desc = curr_kp, curr_desc
            continue
        # 3. Update global pose : R_world = R_world @ R^T ; t_world = t_world - R_world @ t
        # (camera convention: R maps prev to curr in cam frame, t is unit baseline)
        # Pose update for VO:
        t_world = t_world + R_world @ t
        R_world = R_world @ R
        out_rows.append({'label': args.label, 'frame_idx': i, 'ts_s': t0 + i/args.fps, 
                         'x': t_world[0,0], 'y': t_world[1,0], 'z': t_world[2,0], 
                         'inliers': n_inliers, 'tracked': n_tracked})
        # carry forward
        prev_gray = curr_gray
        if args.method == 'klt':
            prev_pts = cv2.goodFeaturesToTrack(curr_gray, maxCorners=args.max_features, qualityLevel=0.01, minDistance=7, blockSize=7)
        else:
            prev_kp, prev_desc = curr_kp, curr_desc
        if i % 100 == 0:
            print(f'[vo] frame {i}/{len(frames)} tracked={n_tracked} inliers={n_inliers} pos=({t_world[0,0]:.2f},{t_world[1,0]:.2f},{t_world[2,0]:.2f})', flush=True)
    print(f'[vo] done. Total frames: {len(out_rows)}. Failed pairs: {fail_count}', flush=True)
    with open(args.out, 'w', newline='') as f:
        w = csv.DictWriter(f, fieldnames=['label','frame_idx','ts_s','x','y','z','inliers','tracked'])
        w.writeheader(); w.writerows(out_rows)
    print(f'[out] {args.out}', flush=True)
    if args.plot:
        import matplotlib
        matplotlib.use('Agg')
        import matplotlib.pyplot as plt
        x = [r['x'] for r in out_rows]
        y = [r['y'] for r in out_rows]
        z = [r['z'] for r in out_rows]
        fig, axes = plt.subplots(2, 2, figsize=(14, 12))
        ax_xy, ax_xz, ax_yz, ax_qual = axes[0,0], axes[0,1], axes[1,0], axes[1,1]
        ax_xy.plot(x, y, '-b', linewidth=1, alpha=0.7, label='VO classic')
        ax_xy.plot(x[0], y[0], 'go', markersize=10, label='start')
        ax_xy.plot(x[-1], y[-1], 'r^', markersize=10, label='end')
        if args.ref_csv:
            try:
                with open(args.ref_csv) as ff:
                    refrows = list(csv.DictReader(ff))
                rx = [float(r['x']) for r in refrows if r.get('segment','') == args.label]
                ry = [float(r['y']) for r in refrows if r.get('segment','') == args.label]
                if rx:
                    ax_xy.plot(rx, ry, '-r', linewidth=1, alpha=0.5, label='lingbot')
            except Exception as e:
                print(f'[plot] ref_csv load fail: {e}')
        ax_xy.set_xlabel('X (m, up-to-scale)'); ax_xy.set_ylabel('Y'); ax_xy.set_title(f'Top X-Y — VO classique {args.label}')
        ax_xy.set_aspect('equal'); ax_xy.legend(); ax_xy.grid(True, alpha=0.3)
        ax_xz.plot(x, z, '-b', linewidth=1)
        ax_xz.set_xlabel('X'); ax_xz.set_ylabel('Z'); ax_xz.set_title('Side X-Z'); ax_xz.set_aspect('equal'); ax_xz.grid(True, alpha=0.3)
        ax_yz.plot(y, z, '-b', linewidth=1)
        ax_yz.set_xlabel('Y'); ax_yz.set_ylabel('Z'); ax_yz.set_title('Side Y-Z'); ax_yz.set_aspect('equal'); ax_yz.grid(True, alpha=0.3)
        tracked = [r['tracked'] for r in out_rows]
        inliers = [r['inliers'] for r in out_rows]
        ax_qual.plot(tracked, label='tracked features', color='blue', alpha=0.6)
        ax_qual.plot(inliers, label='RANSAC inliers', color='red', alpha=0.6)
        ax_qual.set_xlabel('Frame'); ax_qual.set_ylabel('Count'); ax_qual.set_title('Tracking quality'); ax_qual.legend(); ax_qual.grid(True, alpha=0.3)
        plt.suptitle(f'Visual Odometry classique (OpenCV {args.method.upper()}) — {args.label}')
        plt.tight_layout()
        plt.savefig(args.plot, dpi=130, bbox_inches='tight')
        print(f'[plot] {args.plot}')
 if __name__ == '__main__': main()