auto-iter 20260513-2231: GX019817 RoPE skip, 4 PLY done ready for stage06
This commit is contained in:
Poulpe
156
scripts/dvl_optical_full.py
Normal file
156
scripts/dvl_optical_full.py
Normal file
@@ -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()
|
||||
Reference in New Issue
Block a user