From 38dbcfd46f1c4653211515aa5fe374fb655dd827 Mon Sep 17 00:00:00 2001 From: Poulpe Date: Wed, 13 May 2026 23:02:31 +0000 Subject: [PATCH] auto-iter 20260513-2231: GX019817 RoPE skip, 4 PLY done ready for stage06 --- pipeline/iteration-log.md | 17 ++ scripts/coverage_swath.py | 139 +++++++++++++++ scripts/dvl_focal_sweep.py | 61 +++++++ scripts/dvl_optical.py | 140 ++++++++++++++++ scripts/dvl_optical_full.py | 156 +++++++++++++++++ scripts/dvl_optical_imu.py | 166 ++++++++++++++++++ scripts/loop_closure_lightglue.py | 252 ++++++++++++++++++++++++++++ scripts/loop_closure_phash.py | 120 +++++++++++++ scripts/loop_closure_sweep.py | 119 +++++++++++++ scripts/photomosaic_overlay.py | 244 +++++++++++++++++++++++++++ scripts/poses_to_csv.py | 61 +++++++ scripts/stage06_align_absolute.py | 232 +++++++++++++++++++++++++ scripts/stage06b_imu_depth_align.py | 179 ++++++++++++++++++++ scripts/vo_classic_opencv.py | 197 ++++++++++++++++++++++ 14 files changed, 2083 insertions(+) create mode 100644 scripts/coverage_swath.py create mode 100644 scripts/dvl_focal_sweep.py create mode 100644 scripts/dvl_optical.py create mode 100644 scripts/dvl_optical_full.py create mode 100644 scripts/dvl_optical_imu.py create mode 100755 scripts/loop_closure_lightglue.py create mode 100644 scripts/loop_closure_phash.py create mode 100644 scripts/loop_closure_sweep.py create mode 100755 scripts/photomosaic_overlay.py create mode 100755 scripts/poses_to_csv.py create mode 100755 scripts/stage06_align_absolute.py create mode 100644 scripts/stage06b_imu_depth_align.py create mode 100644 scripts/vo_classic_opencv.py diff --git a/pipeline/iteration-log.md b/pipeline/iteration-log.md index 18ae57e..0dbd787 100644 --- a/pipeline/iteration-log.md +++ b/pipeline/iteration-log.md @@ -120,3 +120,20 @@ - **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) diff --git a/scripts/coverage_swath.py b/scripts/coverage_swath.py new file mode 100644 index 0000000..569f0c9 --- /dev/null +++ b/scripts/coverage_swath.py @@ -0,0 +1,139 @@ +#!/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 --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() diff --git a/scripts/dvl_optical_full.py b/scripts/dvl_optical_full.py new file mode 100644 index 0000000..8ffbc7d --- /dev/null +++ b/scripts/dvl_optical_full.py @@ -0,0 +1,156 @@ +#!/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() diff --git a/scripts/dvl_optical_imu.py b/scripts/dvl_optical_imu.py new file mode 100644 index 0000000..62cd1d0 --- /dev/null +++ b/scripts/dvl_optical_imu.py @@ -0,0 +1,166 @@ +#!/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 --bag-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() diff --git a/scripts/loop_closure_lightglue.py b/scripts/loop_closure_lightglue.py new file mode 100755 index 0000000..d49a8d5 --- /dev/null +++ b/scripts/loop_closure_lightglue.py @@ -0,0 +1,252 @@ +#!/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() diff --git a/scripts/loop_closure_phash.py b/scripts/loop_closure_phash.py new file mode 100644 index 0000000..7d175d8 --- /dev/null +++ b/scripts/loop_closure_phash.py @@ -0,0 +1,120 @@ +#!/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 --dvl-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() diff --git a/scripts/loop_closure_sweep.py b/scripts/loop_closure_sweep.py new file mode 100644 index 0000000..ed600d9 --- /dev/null +++ b/scripts/loop_closure_sweep.py @@ -0,0 +1,119 @@ +#!/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() diff --git a/scripts/photomosaic_overlay.py b/scripts/photomosaic_overlay.py new file mode 100755 index 0000000..8acd2a0 --- /dev/null +++ b/scripts/photomosaic_overlay.py @@ -0,0 +1,244 @@ +#!/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() diff --git a/scripts/poses_to_csv.py b/scripts/poses_to_csv.py new file mode 100755 index 0000000..774a067 --- /dev/null +++ b/scripts/poses_to_csv.py @@ -0,0 +1,61 @@ +#!/usr/bin/env python3 +"""Extract camera positions (timestamp, x, y, z) from lingbot-map NPZ poses. +Usage: poses_to_csv.py [--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() diff --git a/scripts/stage06_align_absolute.py b/scripts/stage06_align_absolute.py new file mode 100755 index 0000000..088719d --- /dev/null +++ b/scripts/stage06_align_absolute.py @@ -0,0 +1,232 @@ +#!/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() diff --git a/scripts/stage06b_imu_depth_align.py b/scripts/stage06b_imu_depth_align.py new file mode 100644 index 0000000..a7bbbde --- /dev/null +++ b/scripts/stage06b_imu_depth_align.py @@ -0,0 +1,179 @@ +#!/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() diff --git a/scripts/vo_classic_opencv.py b/scripts/vo_classic_opencv.py new file mode 100644 index 0000000..9500484 --- /dev/null +++ b/scripts/vo_classic_opencv.py @@ -0,0 +1,197 @@ +#!/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//frames// \ + --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()