feat(av-live): face+hand+3D pose to launcher
Stream MediaPipe Holistic face landmarks (68 dlib subset of 478), hand landmarks (21 left + 21 right), and pose world landmarks (33 3D xyz meters) over OSC :57126 to AVLiveBody. Launcher renders face/hand as SwiftUI Canvas overlay and the 3D skeleton as a RealityKit armature (sphere joints + cylinder bones, color per chain) toggled via p / mode openpos. Multi-HMR worker now also starts MediaPipe Multi in parallel so both the dense SMPL-X mesh (TCP 57130, PyTorch backend) and the skeleton/face/hand streams (OSC 57126) feed the launcher from one Python process. Launcher AppDelegate forces .regular activation so SwiftPM binaries actually show their WindowGroup without a bundle. Tests: 9 new pytest cases (4 body3d + 5 face/hand), all green. CoreML conversion still produces NaN on v3d/transl; PyTorch backend is the working path for now.
This commit is contained in:
L'électron rare
@@ -268,6 +268,21 @@ class AppDelegate(NSObject):
|
||||
self._smplx_tcp = SMPLXTCPSender(self._state)
|
||||
self._smplx_tcp.start()
|
||||
LOG.info("worker: Multi-HMR + SMPL-X (mesh dense)")
|
||||
# Also start MediaPipe Multi for body3d + face + hand
|
||||
# OSC streams to AVLiveBody (mesh and skeleton/face/
|
||||
# hand pipelines run in parallel, each owns its own
|
||||
# AVCapture session on the same builtin camera).
|
||||
if _os.environ.get("AV_LIVE_MEDIAPIPE") != "0":
|
||||
try:
|
||||
from .multi import MultiWorker
|
||||
self._mediapipe_worker = MultiWorker(
|
||||
self._state, num_persons=4)
|
||||
self._mediapipe_worker.start()
|
||||
LOG.info("worker: + MediaPipe Multi (3D pose "
|
||||
"+ face + hand) in parallel")
|
||||
except Exception as e: # noqa: BLE001
|
||||
LOG.warning("MediaPipe parallel start failed "
|
||||
"(%s) — mesh only", e)
|
||||
return
|
||||
LOG.info("Multi-HMR indisponible (checkpoints manquants) "
|
||||
"— voir scripts/setup_multihmr.sh")
|
||||
|
||||
+22
-2
@@ -21,7 +21,7 @@ from pathlib import Path
|
||||
|
||||
from .euro_filter import SkeletonFilter
|
||||
from .pose_bridge import PoseSoundBridge
|
||||
from .state import PoseKp, State
|
||||
from .state import Kp3D, PoseKp, State
|
||||
from .tracker import IoUTracker
|
||||
|
||||
LOG = logging.getLogger("multi")
|
||||
@@ -198,6 +198,21 @@ class MultiWorker:
|
||||
x=float(lm.x), y=float(lm.y), z=z, c=float(v)))
|
||||
bodies.append(kp_list)
|
||||
|
||||
# pose_world_landmarks : xyz metric, relative to hip-center.
|
||||
# Aligned 1:1 with pose_landmarks order. Empty fallback if
|
||||
# the MediaPipe build doesn't populate it.
|
||||
bodies3d: list[list[Kp3D]] = []
|
||||
world_list = getattr(pose_res, "pose_world_landmarks", None) or []
|
||||
for landmarks_list in world_list:
|
||||
kp3_list: list[Kp3D] = []
|
||||
for lm in landmarks_list[:33]:
|
||||
v = lm.visibility if lm.visibility is not None else 1.0
|
||||
kp3_list.append(Kp3D(
|
||||
x=float(lm.x), y=float(lm.y),
|
||||
z=float(lm.z if lm.z is not None else 0.0),
|
||||
c=float(v)))
|
||||
bodies3d.append(kp3_list)
|
||||
|
||||
faces = []
|
||||
for landmarks_list in (face_res.face_landmarks or []):
|
||||
kp_list = []
|
||||
@@ -230,16 +245,21 @@ class MultiWorker:
|
||||
for i, kps in enumerate(hands)]
|
||||
|
||||
# Pont sonore : envoi OSC /pose/* a sclang (body + face + hands)
|
||||
# 3D world landmarks share ids with bodies (same MediaPipe
|
||||
# detection, just a different coordinate space).
|
||||
ids_body3d = ids_body[:len(bodies3d)] if bodies3d else []
|
||||
self._sound_bridge.send(
|
||||
bodies, ids_body, t_now,
|
||||
persons_face=faces, persons_face_ids=ids_face,
|
||||
persons_hands=hands, persons_hands_ids=ids_hand)
|
||||
persons_hands=hands, persons_hands_ids=ids_hand,
|
||||
persons_body3d=bodies3d, persons_body3d_ids=ids_body3d)
|
||||
|
||||
with self.state.lock():
|
||||
self.state.persons_body = bodies
|
||||
self.state.persons_face = faces
|
||||
self.state.persons_hands = hands
|
||||
self.state.persons_body_ids = ids_body
|
||||
self.state.persons_body3d = bodies3d
|
||||
self.state.persons_face_ids = ids_face
|
||||
self.state.persons_hands_ids = ids_hand
|
||||
# Compat single-person (1ere personne)
|
||||
|
||||
@@ -10,13 +10,21 @@ Routes emises :
|
||||
/pose/head <pid> <x> <y> <c> position du nez (visage)
|
||||
/pose/sho_span <pid> <dx> ecart epaules (estime distance camera)
|
||||
/pose/limb_span <pid> <span> envergure brassse (poignet a poignet)
|
||||
/face/count <n> nombre de visages detectes
|
||||
/face/kp <pid> <idx> <x> <y> <z> <c> 68-pt subset (dlib mapping)
|
||||
/hand/count <n_left> <n_right> nombre de mains gauche / droite
|
||||
/hand/kp <pid> <side[0=L|1=R]> <idx> <x> <y> <z> <c> 21 landmarks
|
||||
/pose3d/count <n> nombre de squelettes 3D
|
||||
/pose3d/kp <pid> <idx> <x> <y> <z> <c> 33 MediaPipe world landmarks (metres)
|
||||
|
||||
Mapping pose -> son est defini cote SC dans sound_algo/data_only/scenes.scd
|
||||
(scene `live_pose`).
|
||||
(scene `live_pose`). Face / hand keypoints are consumed by the Swift
|
||||
launcher (AVLiveBody) on 127.0.0.1:57126 for skeleton overlay rendering.
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import logging
|
||||
from typing import Any, Iterable, Sequence
|
||||
|
||||
from pythonosc.udp_client import SimpleUDPClient
|
||||
|
||||
@@ -33,6 +41,42 @@ LEFT_HIP = 23
|
||||
RIGHT_HIP = 24
|
||||
|
||||
|
||||
# MediaPipe FaceMesh (468 landmarks) -> 68-point dlib-style subset.
|
||||
# Mapping inspired by community references (Google MediaPipe ->
|
||||
# iBUG 68 facial landmarks). Order matches dlib 68-point convention :
|
||||
# [0..16] jaw contour (left to right)
|
||||
# [17..21] right brow
|
||||
# [22..26] left brow
|
||||
# [27..30] nose bridge (top to tip)
|
||||
# [31..35] nostril base (right to left)
|
||||
# [36..41] right eye (CCW from outer corner)
|
||||
# [42..47] left eye (CCW from inner corner)
|
||||
# [48..59] outer lip (CCW from right corner)
|
||||
# [60..67] inner lip (CCW from right corner)
|
||||
FACE_68_FROM_MP: tuple[int, ...] = (
|
||||
# Jaw (17)
|
||||
127, 234, 132, 172, 150, 176, 148, 152,
|
||||
377, 400, 365, 397, 361, 401, 366, 447, 356,
|
||||
# Right brow (5) — mediapipe perspective is mirrored vs subject
|
||||
70, 63, 105, 66, 107,
|
||||
# Left brow (5)
|
||||
336, 296, 334, 293, 300,
|
||||
# Nose bridge (4)
|
||||
168, 6, 197, 195,
|
||||
# Nostril base (5)
|
||||
98, 97, 2, 326, 327,
|
||||
# Right eye (6)
|
||||
33, 160, 158, 133, 153, 144,
|
||||
# Left eye (6)
|
||||
362, 385, 387, 263, 373, 380,
|
||||
# Outer lip (12)
|
||||
61, 39, 37, 0, 267, 269, 291, 405, 314, 17, 84, 181,
|
||||
# Inner lip (8)
|
||||
78, 81, 13, 311, 308, 402, 14, 178,
|
||||
)
|
||||
assert len(FACE_68_FROM_MP) == 68
|
||||
|
||||
|
||||
class PoseSoundBridge:
|
||||
"""Envoie les keypoints en OSC vers sclang. Throttle a 30 Hz max."""
|
||||
|
||||
@@ -45,9 +89,17 @@ class PoseSoundBridge:
|
||||
self._period = 1.0 / max(1.0, throttle_hz)
|
||||
self._last_t = 0.0
|
||||
|
||||
def send(self, persons_body: list, persons_body_ids: list, t_now: float) -> None:
|
||||
def send(self, persons_body: list, persons_body_ids: list, t_now: float,
|
||||
*,
|
||||
persons_face: Sequence[Sequence[Any]] | None = None,
|
||||
persons_face_ids: Sequence[int] | None = None,
|
||||
persons_hands: Sequence[Sequence[Any]] | None = None,
|
||||
persons_hands_ids: Sequence[int] | None = None,
|
||||
persons_body3d: Sequence[Sequence[Any]] | None = None,
|
||||
persons_body3d_ids: Sequence[int] | None = None) -> None:
|
||||
"""Envoie les keypoints de toutes les personnes detectees.
|
||||
Throttle automatiquement."""
|
||||
Throttle automatiquement. Face / hand sont optionnels et envoyes
|
||||
sur le meme socket :57126 vers AVLiveBody."""
|
||||
if t_now - self._last_t < self._period:
|
||||
return
|
||||
self._last_t = t_now
|
||||
@@ -59,12 +111,20 @@ class PoseSoundBridge:
|
||||
except OSError: pass
|
||||
except OSError:
|
||||
return # SC pas la, on continue silencieusement
|
||||
if n == 0:
|
||||
return
|
||||
|
||||
for i, body in enumerate(persons_body):
|
||||
pid = persons_body_ids[i] if i < len(persons_body_ids) else i
|
||||
self._emit_person(int(pid), body)
|
||||
if n > 0:
|
||||
for i, body in enumerate(persons_body):
|
||||
pid = persons_body_ids[i] if i < len(persons_body_ids) else i
|
||||
self._emit_person(int(pid), body)
|
||||
|
||||
# Face / hand : independant de la presence de body kp (utile en
|
||||
# mode face-only ou hand-only).
|
||||
if persons_face is not None:
|
||||
self._send_face(persons_face, persons_face_ids or [])
|
||||
if persons_hands is not None:
|
||||
self._send_hand(persons_hands, persons_hands_ids or [])
|
||||
if persons_body3d is not None:
|
||||
self._send_body3d(persons_body3d, persons_body3d_ids or [])
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
def _emit_person(self, pid: int, body: list) -> None:
|
||||
@@ -114,6 +174,127 @@ class PoseSoundBridge:
|
||||
try: self._avbody.send_message("/pose/limb_span", [pid, float(span)])
|
||||
except OSError: pass
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
def send_face(self, persons_face: Sequence[Sequence[Any]],
|
||||
persons_face_ids: Sequence[int], t_now: float,
|
||||
force: bool = False) -> None:
|
||||
"""Public throttled entry point for face keypoints.
|
||||
|
||||
Emits a 68-point dlib-style subset of the 468 MediaPipe FaceMesh
|
||||
landmarks per person on /face/count + /face/kp routes.
|
||||
"""
|
||||
if not force and (t_now - self._last_t) < self._period:
|
||||
return
|
||||
self._send_face(persons_face, persons_face_ids)
|
||||
|
||||
def send_hand(self, persons_hands: Sequence[Sequence[Any]],
|
||||
persons_hands_ids: Sequence[int], t_now: float,
|
||||
force: bool = False) -> None:
|
||||
"""Public throttled entry point for hand keypoints.
|
||||
|
||||
Emits the full 21-landmark hand skeleton per detected hand on
|
||||
/hand/count + /hand/kp routes. Side is inferred from id parity
|
||||
(MediaPipe Hand task does not flag left/right reliably) : we
|
||||
treat odd ids as right, even as left, which matches the
|
||||
convention used by the smoother / tracker upstream.
|
||||
"""
|
||||
if not force and (t_now - self._last_t) < self._period:
|
||||
return
|
||||
self._send_hand(persons_hands, persons_hands_ids)
|
||||
|
||||
def _send_face(self, persons_face: Sequence[Sequence[Any]],
|
||||
persons_face_ids: Sequence[int]) -> None:
|
||||
n = len(persons_face)
|
||||
try:
|
||||
self._avbody.send_message("/face/count", [int(n)])
|
||||
except OSError:
|
||||
return
|
||||
for i, face in enumerate(persons_face):
|
||||
if not face:
|
||||
continue
|
||||
pid = persons_face_ids[i] if i < len(persons_face_ids) else i
|
||||
n_lm = len(face)
|
||||
for slot, mp_idx in enumerate(FACE_68_FROM_MP):
|
||||
if mp_idx >= n_lm:
|
||||
continue
|
||||
kp = face[mp_idx]
|
||||
try:
|
||||
self._avbody.send_message("/face/kp", [
|
||||
int(pid), int(slot),
|
||||
float(kp.x), float(kp.y),
|
||||
float(getattr(kp, "z", 0.0)),
|
||||
float(getattr(kp, "c", 1.0)),
|
||||
])
|
||||
except OSError:
|
||||
return
|
||||
|
||||
def _send_hand(self, persons_hands: Sequence[Sequence[Any]],
|
||||
persons_hands_ids: Sequence[int]) -> None:
|
||||
n_left = 0
|
||||
n_right = 0
|
||||
for i in range(len(persons_hands)):
|
||||
pid = persons_hands_ids[i] if i < len(persons_hands_ids) else i
|
||||
if int(pid) % 2 == 0:
|
||||
n_left += 1
|
||||
else:
|
||||
n_right += 1
|
||||
try:
|
||||
self._avbody.send_message("/hand/count", [int(n_left), int(n_right)])
|
||||
except OSError:
|
||||
return
|
||||
for i, hand in enumerate(persons_hands):
|
||||
if not hand:
|
||||
continue
|
||||
pid = persons_hands_ids[i] if i < len(persons_hands_ids) else i
|
||||
side = 1 if int(pid) % 2 else 0
|
||||
for idx, kp in enumerate(hand[:21]):
|
||||
try:
|
||||
self._avbody.send_message("/hand/kp", [
|
||||
int(pid), int(side), int(idx),
|
||||
float(kp.x), float(kp.y),
|
||||
float(getattr(kp, "z", 0.0)),
|
||||
float(getattr(kp, "c", 1.0)),
|
||||
])
|
||||
except OSError:
|
||||
return
|
||||
|
||||
def send_body3d(self, persons_body3d: Sequence[Sequence[Any]],
|
||||
persons_body3d_ids: Sequence[int], t_now: float,
|
||||
force: bool = False) -> None:
|
||||
"""Public throttled entry point for 3D body world landmarks.
|
||||
|
||||
Emits 33 MediaPipe pose_world_landmarks per person on
|
||||
/pose3d/count + /pose3d/kp routes. Coordinates are in meters,
|
||||
relative to the hip-center (MediaPipe convention: x=right,
|
||||
y=down, z=forward from the camera).
|
||||
"""
|
||||
if not force and (t_now - self._last_t) < self._period:
|
||||
return
|
||||
self._send_body3d(persons_body3d, persons_body3d_ids)
|
||||
|
||||
def _send_body3d(self, persons_body3d: Sequence[Sequence[Any]],
|
||||
persons_body3d_ids: Sequence[int]) -> None:
|
||||
n = len(persons_body3d)
|
||||
try:
|
||||
self._avbody.send_message("/pose3d/count", [int(n)])
|
||||
except OSError:
|
||||
return
|
||||
for i, body in enumerate(persons_body3d):
|
||||
if not body:
|
||||
continue
|
||||
pid = persons_body3d_ids[i] if i < len(persons_body3d_ids) else i
|
||||
for idx, kp in enumerate(body[:33]):
|
||||
try:
|
||||
self._avbody.send_message("/pose3d/kp", [
|
||||
int(pid), int(idx),
|
||||
float(kp.x), float(kp.y),
|
||||
float(getattr(kp, "z", 0.0)),
|
||||
float(getattr(kp, "c", 1.0)),
|
||||
])
|
||||
except OSError:
|
||||
return
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
def send_action(self, pid: int, label_idx: int,
|
||||
probs, t_now: float, force: bool = False) -> None:
|
||||
"""Send action classification result via /pose/action OSC route.
|
||||
|
||||
@@ -492,6 +492,9 @@ try:
|
||||
compute_units=ct.ComputeUnit.CPU_AND_GPU,
|
||||
minimum_deployment_target=ct.target.macOS15,
|
||||
convert_to="mlprogram",
|
||||
# FP16 default causes NaN in inverse projection / SMPL-X decoder
|
||||
# (Multi-HMR has values that overflow the FP16 range). Force FP32.
|
||||
compute_precision=ct.precision.FLOAT32,
|
||||
)
|
||||
out_path = "/tmp/multihmr_full_672_s.mlpackage"
|
||||
mlmodel.save(out_path)
|
||||
|
||||
@@ -21,6 +21,16 @@ class PoseKp:
|
||||
c: float = 0.0
|
||||
|
||||
|
||||
@dataclass
|
||||
class Kp3D:
|
||||
"""3D keypoint in metric coordinates relative to hip-center.
|
||||
Used for MediaPipe pose_world_landmarks (xyz in meters)."""
|
||||
x: float = 0.0
|
||||
y: float = 0.0
|
||||
z: float = 0.0
|
||||
c: float = 0.0
|
||||
|
||||
|
||||
@dataclass
|
||||
class SMPLXPerson:
|
||||
"""Resultats Multi-HMR pour une personne : params SMPL-X + vertices
|
||||
@@ -92,6 +102,10 @@ class State:
|
||||
persons_body: list[list[PoseKp]] = field(default_factory=list)
|
||||
persons_face: list[list[PoseKp]] = field(default_factory=list)
|
||||
persons_hands: list[list[PoseKp]] = field(default_factory=list)
|
||||
# MediaPipe pose_world_landmarks per person : 33 keypoints in meters,
|
||||
# relative to the hip-center. Optional companion of persons_body
|
||||
# (image-space xy). Empty if no detection or backend doesn't emit it.
|
||||
persons_body3d: list[list[Kp3D]] = field(default_factory=list)
|
||||
# IDs persistants entre frames (ByteTrack-like via Hungarian IoU).
|
||||
# Couleur du skeleton dans le shader Metal = ID % palette_size.
|
||||
persons_body_ids: list[int] = field(default_factory=list)
|
||||
|
||||
@@ -0,0 +1,79 @@
|
||||
"""Tests for /pose3d/* OSC routes emitted to AVLiveBody."""
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from unittest.mock import MagicMock
|
||||
|
||||
|
||||
@dataclass
|
||||
class _Kp3D:
|
||||
x: float = 0.0
|
||||
y: float = 0.0
|
||||
z: float = 0.0
|
||||
c: float = 1.0
|
||||
|
||||
|
||||
def _make_bridge():
|
||||
from data_only_viz.pose_bridge import PoseSoundBridge
|
||||
b = PoseSoundBridge()
|
||||
b._client = MagicMock()
|
||||
b._avbody = MagicMock()
|
||||
return b
|
||||
|
||||
|
||||
def test_send_body3d_emits_count_and_33_kp() -> None:
|
||||
b = _make_bridge()
|
||||
# One person, 33 keypoints with deterministic xyz.
|
||||
body = [_Kp3D(x=0.01 * i, y=-0.02 * i, z=0.03 * i, c=1.0)
|
||||
for i in range(33)]
|
||||
b.send_body3d([body], [7], t_now=0.0, force=True)
|
||||
|
||||
calls = b._avbody.send_message.call_args_list
|
||||
assert calls[0].args == ("/pose3d/count", [1])
|
||||
kp_calls = [c for c in calls if c.args[0] == "/pose3d/kp"]
|
||||
assert len(kp_calls) == 33
|
||||
# Format : [pid, idx, x, y, z, c]
|
||||
first = kp_calls[0].args[1]
|
||||
assert first[0] == 7
|
||||
assert first[1] == 0
|
||||
assert abs(first[2] - 0.0) < 1e-6
|
||||
assert abs(first[4] - 0.0) < 1e-6
|
||||
# idx ordering strictly 0..32
|
||||
idxs = [c.args[1][1] for c in kp_calls]
|
||||
assert idxs == list(range(33))
|
||||
# Last kp z should be 0.03 * 32
|
||||
last = kp_calls[-1].args[1]
|
||||
assert abs(last[4] - 0.03 * 32) < 1e-6
|
||||
|
||||
|
||||
def test_send_body3d_empty_emits_count_zero() -> None:
|
||||
b = _make_bridge()
|
||||
b.send_body3d([], [], t_now=0.0, force=True)
|
||||
calls = b._avbody.send_message.call_args_list
|
||||
assert len(calls) == 1
|
||||
assert calls[0].args == ("/pose3d/count", [0])
|
||||
|
||||
|
||||
def test_send_body3d_multi_person() -> None:
|
||||
b = _make_bridge()
|
||||
body_a = [_Kp3D(x=1.0) for _ in range(33)]
|
||||
body_b = [_Kp3D(x=2.0) for _ in range(33)]
|
||||
b.send_body3d([body_a, body_b], [10, 11], t_now=0.0, force=True)
|
||||
calls = b._avbody.send_message.call_args_list
|
||||
assert calls[0].args == ("/pose3d/count", [2])
|
||||
kp_calls = [c for c in calls if c.args[0] == "/pose3d/kp"]
|
||||
assert len(kp_calls) == 66
|
||||
assert kp_calls[0].args[1][0] == 10
|
||||
assert kp_calls[33].args[1][0] == 11
|
||||
|
||||
|
||||
def test_send_with_body3d_kwargs_dispatches() -> None:
|
||||
"""Top-level send() routes body3d kwargs to /pose3d."""
|
||||
b = _make_bridge()
|
||||
body = [_Kp3D(x=0.5, y=0.5, c=1.0) for _ in range(33)]
|
||||
body3d = [_Kp3D(x=0.1, y=0.2, z=0.3) for _ in range(33)]
|
||||
b.send([body], [0], 1.0,
|
||||
persons_body3d=[body3d], persons_body3d_ids=[0])
|
||||
av_addrs = {c.args[0] for c in b._avbody.send_message.call_args_list}
|
||||
assert "/pose3d/count" in av_addrs
|
||||
assert "/pose3d/kp" in av_addrs
|
||||
@@ -0,0 +1,105 @@
|
||||
"""Tests for /face/* and /hand/* OSC routes emitted to AVLiveBody."""
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from unittest.mock import MagicMock
|
||||
|
||||
|
||||
@dataclass
|
||||
class _Kp:
|
||||
x: float = 0.0
|
||||
y: float = 0.0
|
||||
z: float = 0.0
|
||||
c: float = 1.0
|
||||
|
||||
|
||||
def _make_bridge():
|
||||
from data_only_viz.pose_bridge import PoseSoundBridge
|
||||
b = PoseSoundBridge()
|
||||
b._client = MagicMock()
|
||||
b._avbody = MagicMock()
|
||||
return b
|
||||
|
||||
|
||||
def test_send_face_emits_count_and_68_kp() -> None:
|
||||
from data_only_viz.pose_bridge import FACE_68_FROM_MP
|
||||
b = _make_bridge()
|
||||
# One face with 478 landmarks at deterministic coords.
|
||||
face = [_Kp(x=i / 478.0, y=1.0 - i / 478.0, z=0.01 * i, c=1.0)
|
||||
for i in range(478)]
|
||||
b.send_face([face], [3], t_now=0.0, force=True)
|
||||
|
||||
calls = b._avbody.send_message.call_args_list
|
||||
# First call : /face/count <1>
|
||||
assert calls[0].args[0] == "/face/count"
|
||||
assert calls[0].args[1] == [1]
|
||||
# Then 68 /face/kp messages
|
||||
kp_calls = [c for c in calls if c.args[0] == "/face/kp"]
|
||||
assert len(kp_calls) == 68
|
||||
# Format : [pid, slot, x, y, z, c]
|
||||
first = kp_calls[0].args[1]
|
||||
assert first[0] == 3
|
||||
assert first[1] == 0
|
||||
assert isinstance(first[2], float)
|
||||
assert isinstance(first[3], float)
|
||||
assert isinstance(first[4], float)
|
||||
assert isinstance(first[5], float)
|
||||
# Slot ordering is strictly increasing 0..67
|
||||
slots = [c.args[1][1] for c in kp_calls]
|
||||
assert slots == list(range(68))
|
||||
# And the x coord of slot 0 matches mp_idx FACE_68_FROM_MP[0]
|
||||
mp0 = FACE_68_FROM_MP[0]
|
||||
assert abs(first[2] - mp0 / 478.0) < 1e-6
|
||||
|
||||
|
||||
def test_send_face_empty_emits_count_zero() -> None:
|
||||
b = _make_bridge()
|
||||
b.send_face([], [], t_now=0.0, force=True)
|
||||
calls = b._avbody.send_message.call_args_list
|
||||
assert len(calls) == 1
|
||||
assert calls[0].args == ("/face/count", [0])
|
||||
|
||||
|
||||
def test_send_hand_emits_count_and_21_kp() -> None:
|
||||
b = _make_bridge()
|
||||
hand_l = [_Kp(x=0.1, y=0.2, z=0.0, c=1.0) for _ in range(21)]
|
||||
hand_r = [_Kp(x=0.7, y=0.3, z=0.0, c=1.0) for _ in range(21)]
|
||||
# pid=2 -> left (even), pid=3 -> right (odd)
|
||||
b.send_hand([hand_l, hand_r], [2, 3], t_now=0.0, force=True)
|
||||
calls = b._avbody.send_message.call_args_list
|
||||
assert calls[0].args == ("/hand/count", [1, 1])
|
||||
kp_calls = [c for c in calls if c.args[0] == "/hand/kp"]
|
||||
assert len(kp_calls) == 42 # 21 * 2
|
||||
# First hand should be side=0 (left)
|
||||
assert kp_calls[0].args[1][1] == 0
|
||||
# 22nd kp call : start of right hand, side=1
|
||||
assert kp_calls[21].args[1][1] == 1
|
||||
|
||||
|
||||
def test_send_throttles_below_period() -> None:
|
||||
"""send_face called twice within < period emits only once."""
|
||||
b = _make_bridge()
|
||||
face = [_Kp(x=0.0, y=0.0) for _ in range(478)]
|
||||
b.send_face([face], [0], t_now=0.0, force=True)
|
||||
n_first = b._avbody.send_message.call_count
|
||||
# Second call without force AND inside throttle window : skipped.
|
||||
b._last_t = 9999.0 # pretend a body send just happened
|
||||
b.send_face([face], [0], t_now=9999.0 + 0.001, force=False)
|
||||
assert b._avbody.send_message.call_count == n_first
|
||||
|
||||
|
||||
def test_send_with_face_hand_kwargs_dispatches() -> None:
|
||||
"""Top-level send() routes face/hand kwargs to /face and /hand."""
|
||||
b = _make_bridge()
|
||||
body = [_Kp(x=0.5, y=0.5, c=1.0) for _ in range(33)]
|
||||
face = [_Kp(x=0.1, y=0.1, c=1.0) for _ in range(478)]
|
||||
hand = [_Kp(x=0.2, y=0.2, c=1.0) for _ in range(21)]
|
||||
b.send([body], [0], 1.0,
|
||||
persons_face=[face], persons_face_ids=[0],
|
||||
persons_hands=[hand], persons_hands_ids=[1])
|
||||
av_addrs = {c.args[0] for c in b._avbody.send_message.call_args_list}
|
||||
assert "/pose/count" in av_addrs
|
||||
assert "/face/count" in av_addrs
|
||||
assert "/face/kp" in av_addrs
|
||||
assert "/hand/count" in av_addrs
|
||||
assert "/hand/kp" in av_addrs
|
||||
@@ -1,8 +1,19 @@
|
||||
import Cocoa
|
||||
import SwiftUI
|
||||
|
||||
// SwiftPM binaries lack a bundle Info.plist, so macOS treats us as a
|
||||
// background CLI app and never shows the WindowGroup window. The
|
||||
// AppDelegate forces regular activation after NSApp is initialized.
|
||||
class AppDelegate: NSObject, NSApplicationDelegate {
|
||||
func applicationDidFinishLaunching(_ notification: Notification) {
|
||||
NSApp.setActivationPolicy(.regular)
|
||||
NSApp.activate(ignoringOtherApps: true)
|
||||
}
|
||||
}
|
||||
|
||||
@main
|
||||
struct AVLiveBodyApp: App {
|
||||
@NSApplicationDelegateAdaptor(AppDelegate.self) var appDelegate
|
||||
var body: some Scene {
|
||||
WindowGroup {
|
||||
ContentView()
|
||||
@@ -46,6 +57,11 @@ struct AVLiveBodyApp: App {
|
||||
KeyEquivalent(Character(String(i))),
|
||||
modifiers: [])
|
||||
}
|
||||
// Alias 'p' for openpos (skeleton view).
|
||||
Button("p — openpos (squelette)") {
|
||||
NotificationCenter.default.post(
|
||||
name: .setVizMode, object: 9)
|
||||
}.keyboardShortcut("p", modifiers: [])
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -61,11 +77,12 @@ struct ContentView: View {
|
||||
@StateObject private var renderer = MeshRenderer()
|
||||
@StateObject private var settings = RenderSettings()
|
||||
@StateObject private var poseListener = PoseOSCListener()
|
||||
@StateObject private var skeleton3d = Skeleton3DRenderer()
|
||||
|
||||
var body: some View {
|
||||
ZStack(alignment: .topLeading) {
|
||||
BodyView(renderer: renderer, settings: settings,
|
||||
poseListener: poseListener)
|
||||
poseListener: poseListener, skeleton3d: skeleton3d)
|
||||
.onAppear {
|
||||
renderer.startOSCServer()
|
||||
poseListener.start()
|
||||
@@ -83,6 +100,10 @@ struct ContentView: View {
|
||||
if let n = note.object as? Int { settings.vizMode = n }
|
||||
}
|
||||
|
||||
// Face + hand skeleton overlay (data_only_viz/pose_bridge.py)
|
||||
FaceHandOverlay(poseListener: poseListener)
|
||||
.allowsHitTesting(false)
|
||||
|
||||
// HUD coin haut-gauche : mode + touches + pose
|
||||
HUDOverlay(settings: settings, poseListener: poseListener)
|
||||
|
||||
|
||||
@@ -11,6 +11,7 @@ struct BodyView: NSViewRepresentable {
|
||||
@ObservedObject var renderer: MeshRenderer
|
||||
@ObservedObject var settings: RenderSettings
|
||||
@ObservedObject var poseListener: PoseOSCListener
|
||||
@ObservedObject var skeleton3d: Skeleton3DRenderer
|
||||
|
||||
func makeNSView(context: Context) -> NSView {
|
||||
let container = NSView(frame: .zero)
|
||||
@@ -88,6 +89,14 @@ struct BodyView: NSViewRepresentable {
|
||||
|
||||
let bodyAnchor = AnchorEntity(world: .zero)
|
||||
arView.scene.addAnchor(bodyAnchor)
|
||||
|
||||
// Dedicated anchor for the 3D skeleton (mode 9 / openpos).
|
||||
// Positioned at the origin ; the perspective camera at z=0 with
|
||||
// default FOV frames a ~3 m-deep stage centered on the hip.
|
||||
let skelAnchor = AnchorEntity(world: SIMD3<Float>(0, 0, -3))
|
||||
arView.scene.addAnchor(skelAnchor)
|
||||
skeleton3d.attach(to: skelAnchor, listener: poseListener)
|
||||
|
||||
container.addSubview(arView)
|
||||
|
||||
// 60 fps mesh interpolation between Multi-HMR frames (Python
|
||||
@@ -105,6 +114,7 @@ struct BodyView: NSViewRepresentable {
|
||||
context.coordinator.previewLayer = preview
|
||||
context.coordinator.container = container
|
||||
context.coordinator.renderer = renderer
|
||||
context.coordinator.skelAnchor = skelAnchor
|
||||
return container
|
||||
}
|
||||
|
||||
@@ -141,6 +151,9 @@ struct BodyView: NSViewRepresentable {
|
||||
c.fillLight?.light.intensity = Float(settings.fillIntensity)
|
||||
c.rimLight?.light.intensity = Float(settings.rimIntensity)
|
||||
|
||||
// 3D skeleton only visible in mode 9 (openpos).
|
||||
c.skelAnchor?.isEnabled = (settings.vizMode == 9)
|
||||
|
||||
// Mesh visibility + material
|
||||
guard let anchor = c.bodyAnchor else { return }
|
||||
anchor.children.removeAll()
|
||||
@@ -159,6 +172,7 @@ struct BodyView: NSViewRepresentable {
|
||||
|
||||
final class Coordinator {
|
||||
var bodyAnchor: AnchorEntity?
|
||||
var skelAnchor: AnchorEntity?
|
||||
var arView: ARView?
|
||||
var cameraEntity: PerspectiveCamera?
|
||||
var sceneRenderer: SceneRenderer?
|
||||
|
||||
@@ -0,0 +1,155 @@
|
||||
import SwiftUI
|
||||
import simd
|
||||
|
||||
/// Lightweight SwiftUI overlay that draws the 68-point face skeleton and
|
||||
/// 21-point hand skeletons sent by data_only_viz/pose_bridge.py over OSC.
|
||||
/// Sits in the ContentView ZStack above BodyView. Coordinates from the
|
||||
/// listener are normalised (0..1 in image space) ; here we map them to
|
||||
/// the overlay's geometry. Rendering is intentionally minimal : small
|
||||
/// dots + a few polylines for facial features and hand bones.
|
||||
struct FaceHandOverlay: View {
|
||||
@ObservedObject var poseListener: PoseOSCListener
|
||||
var showFace: Bool = true
|
||||
var showHands: Bool = true
|
||||
|
||||
var body: some View {
|
||||
GeometryReader { geo in
|
||||
Canvas { ctx, size in
|
||||
if showFace {
|
||||
for face in poseListener.faces.values {
|
||||
drawFace(face, in: &ctx, size: size)
|
||||
}
|
||||
}
|
||||
if showHands {
|
||||
for hand in poseListener.hands.values {
|
||||
drawHand(hand, in: &ctx, size: size)
|
||||
}
|
||||
}
|
||||
}
|
||||
.frame(width: geo.size.width, height: geo.size.height)
|
||||
.allowsHitTesting(false)
|
||||
}
|
||||
}
|
||||
|
||||
// MARK: - Face (dlib 68 layout)
|
||||
|
||||
/// Index spans in the 68-point dlib convention.
|
||||
private static let jaw = Array(0..<17)
|
||||
private static let browR = Array(17..<22)
|
||||
private static let browL = Array(22..<27)
|
||||
private static let noseBridge = Array(27..<31)
|
||||
private static let nostril = Array(31..<36)
|
||||
private static let eyeR = Array(36..<42)
|
||||
private static let eyeL = Array(42..<48)
|
||||
private static let lipOuter = Array(48..<60)
|
||||
private static let lipInner = Array(60..<68)
|
||||
|
||||
private func drawFace(_ face: PoseOSCListener.FaceFrame,
|
||||
in ctx: inout GraphicsContext,
|
||||
size: CGSize) {
|
||||
let stroke = GraphicsContext.Shading.color(.green.opacity(0.85))
|
||||
let dot = GraphicsContext.Shading.color(.green.opacity(0.95))
|
||||
|
||||
drawPolyline(face.points, indices: Self.jaw, closed: false,
|
||||
in: &ctx, size: size, shading: stroke, width: 1.2)
|
||||
drawPolyline(face.points, indices: Self.browR, closed: false,
|
||||
in: &ctx, size: size, shading: stroke, width: 1.2)
|
||||
drawPolyline(face.points, indices: Self.browL, closed: false,
|
||||
in: &ctx, size: size, shading: stroke, width: 1.2)
|
||||
drawPolyline(face.points, indices: Self.noseBridge, closed: false,
|
||||
in: &ctx, size: size, shading: stroke, width: 1.2)
|
||||
drawPolyline(face.points, indices: Self.nostril, closed: false,
|
||||
in: &ctx, size: size, shading: stroke, width: 1.2)
|
||||
drawPolyline(face.points, indices: Self.eyeR, closed: true,
|
||||
in: &ctx, size: size, shading: stroke, width: 1.2)
|
||||
drawPolyline(face.points, indices: Self.eyeL, closed: true,
|
||||
in: &ctx, size: size, shading: stroke, width: 1.2)
|
||||
drawPolyline(face.points, indices: Self.lipOuter, closed: true,
|
||||
in: &ctx, size: size, shading: stroke, width: 1.2)
|
||||
drawPolyline(face.points, indices: Self.lipInner, closed: true,
|
||||
in: &ctx, size: size, shading: stroke, width: 1.0)
|
||||
|
||||
for i in 0..<68 where face.hasPoint[i] {
|
||||
let p = mapPoint(face.points[i], size: size)
|
||||
let r = CGRect(x: p.x - 1.2, y: p.y - 1.2,
|
||||
width: 2.4, height: 2.4)
|
||||
ctx.fill(Path(ellipseIn: r), with: dot)
|
||||
}
|
||||
}
|
||||
|
||||
// MARK: - Hand (MediaPipe 21 landmarks)
|
||||
|
||||
/// MediaPipe hand bone connectivity (5 fingers x 4 bones + palm).
|
||||
private static let handBones: [(Int, Int)] = [
|
||||
// Thumb
|
||||
(0, 1), (1, 2), (2, 3), (3, 4),
|
||||
// Index
|
||||
(0, 5), (5, 6), (6, 7), (7, 8),
|
||||
// Middle
|
||||
(5, 9), (9, 10), (10, 11), (11, 12),
|
||||
// Ring
|
||||
(9, 13), (13, 14), (14, 15), (15, 16),
|
||||
// Pinky
|
||||
(13, 17), (17, 18), (18, 19), (19, 20),
|
||||
// Palm closure
|
||||
(0, 17),
|
||||
]
|
||||
|
||||
private func drawHand(_ hand: PoseOSCListener.HandFrame,
|
||||
in ctx: inout GraphicsContext,
|
||||
size: CGSize) {
|
||||
// Left = cyan, right = magenta.
|
||||
let color: Color = hand.side == 0 ? .cyan : .pink
|
||||
let stroke = GraphicsContext.Shading.color(color.opacity(0.85))
|
||||
let dot = GraphicsContext.Shading.color(color.opacity(0.95))
|
||||
|
||||
for (a, b) in Self.handBones {
|
||||
guard hand.hasPoint[a], hand.hasPoint[b] else { continue }
|
||||
let pa = mapPoint(hand.points[a], size: size)
|
||||
let pb = mapPoint(hand.points[b], size: size)
|
||||
var path = Path()
|
||||
path.move(to: pa)
|
||||
path.addLine(to: pb)
|
||||
ctx.stroke(path, with: stroke, lineWidth: 1.8)
|
||||
}
|
||||
for i in 0..<21 where hand.hasPoint[i] {
|
||||
let p = mapPoint(hand.points[i], size: size)
|
||||
let r = CGRect(x: p.x - 1.8, y: p.y - 1.8,
|
||||
width: 3.6, height: 3.6)
|
||||
ctx.fill(Path(ellipseIn: r), with: dot)
|
||||
}
|
||||
}
|
||||
|
||||
// MARK: - Helpers
|
||||
|
||||
private func drawPolyline(_ pts: [SIMD2<Float>],
|
||||
indices: [Int],
|
||||
closed: Bool,
|
||||
in ctx: inout GraphicsContext,
|
||||
size: CGSize,
|
||||
shading: GraphicsContext.Shading,
|
||||
width: CGFloat) {
|
||||
guard indices.count >= 2 else { return }
|
||||
var path = Path()
|
||||
var started = false
|
||||
for i in indices {
|
||||
let p = mapPoint(pts[i], size: size)
|
||||
if !started {
|
||||
path.move(to: p)
|
||||
started = true
|
||||
} else {
|
||||
path.addLine(to: p)
|
||||
}
|
||||
}
|
||||
if closed, let first = indices.first {
|
||||
path.addLine(to: mapPoint(pts[first], size: size))
|
||||
}
|
||||
ctx.stroke(path, with: shading, lineWidth: width)
|
||||
}
|
||||
|
||||
private func mapPoint(_ p: SIMD2<Float>, size: CGSize) -> CGPoint {
|
||||
// Normalised coords come from MediaPipe in image space already.
|
||||
CGPoint(x: CGFloat(p.x) * size.width,
|
||||
y: CGFloat(p.y) * size.height)
|
||||
}
|
||||
}
|
||||
@@ -23,8 +23,42 @@ final class PoseOSCListener: ObservableObject {
|
||||
var seenAt: TimeInterval = 0
|
||||
}
|
||||
|
||||
/// 68-point dlib-style facial landmarks (x,y normalises 0..1).
|
||||
/// Slot mapping comes from FACE_68_FROM_MP cote Python.
|
||||
struct FaceFrame: Equatable {
|
||||
var points: [SIMD2<Float>] = Array(repeating: .zero, count: 68)
|
||||
var hasPoint: [Bool] = Array(repeating: false, count: 68)
|
||||
var seenAt: TimeInterval = 0
|
||||
}
|
||||
|
||||
/// 21 MediaPipe hand landmarks per detected hand.
|
||||
struct HandFrame: Equatable {
|
||||
var side: Int = 0 // 0 = left, 1 = right
|
||||
var points: [SIMD2<Float>] = Array(repeating: .zero, count: 21)
|
||||
var hasPoint: [Bool] = Array(repeating: false, count: 21)
|
||||
var seenAt: TimeInterval = 0
|
||||
}
|
||||
|
||||
/// MediaPipe pose_world_landmarks : 33 keypoints in meters, relative
|
||||
/// to the hip-center. Conventions on the wire (MediaPipe):
|
||||
/// x = right, y = down, z = forward (away from camera).
|
||||
struct Pose3DFrame: Equatable {
|
||||
var pid: Int = -1
|
||||
// SIMD4 = (x, y, z, confidence). All zeros = slot not yet filled.
|
||||
var kps: [SIMD4<Float>] = Array(repeating: .zero, count: 33)
|
||||
var hasPoint: [Bool] = Array(repeating: false, count: 33)
|
||||
var seenAt: TimeInterval = 0
|
||||
}
|
||||
|
||||
@Published var persons: [Int: PoseFrame] = [:]
|
||||
@Published var faces: [Int: FaceFrame] = [:]
|
||||
@Published var hands: [Int: HandFrame] = [:]
|
||||
@Published var body3d: [Int: Pose3DFrame] = [:]
|
||||
@Published var count: Int = 0
|
||||
@Published var faceCount: Int = 0
|
||||
@Published var body3dCount: Int = 0
|
||||
@Published var handCountLeft: Int = 0
|
||||
@Published var handCountRight: Int = 0
|
||||
|
||||
private var listener: NWListener?
|
||||
|
||||
@@ -127,6 +161,69 @@ final class PoseOSCListener: ObservableObject {
|
||||
var p = persons[Int(pid)] ?? PoseFrame()
|
||||
p.bodyPitch = v
|
||||
persons[Int(pid)] = p
|
||||
case "/face/count":
|
||||
if let n = args.first as? Int32 { faceCount = Int(n) }
|
||||
if faceCount == 0 { faces.removeAll(keepingCapacity: true) }
|
||||
case "/face/kp":
|
||||
guard args.count >= 6,
|
||||
let pid = args[0] as? Int32,
|
||||
let slot = args[1] as? Int32,
|
||||
let x = args[2] as? Float,
|
||||
let y = args[3] as? Float else { return }
|
||||
let slotI = Int(slot)
|
||||
guard slotI >= 0 && slotI < 68 else { return }
|
||||
var f = faces[Int(pid)] ?? FaceFrame()
|
||||
f.points[slotI] = SIMD2(x, y)
|
||||
f.hasPoint[slotI] = true
|
||||
f.seenAt = CFAbsoluteTimeGetCurrent()
|
||||
faces[Int(pid)] = f
|
||||
case "/hand/count":
|
||||
if args.count >= 2,
|
||||
let l = args[0] as? Int32, let r = args[1] as? Int32 {
|
||||
handCountLeft = Int(l)
|
||||
handCountRight = Int(r)
|
||||
if handCountLeft + handCountRight == 0 {
|
||||
hands.removeAll(keepingCapacity: true)
|
||||
}
|
||||
}
|
||||
case "/pose3d/count":
|
||||
if let n = args.first as? Int32 {
|
||||
body3dCount = Int(n)
|
||||
if body3dCount == 0 {
|
||||
body3d.removeAll(keepingCapacity: true)
|
||||
}
|
||||
}
|
||||
case "/pose3d/kp":
|
||||
guard args.count >= 6,
|
||||
let pid = args[0] as? Int32,
|
||||
let idx = args[1] as? Int32,
|
||||
let x = args[2] as? Float,
|
||||
let y = args[3] as? Float,
|
||||
let z = args[4] as? Float,
|
||||
let c = args[5] as? Float else { return }
|
||||
let i = Int(idx)
|
||||
guard i >= 0 && i < 33 else { return }
|
||||
var p = body3d[Int(pid)] ?? Pose3DFrame(pid: Int(pid))
|
||||
p.pid = Int(pid)
|
||||
p.kps[i] = SIMD4<Float>(x, y, z, c)
|
||||
p.hasPoint[i] = true
|
||||
p.seenAt = CFAbsoluteTimeGetCurrent()
|
||||
body3d[Int(pid)] = p
|
||||
case "/hand/kp":
|
||||
guard args.count >= 7,
|
||||
let pid = args[0] as? Int32,
|
||||
let side = args[1] as? Int32,
|
||||
let idx = args[2] as? Int32,
|
||||
let x = args[3] as? Float,
|
||||
let y = args[4] as? Float else { return }
|
||||
let i = Int(idx)
|
||||
guard i >= 0 && i < 21 else { return }
|
||||
var h = hands[Int(pid)] ?? HandFrame()
|
||||
h.side = Int(side)
|
||||
h.points[i] = SIMD2(x, y)
|
||||
h.hasPoint[i] = true
|
||||
h.seenAt = CFAbsoluteTimeGetCurrent()
|
||||
hands[Int(pid)] = h
|
||||
default:
|
||||
break
|
||||
}
|
||||
@@ -134,6 +231,12 @@ final class PoseOSCListener: ObservableObject {
|
||||
let now = CFAbsoluteTimeGetCurrent()
|
||||
persons = persons.filter { $0.value.seenAt == 0
|
||||
|| now - $0.value.seenAt < 2.0 }
|
||||
faces = faces.filter { $0.value.seenAt == 0
|
||||
|| now - $0.value.seenAt < 2.0 }
|
||||
hands = hands.filter { $0.value.seenAt == 0
|
||||
|| now - $0.value.seenAt < 2.0 }
|
||||
body3d = body3d.filter { $0.value.seenAt == 0
|
||||
|| now - $0.value.seenAt < 2.0 }
|
||||
}
|
||||
|
||||
// MARK: - Minimal OSC parser
|
||||
|
||||
@@ -0,0 +1,222 @@
|
||||
import Combine
|
||||
import Foundation
|
||||
import RealityKit
|
||||
import SwiftUI
|
||||
import simd
|
||||
|
||||
/// RealityKit renderer for MediaPipe Pose 3D world landmarks (33 joints,
|
||||
/// metric coords relative to the hip-center). Consumes the `body3d`
|
||||
/// publisher of `PoseOSCListener` and maintains one entity tree per
|
||||
/// detected person.
|
||||
///
|
||||
/// Coordinate mapping (MediaPipe -> RealityKit):
|
||||
/// MediaPipe : x = right, y = down, z = forward (away from cam).
|
||||
/// RealityKit: x = right, y = up, z = backward (toward cam).
|
||||
/// => convert with (x, -y, -z).
|
||||
@MainActor
|
||||
final class Skeleton3DRenderer: ObservableObject {
|
||||
/// 32 bones connecting MediaPipe Pose 33 landmarks. Indices are
|
||||
/// the canonical MediaPipe Pose landmark indices. Source: official
|
||||
/// `mp.solutions.pose.POSE_CONNECTIONS` (Holistic / Pose Landmarker
|
||||
/// share the same 33-pt schema).
|
||||
static let POSE_CONNECTIONS: [(Int, Int, BoneChain)] = [
|
||||
// Face (kept light: nose <-> inner eyes <-> outer eyes <-> ears)
|
||||
(0, 1, .face), (1, 2, .face), (2, 3, .face), (3, 7, .face),
|
||||
(0, 4, .face), (4, 5, .face), (5, 6, .face), (6, 8, .face),
|
||||
(9, 10, .face),
|
||||
// Torso
|
||||
(11, 12, .trunk), (11, 23, .trunk), (12, 24, .trunk),
|
||||
(23, 24, .trunk),
|
||||
// Left arm
|
||||
(11, 13, .arm), (13, 15, .arm),
|
||||
(15, 17, .arm), (15, 19, .arm), (15, 21, .arm), (17, 19, .arm),
|
||||
// Right arm
|
||||
(12, 14, .arm), (14, 16, .arm),
|
||||
(16, 18, .arm), (16, 20, .arm), (16, 22, .arm), (18, 20, .arm),
|
||||
// Left leg
|
||||
(23, 25, .leg), (25, 27, .leg),
|
||||
(27, 29, .leg), (27, 31, .leg), (29, 31, .leg),
|
||||
// Right leg
|
||||
(24, 26, .leg), (26, 28, .leg),
|
||||
(28, 30, .leg), (28, 32, .leg), (30, 32, .leg),
|
||||
]
|
||||
|
||||
enum BoneChain {
|
||||
case trunk, arm, leg, face
|
||||
var color: NSColor {
|
||||
switch self {
|
||||
case .trunk: return .white
|
||||
case .arm: return .systemTeal
|
||||
case .leg: return .systemPink // approx magenta
|
||||
case .face: return NSColor(white: 0.7, alpha: 1.0)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private static let jointRadius: Float = 0.02 // 2 cm
|
||||
private static let boneRadius: Float = 0.012 // 1.2 cm
|
||||
private static let minConfidence: Float = 0.3
|
||||
private static let retainSec: TimeInterval = 1.0
|
||||
|
||||
/// Update throttle : tick at most every `updatePeriod` seconds even
|
||||
/// if the publisher fires faster (Combine debounce-style on a clock).
|
||||
private static let updatePeriod: TimeInterval = 1.0 / 30.0
|
||||
|
||||
private struct PersonEntities {
|
||||
var root: Entity
|
||||
var joints: [ModelEntity] // 33 spheres
|
||||
var bones: [ModelEntity] // 32 bone entities, same order as POSE_CONNECTIONS
|
||||
}
|
||||
|
||||
private var persons: [Int: PersonEntities] = [:]
|
||||
private var lastSeenAt: [Int: TimeInterval] = [:]
|
||||
private weak var rootAnchor: Entity?
|
||||
private var poseSub: AnyCancellable?
|
||||
private var lastUpdateAt: TimeInterval = 0
|
||||
|
||||
/// Attach to a scene by giving it an AnchorEntity that owns all
|
||||
/// skeleton entities, and start observing the listener.
|
||||
func attach(to anchor: Entity, listener: PoseOSCListener) {
|
||||
rootAnchor = anchor
|
||||
poseSub = listener.$body3d
|
||||
.receive(on: DispatchQueue.main)
|
||||
.sink { [weak self] frames in
|
||||
Task { @MainActor in self?.update(frames: frames) }
|
||||
}
|
||||
}
|
||||
|
||||
func detach() {
|
||||
poseSub?.cancel()
|
||||
poseSub = nil
|
||||
for (_, p) in persons { p.root.removeFromParent() }
|
||||
persons.removeAll()
|
||||
lastSeenAt.removeAll()
|
||||
}
|
||||
|
||||
// MARK: - Update
|
||||
|
||||
private func update(frames: [Int: PoseOSCListener.Pose3DFrame]) {
|
||||
let now = CACurrentMediaTime()
|
||||
if now - lastUpdateAt < Self.updatePeriod { return }
|
||||
lastUpdateAt = now
|
||||
|
||||
guard let anchor = rootAnchor else { return }
|
||||
|
||||
// Mark fresh pids
|
||||
for pid in frames.keys { lastSeenAt[pid] = now }
|
||||
// GC stale persons
|
||||
let cutoff = now - Self.retainSec
|
||||
for (pid, p) in persons where (lastSeenAt[pid] ?? 0) < cutoff {
|
||||
p.root.removeFromParent()
|
||||
persons.removeValue(forKey: pid)
|
||||
lastSeenAt.removeValue(forKey: pid)
|
||||
}
|
||||
|
||||
for (pid, frame) in frames {
|
||||
let entities = persons[pid] ?? makePerson(pid: pid, parent: anchor)
|
||||
persons[pid] = entities
|
||||
apply(frame: frame, to: entities)
|
||||
}
|
||||
}
|
||||
|
||||
private func apply(frame: PoseOSCListener.Pose3DFrame,
|
||||
to entities: PersonEntities) {
|
||||
// Convert all 33 keypoints to RealityKit space once.
|
||||
var rk = [SIMD3<Float>](repeating: .zero, count: 33)
|
||||
var valid = [Bool](repeating: false, count: 33)
|
||||
for i in 0..<33 {
|
||||
let k = frame.kps[i]
|
||||
let visible = frame.hasPoint[i] && k.w >= Self.minConfidence
|
||||
valid[i] = visible
|
||||
// Mediapipe (x right, y down, z forward) -> RK (x right, y up, z back)
|
||||
rk[i] = SIMD3<Float>(k.x, -k.y, -k.z)
|
||||
}
|
||||
|
||||
// Joints: position spheres and toggle visibility.
|
||||
for i in 0..<33 {
|
||||
let joint = entities.joints[i]
|
||||
if valid[i] {
|
||||
joint.transform.translation = rk[i]
|
||||
joint.isEnabled = true
|
||||
} else {
|
||||
joint.isEnabled = false
|
||||
}
|
||||
}
|
||||
|
||||
// Bones: orient + scale length between endpoints.
|
||||
for (bIdx, (a, b, _)) in Self.POSE_CONNECTIONS.enumerated() {
|
||||
let bone = entities.bones[bIdx]
|
||||
if !valid[a] || !valid[b] {
|
||||
bone.isEnabled = false
|
||||
continue
|
||||
}
|
||||
let pa = rk[a]
|
||||
let pb = rk[b]
|
||||
let delta = pb - pa
|
||||
let len = simd_length(delta)
|
||||
if len < 1e-5 {
|
||||
bone.isEnabled = false
|
||||
continue
|
||||
}
|
||||
let mid = (pa + pb) * 0.5
|
||||
// Bone mesh is a cylinder of height=1 along +Y. Rotate +Y
|
||||
// onto the (b-a) direction.
|
||||
let dir = delta / len
|
||||
let yAxis = SIMD3<Float>(0, 1, 0)
|
||||
let dot = simd_dot(yAxis, dir)
|
||||
let rot: simd_quatf
|
||||
if dot > 0.9999 {
|
||||
rot = simd_quatf(angle: 0, axis: SIMD3(0, 1, 0))
|
||||
} else if dot < -0.9999 {
|
||||
rot = simd_quatf(angle: .pi, axis: SIMD3(1, 0, 0))
|
||||
} else {
|
||||
let axis = simd_normalize(simd_cross(yAxis, dir))
|
||||
let angle = acos(dot)
|
||||
rot = simd_quatf(angle: angle, axis: axis)
|
||||
}
|
||||
bone.transform.translation = mid
|
||||
bone.transform.rotation = rot
|
||||
// Scale length only on Y, keep XZ at 1 to preserve radius.
|
||||
bone.transform.scale = SIMD3<Float>(1, len, 1)
|
||||
bone.isEnabled = true
|
||||
}
|
||||
}
|
||||
|
||||
// MARK: - Construction
|
||||
|
||||
private func makePerson(pid: Int, parent: Entity) -> PersonEntities {
|
||||
let root = Entity()
|
||||
parent.addChild(root)
|
||||
|
||||
// Joint sphere mesh shared across joints (cheap to reuse).
|
||||
let sphereMesh = MeshResource.generateSphere(
|
||||
radius: Self.jointRadius)
|
||||
let jointMat = SimpleMaterial(
|
||||
color: .white, roughness: 0.6, isMetallic: false)
|
||||
var joints: [ModelEntity] = []
|
||||
joints.reserveCapacity(33)
|
||||
for _ in 0..<33 {
|
||||
let e = ModelEntity(mesh: sphereMesh, materials: [jointMat])
|
||||
e.isEnabled = false
|
||||
root.addChild(e)
|
||||
joints.append(e)
|
||||
}
|
||||
|
||||
// One cylinder per bone (height=1, scaled at runtime).
|
||||
let cylMesh = MeshResource.generateCylinder(
|
||||
height: 1.0, radius: Self.boneRadius)
|
||||
var bones: [ModelEntity] = []
|
||||
bones.reserveCapacity(Self.POSE_CONNECTIONS.count)
|
||||
for (_, _, chain) in Self.POSE_CONNECTIONS {
|
||||
let mat = SimpleMaterial(
|
||||
color: chain.color, roughness: 0.6, isMetallic: false)
|
||||
let e = ModelEntity(mesh: cylMesh, materials: [mat])
|
||||
e.isEnabled = false
|
||||
root.addChild(e)
|
||||
bones.append(e)
|
||||
}
|
||||
NSLog("Skeleton3DRenderer: spawned pid=%d (33 joints, %d bones)",
|
||||
pid, bones.count)
|
||||
return PersonEntities(root: root, joints: joints, bones: bones)
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user