45 lines
2.8 KiB
Python
45 lines
2.8 KiB
Python
"""Fit of the target drifts by a camera translation: lateral (tx, ty) and along the optical axis (tz, radial from the
|
|
image centre), each scaled by p = 1/d - mean 1/d_ref (what the similarity fitted on the references leaves)."""
|
|
import sys, statistics as st
|
|
sys.path.insert(0, "tools/scripts")
|
|
import data
|
|
F = "data/BREITHORN"; M = data.load(F); T = data.load_targets(F)
|
|
refs = [s for s in M if data.is_reference(M[s])]
|
|
inv_ref = st.mean(1000 / T[s]["distance_mm"] for s in refs)
|
|
allx = [m["x"] for s in M for m in M[s] if m["x"] is not None]; ally = [m["y"] for s in M for m in M[s] if m["y"] is not None]
|
|
print("image x range %.0f-%.0f, y range %.0f-%.0f" % (min(allx), max(allx), min(ally), max(ally)))
|
|
cx, cy = float(sys.argv[1]), float(sys.argv[2]); N = 6
|
|
def solve(A, b): # normal equations
|
|
n = len(A[0]); G = [[sum(r[i]*r[j] for r in A) for j in range(n)] for i in range(n)]; h = [sum(r[i]*y for r, y in zip(A, b)) for i in range(n)]
|
|
for i in range(n):
|
|
piv = G[i][i]
|
|
for j in range(i+1, n):
|
|
f = G[j][i]/piv; G[j] = [a - f*c for a, c in zip(G[j], G[i])]; h[j] -= f*h[i]
|
|
x = [0]*n
|
|
for i in reversed(range(n)): x[i] = (h[i] - sum(G[i][j]*x[j] for j in range(i+1, n)))/G[i][i]
|
|
return x
|
|
for a, b in [(sys.argv[i], sys.argv[i+1]) for i in range(3, len(sys.argv), 2)]:
|
|
A0, B0 = data.parse_time(a), data.parse_time(b); rows = []
|
|
for s in M:
|
|
if s in refs: continue
|
|
w = [m for m in data.filter_window(M[s], A0, B0) if m["corr_x"] is not None]
|
|
if len(w) < 2*N: continue
|
|
p = 1000/T[s]["distance_mm"] - inv_ref; x = st.median(m["x"] for m in w); y = st.median(m["y"] for m in w)
|
|
dx = st.median(m["corr_x"] for m in w[-N:]) - st.median(m["corr_x"] for m in w[:N])
|
|
dy = st.median(m["corr_y"] for m in w[-N:]) - st.median(m["corr_y"] for m in w[:N])
|
|
rows.append((s, p, x, y, dx, dy))
|
|
A = []; bb = []
|
|
for s, p, x, y, dx, dy in rows:
|
|
A.append([p, 0, p*(x-cx)]); bb.append(dx); A.append([0, p, p*(y-cy)]); bb.append(dy)
|
|
tx, ty, tz = solve(A, bb)
|
|
lat = solve([[r[0], r[1]] for r in A], bb)
|
|
print(f"\n== {a} -> {b}: tx {tx:+.3f} ty {ty:+.3f} px*m, tz {tz:+.6f} m (radial)")
|
|
print(f"{'sensor':10} {'d':>5} {'x':>6} {'y':>6} {'dcx':>8} {'fit':>8} {'dcy':>8} {'fit':>8} {'res_x':>7} {'res_y':>7}")
|
|
sr = st0 = 0
|
|
for s, p, x, y, dx, dy in sorted(rows, key=lambda r: -r[1]):
|
|
fx = p*tx + p*tz*(x-cx); fy = p*ty + p*tz*(y-cy)
|
|
sr += (dx-fx)**2 + (dy-fy)**2; st0 += dx**2 + dy**2
|
|
print(f"{s:10} {1/(p+inv_ref):5.1f} {x:6.0f} {y:6.0f} {dx:+8.4f} {fx:+8.4f} {dy:+8.4f} {fy:+8.4f} {dx-fx:+7.4f} {dy-fy:+7.4f}")
|
|
srl = sum((r[4]-r[1]*lat[0])**2 + (r[5]-r[1]*lat[1])**2 for r in rows)
|
|
print(f"share of the drift explained: lateral+axial {1-sr/st0:.2f}, lateral only {1-srl/st0:.2f}")
|