Buckets:
| """cwm3/data/build_kin_causal.py — CAUSAL driver-kinematics targets/history on the s12 anchor grid, from the causal | |
| RTMW pose (results/pose_causal/<clip_id>.npz, HALPE-136 → COCO-17 body = indices 0–16) of the 15 s clip windows. | |
| Causal analogue of cwm2/results/targets_kin/{kin,kinhist}_{regime}_{split}.npz (cwm2/data/build_kinematics_targets.py). | |
| Units — "crop-normalised equivalent" of the old cache so every threshold carries over unchanged: | |
| the old pose10hz cache decoded every video to 640×400 and normalised by a 400 px crop, i.e. x_old = (x_px·640/W − crop_x0)/400, | |
| y_old = y_px/H. Here x_c = x_norm·1.6 (= x_px·640/W/400, offset 0 — no route-level crop exists), y_c = y_norm. | |
| Distances / deviations are therefore in identical units (WHEEL_TH 0.10, head-turn thresholds); absolute x channels differ | |
| from the old ones by the per-route constant crop_x0/400 (qc_kin_causal.py removes it before comparing). | |
| Channels D=11 (same order/definitions as the old build, mirroring s8_pose/posefeat.per_frame): | |
| head_cx, head_cy (mean of conf>0.3 face points {nose, eyes, ears}; requires nose), Lwri_x, Lwri_y, Rwri_x, Rwri_y, | |
| head_turn = (nose_x − shoulder_centre_x)/shoulder_width, head_turn_dev = head_turn − REF_head_turn, | |
| Lhand_rest_dist, Rhand_rest_dist = hypot(wrist − REF_wrist), min_hand_rest_dist. | |
| CAUSAL REFERENCES (the old build used whole-route medians = transductive). For clip c on device d, route r, anchor t0, for each | |
| quantity q ∈ {Lwri_x, Lwri_y, Rwri_x, Rwri_y, head_turn}: | |
| REF_q(k) = median( POOL_q(c) ∪ { q(j) : j ≤ k, conf>0.3 } ) (expanding over the clip's own frames ≤ current time) | |
| POOL_q(c) = valid 5 Hz values of (i) every clip of device d on a route ordered strictly BEFORE r (route order: date-named | |
| routes first (older openpilot naming) by date, then counter-named routes by counter, then segment start), and | |
| (ii) clips on the same route r whose window ENDED before this anchor: t0' + 10 ≤ t0; | |
| capped to the most recent POOL_CAP values. REF undefined (NaN) while |POOL ∪ own| < N_MIN. | |
| The reference used is stored per clip and frame in clip_refs.npz (ref[C,75,5], n_pool[C,5], n_prior_clips[C]). | |
| States: hands_on = min_hand_rest_dist < 0.10 (NaN if NaN); head_turned = hysteresis on |head_turn_dev| with the OLD build's | |
| TRAIN thresholds (cwm2/results/targets_kin/build_stats_latest.json head_turn_on/off; state reset after >1 s gap), run causally | |
| along the clip's 10 Hz window series (state initialised at the first valid frame of the window — the old build carried state | |
| across the whole route). | |
| Validity: V = frame present ∧ conf>0.3 for nose, both shoulders, both wrists (old Kvalid rule) ∧ finite head_turn ∧ causal reference | |
| defined (so every Kvalid cell has all 11 channels). States are derived from the f16-rounded stored channel values. | |
| Time grid: pose at t0 − 5 + 0.2k (k=0..74). The window is re-gridded to 10 Hz (j=0..148, t0 − 5 + 0.1j): even j = pose sample, | |
| odd j = LINEAR interpolation of both neighbours (flag interp; V/frame at odd j require both neighbours). Anchors t (0.1 s grid) | |
| inside a clip with t0 − 1 ≤ t ≤ t0 + 5 (≥ 4 s history) are assigned to the clip with the nearest t0. The pose grid ends at | |
| t0 + 9.8, so for anchors with t > t0 + 4.8 the last future steps (t+τ > t0+9.8) are ABSENT (Kframe/Kvalid False, NaN) — counted | |
| in build_stats as n_rows_future_truncated. | |
| Outputs results/targets_kin_causal/ (row-aligned with s12_wm/results/targets_{regime}_{split}.npz; uid/t equality asserted): | |
| kin_causal_{regime}_{split}.npz : uid, dongle, t, row_ok[N], clip_id[N] ('' if !row_ok), dt_anchor[N] = t − t0 (NaN), | |
| K_c f16[N,25,D] (+0.2..+5.0 s), Kvalid[N,25] (V ∧ s12 M), Kframe[N,25], interp[N,25], K0_c f32[N,D], K0valid[N], interp0[N], | |
| S_c f32[N,25,2] {hands_on, head_turned} (NaN invalid / s12-censored), S0_c[N,2], E_c u8[N,25,3], EM_c[N,25,3], E0_c[N,3], | |
| channels, event_names, state_names, units | |
| kinhist_causal_{regime}_{split}.npz : uid, t, Kh_c f16[N,20,D] (t−3.8..t @ 5 Hz, incl. t), Khvalid[N,20], interp_h[N,20], row_ok | |
| clip_refs.npz, build_stats_<utc>.json (+ _latest), run_build_kin_causal_<utc>.json | |
| Rows outside every clip window: all NaN / False with row_ok=False (val/test have NO clips: coverage 0 — reported, not hidden).""" | |
| import argparse, bisect, json, re, sys, time | |
| from collections import defaultdict | |
| from datetime import datetime | |
| from pathlib import Path | |
| import numpy as np | |
| ROOT = Path(__file__).resolve().parents[1] | |
| sys.path.insert(0, str(ROOT / "common")) | |
| from runmeta import write_run_json | |
| EXPLORE = Path("/home/USER/explore") | |
| S12R = EXPLORE / "s12_wm" / "results"; POSE = ROOT / "results" / "pose_causal"; OUT = ROOT / "results" / "targets_kin_causal" | |
| import os | |
| S12_SPLITS = tuple(os.environ.get("CWM3_S12_SPLITS", "train,val,test").split(",")) # 2026-09-16 WP5: split enumeration override (default = the benchmark; the holdout lane sets CWM3_S12_SPLITS=holdout) | |
| CLIP_MAP = EXPLORE / "cwm2" / "private" / "clip_map_full.json" | |
| OLD_STATS = EXPLORE / "cwm2" / "results" / "targets_kin" / "build_stats_latest.json" | |
| CONF_TH, WHEEL_TH = 0.30, 0.10 | |
| NOSE, LEYE, REYE, LEAR, REAR, LSHO, RSHO, LELB, RELB, LWRI, RWRI = range(11) | |
| CHANNELS = ["head_cx", "head_cy", "Lwri_x", "Lwri_y", "Rwri_x", "Rwri_y", "head_turn", "head_turn_dev", "Lhand_rest_dist", "Rhand_rest_dist", "min_hand_rest_dist"] | |
| D = len(CHANNELS); EVENTS = ["hand_return", "hand_leave", "head_turn_onset"]; STATES = ["hands_on", "head_turned"] | |
| REFQ = ["Lwri_x", "Lwri_y", "Rwri_x", "Rwri_y", "head_turn"] | |
| NK, NJ, H, HIST = 75, 149, 25, 20 # 75 pose samples @0.2 s = t0-5..t0+9.8 -> 149 ten-Hz points (t0-5..t0+9.8) | |
| XSCALE = 640.0 / 400.0 # x_norm -> old crop-normalised units (640-wide decode / 400 px crop); y unchanged (400/400) | |
| GAP_RESET = 10 # 10 Hz steps (> 1 s of missing frames resets the hysteresis) | |
| POOL_CAP, N_MIN = 4000, 25 | |
| UNITS = "crop-normalised-equivalent: x = x_norm*1.6 (offset 0), y = y_norm; distances/angles dimensionless" | |
| def route_key(uid): | |
| """uid = <CAR>__<dongle>__<routeid>__route_<a>-<b>; chronological key within a device.""" | |
| parts = uid.split("__"); rid = parts[2] if len(parts) > 3 else uid; seg = parts[3] if len(parts) > 3 else "" | |
| m = re.match(r"(\d{4}-\d{2}-\d{2})--(\d{2})-(\d{2})-(\d{2})", rid) | |
| a = int(re.search(r"route_(\d+)-", seg).group(1)) if re.search(r"route_(\d+)-", seg) else 0 | |
| if m: return (0, datetime.strptime(f"{m.group(1)} {m.group(2)}:{m.group(3)}:{m.group(4)}", "%Y-%m-%d %H:%M:%S").timestamp(), a) | |
| m2 = re.match(r"([0-9a-fA-F]{8})--", rid) | |
| return (1, int(m2.group(1), 16) if m2 else 0, a) | |
| def hysteresis(x, on, off, gap_reset=GAP_RESET): | |
| s = np.full(len(x), np.nan, np.float32); cur, gap, init = 0.0, gap_reset + 1, True | |
| for i in range(len(x)): | |
| v = x[i] | |
| if not np.isfinite(v): gap += 1; continue | |
| if init or gap > gap_reset: cur = 1.0 if v >= on else 0.0; init = False | |
| elif v >= on: cur = 1.0 | |
| elif v < off: cur = 0.0 | |
| gap = 0; s[i] = cur | |
| return s | |
| def expanding_median(pool, vals): | |
| """pool: sorted 1-D array (may be empty); vals[75] with NaN -> ref[75] (NaN while n < N_MIN).""" | |
| out = np.full(len(vals), np.nan, np.float32); own = []; last = np.nan; changed = True | |
| for k, v in enumerate(vals): | |
| if np.isfinite(v): bisect.insort(own, float(v)); changed = True | |
| n = len(pool) + len(own) | |
| if n < N_MIN: continue | |
| if changed: last = float(np.median(np.concatenate([pool, np.asarray(own, np.float32)]))) if len(own) else float(np.median(pool)); changed = False | |
| out[k] = last | |
| return out | |
| def clip_raw(cid): | |
| """-> dict: q[75,5] raw quantities (Lx,Ly,Rx,Ry,head_turn), head[75,2], V[75], F[75] (units: old crop-normalised).""" | |
| d = np.load(POSE / f"{cid}.npz"); k = d["kpts"].astype(np.float32)[:, :17] | |
| xy, cf = k[:, :, :2].copy(), k[:, :, 2]; fr = d["frame_ok"].astype(bool) | |
| xy[:, :, 0] *= XSCALE | |
| xy[cf < CONF_TH] = np.nan; xy[~fr] = np.nan | |
| with np.errstate(invalid="ignore"): | |
| head = np.nanmean(xy[:, [NOSE, LEYE, REYE, LEAR, REAR]], axis=1) | |
| head[np.isnan(xy[:, NOSE, 0])] = np.nan | |
| sho_mid = (xy[:, LSHO] + xy[:, RSHO]) / 2; sw = np.linalg.norm(xy[:, LSHO] - xy[:, RSHO], axis=1); sw = np.where(sw < 1e-3, np.nan, sw) | |
| ht = (xy[:, NOSE, 0] - sho_mid[:, 0]) / sw | |
| q = np.stack([xy[:, LWRI, 0], xy[:, LWRI, 1], xy[:, RWRI, 0], xy[:, RWRI, 1], ht], 1).astype(np.float32) | |
| V = fr & (np.nan_to_num(cf[:, [NOSE, LSHO, RSHO, LWRI, RWRI]]) >= CONF_TH).all(1) & np.isfinite(xy[:, [NOSE, LSHO, RSHO, LWRI, RWRI], 0]).all(1) | |
| return dict(q=q, head=head, V=V, F=fr, t0=float(d["t0"])) | |
| def clip_series(raw, ref): | |
| """5 Hz channels [75,D] from raw quantities + per-frame references [75,5].""" | |
| q, head = raw["q"], raw["head"] | |
| G = np.full((len(q), D), np.nan, np.float32) # 75 for clip windows, N for whole routes | |
| G[:, 0], G[:, 1] = head[:, 0], head[:, 1] | |
| G[:, 2], G[:, 3], G[:, 4], G[:, 5] = q[:, 0], q[:, 1], q[:, 2], q[:, 3] | |
| G[:, 6] = q[:, 4]; G[:, 7] = q[:, 4] - ref[:, 4] | |
| G[:, 8] = np.hypot(q[:, 0] - ref[:, 0], q[:, 1] - ref[:, 1]); G[:, 9] = np.hypot(q[:, 2] - ref[:, 2], q[:, 3] - ref[:, 3]) | |
| G[:, 10] = np.fmin(G[:, 8], G[:, 9]) | |
| return G | |
| def to_10hz(G5, V5, F5): | |
| """[75,·] 5 Hz -> [151,·] 10 Hz: even j = sample k=j/2; odd j = linear interpolation (both neighbours needed).""" | |
| G = np.full((NJ, D), np.nan, np.float32); V = np.zeros(NJ, bool); F = np.zeros(NJ, bool); I = np.zeros(NJ, bool) | |
| G[0::2] = G5; V[0::2] = V5; F[0::2] = F5 | |
| mid = (G5[:-1] + G5[1:]) / 2; G[1::2] = mid # NaN if either neighbour NaN | |
| V[1::2] = V5[:-1] & V5[1:]; F[1::2] = F5[:-1] & F5[1:]; I[1::2] = True | |
| return G, V, F, I | |
| def onsets(z0, z): | |
| zp = np.concatenate([z0[:, None], z[:, :-1]], 1); em = np.isfinite(zp) & np.isfinite(z) | |
| return (em & (zp < 0.5) & (z > 0.5)).astype(np.uint8), em | |
| def main_clips(): | |
| ap = argparse.ArgumentParser(); ap.add_argument("--source", default="clips"); ap.add_argument("--regimes", default="manual,engaged"); ap.add_argument("--splits", default="train,val,test"); ap.add_argument("--limit-clips", type=int, default=0) | |
| a = ap.parse_args(); t_start = time.time(); OUT.mkdir(parents=True, exist_ok=True) | |
| old = json.load(open(OLD_STATS)); head_on, head_off = float(old["head_turn_on"]), float(old["head_turn_off"]) | |
| cm = json.load(open(CLIP_MAP)) | |
| cids = sorted(c for c in cm if (POSE / f"{c}.npz").exists()) | |
| if a.limit_clips: cids = cids[:a.limit_clips] | |
| print(f"clips with causal pose: {len(cids)}/{len(cm)} head thresholds (old TRAIN fit) on={head_on:.4f} off={head_off:.4f}", flush=True) | |
| # ---- per-device chronological processing with causal reference pools | |
| by_dev = defaultdict(list) | |
| for c in cids: by_dev[cm[c]["dongle"]].append(c) | |
| C = len(cids); cindex = {c: i for i, c in enumerate(cids)} | |
| G10 = np.full((C, NJ, D), np.nan, np.float32); V10 = np.zeros((C, NJ), bool); F10 = np.zeros((C, NJ), bool); I10 = np.zeros((C, NJ), bool) | |
| HON = np.full((C, NJ), np.nan, np.float32); HT = np.full((C, NJ), np.nan, np.float32) | |
| REF = np.full((C, NK, 5), np.nan, np.float32); NPOOL = np.zeros((C, 5), np.int32); NPRIOR = np.zeros(C, np.int32); T0 = np.zeros(C); UID = np.empty(C, object) | |
| n_ref_undef = 0; t_last = time.time() | |
| for di, (dev, dc) in enumerate(sorted(by_dev.items())): | |
| dc.sort(key=lambda c: (route_key(cm[c]["uid"]), cm[c]["t0"], c)) | |
| done = [] # (route_key, t0, q_valid_lists[5]) of processed clips of this device | |
| for c in dc: | |
| raw = clip_raw(c); rk = route_key(cm[c]["uid"]); t0 = raw["t0"]; i = cindex[c] | |
| pools = [[] for _ in range(5)]; nprior = 0 | |
| for rk2, t02, qv in done: | |
| if rk2 < rk or (rk2 == rk and t02 + 10.0 <= t0): | |
| nprior += 1 | |
| for j in range(5): pools[j].append(qv[j]) | |
| ref = np.full((NK, 5), np.nan, np.float32) | |
| for j in range(5): | |
| pool = np.concatenate(pools[j])[-POOL_CAP:] if pools[j] else np.zeros(0, np.float32) | |
| pool = np.sort(pool); NPOOL[i, j] = len(pool) | |
| ref[:, j] = expanding_median(pool, raw["q"][:, j]) | |
| n_ref_undef += int(np.isnan(ref[:, 4]).sum()) | |
| REF[i] = ref; NPRIOR[i] = nprior; T0[i] = t0; UID[i] = cm[c]["uid"] | |
| G5 = clip_series(raw, ref) | |
| V5 = raw["V"] & np.isfinite(G5[:, [0, 6, 7, 10]]).all(1) # + finite head_turn (shoulder width > 1e-3) + causal reference defined | |
| G, V, F, I = to_10hz(G5, V5, raw["F"]) | |
| G = G.astype(np.float16).astype(np.float32) # states below are derived from the STORED (f16) values | |
| G10[i], V10[i], F10[i], I10[i] = G, V, F, I | |
| HON[i] = np.where(np.isnan(G[:, 10]), np.nan, (G[:, 10] < WHEEL_TH).astype(np.float32)) | |
| HT[i] = hysteresis(np.abs(G[:, 7]), head_on, head_off) | |
| qv = [raw["q"][:, j][np.isfinite(raw["q"][:, j])].astype(np.float32) for j in range(5)] | |
| done.append((rk, t0, qv)) | |
| if time.time() - t_last > 60: t_last = time.time(); print(f" devices {di+1}/{len(by_dev)} clips {sum(1 for x in UID if x is not None)}/{C} {time.time()-t_start:.0f}s", flush=True) | |
| np.savez_compressed(OUT / "clip_refs.npz", clip_id=np.array(cids), uid=np.array(UID.tolist()), t0=T0, ref=REF, ref_quantities=np.array(REFQ), n_pool=NPOOL, n_prior_clips=NPRIOR, | |
| rule=np.array(f"expanding median over own frames <= current time, initialised from device clips on earlier routes or same-route clips with t0'+10 <= t0; POOL_CAP={POOL_CAP} most recent values; N_MIN={N_MIN}"), units=np.array(UNITS)) | |
| print(f"series built for {C} clips in {time.time()-t_start:.0f}s; ref-undefined frames (head_turn) {n_ref_undef}/{C*NK} ({n_ref_undef/(C*NK)*100:.1f}%); clips with no prior pool {(NPRIOR==0).sum()}", flush=True) | |
| # ---- anchors | |
| by_uid = defaultdict(list) | |
| for c in cids: by_uid[cm[c]["uid"]].append((cm[c]["t0"], cindex[c])) | |
| for u in by_uid: by_uid[u].sort() | |
| offs_f = np.arange(1, H + 1) * 2; offs_h = np.arange(-(HIST - 1), 1) * 2 | |
| stats = dict(head_turn_on=head_on, head_turn_off=head_off, thresholds_source=str(OLD_STATS), n_clips=C, pool_cap=POOL_CAP, n_min=N_MIN, | |
| ref_undefined_frame_frac=n_ref_undef / (C * NK), clips_without_prior_pool=int((NPRIOR == 0).sum()), units=UNITS, splits={}) | |
| for reg in a.regimes.split(","): | |
| for sp in a.splits.split(","): | |
| d = np.load(S12R / f"targets_{reg}_{sp}.npz", allow_pickle=True); uid, don, t, Mc = d["uid"], d["dongle"], d["t"], d["M"].astype(bool); N = len(t) | |
| cidx = np.full(N, -1, np.int64); dt = np.full(N, np.nan, np.float32); n_multi = 0 | |
| order = np.argsort(uid, kind="stable"); bounds = np.flatnonzero(np.r_[1, uid[order][1:] != uid[order][:-1], 1]) | |
| for a_, b_ in zip(bounds[:-1], bounds[1:]): | |
| rows = order[a_:b_]; u = uid[rows[0]] | |
| if u not in by_uid: continue | |
| t0s = np.array([x[0] for x in by_uid[u]]); ci = np.array([x[1] for x in by_uid[u]]); tt = t[rows] | |
| hit = (tt[:, None] >= t0s[None] - 1.0 - 1e-9) & (tt[:, None] <= t0s[None] + 5.0 + 1e-9) | |
| n_multi += int((hit.sum(1) > 1).sum()) | |
| dist = np.where(hit, np.abs(tt[:, None] - t0s[None]), np.inf); best = dist.argmin(1); ok = hit.any(1) | |
| cidx[rows[ok]] = ci[best[ok]]; dt[rows[ok]] = (tt - t0s[best])[ok] | |
| row_ok = cidx >= 0; R = np.flatnonzero(row_ok); ci = cidx[R] | |
| ja = np.round((t[R] - (T0[ci] - 5.0)) * 10).astype(np.int64) | |
| assert (ja - 2 * (HIST - 1) >= 0).all() and (ja <= NJ - 1).all(), "anchor outside the usable window" | |
| n_trunc = int((ja + 2 * H > NJ - 1).sum()) | |
| def gather(A, offs): | |
| idx = ja[:, None] + offs[None, :]; inside = idx <= NJ - 1 | |
| out = A[ci[:, None], np.clip(idx, 0, NJ - 1)] | |
| fill = np.nan if A.dtype.kind == "f" else False | |
| return np.where(inside[..., None] if out.ndim == 3 else inside, out, fill) | |
| K = np.full((N, H, D), np.nan, np.float16); Kh = np.full((N, HIST, D), np.nan, np.float16); Kvalid = np.zeros((N, H), bool); Kframe = np.zeros((N, H), bool); interp = np.zeros((N, H), bool) | |
| Khvalid = np.zeros((N, HIST), bool); interp_h = np.zeros((N, HIST), bool); K0 = np.full((N, D), np.nan, np.float32); K0valid = np.zeros(N, bool); interp0 = np.zeros(N, bool) | |
| S = np.full((N, H, 2), np.nan, np.float32); S0 = np.full((N, 2), np.nan, np.float32) | |
| if len(R): | |
| K[R] = gather(G10, offs_f).astype(np.float16); Kh[R] = gather(G10, offs_h).astype(np.float16) | |
| Kvalid[R] = gather(V10, offs_f); Kframe[R] = gather(F10, offs_f) | |
| Khvalid[R] = gather(V10, offs_h) | |
| K0[R] = G10[ci, ja]; K0valid[R] = V10[ci, ja]; interp0[R] = (ja % 2) == 1 | |
| interp[R] = np.repeat(interp0[R][:, None], H, 1); interp_h[R] = np.repeat(interp0[R][:, None], HIST, 1) # 0.2 s steps keep the anchor's grid parity | |
| S[R, :, 0] = gather(HON, offs_f); S[R, :, 1] = gather(HT, offs_f); S0[R, 0] = HON[ci, ja]; S0[R, 1] = HT[ci, ja] | |
| n_kvalid_raw = int(Kvalid.sum()); Kvalid &= Mc; S[~Mc] = np.nan | |
| E = np.zeros((N, H, 3), np.uint8); EM = np.zeros((N, H, 3), bool); E0 = np.full((N, 3), np.nan, np.float32) | |
| E[:, :, 0], EM[:, :, 0] = onsets(S0[:, 0], S[:, :, 0]); E[:, :, 1], EM[:, :, 1] = onsets(1 - S0[:, 0], 1 - S[:, :, 0]); E[:, :, 2], EM[:, :, 2] = onsets(S0[:, 1], S[:, :, 1]) | |
| E0[:, 0], E0[:, 1], E0[:, 2] = S0[:, 0], 1 - S0[:, 0], S0[:, 1] | |
| assert np.array_equal(uid, d["uid"]) and np.allclose(t, d["t"]) and not (Kvalid & ~Mc).any() and not (EM & ~Mc[:, :, None]).any() | |
| clip_id = np.array([cids[i] if i >= 0 else "" for i in cidx]) | |
| np.savez_compressed(OUT / f"kin_causal_{reg}_{sp}.npz", uid=uid, dongle=don, t=t, row_ok=row_ok, clip_id=clip_id, dt_anchor=dt, K_c=K, Kvalid=Kvalid, Kframe=Kframe, interp=interp, | |
| K0_c=K0, K0valid=K0valid, interp0=interp0, S_c=S, S0_c=S0, E_c=E, EM_c=EM, E0_c=E0, channels=np.array(CHANNELS), event_names=np.array(EVENTS), state_names=np.array(STATES), units=np.array(UNITS)) | |
| np.savez_compressed(OUT / f"kinhist_causal_{reg}_{sp}.npz", uid=uid, t=t, row_ok=row_ok, Kh_c=Kh, Khvalid=Khvalid, interp_h=interp_h, channels=np.array(CHANNELS), units=np.array(UNITS)) | |
| nr = max(int(row_ok.sum()), 1) | |
| st = dict(N=int(N), routes=int(len(np.unique(uid))), routes_with_clips=int(sum(u in by_uid for u in np.unique(uid))), n_row_ok=int(row_ok.sum()), coverage_row_ok=float(row_ok.mean()), n_multi_clip_anchors=n_multi, n_rows_future_truncated=n_trunc, | |
| n_clips_used=int(len(np.unique(cidx[row_ok]))), Kvalid_frac_all_rows=float(Kvalid.mean()), Kvalid_frac_row_ok=float(Kvalid[row_ok].mean()) if row_ok.any() else None, | |
| Kvalid_all25_frac_row_ok=float(Kvalid[row_ok].all(1).mean()) if row_ok.any() else None, Kvalid_dropped_by_s12_M=int(n_kvalid_raw - Kvalid.sum()), | |
| Kframe_frac_row_ok=float(Kframe[row_ok].mean()) if row_ok.any() else None, K0valid_frac_row_ok=float(K0valid[row_ok].mean()) if row_ok.any() else None, | |
| Khvalid_frac_row_ok=float(Khvalid[row_ok].mean()) if row_ok.any() else None, Khvalid_all20_frac_row_ok=float(Khvalid[row_ok].all(1).mean()) if row_ok.any() else None, | |
| interp_frac_row_ok=float(interp[row_ok].mean()) if row_ok.any() else None, dt_anchor_hist={f"{lo}..{lo+1}": int(((dt >= lo) & (dt < lo + 1)).sum()) for lo in range(-1, 5)}, | |
| hands_on_prev=float(np.nanmean(S[:, :, 0])) if np.isfinite(S[:, :, 0]).any() else None, head_turned_prev=float(np.nanmean(S[:, :, 1])) if np.isfinite(S[:, :, 1]).any() else None, | |
| channel_nan_frac_row_ok={c: float(np.isnan(K[row_ok][:, :, i].astype(np.float32)).mean()) for i, c in enumerate(CHANNELS)} if row_ok.any() else None, events={}) | |
| chains = [(S0[:, 0], S[:, :, 0]), (1 - S0[:, 0], 1 - S[:, :, 0]), (S0[:, 1], S[:, :, 1])] | |
| for j, name in enumerate(EVENTS): | |
| z0, z = chains[j]; zp = np.concatenate([z0[:, None], z[:, :-1]], 1); m = EM[:, :, j]; m_tr = m & (zp < 0.5); inact0 = E0[:, j] < 0.5 | |
| rec = dict(n_inactive_at_anchor=int(inact0.sum()), hazard_per_step=float(E[:, :, j][m_tr].mean()) if m_tr.any() else None) | |
| for tau, k in (("1s", 5), ("3s", 15), ("5s", 25)): | |
| sel = inact0 & m[:, :k].all(1); y = E[sel, :k, j].any(1); rec[f"n@{tau}"] = int(sel.sum()); rec[f"pos@{tau}"] = int(y.sum()); rec[f"base_rate@{tau}"] = float(y.mean()) if sel.any() else None | |
| st["events"][name] = rec | |
| stats["splits"][f"{reg}_{sp}"] = st | |
| print(f"{reg}/{sp}: N={N} row_ok={row_ok.sum()} ({row_ok.mean()*100:.2f}%) clips_used={st['n_clips_used']} Kvalid|row_ok={st['Kvalid_frac_row_ok']} hands_on={st['hands_on_prev']} head_turned={st['head_turned_prev']} " | |
| + " ".join(f"{e}@3s={st['events'][e]['pos@3s']}/{st['events'][e]['n@3s']}" for e in EVENTS), flush=True) | |
| stamp = time.strftime("%Y%m%dT%H%M%SZ", time.gmtime()) | |
| (OUT / f"build_stats_{stamp}.json").write_text(json.dumps(stats, indent=1)); (OUT / "build_stats_latest.json").write_text(json.dumps(stats, indent=1)) | |
| write_run_json(OUT, "build_kin_causal", [Path(__file__)], [S12R / f"targets_{r}_{s}.npz" for r in a.regimes.split(",") for s in a.splits.split(",")] + [OLD_STATS, CLIP_MAP], None, t_start, | |
| extra=dict(summary={k: v for k, v in stats.items() if k != "splits"}, coverage={k: dict(N=v["N"], n_row_ok=v["n_row_ok"], coverage_row_ok=v["coverage_row_ok"], routes_with_clips=v["routes_with_clips"], routes=v["routes"]) for k, v in stats["splits"].items()}, | |
| n_pose_npz_inputs=C, pose_dir=str(POSE), stats_file=f"build_stats_{stamp}.json"), input_listing=True) | |
| print("KIN_CAUSAL_DONE", flush=True) | |
| # ===================================================================================================================== | |
| # --source routes : same targets from the ROUTE-LEVEL causal pose (results/pose_causal_routes/<uid>.npz, 10 Hz, causal box) | |
| # for ALL s12 rows. Histories Kh_c[N,40,D] (t-3.9..t) and Kh8_c[N,80,D] (t-7.9..t) at 10 Hz; no interpolation needed. | |
| # Causal reference: expanding per-route medians over MOVING frames <= t (pose npz `moving` = grid10 vEgo > 2 m/s), initialised | |
| # from the same device's earlier routes (route_key order; valid moving-frame values; POOL_CAP most recent); undefined < N_MIN. | |
| # Output dir results/targets_kin_causal_routes/ (the clip-window build in results/targets_kin_causal/ is left untouched). | |
| # ===================================================================================================================== | |
| POSE_R = ROOT / "results" / "pose_causal_routes"; OUT_R = ROOT / "results" / "targets_kin_causal_routes" | |
| HIST8 = 80 | |
| def raw_from_kpts(k16, frame_ok): | |
| """kpts f16[N,136,3] normalised -> raw quantities in old crop-normalised units (as clip_raw).""" | |
| k = k16.astype(np.float32)[:, :17]; xy, cf = k[:, :, :2].copy(), k[:, :, 2] | |
| xy[:, :, 0] *= XSCALE; xy[cf < CONF_TH] = np.nan; xy[~frame_ok] = np.nan | |
| with np.errstate(invalid="ignore"): head = np.nanmean(xy[:, [NOSE, LEYE, REYE, LEAR, REAR]], axis=1) | |
| head[np.isnan(xy[:, NOSE, 0])] = np.nan | |
| sho_mid = (xy[:, LSHO] + xy[:, RSHO]) / 2; sw = np.linalg.norm(xy[:, LSHO] - xy[:, RSHO], axis=1); sw = np.where(sw < 1e-3, np.nan, sw) | |
| ht = (xy[:, NOSE, 0] - sho_mid[:, 0]) / sw | |
| q = np.stack([xy[:, LWRI, 0], xy[:, LWRI, 1], xy[:, RWRI, 0], xy[:, RWRI, 1], ht], 1).astype(np.float32) | |
| V = frame_ok & (np.nan_to_num(cf[:, [NOSE, LSHO, RSHO, LWRI, RWRI]]) >= CONF_TH).all(1) & np.isfinite(xy[:, [NOSE, LSHO, RSHO, LWRI, RWRI], 0]).all(1) | |
| return dict(q=q, head=head, V=V, F=frame_ok) | |
| def expanding_median_moving(pool, vals, moving): | |
| """pool: 1-D array (device's earlier routes); vals[N] with NaN; moving[N] bool -> ref[N] (causal, expanding over moving frames <= t).""" | |
| arr = sorted(pool.tolist()); out = np.full(len(vals), np.nan, np.float32); last = np.nan | |
| if len(arr) >= N_MIN: n = len(arr); last = arr[n // 2] if n % 2 else 0.5 * (arr[n // 2 - 1] + arr[n // 2]) | |
| for k in range(len(vals)): | |
| v = vals[k] | |
| if moving[k] and np.isfinite(v): | |
| bisect.insort(arr, float(v)); n = len(arr) | |
| if n >= N_MIN: last = arr[n // 2] if n % 2 else 0.5 * (arr[n // 2 - 1] + arr[n // 2]) | |
| out[k] = last | |
| return out | |
| def main_routes(): | |
| global OUT_R | |
| ap = argparse.ArgumentParser(); ap.add_argument("--source", default="routes"); ap.add_argument("--regimes", default="manual,engaged"); ap.add_argument("--splits", default="train,val,test"); ap.add_argument("--limit-routes", type=int, default=0); ap.add_argument("--out-dir", default=""); ap.add_argument("--pose-dir", default="", help="preferred pose dir (e.g. results/pose_causal_routes_v2); routes missing there fall back to results/pose_causal_routes") | |
| a = ap.parse_args(); t_start = time.time() | |
| if a.out_dir: OUT_R = Path(a.out_dir) | |
| OUT_R.mkdir(parents=True, exist_ok=True) | |
| old = json.load(open(OLD_STATS)); head_on, head_off = float(old["head_turn_on"]), float(old["head_turn_off"]) | |
| uids = sorted(set().union(*[set(np.unique(np.load(S12R / f"targets_{r}_{s}.npz", allow_pickle=True)["uid"]).tolist()) for r in ("manual", "engaged") for s in S12_SPLITS])) | |
| P2 = Path(a.pose_dir) if a.pose_dir else None | |
| def pose_path(u): | |
| if P2 is not None and (P2 / f"{u}.npz").exists(): return P2 / f"{u}.npz" | |
| return POSE_R / f"{u}.npz" | |
| have = [u for u in uids if pose_path(u).exists()] | |
| n_from_v2 = sum(1 for u in have if P2 is not None and (P2 / f"{u}.npz").exists()) | |
| if a.limit_routes: have = have[:a.limit_routes] | |
| print(f"s12 routes {len(uids)}, with causal route pose {len(have)}; head thresholds on={head_on:.4f} off={head_off:.4f}", flush=True) | |
| by_dev = defaultdict(list) | |
| for u in have: by_dev[u.split("__")[1]].append(u) | |
| series = {}; refs = {}; n_ref_undef = 0; n_frames_total = 0; n_no_pool = 0; t_last = time.time() | |
| for di, (dev, dus) in enumerate(sorted(by_dev.items())): | |
| dus.sort(key=route_key); pools = [np.zeros(0, np.float32) for _ in range(5)] | |
| for u in dus: | |
| z = np.load(pose_path(u)); t = z["t"].astype(np.float64); F = np.isfinite(z["box"][:, 0]) & np.isfinite(z["kpts"][:, 0, 2].astype(np.float32)); moving = z["moving"].astype(bool) | |
| raw = raw_from_kpts(z["kpts"], F); q = raw["q"] | |
| if all(len(p_) == 0 for p_ in pools): n_no_pool += 1 | |
| ref = np.stack([expanding_median_moving(pools[j], q[:, j], moving) for j in range(5)], 1) | |
| n_ref_undef += int(np.isnan(ref[:, 4]).sum()); n_frames_total += len(t) | |
| G5 = clip_series(raw, ref) # [N,D] (name kept; works for any N) | |
| V = raw["V"] & np.isfinite(G5[:, [0, 6, 7, 10]]).all(1) | |
| G = G5.astype(np.float16).astype(np.float32) | |
| k = np.round(t * 10).astype(np.int64); k0, k1 = int(k.min()), int(k.max()); M = k1 - k0 + 1 | |
| GG = np.full((M, D), np.nan, np.float32); VV = np.zeros(M, bool); FF = np.zeros(M, bool) | |
| GG[k - k0] = G; VV[k - k0] = V; FF[k - k0] = F | |
| hon = np.where(np.isnan(GG[:, 10]), np.nan, (GG[:, 10] < WHEEL_TH).astype(np.float32)); ht = hysteresis(np.abs(GG[:, 7]), head_on, head_off) | |
| series[u] = dict(k0=k0, G=GG, V=VV, F=FF, hon=hon, ht=ht); refs[u] = ref.astype(np.float32) | |
| for j in range(5): | |
| v = q[:, j][moving & np.isfinite(q[:, j])]; pools[j] = np.concatenate([pools[j], v.astype(np.float32)])[-POOL_CAP:] | |
| if time.time() - t_last > 60: t_last = time.time(); print(f" devices {di+1}/{len(by_dev)} routes {len(series)}/{len(have)} {time.time()-t_start:.0f}s", flush=True) | |
| np.savez_compressed(OUT_R / "route_refs.npz", uids=np.array(list(refs)), ref_quantities=np.array(REFQ), **{f"ref__{u}": r for u, r in refs.items()}, | |
| rule=np.array(f"expanding median over MOVING frames (grid10 vEgo > 2 m/s) <= t, initialised from the device's earlier routes (route_key order), POOL_CAP={POOL_CAP}, N_MIN={N_MIN}"), units=np.array(UNITS)) | |
| print(f"series for {len(series)} routes in {time.time()-t_start:.0f}s; ref-undefined frames {n_ref_undef}/{n_frames_total} ({100*n_ref_undef/max(n_frames_total,1):.2f}%); routes without prior pool {n_no_pool}", flush=True) | |
| offs_f = np.arange(1, H + 1) * 2; offs_h = np.arange(-(40 - 1), 1); offs_h8 = np.arange(-(HIST8 - 1), 1) | |
| stats = dict(source="routes", pose_dir=str(P2) if P2 else str(POSE_R), pose_fallback_dir=str(POSE_R), n_routes_from_pose_dir=n_from_v2, head_turn_on=head_on, head_turn_off=head_off, n_routes_s12=len(uids), n_routes_with_pose=len(have), ref_undefined_frame_frac=n_ref_undef / max(n_frames_total, 1), routes_without_prior_pool=n_no_pool, pool_cap=POOL_CAP, n_min=N_MIN, units=UNITS, splits={}) | |
| for reg in a.regimes.split(","): | |
| for sp in a.splits.split(","): | |
| d = np.load(S12R / f"targets_{reg}_{sp}.npz", allow_pickle=True); uid, don, t, Mc = d["uid"], d["dongle"], d["t"], d["M"].astype(bool); N = len(t) | |
| K = np.full((N, H, D), np.nan, np.float16); Kh = np.full((N, 40, D), np.nan, np.float16); Kh8 = np.full((N, HIST8, D), np.nan, np.float16) | |
| Kvalid = np.zeros((N, H), bool); Kframe = np.zeros((N, H), bool); Khvalid = np.zeros((N, 40), bool); Kh8valid = np.zeros((N, HIST8), bool) | |
| K0 = np.full((N, D), np.nan, np.float32); K0valid = np.zeros(N, bool); S = np.full((N, H, 2), np.nan, np.float32); S0 = np.full((N, 2), np.nan, np.float32) | |
| route_dec = np.zeros(N, bool); row_ok = np.zeros(N, bool) | |
| order = np.argsort(uid, kind="stable"); bounds = np.flatnonzero(np.r_[1, uid[order][1:] != uid[order][:-1], 1]) | |
| for a_, b_ in zip(bounds[:-1], bounds[1:]): | |
| rows = order[a_:b_]; u = uid[rows[0]] | |
| if u not in series: continue | |
| rs = series[u]; route_dec[rows] = True; ka = np.round(t[rows] * 10).astype(np.int64) - rs["k0"]; M = len(rs["G"]) | |
| def gather(A, offs): | |
| idx = ka[:, None] + offs[None, :]; ok = (idx >= 0) & (idx < M); out = A[np.clip(idx, 0, M - 1)] | |
| return np.where(ok[..., None] if out.ndim == 3 else ok, out, np.nan if A.dtype.kind == "f" else False) | |
| K[rows] = gather(rs["G"], offs_f).astype(np.float16); Kh[rows] = gather(rs["G"], offs_h).astype(np.float16); Kh8[rows] = gather(rs["G"], offs_h8).astype(np.float16) | |
| Kvalid[rows] = gather(rs["V"], offs_f); Kframe[rows] = gather(rs["F"], offs_f); Khvalid[rows] = gather(rs["V"], offs_h); Kh8valid[rows] = gather(rs["V"], offs_h8) | |
| ok0 = (ka >= 0) & (ka < M); kc = np.clip(ka, 0, M - 1) | |
| K0[rows] = np.where(ok0[:, None], rs["G"][kc], np.nan); K0valid[rows] = ok0 & rs["V"][kc]; row_ok[rows] = ok0 & rs["F"][kc] | |
| S[rows, :, 0] = gather(rs["hon"], offs_f); S[rows, :, 1] = gather(rs["ht"], offs_f); S0[rows, 0] = np.where(ok0, rs["hon"][kc], np.nan); S0[rows, 1] = np.where(ok0, rs["ht"][kc], np.nan) | |
| n_kvalid_raw = int(Kvalid.sum()); Kvalid &= Mc; S[~Mc] = np.nan | |
| E = np.zeros((N, H, 3), np.uint8); EM = np.zeros((N, H, 3), bool); E0 = np.full((N, 3), np.nan, np.float32) | |
| E[:, :, 0], EM[:, :, 0] = onsets(S0[:, 0], S[:, :, 0]); E[:, :, 1], EM[:, :, 1] = onsets(1 - S0[:, 0], 1 - S[:, :, 0]); E[:, :, 2], EM[:, :, 2] = onsets(S0[:, 1], S[:, :, 1]) | |
| E0[:, 0], E0[:, 1], E0[:, 2] = S0[:, 0], 1 - S0[:, 0], S0[:, 1] | |
| assert np.array_equal(uid, d["uid"]) and np.allclose(t, d["t"]) and not (Kvalid & ~Mc).any() and not (EM & ~Mc[:, :, None]).any() | |
| np.savez_compressed(OUT_R / f"kin_causal_{reg}_{sp}.npz", uid=uid, dongle=don, t=t, row_ok=row_ok, route_decoded=route_dec, K_c=K, Kvalid=Kvalid, Kframe=Kframe, K0_c=K0, K0valid=K0valid, | |
| S_c=S, S0_c=S0, E_c=E, EM_c=EM, E0_c=E0, channels=np.array(CHANNELS), event_names=np.array(EVENTS), state_names=np.array(STATES), units=np.array(UNITS), source=np.array("routes")) | |
| np.savez_compressed(OUT_R / f"kinhist_causal_{reg}_{sp}.npz", uid=uid, t=t, row_ok=row_ok, Kh_c=Kh, Khvalid=Khvalid, Kh8_c=Kh8, Kh8valid=Kh8valid, channels=np.array(CHANNELS), units=np.array(UNITS), source=np.array("routes")) | |
| st = dict(N=int(N), routes=int(len(np.unique(uid))), routes_decoded=int(len({u for u in np.unique(uid) if u in series})), n_route_decoded_rows=int(route_dec.sum()), coverage_route_decoded=float(route_dec.mean()), | |
| n_row_ok=int(row_ok.sum()), coverage_row_ok=float(row_ok.mean()), coverage_row_ok_given_decoded=float(row_ok[route_dec].mean()) if route_dec.any() else None, | |
| Kvalid_frac_row_ok=float(Kvalid[row_ok].mean()) if row_ok.any() else None, Kvalid_all25_frac_row_ok=float(Kvalid[row_ok].all(1).mean()) if row_ok.any() else None, Kvalid_dropped_by_s12_M=int(n_kvalid_raw - Kvalid.sum()), | |
| Khvalid_all40_frac_row_ok=float(Khvalid[row_ok].all(1).mean()) if row_ok.any() else None, Kh8valid_all80_frac_row_ok=float(Kh8valid[row_ok].all(1).mean()) if row_ok.any() else None, | |
| K0valid_frac_row_ok=float(K0valid[row_ok].mean()) if row_ok.any() else None, hands_on_prev=float(np.nanmean(S[:, :, 0])) if np.isfinite(S[:, :, 0]).any() else None, head_turned_prev=float(np.nanmean(S[:, :, 1])) if np.isfinite(S[:, :, 1]).any() else None, | |
| channel_nan_frac_row_ok={c: float(np.isnan(K[row_ok][:, :, i].astype(np.float32)).mean()) for i, c in enumerate(CHANNELS)} if row_ok.any() else None, events={}) | |
| chains = [(S0[:, 0], S[:, :, 0]), (1 - S0[:, 0], 1 - S[:, :, 0]), (S0[:, 1], S[:, :, 1])] | |
| for j, name in enumerate(EVENTS): | |
| z0, z = chains[j]; zp = np.concatenate([z0[:, None], z[:, :-1]], 1); m = EM[:, :, j]; m_tr = m & (zp < 0.5); inact0 = E0[:, j] < 0.5 | |
| rec = dict(n_inactive_at_anchor=int(inact0.sum()), hazard_per_step=float(E[:, :, j][m_tr].mean()) if m_tr.any() else None) | |
| for tau, kk in (("1s", 5), ("3s", 15), ("5s", 25)): | |
| sel = inact0 & m[:, :kk].all(1); y = E[sel, :kk, j].any(1); rec[f"n@{tau}"] = int(sel.sum()); rec[f"pos@{tau}"] = int(y.sum()); rec[f"base_rate@{tau}"] = float(y.mean()) if sel.any() else None | |
| st["events"][name] = rec | |
| stats["splits"][f"{reg}_{sp}"] = st | |
| print(f"{reg}/{sp}: N={N} route_decoded={route_dec.sum()} ({route_dec.mean()*100:.2f}%) row_ok={row_ok.sum()} ({row_ok.mean()*100:.2f}%) Kvalid|row_ok={st['Kvalid_frac_row_ok']} Kh8 all80={st['Kh8valid_all80_frac_row_ok']} hands_on={st['hands_on_prev']} head_turned={st['head_turned_prev']} " | |
| + " ".join(f"{e}@3s={st['events'][e]['pos@3s']}/{st['events'][e]['n@3s']}" for e in EVENTS), flush=True) | |
| stamp = time.strftime("%Y%m%dT%H%M%SZ", time.gmtime()) | |
| (OUT_R / f"build_stats_{stamp}.json").write_text(json.dumps(stats, indent=1)); (OUT_R / "build_stats_latest.json").write_text(json.dumps(stats, indent=1)) | |
| write_run_json(OUT_R, "build_kin_causal_routes", [Path(__file__)], [S12R / f"targets_{r}_{s}.npz" for r in a.regimes.split(",") for s in a.splits.split(",")] + [OLD_STATS], None, t_start, | |
| extra=dict(summary={k: v for k, v in stats.items() if k != "splits"}, coverage={k: dict(N=v["N"], route_decoded=v["coverage_route_decoded"], row_ok=v["coverage_row_ok"], routes_decoded=v["routes_decoded"], routes=v["routes"]) for k, v in stats["splits"].items()}, | |
| pose_dir=str(P2) if P2 else str(POSE_R), stats_file=f"build_stats_{stamp}.json"), input_listing=True) | |
| print("KIN_CAUSAL_ROUTES_DONE", flush=True) | |
| def main(): | |
| if "--source" in sys.argv and sys.argv[sys.argv.index("--source") + 1] == "routes": return main_routes() | |
| return main_clips() | |
| if __name__ == "__main__": | |
| main() | |
Xet Storage Details
- Size:
- 36.6 kB
- Xet hash:
- 82a112b30a31302ea4e22e4a32126822a63d9b6ead2979874c60654c83f0a917
·
Xet efficiently stores files, intelligently splitting them into unique chunks and accelerating uploads and downloads. More info.