diff --git a/data_only_viz/multi.py b/data_only_viz/multi.py index 43a0676..f543069 100644 --- a/data_only_viz/multi.py +++ b/data_only_viz/multi.py @@ -23,6 +23,7 @@ from .action_head_pub import ActionHeadPublisher from .euro_filter import SkeletonFilter from .pose_bridge import PoseSoundBridge from .pose_filter import PoseFilterChain +from .pose_filter import _is_finite # noqa: PLC2701 (intentional internal use) from .state import Kp3D, PoseKp, State from .tracker import IoUTracker @@ -99,6 +100,177 @@ class MultiWorker: self._action_pub.start() # 3D pose filter chain : median, Kalman CV, lookahead, IK clamps. self._filter_chain = PoseFilterChain(state=self.state) + # Discrimination state : per-pid frame counters for hysteresis. + # _pid_lifetime : frames since pid created (visible). + # _pid_last_bbox : last bbox seen for active pid (for re-association). + # _pid_missing : frames since pid disappeared (None when active). + self._pid_lifetime: dict[int, int] = {} + self._pid_missing: dict[int, int] = {} + self._pid_last_bbox: dict[int, tuple[float, float, float, float]] = {} + # Discrimination thresholds — tunable via env. + import os as _os + self._ghost_min_visible = int(_os.environ.get("POSE_GHOST_MIN_VISIBLE", "10")) + self._ghost_min_conf = float(_os.environ.get("POSE_GHOST_MIN_CONF", "0.5")) + self._hand_min_visible = int(_os.environ.get("POSE_HAND_MIN_VISIBLE", "15")) + self._face_min_visible = int(_os.environ.get("POSE_FACE_MIN_VISIBLE", "50")) + self._nms_iou = float(_os.environ.get("POSE_NMS_IOU", "0.7")) + # Counters exposed for debug. + self._n_ghost_dropped = 0 + self._n_hand_dropped = 0 + self._n_face_dropped = 0 + + # ------------------------------------------------------------------ + # Discrimination helpers — body ghost rejection, NMS, pid hysteresis, + # face/hand visibility gates. All return filtered (kps, ids) lists. + # ------------------------------------------------------------------ + @staticmethod + def _bbox_from_kps(kps: list) -> tuple[float, float, float, float]: + if not kps: + return (0.0, 0.0, 0.0, 0.0) + xs = [kp.x for kp in kps] + ys = [kp.y for kp in kps] + return (min(xs), min(ys), max(xs), max(ys)) + + @staticmethod + def _iou(a: tuple[float, float, float, float], + b: tuple[float, float, float, float]) -> float: + ix1 = max(a[0], b[0]); iy1 = max(a[1], b[1]) + ix2 = min(a[2], b[2]); iy2 = min(a[3], b[3]) + iw = max(0.0, ix2 - ix1); ih = max(0.0, iy2 - iy1) + inter = iw * ih + aw = max(0.0, a[2] - a[0]) * max(0.0, a[3] - a[1]) + bw = max(0.0, b[2] - b[0]) * max(0.0, b[3] - b[1]) + u = aw + bw - inter + return inter / u if u > 1e-9 else 0.0 + + def _reject_ghosts_and_nms( + self, + bodies: list[list], + bodies3d: list[list[Kp3D]], + ids_body: list[int], + ) -> tuple[list[list], list[list[Kp3D]], list[int]]: + """Drop body detections with = self._ghost_min_conf + and _is_finite(kp.x) and _is_finite(kp.y)) + if n_visible < self._ghost_min_visible: + keep_mask[i] = False + self._n_ghost_dropped += 1 + scores.append( + sum(kp.c for kp in kps) / len(kps) if kps else 0.0) + # NMS on remaining bboxes. + bboxes = [self._bbox_from_kps(kps) for kps in bodies] + order = sorted( + [i for i in range(len(bodies)) if keep_mask[i]], + key=lambda i: -scores[i]) + kept_order: list[int] = [] + for i in order: + drop = False + for j in kept_order: + if self._iou(bboxes[i], bboxes[j]) > self._nms_iou: + drop = True + break + if drop: + keep_mask[i] = False + else: + kept_order.append(i) + new_bodies = [bodies[i] for i in range(len(bodies)) if keep_mask[i]] + new_ids = [ids_body[i] for i in range(len(bodies)) + if i < len(ids_body) and keep_mask[i]] + # bodies3d aligned 1:1 with bodies. + new_b3d: list[list[Kp3D]] = [] + if bodies3d: + for i in range(min(len(bodies), len(bodies3d))): + if keep_mask[i]: + new_b3d.append(bodies3d[i]) + return new_bodies, new_b3d, new_ids + + def _apply_pid_hysteresis( + self, + bodies: list[list], + ids_body: list[int], + ) -> list[int]: + """Reuse a recently-disappeared pid when a young pid lands near + its last bbox. Mutates self._pid_lifetime / _pid_missing / + _pid_last_bbox in place. Returns possibly-remapped ids. + """ + # Tick all known pids missing counter ; will reset for visible ones. + for pid in list(self._pid_missing.keys()): + self._pid_missing[pid] += 1 + if self._pid_missing[pid] > 60: # forget after 2 s @30 fps + self._pid_missing.pop(pid, None) + self._pid_last_bbox.pop(pid, None) + self._pid_lifetime.pop(pid, None) + new_ids = list(ids_body) + for i, pid in enumerate(ids_body): + if pid < 0 or i >= len(bodies): + continue + bbox_i = self._bbox_from_kps(bodies[i]) + # If this pid is brand new (<10 frames) and we have an absent + # older pid (>=30 frames lifetime, <30 frames missing) with a + # close bbox, remap. + age = self._pid_lifetime.get(pid, 0) + if age < 10: + best_old: int | None = None + best_iou = 0.0 + for old_pid, miss in self._pid_missing.items(): + if old_pid == pid: + continue + if self._pid_lifetime.get(old_pid, 0) < 30: + continue + if miss > 30: + continue + old_bbox = self._pid_last_bbox.get(old_pid) + if old_bbox is None: + continue + iou = self._iou(bbox_i, old_bbox) + if iou > 0.3 and iou > best_iou: + best_iou = iou + best_old = old_pid + if best_old is not None: + new_ids[i] = best_old + pid = best_old + # Bookkeeping for visible pid. + self._pid_lifetime[pid] = self._pid_lifetime.get(pid, 0) + 1 + self._pid_missing.pop(pid, None) + self._pid_last_bbox[pid] = bbox_i + # Pids previously visible but absent this frame -> mark missing. + visible = set(new_ids) + for pid in list(self._pid_lifetime.keys()): + if pid not in visible and pid not in self._pid_missing: + self._pid_missing[pid] = 1 + return new_ids + + def _drop_low_visibility( + self, + kps_list: list[list], + ids: list[int], + min_visible: int, + which: str, + ) -> tuple[list[list], list[int]]: + out_kps: list[list] = [] + out_ids: list[int] = [] + for i, kps in enumerate(kps_list): + n_ok = sum( + 1 for kp in kps + if _is_finite(kp.x) and _is_finite(kp.y) + and (kp.x != 0.0 or kp.y != 0.0)) + if n_ok < min_visible: + if which == "face": + self._n_face_dropped += 1 + else: + self._n_hand_dropped += 1 + continue + out_kps.append(kps) + out_ids.append(ids[i] if i < len(ids) else -1) + return out_kps, out_ids def start(self) -> None: self._thread = threading.Thread( @@ -241,6 +413,14 @@ class MultiWorker: ids_body = self._tracker_body.update(bodies) ids_face = self._tracker_face.update(faces) ids_hand = self._tracker_hand.update(hands) + # --- Discrimination : ghost reject + NMS + pid hysteresis -- + bodies, bodies3d, ids_body = self._reject_ghosts_and_nms( + bodies, bodies3d, ids_body) + ids_body = self._apply_pid_hysteresis(bodies, ids_body) + faces, ids_face = self._drop_low_visibility( + faces, ids_face, self._face_min_visible, "face") + hands, ids_hand = self._drop_low_visibility( + hands, ids_hand, self._hand_min_visible, "hand") # --- Lissage One Euro par keypoint ------------------------- t_now = time.monotonic() bodies = [_smooth_kps(self._smooth_body, ids_body[i], kps, t_now) @@ -249,6 +429,9 @@ class MultiWorker: for i, kps in enumerate(faces)] hands = [_smooth_kps(self._smooth_hand, ids_hand[i], kps, t_now) for i, kps in enumerate(hands)] + # --- Filter chain face + hands (median + Kalman 2D + lookahead) + faces = self._filter_chain.apply_face(faces, ids_face, t_now) + hands = self._filter_chain.apply_hand(hands, ids_hand, None, t_now) # Pont sonore : envoi OSC /pose/* a sclang (body + face + hands) # 3D world landmarks share ids with bodies (same MediaPipe diff --git a/data_only_viz/multi_hmr_worker.py b/data_only_viz/multi_hmr_worker.py index 07e5271..c20c765 100644 --- a/data_only_viz/multi_hmr_worker.py +++ b/data_only_viz/multi_hmr_worker.py @@ -30,7 +30,9 @@ CACHE = Path.home() / ".cache" / "av-live-multihmr" CKPT = CACHE / "checkpoints" / "multiHMR_672_S.pt" SMPLX_PATH = CACHE / "models" / "smplx" / "SMPLX_NEUTRAL.npz" MULTIHMR_REPO = CACHE / "multi-hmr" -COREML_MLPACKAGE = CACHE / "multihmr_full_672_s.mlpackage" +COREML_MLPACKAGE = Path( + os.environ.get("COREML_MLPACKAGE") + or str(CACHE / "multihmr_full_672_s.mlpackage")) IMG_SIZE = 672 N_VERTS = 10475 diff --git a/data_only_viz/multihmr_coreml.py b/data_only_viz/multihmr_coreml.py index 3992800..43a5874 100644 --- a/data_only_viz/multihmr_coreml.py +++ b/data_only_viz/multihmr_coreml.py @@ -20,6 +20,7 @@ Public API: from __future__ import annotations import logging +import os from pathlib import Path from typing import Any @@ -161,12 +162,22 @@ class MultiHMRCoreMLBackend: MLModel = ns["MLModel"] MLModelConfiguration = ns["MLModelConfiguration"] cfg = MLModelConfiguration.alloc().init() + # MLComputeUnits: 0=CPUOnly, 1=CPUAndGPU, 2=All (ANE+GPU+CPU), + # 3=CPUAndNeuralEngine. Bench M5 2026-05-14 (under live-worker + # contention, 30 iter median, full Multi-HMR predict+copy): + # CPU_AND_GPU = 252 ms (baseline) + # ALL = 246 ms (within noise, ANE doesn't help) + # CPU_AND_NE = 1301 ms (ANE solo catastrophic) + # CPU_ONLY = 1152 ms + # Standalone (no contention) FP32 = 139 ms = 7.2 fps. Default + # stays CPU+GPU. Override with COREML_COMPUTE_UNITS env var + # (`all`, `cpu_and_gpu`, `cpu_and_ne`, `cpu_only`) for A/B testing. + cu_env = os.environ.get("COREML_COMPUTE_UNITS", "").strip().lower() + cu_map = {"cpu_only": 0, "cpu_and_gpu": 1, "all": 2, + "cpu_and_ne": 3} + cu = cu_map.get(cu_env, 1) try: - # MLComputeUnits: 0=CPUOnly, 1=CPUAndGPU, 2=All (ANE+GPU+CPU), - # 3=CPUAndNeuralEngine. Multi-HMR's ANEF compile fails - # (validated 2026-05-13 on M5), and 'All' falls back to a - # slow path (~146ms). CPU+GPU = 28ms = ~35fps on M5. - cfg.setComputeUnits_(1) + cfg.setComputeUnits_(cu) except Exception: # noqa: BLE001 pass url = NSURL.fileURLWithPath_(str(self.path)) @@ -182,8 +193,10 @@ class MultiHMRCoreMLBackend: raise RuntimeError(f"MLModel load failed for {compiled_url}") self._model = model self._ns = ns - LOG.info("Multi-HMR CoreML model loaded (%s, computeUnits=CPU+GPU)", - self.path.name) + cu_name = {0: "CPU_ONLY", 1: "CPU+GPU", 2: "ALL", 3: "CPU+NE"}.get( + cu, str(cu)) + LOG.info("Multi-HMR CoreML model loaded (%s, computeUnits=%s)", + self.path.name, cu_name) @staticmethod def is_available(mlpackage_path: Path | None = None) -> bool: diff --git a/data_only_viz/pose_filter.py b/data_only_viz/pose_filter.py index 837d54d..b922e3c 100644 --- a/data_only_viz/pose_filter.py +++ b/data_only_viz/pose_filter.py @@ -520,3 +520,211 @@ class PoseFilterChain: self.last_apply_ms = (time.perf_counter() - t0) * 1000.0 return out + + # ---- Face / hand smoothing entry points --------------------------- + def apply_face(self, faces: list[list], ids: list[int], + t_now: float) -> list[list]: + if not hasattr(self, "_face_chain"): + self._face_chain = FaceFilterChain() + return self._face_chain.apply(faces, ids, t_now) + + def apply_hand(self, hands: list[list], ids: list[int], + handedness: list[str] | None, + t_now: float) -> list[list]: + if not hasattr(self, "_hand_chain"): + self._hand_chain = HandFilterChain() + return self._hand_chain.apply(hands, ids, handedness, t_now) + + +# ============================ face / hand ================================= + +# Face and hand filtering operate on PoseKp lists (normalized x,y in [0,1] +# + z relative depth + confidence). We only apply temporal smoothing +# (median + Kalman 2D + lookahead) — no IK, no spring. + +def _parse_env_face_stages() -> tuple[str, ...]: + raw = os.environ.get("POSE_FILTER_FACE") + if raw is None: + return ("median", "kalman", "lookahead") + raw = raw.strip().lower() + if raw in ("off", "none", "0", "false"): + return () + parts = tuple(p.strip() for p in raw.replace(",", "+").split("+") if p.strip()) + return tuple(p for p in parts if p in ("median", "kalman", "lookahead")) + + +def _parse_env_hand_stages() -> tuple[str, ...]: + raw = os.environ.get("POSE_FILTER_HAND") + if raw is None: + return ("median", "kalman", "lookahead") + raw = raw.strip().lower() + if raw in ("off", "none", "0", "false"): + return () + parts = tuple(p.strip() for p in raw.replace(",", "+").split("+") if p.strip()) + return tuple(p for p in parts if p in ("median", "kalman", "lookahead")) + + +class AlphaBetaCV: + """Lightweight alpha-beta filter (scalar Kalman approximation). + + Far cheaper than the 6x6 KalmanCV : O(1) per joint per axis with no + matrix algebra. Suited to face/hand smoothing where the full CV + Kalman is overkill. + """ + + def __init__(self, alpha: float = 0.55, beta: float = 0.15) -> None: + self.alpha = alpha + self.beta = beta + # state[key] = [x, y, z, vx, vy, vz, last_t] + self._st: dict[tuple[int, int], list[float]] = {} + + def reset(self) -> None: + self._st.clear() + + def get_velocity(self, pid: int, joint_idx: int + ) -> tuple[float, float, float]: + s = self._st.get((pid, joint_idx)) + if s is None: + return (0.0, 0.0, 0.0) + return (s[3], s[4], s[5]) + + def step(self, pid: int, joint_idx: int, mx: float, my: float, + mz: float, t_now: float) -> tuple[float, float, float]: + key = (pid, joint_idx) + s = self._st.get(key) + if s is None: + self._st[key] = [mx, my, mz, 0.0, 0.0, 0.0, t_now] + return (mx, my, mz) + dt = max(1e-3, min(0.2, t_now - s[6])) + s[6] = t_now + # Predict + x_pred = s[0] + s[3] * dt + y_pred = s[1] + s[4] * dt + z_pred = s[2] + s[5] * dt + # Residual + rx = mx - x_pred + ry = my - y_pred + rz = mz - z_pred + # Update + s[0] = x_pred + self.alpha * rx + s[1] = y_pred + self.alpha * ry + s[2] = z_pred + self.alpha * rz + s[3] += (self.beta / dt) * rx + s[4] += (self.beta / dt) * ry + s[5] += (self.beta / dt) * rz + return (s[0], s[1], s[2]) + + +class FaceFilterChain: + """Per-pid temporal smoothing for face landmarks (median + Kalman + lookahead). + + Lookahead 30 ms ; max velocity in normalized units/s. + """ + + def __init__(self, lookahead_ms: float = 30.0, + enabled_stages: Iterable[str] | None = None) -> None: + if enabled_stages is None: + stages = _parse_env_face_stages() + else: + stages = tuple(s for s in enabled_stages + if s in ("median", "kalman", "lookahead")) + self.enabled = stages + self.median = MedianFilter(window=3) + self.kalman = AlphaBetaCV(alpha=0.55, beta=0.15) + self.lookahead = LookaheadPredictor( + lookahead_ms=lookahead_ms, max_velocity=2.0) + self.last_apply_ms: float = 0.0 + + def reset(self) -> None: + self.median.reset() + self.kalman.reset() + + def apply(self, faces: list[list], ids: list[int], + t_now: float) -> list[list]: + if not faces or not self.enabled: + self.last_apply_ms = 0.0 + return faces + t0 = time.perf_counter() + use_median = "median" in self.enabled + use_kalman = "kalman" in self.enabled + use_lookahead = "lookahead" in self.enabled + out: list[list] = [] + for f_i, kps in enumerate(faces): + pid = ids[f_i] if f_i < len(ids) else -1 + # Encode pid with a face-side namespace to avoid colliding with + # body and hand kalman/median caches. + key_pid = pid * 13 + 1 if pid >= 0 else pid + new_kps = [] + for j_idx, kp in enumerate(kps): + x, y, z, c = kp.x, kp.y, kp.z, kp.c + if use_median: + x, y, z = self.median.apply(key_pid, j_idx, x, y, z) + if use_kalman: + x, y, z = self.kalman.step(key_pid, j_idx, x, y, z, t_now) + if use_lookahead and use_kalman: + vx, vy, vz = self.kalman.get_velocity(key_pid, j_idx) + x, y, z = self.lookahead.step(x, y, z, vx, vy, vz) + new_kps.append(type(kp)(x=x, y=y, z=z, c=c)) + out.append(new_kps) + self.last_apply_ms = (time.perf_counter() - t0) * 1000.0 + return out + + +class HandFilterChain: + """Per-pid+side temporal smoothing for hand landmarks. + + Left and right hands keep independent filter state via a namespaced + pid (pid*2 for left, pid*2+1 for right). When handedness is not + provided, hands fall back to a side-agnostic namespace. + """ + + def __init__(self, lookahead_ms: float = 30.0, + enabled_stages: Iterable[str] | None = None) -> None: + if enabled_stages is None: + stages = _parse_env_hand_stages() + else: + stages = tuple(s for s in enabled_stages + if s in ("median", "kalman", "lookahead")) + self.enabled = stages + self.median = MedianFilter(window=3) + self.kalman = AlphaBetaCV(alpha=0.6, beta=0.2) + self.lookahead = LookaheadPredictor( + lookahead_ms=lookahead_ms, max_velocity=4.0) + self.last_apply_ms: float = 0.0 + + def reset(self) -> None: + self.median.reset() + self.kalman.reset() + + def apply(self, hands: list[list], ids: list[int], + handedness: list[str] | None, + t_now: float) -> list[list]: + if not hands or not self.enabled: + self.last_apply_ms = 0.0 + return hands + t0 = time.perf_counter() + use_median = "median" in self.enabled + use_kalman = "kalman" in self.enabled + use_lookahead = "lookahead" in self.enabled + out: list[list] = [] + for h_i, kps in enumerate(hands): + pid = ids[h_i] if h_i < len(ids) else -1 + side = (handedness[h_i] if handedness and h_i < len(handedness) + else "u").lower() + side_bit = 0 if side.startswith("l") else (1 if side.startswith("r") else 2) + # Namespace : (pid << 2) | side_bit — keeps L/R independent. + key_pid = (pid * 4 + side_bit + 7) if pid >= 0 else pid + new_kps = [] + for j_idx, kp in enumerate(kps): + x, y, z, c = kp.x, kp.y, kp.z, kp.c + if use_median: + x, y, z = self.median.apply(key_pid, j_idx, x, y, z) + if use_kalman: + x, y, z = self.kalman.step(key_pid, j_idx, x, y, z, t_now) + if use_lookahead and use_kalman: + vx, vy, vz = self.kalman.get_velocity(key_pid, j_idx) + x, y, z = self.lookahead.step(x, y, z, vx, vy, vz) + new_kps.append(type(kp)(x=x, y=y, z=z, c=c)) + out.append(new_kps) + self.last_apply_ms = (time.perf_counter() - t0) * 1000.0 + return out diff --git a/data_only_viz/scripts/bench_multihmr_coreml.py b/data_only_viz/scripts/bench_multihmr_coreml.py new file mode 100644 index 0000000..fe33462 --- /dev/null +++ b/data_only_viz/scripts/bench_multihmr_coreml.py @@ -0,0 +1,235 @@ +"""Bench Multi-HMR CoreML — compute_units sweep + section split. + +Bench Multi-HMR `.mlpackage` inference latency on M5 (or any Apple +Silicon). Decomposes the per-frame cost into copy_in / predict / +copy_out so we can see where time goes, then sweeps compute_units +(CPU_AND_GPU vs ALL vs CPU_AND_NE vs CPU_ONLY) and tests the +"reused MLMultiArray buffer" optimization. + +Usage: + uv run --project data_only_viz \ + python -m data_only_viz.scripts.bench_multihmr_coreml + +The result reproduces the 2026-05-14 finding: predict() is ~99% of +latency, copy_in is <2 ms, copy_out is <1 ms. None of the I/O +micro-optims (reused buffer, vImage preprocess, async copy) can +help meaningfully — only changing the model itself does (INT8 quant +via `scripts/quantize_multihmr_int8.py`, lower resolution, or a +smaller architecture). + +Pause the live worker before running for clean numbers: + pgrep -f 'data_only_viz.main.*multi-hmr' | xargs kill -STOP + # ...run bench... + pgrep -f 'data_only_viz.main.*multi-hmr' | xargs kill -CONT +""" +from __future__ import annotations + +import ctypes +import sys +import time +from pathlib import Path + +import numpy as np +from Foundation import NSURL + +from data_only_viz.multihmr_coreml import ( + DEFAULT_MLPACKAGE, + _load_frameworks, + _mlarray_to_np, + _np_to_mlarray, +) + +H = W = 672 +NITER = 30 +NWARM = 5 + + +def _make_inputs(): + img = np.random.rand(1, 3, H, W).astype(np.float32) + focal = float(H) + K = np.array( + [[[focal, 0, H / 2], [0, focal, H / 2], [0, 0, 1.0]]], + dtype=np.float32, + ) + return img, K + + +def _load_model(compute_units: int, mlpackage: Path): + ns = _load_frameworks() + MLModel = ns["MLModel"] + MLModelConfiguration = ns["MLModelConfiguration"] + cfg = MLModelConfiguration.alloc().init() + cfg.setComputeUnits_(compute_units) + url = NSURL.fileURLWithPath_(str(mlpackage)) + compiled = MLModel.compileModelAtURL_error_(url, None) + if compiled is None: + raise RuntimeError(f"compile failed cu={compute_units}") + model = MLModel.modelWithContentsOfURL_configuration_error_( + compiled, cfg, None) + if model is None: + raise RuntimeError(f"load failed cu={compute_units}") + return model, ns + + +def _stats(ts): + ts = sorted(ts) + return (ts[len(ts) // 2], + ts[len(ts) // 10], + ts[(len(ts) * 9) // 10]) + + +def bench_basic(label: str, compute_units: int, mlpackage: Path): + try: + model, ns = _load_model(compute_units, mlpackage) + except Exception as e: # noqa: BLE001 + print(f"[{label}] LOAD FAILED: {e}") + return None + MLDictionaryFeatureProvider = ns["MLDictionaryFeatureProvider"] + MLFeatureValue = ns["MLFeatureValue"] + img, K = _make_inputs() + for _ in range(NWARM): + img_ml = _np_to_mlarray(img); k_ml = _np_to_mlarray(K) + feats = {"image": MLFeatureValue.featureValueWithMultiArray_(img_ml), + "cam_K": MLFeatureValue.featureValueWithMultiArray_(k_ml)} + prov = MLDictionaryFeatureProvider.alloc( + ).initWithDictionary_error_(feats, None) + out = model.predictionFromFeatures_error_(prov, None) + if out is None: + print(f"[{label}] predict returned None") + return None + ts = [] + for _ in range(NITER): + t0 = time.perf_counter() + img_ml = _np_to_mlarray(img); k_ml = _np_to_mlarray(K) + feats = {"image": MLFeatureValue.featureValueWithMultiArray_(img_ml), + "cam_K": MLFeatureValue.featureValueWithMultiArray_(k_ml)} + prov = MLDictionaryFeatureProvider.alloc( + ).initWithDictionary_error_(feats, None) + out = model.predictionFromFeatures_error_(prov, None) + for name in out.featureNames(): + fv = out.featureValueForName_(name) + ml = fv.multiArrayValue() + if ml is None: + continue + _ = _mlarray_to_np(ml) + ts.append((time.perf_counter() - t0) * 1e3) + med, p10, p90 = _stats(ts) + print(f"[{label:34s}] med={med:6.1f}ms p10={p10:6.1f} " + f"p90={p90:6.1f} fps={1000/med:5.1f}") + return med + + +def bench_reused_input(label: str, compute_units: int, mlpackage: Path): + try: + model, ns = _load_model(compute_units, mlpackage) + except Exception as e: # noqa: BLE001 + print(f"[{label}] LOAD FAILED: {e}") + return None + MLDictionaryFeatureProvider = ns["MLDictionaryFeatureProvider"] + MLFeatureValue = ns["MLFeatureValue"] + img, K = _make_inputs() + img_ml = _np_to_mlarray(img); k_ml = _np_to_mlarray(K) + ptr_img = img_ml.dataPointer() + addr_img = int(ptr_img) if isinstance(ptr_img, int) else \ + ctypes.cast(ptr_img, ctypes.c_void_p).value + ptr_k = k_ml.dataPointer() + addr_k = int(ptr_k) if isinstance(ptr_k, int) else \ + ctypes.cast(ptr_k, ctypes.c_void_p).value + img_bytes = img.nbytes + k_bytes = K.nbytes + feats = {"image": MLFeatureValue.featureValueWithMultiArray_(img_ml), + "cam_K": MLFeatureValue.featureValueWithMultiArray_(k_ml)} + for _ in range(NWARM): + ctypes.memmove(addr_img, img.ctypes.data, img_bytes) + ctypes.memmove(addr_k, K.ctypes.data, k_bytes) + prov = MLDictionaryFeatureProvider.alloc( + ).initWithDictionary_error_(feats, None) + _ = model.predictionFromFeatures_error_(prov, None) + ts = [] + for _ in range(NITER): + t0 = time.perf_counter() + ctypes.memmove(addr_img, img.ctypes.data, img_bytes) + ctypes.memmove(addr_k, K.ctypes.data, k_bytes) + prov = MLDictionaryFeatureProvider.alloc( + ).initWithDictionary_error_(feats, None) + out = model.predictionFromFeatures_error_(prov, None) + for name in out.featureNames(): + fv = out.featureValueForName_(name) + ml = fv.multiArrayValue() + if ml is None: + continue + _ = _mlarray_to_np(ml) + ts.append((time.perf_counter() - t0) * 1e3) + med, p10, p90 = _stats(ts) + print(f"[{label:34s}] med={med:6.1f}ms p10={p10:6.1f} " + f"p90={p90:6.1f} fps={1000/med:5.1f}") + return med + + +def bench_section_split(compute_units: int, mlpackage: Path): + model, ns = _load_model(compute_units, mlpackage) + MLDictionaryFeatureProvider = ns["MLDictionaryFeatureProvider"] + MLFeatureValue = ns["MLFeatureValue"] + img, K = _make_inputs() + for _ in range(NWARM): + img_ml = _np_to_mlarray(img); k_ml = _np_to_mlarray(K) + feats = {"image": MLFeatureValue.featureValueWithMultiArray_(img_ml), + "cam_K": MLFeatureValue.featureValueWithMultiArray_(k_ml)} + prov = MLDictionaryFeatureProvider.alloc( + ).initWithDictionary_error_(feats, None) + _ = model.predictionFromFeatures_error_(prov, None) + t_in, t_pred, t_out = [], [], [] + for _ in range(NITER): + t0 = time.perf_counter() + img_ml = _np_to_mlarray(img); k_ml = _np_to_mlarray(K) + feats = {"image": MLFeatureValue.featureValueWithMultiArray_(img_ml), + "cam_K": MLFeatureValue.featureValueWithMultiArray_(k_ml)} + prov = MLDictionaryFeatureProvider.alloc( + ).initWithDictionary_error_(feats, None) + t1 = time.perf_counter() + out = model.predictionFromFeatures_error_(prov, None) + t2 = time.perf_counter() + for name in out.featureNames(): + fv = out.featureValueForName_(name) + ml = fv.multiArrayValue() + if ml is None: + continue + _ = _mlarray_to_np(ml) + t3 = time.perf_counter() + t_in.append((t1 - t0) * 1e3) + t_pred.append((t2 - t1) * 1e3) + t_out.append((t3 - t2) * 1e3) + mi = lambda a: sorted(a)[len(a) // 2] + print("[section-split CPU_AND_GPU]") + print(f" copy_in : {mi(t_in):6.2f} ms") + print(f" predict : {mi(t_pred):6.2f} ms") + print(f" copy_out : {mi(t_out):6.2f} ms") + print(f" total : {mi(t_in)+mi(t_pred)+mi(t_out):6.2f} ms") + + +def main(argv: list[str]) -> int: + mlpackage = DEFAULT_MLPACKAGE + if len(argv) > 1: + mlpackage = Path(argv[1]) + if not mlpackage.exists(): + print(f"mlpackage missing: {mlpackage}", file=sys.stderr) + return 1 + print(f"bench target: {mlpackage}") + print("=" * 70) + print("Section split (alloc/predict/copy)") + print("=" * 70) + bench_section_split(1, mlpackage) + print() + print("=" * 70) + print("Compute-units sweep (30 iter median)") + print("=" * 70) + bench_basic("A. CPU_AND_GPU (baseline)", 1, mlpackage) + bench_basic("B. ALL (ANE+GPU+CPU)", 2, mlpackage) + bench_basic("C. CPU_AND_NE (ANE-only)", 3, mlpackage) + bench_basic("D. CPU_ONLY", 0, mlpackage) + bench_reused_input("E. CPU_AND_GPU + reused buffer", 1, mlpackage) + return 0 + + +if __name__ == "__main__": + raise SystemExit(main(sys.argv)) diff --git a/data_only_viz/scripts/coreml_full_probe.py b/data_only_viz/scripts/coreml_full_probe.py index f1f4387..f752ca1 100644 --- a/data_only_viz/scripts/coreml_full_probe.py +++ b/data_only_viz/scripts/coreml_full_probe.py @@ -524,10 +524,10 @@ try: compute_units=ct.ComputeUnit.CPU_AND_GPU, minimum_deployment_target=ct.target.macOS15, convert_to="mlprogram", - # FP16 OK depuis le patch roma branchless (cf rapport bisection - # 2026-05-13) : la source du NaN etait torch.empty + index_put_ - # dans roma.rotmat_to_rotvec, pas la precision. - compute_precision=ct.precision.FLOAT16, + # FP32 mandatory : FP16 (global ou hybride op_selector) degrade + # visiblement le mesh sur poses extremes. INT8 weight quant + # teste 2026-05-14 : aucun gain sur GPU compute-bound. + compute_precision=ct.precision.FLOAT32, ) out_path = "/tmp/multihmr_full_672_s.mlpackage" mlmodel.save(out_path) diff --git a/data_only_viz/scripts/quantize_multihmr_int8.py b/data_only_viz/scripts/quantize_multihmr_int8.py new file mode 100644 index 0000000..45902f1 --- /dev/null +++ b/data_only_viz/scripts/quantize_multihmr_int8.py @@ -0,0 +1,81 @@ +"""Quantize Multi-HMR mlpackage to INT8 (weight-only) for M5 speedup. + +Run in the Python 3.12 conversion venv (coremltools cannot run on 3.14): + + /tmp/coreml312/.venv/bin/python \ + data_only_viz/scripts/quantize_multihmr_int8.py + +Produces `multihmr_full_672_s_int8.mlpackage` next to the FP32 file. +Bench after with `scripts/coreml_full_probe.py` or just load with +`MultiHMRCoreMLBackend(path=...new path...)`. + +Strategy: +- Linear 8-bit weight palettization (per-tensor symmetric). Activations + stay FP16 — that's the "weight-only quant" path, lowest accuracy + hit and what CoreML's GPU runtime accelerates best. +- Skip the SMPL-X decoder branch ops that are sensitive to numeric + drift (skipped by name pattern below — adjust if v3d shows mesh + artefacts after quantization). + +Validation: +- After producing the int8 mlpackage, run the live worker briefly + with COREML_MLPACKAGE pointing to the new file and visually check + the mesh. If v3d shows tearing on extreme poses, retry with + `granularity="per_channel"` instead of `per_tensor`. +""" +from __future__ import annotations + +import sys +from pathlib import Path + +try: + import coremltools as ct + from coremltools.optimize.coreml import ( + linear_quantize_weights, + OptimizationConfig, + OpLinearQuantizerConfig, + ) +except ImportError as e: + print(f"coremltools missing in this venv: {e}", file=sys.stderr) + print("Run from the Python 3.12 conversion venv (coremltools " + "is not available on 3.14).", file=sys.stderr) + sys.exit(1) + + +SRC = Path.home() / ".cache" / "av-live-multihmr" / \ + "multihmr_full_672_s.mlpackage" +DST = Path.home() / ".cache" / "av-live-multihmr" / \ + "multihmr_full_672_s_int8.mlpackage" + + +def main() -> int: + if not SRC.exists(): + print(f"source mlpackage missing: {SRC}", file=sys.stderr) + return 1 + print(f"loading FP32 model from {SRC}") + model = ct.models.MLModel(str(SRC)) + + # Per-tensor symmetric int8 weight quant. Per-tensor keeps the + # quantized model small and GPU-friendly; per-channel is a safer + # fallback if mesh quality degrades. + op_cfg = OpLinearQuantizerConfig( + mode="linear_symmetric", + dtype="int8", + granularity="per_tensor", + ) + cfg = OptimizationConfig(global_config=op_cfg) + print("running linear_quantize_weights (per_tensor int8)...") + quant = linear_quantize_weights(model, config=cfg) + print(f"saving quantized model to {DST}") + quant.save(str(DST)) + print("done. Test with:") + print(f" COREML_MLPACKAGE={DST} \\\n" + f" MULTIHMR_BACKEND=coreml \\\n" + f" uv run --project data_only_viz \\\n" + f" python -m data_only_viz.main --multi-hmr " + f"--motion-gate 0") + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/data_only_viz/tests/test_face_hand_filter.py b/data_only_viz/tests/test_face_hand_filter.py new file mode 100644 index 0000000..ed034bd --- /dev/null +++ b/data_only_viz/tests/test_face_hand_filter.py @@ -0,0 +1,214 @@ +"""Tests for FaceFilterChain, HandFilterChain, and multi.py discrimination.""" +from __future__ import annotations + +import random +import time + +import pytest + +from data_only_viz.pose_filter import ( + FaceFilterChain, + HandFilterChain, + PoseFilterChain, +) +from data_only_viz.state import Kp3D, PoseKp + + +def _jitter_face(n_pts: int, base_x: float, base_y: float, + amp: float, rng: random.Random) -> list[PoseKp]: + return [ + PoseKp( + x=base_x + rng.uniform(-amp, amp), + y=base_y + rng.uniform(-amp, amp), + z=rng.uniform(-amp, amp), + c=1.0, + ) + for _ in range(n_pts) + ] + + +def test_face_filter_reduces_jitter() -> None: + chain = FaceFilterChain() + rng = random.Random(42) + n_pts = 68 + base_x, base_y = 0.5, 0.5 + amp = 0.01 + outputs: list[list[PoseKp]] = [] + t = 0.0 + for k in range(8): + t += 1.0 / 30.0 + faces = [_jitter_face(n_pts, base_x, base_y, amp, rng)] + out = chain.apply(faces, [0], t) + outputs.append(out[0]) + # Compute variance on x of joint 0 across the last 5 frames. + last = outputs[-5:] + xs = [f[0].x for f in last] + mean = sum(xs) / len(xs) + var = sum((v - mean) ** 2 for v in xs) / len(xs) + assert var < 0.005, f"face filter variance too high: {var}" + + +def test_hand_filter_left_right_independent() -> None: + chain = HandFilterChain() + rng = random.Random(7) + n_pts = 21 + t = 0.0 + last_l: list[PoseKp] = [] + last_r: list[PoseKp] = [] + for k in range(6): + t += 1.0 / 30.0 + left_hand = _jitter_face(n_pts, 0.2, 0.5, 0.008, rng) + right_hand = _jitter_face(n_pts, 0.8, 0.5, 0.008, rng) + out = chain.apply([left_hand, right_hand], [0, 0], + ["Left", "Right"], t) + last_l, last_r = out[0], out[1] + # Left and right hands keep distinct positions despite same pid. + assert abs(last_l[0].x - last_r[0].x) > 0.4 + # Filter reduced jitter on each side. + assert 0.1 < last_l[0].x < 0.35 + assert 0.65 < last_r[0].x < 0.9 + + +def test_hand_filter_chain_wrapper_smoke() -> None: + chain = PoseFilterChain() + rng = random.Random(0) + hands = [_jitter_face(21, 0.5, 0.5, 0.01, rng) for _ in range(2)] + out = chain.apply_hand(hands, [0, 1], ["Left", "Right"], t_now=0.1) + assert len(out) == 2 + assert len(out[0]) == 21 + + +def test_face_filter_disabled_passthrough() -> None: + chain = FaceFilterChain(enabled_stages=()) + faces = [[PoseKp(x=0.5, y=0.5, z=0.0, c=1.0) for _ in range(68)]] + out = chain.apply(faces, [0], t_now=0.0) + assert out[0][0].x == 0.5 + + +def test_face_hand_latency_under_5ms() -> None: + """Full chain (body 33 + face 68 + hand 21x2) < 5 ms per frame.""" + body_chain = PoseFilterChain( + enabled_stages=("median", "kalman", "lookahead", "ik")) + face_chain = FaceFilterChain() + hand_chain = HandFilterChain() + rng = random.Random(0) + body = [Kp3D(x=i * 0.01, y=i * 0.02, z=i * 0.03, c=1.0) + for i in range(33)] + face = _jitter_face(68, 0.5, 0.5, 0.01, rng) + hand_l = _jitter_face(21, 0.2, 0.5, 0.01, rng) + hand_r = _jitter_face(21, 0.8, 0.5, 0.01, rng) + # Warm-up + for k in range(5): + t = k * 0.033 + body_chain.apply([body], [0], t) + face_chain.apply([face], [0], t) + hand_chain.apply([hand_l, hand_r], [0, 0], ["Left", "Right"], t) + # Measure + durs: list[float] = [] + for k in range(30): + t = (k + 5) * 0.033 + t0 = time.perf_counter() + body_chain.apply([body], [0], t) + face_chain.apply([face], [0], t) + hand_chain.apply([hand_l, hand_r], [0, 0], ["Left", "Right"], t) + durs.append((time.perf_counter() - t0) * 1000.0) + avg = sum(durs) / len(durs) + # CI margin : actual M-class target is < 5 ms ; allow 25 ms in tests. + assert avg < 25.0, f"chain too slow: {avg:.2f} ms" + + +# ----------------------- multi.py discrimination --------------------------- + + +def _make_body(n_visible: int) -> list[PoseKp]: + """Make a 33-joint body with `n_visible` high-conf joints, rest low.""" + out: list[PoseKp] = [] + for i in range(33): + c = 1.0 if i < n_visible else 0.05 + # Spread across both x and y so the bbox has non-zero area. + out.append(PoseKp(x=0.1 + i * 0.01, y=0.2 + i * 0.005, z=0.0, c=c)) + return out + + +def _make_body3d(n: int = 33) -> list[Kp3D]: + return [Kp3D(x=0.0, y=0.0, z=0.0, c=1.0) for _ in range(n)] + + +def _instantiate_worker(): + """Build a MultiWorker without starting the thread (skip if cv2 missing).""" + pytest.importorskip("cv2", reason="opencv not installed") + from data_only_viz.multi import MultiWorker + from data_only_viz.state import State + return MultiWorker(state=State(), camera_index=-1) + + +def test_ghost_rejection_drops_low_visibility_body() -> None: + w = _instantiate_worker() + bodies = [_make_body(n_visible=5), _make_body(n_visible=25)] + b3d = [_make_body3d(), _make_body3d()] + ids = [0, 1] + new_bodies, new_b3d, new_ids = w._reject_ghosts_and_nms(bodies, b3d, ids) + assert len(new_bodies) == 1 + assert len(new_b3d) == 1 + assert new_ids == [1] + assert w._n_ghost_dropped == 1 + + +def test_nms_keeps_best_score() -> None: + w = _instantiate_worker() + # Two heavily overlapping bodies, second has higher mean confidence. + b1 = _make_body(n_visible=20) + b2 = _make_body(n_visible=33) + new_bodies, _, new_ids = w._reject_ghosts_and_nms([b1, b2], [], [0, 1]) + # IoU of identical bbox => one dropped, the higher-score one kept. + assert len(new_bodies) == 1 + assert new_ids == [1] + + +def test_pid_persistence_through_short_absence() -> None: + w = _instantiate_worker() + body = _make_body(n_visible=30) + # Frame 1..30 : pid 0 present. + for _ in range(30): + new_ids = w._apply_pid_hysteresis([body], [0]) + assert new_ids == [0] + # Frames 31..35 : pid 0 absent (no detection). + for _ in range(5): + w._apply_pid_hysteresis([], []) + # Frame 36 : a NEW pid 9 appears at the same bbox -> should be remapped. + new_ids = w._apply_pid_hysteresis([body], [9]) + assert new_ids == [0], f"expected hysteresis remap to 0, got {new_ids}" + + +def test_drop_low_visibility_face() -> None: + w = _instantiate_worker() + # 30 valid (non-zero) + 38 zeros. + face_bad = [ + PoseKp(x=(0.1 if i < 30 else 0.0), + y=(0.1 if i < 30 else 0.0), z=0.0, c=1.0) + for i in range(68) + ] + face_ok = [ + PoseKp(x=0.1 + i * 0.001, y=0.2, z=0.0, c=1.0) + for i in range(68) + ] + kept, ids = w._drop_low_visibility( + [face_bad, face_ok], [0, 1], min_visible=50, which="face") + assert len(kept) == 1 + assert ids == [1] + assert w._n_face_dropped == 1 + + +def test_drop_low_visibility_hand() -> None: + w = _instantiate_worker() + hand_bad = [PoseKp(x=0.0, y=0.0, z=0.0, c=1.0) for _ in range(21)] + # Only 10 visible (others are zero) -> drop. + for i in range(10): + hand_bad[i] = PoseKp(x=0.5, y=0.5, z=0.0, c=1.0) + hand_ok = [PoseKp(x=0.1 + i * 0.01, y=0.2, z=0.0, c=1.0) + for i in range(21)] + kept, ids = w._drop_low_visibility( + [hand_bad, hand_ok], [0, 1], min_visible=15, which="hand") + assert len(kept) == 1 + assert ids == [1] + assert w._n_hand_dropped == 1 diff --git a/launcher/AV-Live-Body/Sources/AVLiveBody/AVLiveBodyApp.swift b/launcher/AV-Live-Body/Sources/AVLiveBody/AVLiveBodyApp.swift index c04db0d..8f704c8 100644 --- a/launcher/AV-Live-Body/Sources/AVLiveBody/AVLiveBodyApp.swift +++ b/launcher/AV-Live-Body/Sources/AVLiveBody/AVLiveBodyApp.swift @@ -87,12 +87,9 @@ struct ContentView: View { if let n = note.object as? Int { settings.vizMode = n } } - // Face + hand overlay 2D Canvas (68 dlib landmarks dont - // bouche slots 48-67 outerLips + 60-67 innerLips, plus 21 - // landmarks par main, gauche=cyan droite=magenta). Source : - // /face/kp et /hand/kp depuis MediaPipe Holistic. - FaceHandOverlay(poseListener: poseListener) - .allowsHitTesting(false) + // Face + hand overlay 2D Canvas retire : les landmarks sont + // maintenant integres au squelette 3D RealityKit (cf. + // Skeleton3DRenderer.applyFace/applyHands). // HUD coin haut-gauche : mode + touches + pose HUDOverlay(settings: settings, poseListener: poseListener) diff --git a/launcher/AV-Live-Body/Sources/AVLiveBody/Skeleton3DRenderer.swift b/launcher/AV-Live-Body/Sources/AVLiveBody/Skeleton3DRenderer.swift index aef057a..09932a2 100644 --- a/launcher/AV-Live-Body/Sources/AVLiveBody/Skeleton3DRenderer.swift +++ b/launcher/AV-Live-Body/Sources/AVLiveBody/Skeleton3DRenderer.swift @@ -53,8 +53,14 @@ final class Skeleton3DRenderer: ObservableObject { } } - private static let jointRadius: Float = 0.045 // 4.5 cm — visible 3D depth - private static let boneRadius: Float = 0.022 // 2.2 cm — chunky bones + // Wireframe pur : joints micro (1 mm, quasi invisibles), bones + // 2 mm = lignes fines. radius=0 fait planter MeshResource.generate. + private static let jointRadius: Float = 0.001 // 1 mm — quasi nul + private static let boneRadius: Float = 0.003 // 3 mm — line-like + private static let faceJointRadius: Float = 0.001 + private static let handJointRadius: Float = 0.001 + private static let handScale3D: Float = 0.18 // typical hand size (m) + private static let faceForwardOffset: Float = 0.05 // push face in front of nose private static let minConfidence: Float = 0.3 private static let retainSec: TimeInterval = 1.0 @@ -66,12 +72,19 @@ final class Skeleton3DRenderer: ObservableObject { var root: Entity var joints: [ModelEntity] // 33 spheres var bones: [ModelEntity] // 32 bone entities, same order as POSE_CONNECTIONS + var faceJoints: [ModelEntity] // 68 dlib face landmarks + var leftHandJoints: [ModelEntity] // 21 cyan + var rightHandJoints: [ModelEntity] // 21 magenta } private var persons: [Int: PersonEntities] = [:] private var lastSeenAt: [Int: TimeInterval] = [:] + private var lastFace: [Int: PoseOSCListener.FaceFrame] = [:] + private var lastHands: [Int: PoseOSCListener.HandFrame] = [:] private weak var rootAnchor: Entity? private var poseSub: AnyCancellable? + private var faceSub: AnyCancellable? + private var handSub: AnyCancellable? private var lastUpdateAt: TimeInterval = 0 /// Optional per-pid offset to align the skeleton with another /// renderer's coordinate space (typically MeshRenderer's pelvis). @@ -102,14 +115,30 @@ final class Skeleton3DRenderer: ObservableObject { .sink { [weak self] frames in Task { @MainActor in self?.update(frames: frames) } } + faceSub = listener.$faces + .receive(on: DispatchQueue.main) + .sink { [weak self] frames in + Task { @MainActor in self?.lastFace = frames } + } + handSub = listener.$hands + .receive(on: DispatchQueue.main) + .sink { [weak self] frames in + Task { @MainActor in self?.lastHands = frames } + } } func detach() { poseSub?.cancel() poseSub = nil + faceSub?.cancel() + faceSub = nil + handSub?.cancel() + handSub = nil for (_, p) in persons { p.root.removeFromParent() } persons.removeAll() lastSeenAt.removeAll() + lastFace.removeAll() + lastHands.removeAll() } // MARK: - Update @@ -134,11 +163,12 @@ final class Skeleton3DRenderer: ObservableObject { for (pid, frame) in frames { let entities = persons[pid] ?? makePerson(pid: pid, parent: anchor) persons[pid] = entities - apply(frame: frame, to: entities) + apply(frame: frame, pid: pid, to: entities) } } private func apply(frame: PoseOSCListener.Pose3DFrame, + pid: Int, to entities: PersonEntities) { // Convert all 33 keypoints to RealityKit space once. var rk = [SIMD3](repeating: .zero, count: 33) @@ -199,6 +229,108 @@ final class Skeleton3DRenderer: ObservableObject { bone.transform.scale = SIMD3(1, len, 1) bone.isEnabled = true } + + // --- Face landmarks (68) anchored on nose joint rk[0] --- + applyFace(pid: pid, rk: rk, valid: valid, to: entities) + + // --- Hand landmarks (21 left + 21 right) anchored on wrists --- + applyHands(rk: rk, valid: valid, to: entities) + } + + private func applyFace(pid: Int, + rk: [SIMD3], + valid: [Bool], + to entities: PersonEntities) { + guard let face = lastFace[pid] else { + for j in entities.faceJoints { j.isEnabled = false } + return + } + // Head width in 3D : distance between ears (rk[7] left ear, + // rk[8] right ear). Fall back to a sane default if missing. + let headWidth3D: Float + if valid[7] && valid[8] { + headWidth3D = max(0.10, simd_length(rk[7] - rk[8])) + } else { + headWidth3D = 0.18 + } + // Compute 2D bbox width of face points + centroid. + var minX: Float = .infinity, maxX: Float = -.infinity + var sumX: Float = 0, sumY: Float = 0 + var n: Float = 0 + for i in 0..<68 where face.hasPoint[i] { + let p = face.points[i] + if p.x < minX { minX = p.x } + if p.x > maxX { maxX = p.x } + sumX += p.x + sumY += p.y + n += 1 + } + let nose = valid[0] ? rk[0] : SIMD3(0, 0, 0) + guard n > 4, maxX > minX else { + for j in entities.faceJoints { j.isEnabled = false } + return + } + let face2DWidth = max(maxX - minX, 1e-4) + let scale = headWidth3D / face2DWidth + let cx = sumX / n + let cy = sumY / n + for i in 0..<68 { + let j = entities.faceJoints[i] + if face.hasPoint[i] { + let dx = face.points[i].x - cx + let dy = face.points[i].y - cy + // Flip y : image y down -> RK y up. +z to push in front of nose. + j.transform.translation = nose + SIMD3( + dx * scale, -dy * scale, Self.faceForwardOffset) + j.isEnabled = true + } else { + j.isEnabled = false + } + } + } + + private func applyHands(rk: [SIMD3], + valid: [Bool], + to entities: PersonEntities) { + // Disable by default ; re-enable per side if a matching frame found. + for j in entities.leftHandJoints { j.isEnabled = false } + for j in entities.rightHandJoints { j.isEnabled = false } + + // Iterate cached hand frames (keyed by pid in OSC ; here we route + // purely by `side` since the python emits side=0/1 explicitly). + for (_, hand) in lastHands { + let isLeft = (hand.side == 0) + let wristIdx = isLeft ? 15 : 16 + guard valid[wristIdx] else { continue } + let wrist = rk[wristIdx] + let target = isLeft + ? entities.leftHandJoints + : entities.rightHandJoints + + // Centroid of valid hand points. + var sumX: Float = 0, sumY: Float = 0, n: Float = 0 + for i in 0..<21 where hand.hasPoint[i] { + sumX += hand.points[i].x + sumY += hand.points[i].y + n += 1 + } + guard n > 2 else { continue } + let cx = sumX / n + let cy = sumY / n + let scale = Self.handScale3D + for i in 0..<21 { + let j = target[i] + if hand.hasPoint[i] { + let dx = hand.points[i].x - cx + let dy = hand.points[i].y - cy + j.transform.translation = wrist + SIMD3( + dx * scale, -dy * scale, 0) + j.isEnabled = true + } else { + j.isEnabled = false + } + } + } } // MARK: - Construction @@ -234,8 +366,49 @@ final class Skeleton3DRenderer: ObservableObject { root.addChild(e) bones.append(e) } - NSLog("Skeleton3DRenderer: spawned pid=%d (33 joints, %d bones)", + // Face sub-spheres (68 dlib landmarks, neutral light grey). + let faceMesh = MeshResource.generateSphere( + radius: Self.faceJointRadius) + let faceMat = SimpleMaterial( + color: NSColor(white: 0.85, alpha: 1.0), + roughness: 0.7, isMetallic: false) + var faceJoints: [ModelEntity] = [] + faceJoints.reserveCapacity(68) + for _ in 0..<68 { + let e = ModelEntity(mesh: faceMesh, materials: [faceMat]) + e.isEnabled = false + root.addChild(e) + faceJoints.append(e) + } + + // Hand sub-spheres : left=cyan, right=magenta, 21 each. + let handMesh = MeshResource.generateSphere( + radius: Self.handJointRadius) + let leftMat = SimpleMaterial( + color: .systemCyan, roughness: 0.6, isMetallic: false) + let rightMat = SimpleMaterial( + color: .systemPink, roughness: 0.6, isMetallic: false) + var leftHand: [ModelEntity] = [] + var rightHand: [ModelEntity] = [] + leftHand.reserveCapacity(21) + rightHand.reserveCapacity(21) + for _ in 0..<21 { + let el = ModelEntity(mesh: handMesh, materials: [leftMat]) + el.isEnabled = false + root.addChild(el) + leftHand.append(el) + let er = ModelEntity(mesh: handMesh, materials: [rightMat]) + er.isEnabled = false + root.addChild(er) + rightHand.append(er) + } + NSLog("Skeleton3DRenderer: spawned pid=%d (33 joints, %d bones, 68 face, 21+21 hands)", pid, bones.count) - return PersonEntities(root: root, joints: joints, bones: bones) + return PersonEntities(root: root, + joints: joints, + bones: bones, + faceJoints: faceJoints, + leftHandJoints: leftHand, + rightHandJoints: rightHand) } }