feat(pose): decode iphone 2D skeleton
Add TAG_SKELETON2D=6, SKEL2D_BYTES=819, decode_skeleton2D() to iphone_usb_bridge.py; handler in IphoneUSBSource writing persons_arkit_2d / persons_arkit_2d_t to state; new state fields in State dataclass. TDD: test_arkit_body.py (round-trip + wrong length → None). Import check verified.
This commit is contained in:
@@ -18,7 +18,8 @@ import cv2
|
||||
import numpy as np
|
||||
|
||||
from data_only_viz.scripts.iphone_usb_bridge import (
|
||||
connect_device, decode_skeleton, iter_frames, TAG_SKELETON,
|
||||
connect_device, decode_skeleton, decode_skeleton2D,
|
||||
iter_frames, TAG_SKELETON, TAG_SKELETON2D,
|
||||
)
|
||||
from data_only_viz.state import PoseKp
|
||||
|
||||
@@ -148,6 +149,13 @@ class IphoneUSBSource:
|
||||
arr[i] = (x, y, z)
|
||||
self.state.persons_arkit_joints[pid] = arr
|
||||
self.state.persons_arkit_last_t[pid] = time.perf_counter()
|
||||
elif tag == TAG_SKELETON2D and self.state is not None:
|
||||
pts = decode_skeleton2D(payload)
|
||||
if pts is not None:
|
||||
arr = np.array([[x, y] for (x, y, _v) in pts], dtype=np.float32)
|
||||
with self.state.lock():
|
||||
self.state.persons_arkit_2d[pid] = arr
|
||||
self.state.persons_arkit_2d_t[pid] = time.perf_counter()
|
||||
elif tag == TAG_HANDS:
|
||||
hands = _decode_hands(payload)
|
||||
if hands is not None and self.state is not None:
|
||||
|
||||
@@ -44,6 +44,9 @@ TAG_SKELETON = 1
|
||||
JOINT_COUNT = 91
|
||||
SKEL_FLOAT_BYTES = JOINT_COUNT * 3 * 4 # 1092
|
||||
SKEL_BYTES = SKEL_FLOAT_BYTES + JOINT_COUNT # 1183
|
||||
TAG_SKELETON2D = 6 # wire: skeleton2D (face=5 taken)
|
||||
SKEL2D_FLOAT_BYTES = JOINT_COUNT * 2 * 4 # 728
|
||||
SKEL2D_BYTES = SKEL2D_FLOAT_BYTES + JOINT_COUNT # 819
|
||||
MAX_PAYLOAD = 8 * 1024 * 1024
|
||||
|
||||
LOG = logging.getLogger("iphone_usb_bridge")
|
||||
@@ -169,6 +172,19 @@ def decode_skeleton(payload: bytes):
|
||||
]
|
||||
|
||||
|
||||
def decode_skeleton2D(payload: bytes):
|
||||
"""Return [(x, y, valid), ...] of length 91 (normalized 0..1), or None.
|
||||
|
||||
Wire layout: 91 × (x f32 BE, y f32 BE) = 728 bytes, then 91 validity bytes.
|
||||
Total: SKEL2D_BYTES = 819.
|
||||
"""
|
||||
if len(payload) != SKEL2D_BYTES:
|
||||
return None
|
||||
floats = struct.unpack(">" + "f" * (JOINT_COUNT * 2), payload[:SKEL2D_FLOAT_BYTES])
|
||||
valid = payload[SKEL2D_FLOAT_BYTES:]
|
||||
return [(floats[i * 2], floats[i * 2 + 1], valid[i] != 0) for i in range(JOINT_COUNT)]
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------
|
||||
# main loop
|
||||
# --------------------------------------------------------------------------
|
||||
|
||||
@@ -144,6 +144,10 @@ class State:
|
||||
# (metres, hip-centered). Fresh = updated within < 1 s.
|
||||
persons_arkit_joints: dict = field(default_factory=dict)
|
||||
persons_arkit_last_t: dict = field(default_factory=dict)
|
||||
# ARKit 2D projected skeleton (Task 3): 91×2 float32 arrays (normalized
|
||||
# screen coords 0..1) per pid. Updated by IphoneUSBSource on TAG_SKELETON2D.
|
||||
persons_arkit_2d: dict[int, "np.ndarray"] = field(default_factory=dict)
|
||||
persons_arkit_2d_t: dict[int, float] = field(default_factory=dict)
|
||||
|
||||
# ---- LiDAR / ICP mesh fusion (Task 8 - 2026-05-14) ----
|
||||
# Set by the LidarTCPReader poller; consumed by FusionWorker.run_once.
|
||||
|
||||
@@ -0,0 +1,29 @@
|
||||
"""Task 3: decode_skeleton2D round-trip tests.
|
||||
|
||||
Wire layout (TAG_SKELETON2D = 6):
|
||||
91 joints * (x: f32 BE, y: f32 BE) = 728 bytes of floats
|
||||
+ 91 validity bytes
|
||||
= 819 bytes total
|
||||
"""
|
||||
import struct
|
||||
|
||||
from data_only_viz.scripts.iphone_usb_bridge import decode_skeleton2D, JOINT_COUNT
|
||||
|
||||
|
||||
def test_decode_skeleton2d_roundtrip():
|
||||
floats = []
|
||||
for i in range(JOINT_COUNT):
|
||||
floats += [i * 0.001, i * 0.002]
|
||||
payload = struct.pack(">" + "f" * (JOINT_COUNT * 2), *floats)
|
||||
payload += bytes(1 if i % 2 == 0 else 0 for i in range(JOINT_COUNT))
|
||||
out = decode_skeleton2D(payload)
|
||||
assert out is not None and len(out) == JOINT_COUNT
|
||||
assert abs(out[10][0] - 0.010) < 1e-5 and out[10][2] is True
|
||||
assert out[11][2] is False
|
||||
|
||||
|
||||
def test_decode_skeleton2d_wrong_length_returns_none():
|
||||
assert decode_skeleton2D(b"\x00" * 10) is None
|
||||
assert decode_skeleton2D(b"\x00" * 818) is None
|
||||
assert decode_skeleton2D(b"\x00" * 820) is None
|
||||
assert decode_skeleton2D(b"") is None
|
||||
Reference in New Issue
Block a user