141 lines
6.7 KiB
Python
141 lines
6.7 KiB
Python
#!/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()
|