AsianPlayer/TriDrive / code /data /build_face_causal.py
AsianPlayer's picture
download
raw
20.3 kB
"""cwm3/data/build_face_causal.py — CAUSAL face-geometry channels on the s12 anchor grid from OUR OWN RTMW HALPE-136 face landmarks of the
route-level causal pose (results/pose_causal_routes[_v2]/<uid>.npz; roadmap §B3 item 1b, 2026-09-12). Mirrors build_kin_causal.main_routes
(same route_key order, expanding_median_moving references with the device's earlier routes as the pool, POOL_CAP 4000, N_MIN 25, 10 Hz grid,
Fh8 = 80 steps t-7.9..t). Nothing here reads an openpilot DMS quantity; the channel names deliberately avoid the DMS vocabulary (`lm_` = landmark).
python data/build_face_causal.py --source routes --pose-dir results/pose_causal_routes_v2 [--out-dir results/targets_face_causal_routes_v2]
Landmarks (HALPE-136 = COCO-17 body 0-16 + ... + face 26-93 in the iBUG-68 order; verified on the stored npz: nose tip 56 = body nose 0, right eye
62-67 = body 2, left eye 68-73 = body 1): nose tip NT = 56; right eye RE = 62..67 (iBUG 36-41: p1 outer corner, p2 p3 upper lid, p4 inner corner,
p5 p6 lower lid), left eye LE = 68..73 (iBUG 42-47); ears = body 3 (left) / 4 (right). Coordinates x/W, y/H are put on a common H-unit scale
(x' = x·W/H) before distances / angles.
Channels F = 9 (`channels`), per 10 Hz frame:
lm_yaw (NT_x − eye_mid_x) / IOD eye_mid = midpoint of the two eye centres (mean of the >= 5 valid landmarks); IOD = |LE_c − RE_c|
lm_yaw_dev lm_yaw − REF_yaw REF = causal expanding per-route median over MOVING valid frames <= t, pool = the same device's
lm_pitch_dev (NT_y − eye_mid_y) / IOD − REF_pitch earlier routes (build_kin_causal.expanding_median_moving; NaN while |pool ∪ own| < N_MIN)
lm_roll_dev atan2(LE_c_y − RE_c_y, LE_c_x − RE_c_x) − REF_roll [rad]
ear_conf_asym conf(left ear) − conf(right ear) (a turned head hides one ear; confidences of body points 3 / 4, NaN -> 0)
eye_open mean EAR over the eyes with all 6 landmarks valid ÷ REF_EAR; EAR = (|p2−p6| + |p3−p5|) / (2·|p1−p4|)
blink_rate_4s blinks / s over the trailing 4 s; blink = eye_open < BLINK_TH (0.6) on 1..5 consecutive valid frames followed by a valid frame >= BLINK_TH
gaze_off hysteresis state on |lm_yaw_dev| (build_kin_causal.hysteresis, > 1 s gap resets) with on / off = the 90th / 75th percentile of
|lm_yaw_dev| over MOVING valid frames of the TRAIN-split routes (first pass; stored in build_stats / the npz `thresholds`)
gaze_off_dwell_s seconds since the gaze_off state last became 1 (0 when the state is 0; cap DWELL_CAP 30 s; NaN where the state is NaN)
Validity V(frame) = box present ∧ nose conf >= 0.3 ∧ >= 5 of 6 landmarks per eye with conf >= 0.3 ∧ IOD >= IOD_MIN_PX (10 px ≈ 0.005 W) ∧ finite
yaw / pitch / roll / EAR ∧ references defined. eye_open additionally needs one eye with all 6 landmarks valid (else NaN, V unchanged).
Outputs (row-aligned with s12_wm/results/targets_{regime}_{split}.npz; uid / t equality asserted) -> <out-dir>/facehist_causal_{regime}_{split}.npz:
uid, t, row_ok[N] (anchor inside the decoded route series with a frame present), F0_c f32[N,F] (NaN invalid), F0valid[N] (row_ok ∧ V at t),
Fh8_c f16[N,80,F] (t-7.9..t @ 10 Hz; NaN outside / invalid), Fh8valid[N,80], channels, units, source, thresholds (json), pose_dir
+ route_face_refs.npz (per-route REF series, 4 quantities), build_stats_<utc>.json (+ _latest), run_build_face_causal_<utc>.json.
"""
from __future__ import annotations
import argparse
import json
import sys
import time
from collections import defaultdict
from pathlib import Path
import numpy as np
ROOT = Path(__file__).resolve().parents[1]
sys.path.insert(0, str(ROOT / "common"))
sys.path.insert(0, str(ROOT / "data"))
from runmeta import write_run_json # noqa: E402
import build_kin_causal as BK # noqa: E402 (route_key, expanding_median_moving, hysteresis, POOL_CAP, N_MIN, S12R)
S12R = BK.S12R
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)
POSE_R = ROOT / "results" / "pose_causal_routes"
OUT_DEFAULT = ROOT / "results" / "targets_face_causal_routes_v2"
CONF_TH = 0.30
NT, LEAR, REAR = 56, 3, 4
RE = list(range(62, 68))
LE = list(range(68, 74))
IDX = [0, LEAR, REAR, NT] + RE + LE # the 16 keypoints read from every route
CHANNELS = ["lm_yaw", "lm_yaw_dev", "lm_pitch_dev", "lm_roll_dev", "ear_conf_asym", "eye_open", "blink_rate_4s", "gaze_off", "gaze_off_dwell_s"]
F = len(CHANNELS)
REFQ = ["lm_yaw", "lm_pitch", "lm_roll", "ear"]
IOD_MIN_PX, BLINK_TH, BLINK_MAX_FRAMES, BLINK_WIN, DWELL_CAP, HIST8 = 10.0, 0.6, 5, 40, 30.0, 80
GAZE_PCT_ON, GAZE_PCT_OFF = 90.0, 75.0
UNITS = "lm_yaw / lm_*_dev dimensionless (IOD-normalised; roll in rad), ear_conf_asym conf units, eye_open ratio to the route reference EAR, blink_rate_4s 1/s, gaze_off {0,1}, dwell s"
def raw_face(kp16, W, H):
"""kp16 [T,16,3] (IDX order) -> dict of per-frame raw quantities (H-unit coordinates)."""
k = kp16.astype(np.float32)
xy, cf = k[:, :, :2].copy(), np.nan_to_num(k[:, :, 2], nan=0.0)
xy[:, :, 0] *= float(W) / float(H)
ok = np.isfinite(xy).all(-1) & (cf >= CONF_TH)
T = len(k)
nose_ok = ok[:, 0] & ok[:, 3]
re_ok, le_ok = ok[:, 4:10], ok[:, 10:16]
n_re, n_le = re_ok.sum(1), le_ok.sum(1)
def centre(sl, m):
w = m.astype(np.float32)[:, :, None]
with np.errstate(invalid="ignore", divide="ignore"):
return (xy[:, sl] * w).sum(1) / np.maximum(w.sum(1), 1e-6)
re_c, le_c = centre(slice(4, 10), re_ok), centre(slice(10, 16), le_ok)
eyes_ok = (n_re >= 5) & (n_le >= 5)
d = le_c - re_c
iod = np.hypot(d[:, 0], d[:, 1])
iod_ok = eyes_ok & (iod * H >= IOD_MIN_PX)
mid = (re_c + le_c) / 2
with np.errstate(invalid="ignore", divide="ignore"):
yaw = (xy[:, 3, 0] - mid[:, 0]) / iod
pitch = (xy[:, 3, 1] - mid[:, 1]) / iod
roll = np.arctan2(d[:, 1], d[:, 0])
def ear(sl, m):
p = xy[:, sl] # p1..p6
all6 = m.all(1)
with np.errstate(invalid="ignore", divide="ignore"):
e = (np.hypot(*(p[:, 1] - p[:, 5]).T) + np.hypot(*(p[:, 2] - p[:, 4]).T)) / (2 * np.hypot(*(p[:, 0] - p[:, 3]).T))
return np.where(all6 & np.isfinite(e), e, np.nan)
e_r, e_l = ear(slice(4, 10), re_ok), ear(slice(10, 16), le_ok)
with np.errstate(invalid="ignore"):
ear_m = np.nanmean(np.stack([e_r, e_l], 1), 1)
V = nose_ok & iod_ok & np.isfinite(yaw) & np.isfinite(pitch) & np.isfinite(roll)
asym = cf[:, 1] - cf[:, 2]
q = np.stack([yaw, pitch, roll, ear_m], 1).astype(np.float32) # REFQ order
q[~V] = np.nan
return dict(q=q, asym=asym.astype(np.float32), V=V, T=T)
def blink_rate(eye_open, valid):
"""blinks / s over the trailing BLINK_WIN frames; a blink = a run of 1..BLINK_MAX_FRAMES consecutive frames with valid & eye_open < BLINK_TH followed by a
valid frame >= BLINK_TH (invalid frames break a run). NaN where the frame is invalid."""
T = len(eye_open)
low = valid & np.isfinite(eye_open) & (eye_open < BLINK_TH)
high = valid & np.isfinite(eye_open) & (eye_open >= BLINK_TH)
marks = np.zeros(T, np.float32)
d = np.diff(np.r_[0, low.astype(np.int8), 0])
starts, ends = np.flatnonzero(d == 1), np.flatnonzero(d == -1) # low run = [s, e)
for s, e in zip(starts, ends):
if 1 <= e - s <= BLINK_MAX_FRAMES and e < T and high[e]:
marks[e] = 1.0
c = np.cumsum(np.r_[0.0, marks])
k = np.arange(T)
cnt = c[k + 1] - c[np.maximum(k + 1 - BLINK_WIN, 0)]
out = (cnt / (BLINK_WIN / 10.0)).astype(np.float32)
out[~valid] = np.nan
return out
def dwell_of_state(state, hz=10.0, cap=DWELL_CAP):
"""seconds since the state last became 1 (0 while the state is 0); NaN where the state is NaN; run length counted over consecutive samples."""
T = len(state)
out = np.full(T, np.nan, np.float32)
run = 0.0
for i in range(T):
s = state[i]
if not np.isfinite(s):
continue
run = run + 1.0 if (s > 0.5 and run > 0) else (1.0 if s > 0.5 else 0.0)
out[i] = min((run - 1) / hz, cap) if run > 0 else 0.0
return out
def main():
ap = argparse.ArgumentParser()
ap.add_argument("--source", default="routes")
ap.add_argument("--pose-dir", default=str(ROOT / "results" / "pose_causal_routes_v2"), help="preferred pose dir; routes missing there fall back to results/pose_causal_routes")
ap.add_argument("--out-dir", default=str(OUT_DEFAULT))
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("--thresholds-from", default="", help="2026-09-16 WP5: facehist npz whose `thresholds` (gaze_off_on / gaze_off_off) are reused instead of recomputing TRAIN percentiles (holdout runs)")
a = ap.parse_args()
assert a.source == "routes", "only --source routes is implemented (route-level causal pose)"
t_start = time.time()
OUT = Path(a.out_dir)
OUT.mkdir(parents=True, exist_ok=True)
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"
regs, sps = a.regimes.split(","), a.splits.split(",")
uids_by_split = {sp: set() for sp in S12_SPLITS}; uids_by_split.setdefault("train", set())
for r in ("manual", "engaged"):
for sp in S12_SPLITS:
uids_by_split[sp] |= set(np.unique(np.load(S12R / f"targets_{r}_{sp}.npz", allow_pickle=True)["uid"]).tolist())
uids = sorted(set().union(*uids_by_split.values()))
have = [u for u in uids if pose_path(u).exists()]
n_from_p2 = 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)} ({n_from_p2} from {P2}); train-split routes {len(uids_by_split['train'])}", flush=True)
by_dev = defaultdict(list)
for u in have:
by_dev[u.split("__")[1]].append(u)
# ---- pass 1: per device in route order — raw quantities, causal references, deviation channels; collect TRAIN |yaw_dev| on moving valid frames
series, refs = {}, {}
n_ref_undef = n_frames = n_no_pool = n_valid = 0
train_absdev = []
t_last = time.time()
for di, (dev, dus) in enumerate(sorted(by_dev.items())):
dus.sort(key=BK.route_key)
pools = [np.zeros(0, np.float32) for _ in range(len(REFQ))]
for u in dus:
z = np.load(pose_path(u))
t = z["t"].astype(np.float64)
W, H = [int(v) for v in z["native_wh"]]
kp = z["kpts"][:, IDX]
frame = np.isfinite(z["box"][:, 0]) & np.isfinite(kp[:, 0, 2].astype(np.float32))
moving = z["moving"].astype(bool)
raw = raw_face(kp, W, H)
raw["V"] &= frame
raw["q"][~raw["V"]] = np.nan
q = raw["q"]
if all(len(p_) == 0 for p_ in pools):
n_no_pool += 1
ref = np.stack([BK.expanding_median_moving(pools[j], q[:, j], moving) for j in range(len(REFQ))], 1).astype(np.float32)
V = raw["V"] & np.isfinite(ref[:, 0]) & np.isfinite(ref[:, 1]) & np.isfinite(ref[:, 2])
n_ref_undef += int((raw["V"] & ~V).sum())
n_frames += len(t)
n_valid += int(V.sum())
G = np.full((len(t), F), np.nan, np.float32)
G[:, 0] = q[:, 0]
G[:, 1] = q[:, 0] - ref[:, 0]
G[:, 2] = q[:, 1] - ref[:, 1]
G[:, 3] = q[:, 2] - ref[:, 2]
G[:, 4] = np.where(frame, raw["asym"], np.nan)
with np.errstate(invalid="ignore", divide="ignore"):
G[:, 5] = np.where(np.isfinite(ref[:, 3]) & (ref[:, 3] > 1e-3), q[:, 3] / ref[:, 3], np.nan)
G[~V] = np.nan
G[:, 6] = blink_rate(G[:, 5], V)
if u in uids_by_split["train"]:
v = np.abs(G[moving & V, 1])
train_absdev.append(v[np.isfinite(v)].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, F), 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] = frame
series[u] = dict(k0=k0, G=GG, V=VV, F=FF)
refs[u] = ref
for j in range(len(REFQ)):
v = q[:, j][moving & np.isfinite(q[:, j])]
pools[j] = np.concatenate([pools[j], v.astype(np.float32)])[-BK.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)
absdev = np.concatenate(train_absdev) if train_absdev else np.zeros(0, np.float32)
gaze_on, gaze_off = (float(np.percentile(absdev, GAZE_PCT_ON)), float(np.percentile(absdev, GAZE_PCT_OFF))) if len(absdev) else (np.nan, np.nan)
thresholds_source = "train_split_percentiles"
if a.thresholds_from: # 2026-09-16 WP5 holdout: reuse the FROZEN train thresholds of an existing face pack
_fz = json.loads(str(np.load(a.thresholds_from, allow_pickle=True)["thresholds"])); gaze_on, gaze_off = float(_fz["gaze_off_on"]), float(_fz["gaze_off_off"]); thresholds_source = f"frozen:{a.thresholds_from}"
print(f"thresholds overridden from {a.thresholds_from}: on {gaze_on:.4f} off {gaze_off:.4f}", flush=True)
print(f"pass 1: {len(series)} routes, {n_frames} frames, valid {n_valid / max(n_frames, 1):.3f}, ref-undefined among valid {n_ref_undef}; TRAIN |yaw_dev| moving valid n={len(absdev)} -> "
f"gaze_off on (P{GAZE_PCT_ON:.0f}) {gaze_on:.4f} / off (P{GAZE_PCT_OFF:.0f}) {gaze_off:.4f} in {time.time() - t_start:.0f}s", flush=True)
# ---- pass 2: gaze_off hysteresis (TRAIN thresholds) + dwell on every route
for u, rs in series.items():
st = BK.hysteresis(np.abs(rs["G"][:, 1]), gaze_on, gaze_off)
rs["G"][:, 7] = st
rs["G"][:, 8] = dwell_of_state(st)
rs["G"] = rs["G"].astype(np.float16).astype(np.float32) # stored precision = f16 (anchors read the same values)
thresholds = dict(gaze_off_on=gaze_on, gaze_off_off=gaze_off, gaze_pct_on=GAZE_PCT_ON, gaze_pct_off=GAZE_PCT_OFF, n_train_frames_for_threshold=int(len(absdev)), thresholds_source=thresholds_source,
conf_th=CONF_TH, iod_min_px=IOD_MIN_PX, blink_th=BLINK_TH, blink_max_frames=BLINK_MAX_FRAMES, blink_win_frames=BLINK_WIN, dwell_cap_s=DWELL_CAP,
pool_cap=BK.POOL_CAP, n_min=BK.N_MIN, landmarks=dict(nose_tip=NT, right_eye=RE, left_eye=LE, left_ear=LEAR, right_ear=REAR))
np.savez_compressed(OUT / "route_face_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 valid frames <= t, initialised from the device's earlier routes (route_key order), POOL_CAP={BK.POOL_CAP}, N_MIN={BK.N_MIN}"))
stats = dict(source="routes", pose_dir=str(P2), pose_fallback_dir=str(POSE_R), n_routes_from_pose_dir=n_from_p2, n_routes_s12=len(uids), n_routes_with_pose=len(have),
n_frames=n_frames, valid_frame_frac=n_valid / max(n_frames, 1), ref_undefined_valid_frames=n_ref_undef, routes_without_prior_pool=n_no_pool,
thresholds=thresholds, channels=CHANNELS, units=UNITS, splits={})
offs_h8 = np.arange(-(HIST8 - 1), 1)
for reg in regs:
for sp in sps:
d = np.load(S12R / f"targets_{reg}_{sp}.npz", allow_pickle=True)
uid, t = d["uid"], d["t"]
N = len(t)
Fh8 = np.full((N, HIST8, F), np.nan, np.float16)
Fh8valid = np.zeros((N, HIST8), bool)
F0 = np.full((N, F), np.nan, np.float32)
F0valid = np.zeros(N, bool)
row_ok = np.zeros(N, bool)
route_dec = 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"])
idx = ka[:, None] + offs_h8[None, :]
ok = (idx >= 0) & (idx < M)
Gh = rs["G"][np.clip(idx, 0, M - 1)]
Fh8[rows] = np.where(ok[..., None], Gh, np.nan).astype(np.float16)
Fh8valid[rows] = np.where(ok, rs["V"][np.clip(idx, 0, M - 1)], False)
ok0 = (ka >= 0) & (ka < M)
kc = np.clip(ka, 0, M - 1)
F0[rows] = np.where(ok0[:, None], rs["G"][kc], np.nan)
F0valid[rows] = ok0 & rs["V"][kc]
row_ok[rows] = ok0 & rs["F"][kc]
F0[~F0valid] = np.nan
assert np.array_equal(uid, d["uid"]) and np.allclose(t, d["t"])
np.savez_compressed(OUT / f"facehist_causal_{reg}_{sp}.npz", uid=uid, t=t, row_ok=row_ok, route_decoded=route_dec, F0_c=F0, F0valid=F0valid, Fh8_c=Fh8, Fh8valid=Fh8valid,
channels=np.array(CHANNELS), units=np.array(UNITS), source=np.array("routes"), thresholds=np.array(json.dumps(thresholds)), pose_dir=np.array(str(P2)))
R = row_ok
st = dict(N=int(N), routes=int(len(np.unique(uid))), routes_decoded=int(len({x for x in np.unique(uid) if x in series})), coverage_route_decoded=float(route_dec.mean()),
n_row_ok=int(R.sum()), coverage_row_ok=float(R.mean()), F0valid_frac_row_ok=float(F0valid[R].mean()) if R.any() else None,
Fh8valid_frac_row_ok=float(Fh8valid[R].mean()) if R.any() else None, Fh8valid_all80_frac_row_ok=float(Fh8valid[R].all(1).mean()) if R.any() else None,
gaze_off_prev=float(np.nanmean(F0[F0valid, 7])) if F0valid.any() else None, eye_open_median=float(np.nanmedian(F0[F0valid, 5])) if F0valid.any() else None,
blink_rate_mean=float(np.nanmean(F0[F0valid, 6])) if F0valid.any() else None,
channel_nan_frac_F0valid={c: float(np.isnan(F0[F0valid][:, i]).mean()) for i, c in enumerate(CHANNELS)} if F0valid.any() else None)
stats["splits"][f"{reg}_{sp}"] = st
print(f"{reg}/{sp}: N={N} route_decoded={route_dec.mean() * 100:.2f}% row_ok={R.mean() * 100:.2f}% F0valid|row_ok={st['F0valid_frac_row_ok']} Fh8 all80={st['Fh8valid_all80_frac_row_ok']} "
f"gaze_off={st['gaze_off_prev']} eye_open med={st['eye_open_median']} blink/s={st['blink_rate_mean']}", 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_face_causal", [Path(__file__), Path(BK.__file__)], [S12R / f"targets_{r}_{s}.npz" for r in regs for s in sps], None, t_start,
extra=dict(summary={k: v for k, v in stats.items() if k != "splits"}, coverage={k: dict(N=v["N"], row_ok=v["coverage_row_ok"], F0valid=v["F0valid_frac_row_ok"]) for k, v in stats["splits"].items()},
pose_dir=str(P2), stats_file=f"build_stats_{stamp}.json", dms_fields_used=[]), input_listing=True)
print("FACE_CAUSAL_ROUTES_DONE", flush=True)
if __name__ == "__main__":
main()

Xet Storage Details

Size:
20.3 kB
·
Xet hash:
6da23323a0a243914d42c3407cf71bb32a550f749c0522aac31261ba99fb7813

Xet efficiently stores files, intelligently splitting them into unique chunks and accelerating uploads and downloads. More info.