Buckets:
| """cwm3/data/wheel_ellipse.py — ellipse geometry for the hands_on / hand-event label v4 (pre-registered docs/decisions.md | |
| 2026-09-16T22:18:02Z): robust causal fit of the steering-wheel rim locus from wrist positions, and the exact signed distance to it. | |
| Geometry = (cx, cy, a, b, theta): centre, semi-axes a >= b > 0, rotation of the a-axis in radians (wrapped to [-pi/2, pi/2)). Units are the | |
| crop-normalised-equivalent units of data/build_kin_causal.py (x = x_norm * 1.6, y = y_norm), so distances are comparable to the v2/v3 thresholds. | |
| fit_ellipse_direct(x, y) / fit_ellipse_direct_batch(X, Y) | |
| Fitzgibbon direct least-squares conic fit constrained to an ellipse (Halir & Flusser 1998 numerically stable formulation) on | |
| isotropically normalised coordinates (mean-centred, RMS-radius scaled); -> geometry (NaN row / None when degenerate or < 5 points). | |
| geom_to_conic / conic_to_geom | |
| conics are (A, B, C, D, E, F) with Q(p) = A x^2 + B x y + C y^2 + D x + E y + F, normalised so that Q < 0 INSIDE the ellipse. | |
| sampson_distance(conic(s), x, y) | |
| first-order signed geometric distance Q / |grad Q| (used for RANSAC scoring / inlier selection; accurate near the rim). | |
| signed_distance(geom, x, y) | |
| EXACT signed Euclidean distance to the ellipse (Eberly, "Distance from a point to an ellipse", robust bisection on the canonical | |
| root function): 0 on the rim, < 0 inside (distance to the rim, negated), > 0 outside. Per-point geometry arrays broadcast against x, y, | |
| so a piecewise-constant causal ellipse series can be evaluated in one call. | |
| arc_coverage_deg(geom, x, y) | |
| angular coverage of points ON THE FITTED ELLIPSE: each point's angle in the ellipse frame (u, v) is binned in COV_BIN_DEG = 10 deg bins; | |
| a bin is occupied when it holds >= max(MIN_PER_BIN = 5, COV_FRAC = 1 % of the points) points; coverage = 10 deg x occupied bins, | |
| taken as the MIN over the two angle parametrisations — parametric phi = atan2(v / b, u / a) and central psi = atan2(v, u) — which | |
| agree for a correct fit but disagree when a semi-axis is wrong (an under-estimated b stretches phi near the vertices, so two grip | |
| clusters at the wheel's sides would otherwise look like 100 deg). Contiguity is NOT required: two opposite arcs add up (e.g. two 70 deg | |
| arcs at the sides -> 140 deg parametric / >= 110 deg central on a 1.67:1 ellipse), which constrains the ellipse better than one 90 deg arc. | |
| ransac_ellipse(P, rng, prev=None) | |
| RANSAC on the point set P[n, 2]: N_ITER = 100 random N_SAMPLE = 5-point direct fits + the previous ellipse (warm start) + the plain | |
| direct fit on all points as candidates; candidates with a semi-axis outside [AX_MIN, AX_MAX] = [0.05, 0.6] (the plausible range of a | |
| wheel in the driver camera; also the diagnostics' flag range) or with an aspect ratio b / a < RATIO_MIN = 0.35 (a wheel rim seen from | |
| the driver camera is never that flat; without the floor a thin ellipse threading the two grip clusters and the hand's path across the | |
| wheel wins on real devices) are dropped; MSAC score = sum_i min(sampson_i^2, TAU_IN^2) with | |
| TAU_IN = 0.03 (lower is better: prefers the candidate that fits its inliers tightly over one that merely passes within the band); | |
| inliers = |sampson| <= TAU_IN; the best candidate is refit twice on its inliers (LO-RANSAC style). | |
| Returns dict(geom, inliers, n_inlier, coverage_deg, passed, n_points) with the PRE-REGISTERED gate | |
| passed = (n_inlier >= N_MIN_PTS = 200) and (coverage_deg >= COV_MIN_DEG = 90); a set with fewer than N_MIN_PTS points is not fitted. | |
| """ | |
| import numpy as np | |
| TAU_IN = 0.03 # RANSAC inlier band on the Sampson distance (normalised units) | |
| N_ITER, N_SAMPLE = 100, 5 # random minimal-sample candidates per fit | |
| N_MIN_PTS = 200 # pre-registered: >= 200 inlier points ... | |
| COV_MIN_DEG = 90.0 # ... spanning >= 90 deg of arc on the fitted ellipse | |
| COV_BIN_DEG, MIN_PER_BIN, COV_FRAC = 10.0, 5, 0.01 | |
| AX_MIN, AX_MAX = 0.05, 0.6 # candidate sanity range for the semi-axes (plausible wheel size in the driver camera; = diagnostics flag range) | |
| RATIO_MIN = 0.35 # candidate sanity: minimum aspect ratio b / a | |
| AXIS_EPS = 1e-9 # relative distance below which a point counts as lying on an axis of the canonical ellipse | |
| N_BISECT = 64 # Eberly bisection iterations (float64; interval width / 2^64 << 1e-12) | |
| # ------------------------------------------------------------------------------------------------------------------- conversions | |
| def geom_to_conic(cx, cy, a, b, theta): | |
| """(cx, cy, a, b, theta) -> (A, B, C, D, E, F) with Q < 0 inside. Scalars or broadcastable arrays.""" | |
| c, s = np.cos(theta), np.sin(theta) | |
| A = c * c / (a * a) + s * s / (b * b); C = s * s / (a * a) + c * c / (b * b); B = 2 * c * s * (1 / (a * a) - 1 / (b * b)) | |
| D = -2 * A * cx - B * cy; E = -B * cx - 2 * C * cy; F = A * cx * cx + B * cx * cy + C * cy * cy - 1 | |
| return np.stack(np.broadcast_arrays(A, B, C, D, E, F), -1) | |
| def conic_to_geom_batch(K): | |
| """K[k, 6] conics -> G[k, 5] geometry (NaN rows where the conic is not a real ellipse). a >= b, theta in [-pi/2, pi/2).""" | |
| K = np.asarray(K, np.float64); A, B, C, D, E, F = K.T | |
| G = np.full((len(K), 5), np.nan) | |
| with np.errstate(all="ignore"): | |
| det = A * C - B * B / 4 | |
| ok = np.isfinite(K).all(1) & (det > 0) | |
| sgn = np.where(A + C < 0, -1.0, 1.0) # make the quadratic form positive definite | |
| A, B, C, D, E, F = A * sgn, B * sgn, C * sgn, D * sgn, E * sgn, F * sgn | |
| cx = (B * E - 2 * C * D) / (4 * det); cy = (B * D - 2 * A * E) / (4 * det) | |
| Fp = F + 0.5 * (D * cx + E * cy) # Q(p) = (p-c)^T M (p-c) + Fp | |
| ok &= np.isfinite(Fp) & (Fp < 0) | |
| M = np.stack([np.stack([A, B / 2], -1), np.stack([B / 2, C], -1)], -2) | |
| M = np.where(ok[:, None, None], M, np.eye(2)[None]) | |
| w, V = np.linalg.eigh(M) # ascending eigenvalues; column 0 = major axis | |
| ok &= w[:, 0] > 0 | |
| a = np.sqrt(-Fp / w[:, 0]); b = np.sqrt(-Fp / w[:, 1]) | |
| th = np.arctan2(V[:, 1, 0], V[:, 0, 0]); th = (th + np.pi / 2) % np.pi - np.pi / 2 | |
| G[ok] = np.stack([cx, cy, a, b, th], -1)[ok] | |
| return G | |
| def conic_to_geom(conic): | |
| g = conic_to_geom_batch(np.asarray(conic, np.float64)[None])[0] | |
| return None if not np.isfinite(g).all() else tuple(float(v) for v in g) | |
| # ------------------------------------------------------------------------------------------------------------------- direct fit | |
| def fit_ellipse_direct_batch(X, Y): | |
| """X, Y [k, m] (m >= 5 points per row) -> G[k, 5] geometry per row (NaN row when degenerate). Halir & Flusser direct LS ellipse fit on | |
| mean-centred, RMS-scaled coordinates; the geometry is mapped back (centre = mean + s * centre_n, axes = s * axes_n, theta unchanged).""" | |
| X = np.asarray(X, np.float64); Y = np.asarray(Y, np.float64); k, m = X.shape | |
| G = np.full((k, 5), np.nan) | |
| if m < 5: return G | |
| with np.errstate(all="ignore"): | |
| mx = X.mean(1, keepdims=True); my = Y.mean(1, keepdims=True) | |
| s = np.sqrt(((X - mx) ** 2 + (Y - my) ** 2).mean(1, keepdims=True)); s = np.where(s > 0, s, 1.0) | |
| xn = (X - mx) / s; yn = (Y - my) / s | |
| D1 = np.stack([xn * xn, xn * yn, yn * yn], -1); D2 = np.stack([xn, yn, np.ones_like(xn)], -1) | |
| S1 = np.einsum("kmi,kmj->kij", D1, D1); S2 = np.einsum("kmi,kmj->kij", D1, D2); S3 = np.einsum("kmi,kmj->kij", D2, D2) | |
| ok = np.isfinite(S1).all((1, 2)) & np.isfinite(S3).all((1, 2)) | |
| cond = np.where(ok, np.linalg.cond(np.where(ok[:, None, None], S3, np.eye(3)[None])), np.inf); ok &= cond < 1e12 | |
| S3s = np.where(ok[:, None, None], S3, np.eye(3)[None]) | |
| T = -np.linalg.solve(S3s, np.swapaxes(S2, 1, 2)) # [k,3,3] | |
| M = S1 + S2 @ T | |
| Mred = np.stack([M[:, 2] / 2, -M[:, 1], M[:, 0] / 2], 1) # C1^{-1} M | |
| Mred = np.where(np.isfinite(Mred).all((1, 2))[:, None, None], Mred, np.eye(3)[None]) | |
| w, V = np.linalg.eig(Mred) # [k,3], [k,3,3] (columns) | |
| V = np.real(V) | |
| cnd = 4 * V[:, 0, :] * V[:, 2, :] - V[:, 1, :] ** 2 # ellipse constraint per eigenvector | |
| j = cnd.argmax(1); ok &= cnd[np.arange(k), j] > 0 | |
| a1 = V[np.arange(k), :, j] # [k,3] | |
| a2 = np.einsum("kij,kj->ki", T, a1) | |
| conic_n = np.concatenate([a1, a2], 1) | |
| Gn = conic_to_geom_batch(conic_n) | |
| ok &= np.isfinite(Gn).all(1) | |
| G[:, 0] = mx[:, 0] + s[:, 0] * Gn[:, 0]; G[:, 1] = my[:, 0] + s[:, 0] * Gn[:, 1] | |
| G[:, 2] = s[:, 0] * Gn[:, 2]; G[:, 3] = s[:, 0] * Gn[:, 3]; G[:, 4] = Gn[:, 4] | |
| G[~ok] = np.nan | |
| return G | |
| def fit_ellipse_direct(x, y): | |
| """Direct LS ellipse fit of the points (x, y) (NaN pairs dropped) -> (cx, cy, a, b, theta) or None.""" | |
| x = np.asarray(x, np.float64).ravel(); y = np.asarray(y, np.float64).ravel(); f = np.isfinite(x) & np.isfinite(y) | |
| if f.sum() < 5: return None | |
| g = fit_ellipse_direct_batch(x[f][None], y[f][None])[0] | |
| return None if not np.isfinite(g).all() else tuple(float(v) for v in g) | |
| # -------------------------------------------------------------------------------------------------------------------- distances | |
| def sampson_distance(conics, x, y): | |
| """conics [6] or [k, 6]; x, y [n] -> signed first-order distance [n] or [k, n] (negative inside). Three small matmuls (BLAS).""" | |
| K = np.asarray(conics, np.float64); single = K.ndim == 1 | |
| if single: K = K[None] | |
| x = np.asarray(x, np.float64); y = np.asarray(y, np.float64) | |
| Z = np.stack([x * x, x * y, y * y, x, y, np.ones_like(x)], 0) # [6, n] | |
| with np.errstate(all="ignore"): | |
| Q = K @ Z # [k, n] | |
| gx = np.stack([2 * K[:, 0], K[:, 1], K[:, 3]], 1) @ Z[3:] # 2Ax + By + D | |
| gy = np.stack([K[:, 1], 2 * K[:, 2], K[:, 4]], 1) @ Z[3:] # Bx + 2Cy + E | |
| d = Q / np.sqrt(gx * gx + gy * gy) | |
| return d[0] if single else d | |
| def msac_scores(conics, x, y, tau=TAU_IN): | |
| """conics [k, 6] -> (score[k] = sum_i min(d_i^2, tau^2), inlier count[k], d[k, n]) with d = Sampson distance.""" | |
| d = sampson_distance(conics, x, y); d2 = d * d; inl = d2 <= tau * tau | |
| return np.where(inl, d2, tau * tau).sum(1), inl.sum(1), d | |
| def _dist_canonical(e0, e1, y0, y1, n_iter=N_BISECT): | |
| """Distance from (y0, y1), both >= 0, to the axis-aligned ellipse (x0/e0)^2 + (x1/e1)^2 = 1 with e0 >= e1 > 0 (Eberly's algorithm, | |
| bisection on the robust root function). All arguments broadcast; NaN where any input is not finite.""" | |
| e0, e1, y0, y1 = np.broadcast_arrays(*[np.asarray(v, np.float64) for v in (e0, e1, y0, y1)]) | |
| ok = np.isfinite(e0) & np.isfinite(e1) & np.isfinite(y0) & np.isfinite(y1) & (e1 > 0) & (e0 >= e1) | |
| with np.errstate(all="ignore"): | |
| # --- general case: y0 > 0 and y1 > 0 | |
| z0 = y0 / e0; z1 = y1 / e1; g = z0 * z0 + z1 * z1 - 1 | |
| r0 = (e0 / e1) ** 2; n0 = r0 * z0 | |
| s0 = z1 - 1; s1 = np.where(g < 0, 0.0, np.sqrt(n0 * n0 + z1 * z1) - 1) | |
| s = 0.5 * (s0 + s1) | |
| for _ in range(n_iter): | |
| s = 0.5 * (s0 + s1) | |
| gg = (n0 / (s + r0)) ** 2 + (z1 / (s + 1)) ** 2 - 1 | |
| s0 = np.where(gg > 0, s, s0); s1 = np.where(gg < 0, s, s1) | |
| xA0 = r0 * y0 / (s + r0); xA1 = y1 / (s + 1) | |
| dA = np.where(g == 0, 0.0, np.hypot(xA0 - y0, xA1 - y1)) | |
| # --- y0 == 0, y1 > 0: closest point is the top of the minor axis | |
| dB = np.abs(y1 - e1) | |
| # --- y1 == 0: closest point on the major axis or on the arc | |
| numer0 = e0 * y0; denom0 = e0 * e0 - e1 * e1 | |
| xde0 = numer0 / denom0; inner = numer0 < denom0 | |
| dC_in = np.hypot(e0 * xde0 - y0, e1 * np.sqrt(np.clip(1 - xde0 * xde0, 0, None))) | |
| dC = np.where(inner, dC_in, np.abs(y0 - e0)) | |
| on1 = y1 <= AXIS_EPS * e1; on0 = y0 <= AXIS_EPS * e0 # numerically on an axis: the general branch is ill-conditioned there | |
| d = np.where(~on1, np.where(~on0, dA, dB), dC) | |
| return np.where(ok, d, np.nan) | |
| def signed_distance(geom, x, y): | |
| """geom = (cx, cy, a, b, theta) scalars or per-point arrays; x, y arrays -> exact signed distance (< 0 inside, 0 on the rim, > 0 outside). | |
| NaN where the geometry or the point is undefined.""" | |
| cx, cy, a, b, th = [np.asarray(v, np.float64) for v in geom] | |
| x = np.asarray(x, np.float64); y = np.asarray(y, np.float64) | |
| with np.errstate(all="ignore"): | |
| swap = b > a # canonical frame needs e0 >= e1 | |
| e0 = np.where(swap, b, a); e1 = np.where(swap, a, b); th = np.where(swap, th + np.pi / 2, th) | |
| c, s = np.cos(th), np.sin(th); dx = x - cx; dy = y - cy | |
| u = dx * c + dy * s; v = -dx * s + dy * c | |
| dist = _dist_canonical(e0, e1, np.abs(u), np.abs(v)) | |
| inside = (u / e0) ** 2 + (v / e1) ** 2 < 1 | |
| return np.where(inside, -dist, dist) | |
| def param_angle_deg(geom, x, y, central=False): | |
| """Angle (degrees in [0, 360)) of the points in the ellipse frame: parametric phi = atan2(v / b, u / a), or central psi = atan2(v, u).""" | |
| cx, cy, a, b, th = geom; c, s = np.cos(th), np.sin(th); dx = np.asarray(x, np.float64) - cx; dy = np.asarray(y, np.float64) - cy | |
| u = dx * c + dy * s; v = -dx * s + dy * c | |
| return np.degrees(np.arctan2(v, u) if central else np.arctan2(v / b, u / a)) % 360.0 | |
| def _bin_coverage(ang, bin_deg, thr): | |
| nb = int(round(360.0 / bin_deg)); counts = np.bincount(np.minimum((ang // bin_deg).astype(np.int64), nb - 1), minlength=nb) | |
| return float(bin_deg * int((counts >= thr).sum())) | |
| def arc_coverage_deg(geom, x, y, bin_deg=COV_BIN_DEG, min_per_bin=MIN_PER_BIN, frac=COV_FRAC, both=False): | |
| """Occupied-bin angular coverage (degrees) of the points on the fitted ellipse = min(parametric, central); see the module docstring. | |
| both=True returns (parametric, central).""" | |
| x = np.asarray(x, np.float64); y = np.asarray(y, np.float64); f = np.isfinite(x) & np.isfinite(y); n = int(f.sum()) | |
| if n == 0: return (0.0, 0.0) if both else 0.0 | |
| thr = max(min_per_bin, frac * n) | |
| cp = _bin_coverage(param_angle_deg(geom, x[f], y[f]), bin_deg, thr); cc = _bin_coverage(param_angle_deg(geom, x[f], y[f], central=True), bin_deg, thr) | |
| return (cp, cc) if both else min(cp, cc) | |
| # ----------------------------------------------------------------------------------------------------------------------- RANSAC | |
| def _sane(G, ax_range=(AX_MIN, AX_MAX), ratio_min=RATIO_MIN): | |
| """G[k, 5] -> bool[k]: finite geometry with both semi-axes inside the sanity range and b / a >= ratio_min.""" | |
| G = np.asarray(G, np.float64) | |
| with np.errstate(all="ignore"): | |
| ok = np.isfinite(G).all(1) & (G[:, 2] >= ax_range[0]) & (G[:, 2] <= ax_range[1]) & (G[:, 3] >= ax_range[0]) & (G[:, 3] <= ax_range[1]) | |
| return ok & (G[:, 3] / G[:, 2] >= ratio_min) | |
| def ransac_ellipse(P, rng, prev=None, n_iter=N_ITER, n_sample=N_SAMPLE, tau=TAU_IN, n_min=N_MIN_PTS, cov_min=COV_MIN_DEG, ax_range=(AX_MIN, AX_MAX)): | |
| """Robust ellipse fit of P[n, 2] (NaN rows dropped). rng: numpy Generator (consumed: one integers() draw of shape [n_iter, n_sample]). | |
| prev: previous geometry used as an extra candidate (warm start) or None. See the module docstring for the algorithm and the gate.""" | |
| P = np.asarray(P, np.float64); P = P[np.isfinite(P).all(1)]; n = len(P) | |
| res = dict(geom=None, inliers=np.zeros(n, bool), n_inlier=0, coverage_deg=0.0, passed=False, n_points=n) | |
| if n < max(n_min, n_sample): return res | |
| idx = rng.integers(0, n, size=(n_iter, n_sample)) | |
| Gc = fit_ellipse_direct_batch(P[idx, 0], P[idx, 1]) | |
| cands = [Gc[_sane(Gc, ax_range)]] | |
| if prev is not None: cands.append(np.asarray(prev, np.float64)[None]) | |
| g_all = fit_ellipse_direct_batch(P[None, :, 0], P[None, :, 1]) | |
| if _sane(g_all, ax_range)[0]: cands.append(g_all) | |
| G = np.concatenate(cands, 0) | |
| if len(G) == 0: return res | |
| score, counts, ds = msac_scores(geom_to_conic(*G.T), P[:, 0], P[:, 1], tau) # [k], [k], [k, n] | |
| best = int(score.argmin()) | |
| g = G[best]; inl = np.abs(ds[best]) <= tau | |
| for _ in range(2): # local optimisation: refit on the inliers | |
| if inl.sum() < max(5, n_sample): break | |
| g2 = fit_ellipse_direct_batch(P[None, inl, 0], P[None, inl, 1]) | |
| if not _sane(g2, ax_range)[0]: break | |
| g = g2[0]; inl = np.abs(sampson_distance(geom_to_conic(*g), P[:, 0], P[:, 1])) <= tau | |
| n_in = int(inl.sum()); cov = arc_coverage_deg(tuple(g), P[inl, 0], P[inl, 1]) if n_in else 0.0 | |
| res.update(geom=tuple(float(v) for v in g), inliers=inl, n_inlier=n_in, coverage_deg=cov, passed=bool(n_in >= n_min and cov >= cov_min)) | |
| return res | |
| def ellipse_points(geom, phi_deg): | |
| """Points on the ellipse at parametric angles phi (degrees) -> x[n], y[n].""" | |
| cx, cy, a, b, th = geom; p = np.radians(np.asarray(phi_deg, np.float64)); u = a * np.cos(p); v = b * np.sin(p) | |
| return cx + u * np.cos(th) - v * np.sin(th), cy + u * np.sin(th) + v * np.cos(th) | |
Xet Storage Details
- Size:
- 17.4 kB
- Xet hash:
- 920148d29d5233dcf2b7b29988272d5364a6f5c3c01e904e8d3a03ab3e4e51b5
·
Xet efficiently stores files, intelligently splitting them into unique chunks and accelerating uploads and downloads. More info.