feat(win): T-WIN-004 239 字节关键点帧编解码 + 无头测试框架
This commit is contained in:
@@ -0,0 +1,210 @@
|
||||
class_name KeypointFrame
|
||||
extends RefCounted
|
||||
|
||||
## 239 字节关键点帧编解码 —— 契约 contracts/websocket-keypoints.md §2（冻结）
|
||||
##
|
||||
## 帧布局（小端序）：
|
||||
## 偏移 0 2B magic = 0x4F44 ('OD')
|
||||
## 偏移 2 1B version = 0x01
|
||||
## 偏移 3 1B flags bit0=镜像 bit1=含低置信度点 bit2=上半身模式
|
||||
## 偏移 4 4B frame_id u32
|
||||
## 偏移 8 231B keypoints[33] × 7B
|
||||
## 每点：+0 s16 x +2 s16 y +4 s16 z +6 u8 conf
|
||||
## 合计 239
|
||||
##
|
||||
## 坐标语义见 contracts/skeleton-topology.md §4：
|
||||
## x,y ∈ [0,1] 归一化图像坐标（原点左上）；z 与 x 同尺度，负值朝相机。
|
||||
## 归一化系数统一为 32767。
|
||||
|
||||
const MAGIC := 0x4F44
|
||||
const PROTOCOL_VERSION := 1
|
||||
const BINARY_SIZE := 239
|
||||
const KEYPOINT_COUNT := 33
|
||||
const HEADER_SIZE := 8
|
||||
const POINT_BYTES := 7
|
||||
const SCALE := 32767.0
|
||||
|
||||
## flags 位定义
|
||||
const FLAG_MIRRORED := 1 << 0
|
||||
const FLAG_LOW_CONFIDENCE := 1 << 1
|
||||
const FLAG_UPPER_BODY_ONLY := 1 << 2
|
||||
|
||||
## 低置信度阈值：conf < 0.5 的点坐标无意义，必须插值
|
||||
const CONFIDENCE_THRESHOLD := 0.5
|
||||
|
||||
## MediaPipe 33 点索引 —— 契约 skeleton-topology.md §1（完整 33 项）
|
||||
const NOSE := 0
|
||||
const LEFT_EYE_INNER := 1
|
||||
const LEFT_EYE := 2
|
||||
const LEFT_EYE_OUTER := 3
|
||||
const RIGHT_EYE_INNER := 4
|
||||
const RIGHT_EYE := 5
|
||||
const RIGHT_EYE_OUTER := 6
|
||||
const LEFT_EAR := 7
|
||||
const RIGHT_EAR := 8
|
||||
const MOUTH_LEFT := 9
|
||||
const MOUTH_RIGHT := 10
|
||||
const LEFT_SHOULDER := 11
|
||||
const RIGHT_SHOULDER := 12
|
||||
const LEFT_ELBOW := 13
|
||||
const RIGHT_ELBOW := 14
|
||||
const LEFT_WRIST := 15
|
||||
const RIGHT_WRIST := 16
|
||||
const LEFT_PINKY := 17
|
||||
const RIGHT_PINKY := 18
|
||||
const LEFT_INDEX := 19
|
||||
const RIGHT_INDEX := 20
|
||||
const LEFT_THUMB := 21
|
||||
const RIGHT_THUMB := 22
|
||||
const LEFT_HIP := 23
|
||||
const RIGHT_HIP := 24
|
||||
const LEFT_KNEE := 25
|
||||
const RIGHT_KNEE := 26
|
||||
const LEFT_ANKLE := 27
|
||||
const RIGHT_ANKLE := 28
|
||||
const LEFT_HEEL := 29
|
||||
const RIGHT_HEEL := 30
|
||||
const LEFT_FOOT_INDEX := 31
|
||||
const RIGHT_FOOT_INDEX := 32
|
||||
|
||||
|
||||
## 一帧姿态数据
|
||||
class PoseFrame extends RefCounted:
|
||||
var frame_id: int = 0
|
||||
var flags: int = 0
|
||||
## 33 个归一化坐标；z 为深度
|
||||
var points: PackedVector3Array = PackedVector3Array()
|
||||
## 33 个置信度 [0,1]
|
||||
var confidence: PackedFloat32Array = PackedFloat32Array()
|
||||
## 接收时刻（毫秒，引擎时钟）
|
||||
var received_at_ms: int = 0
|
||||
|
||||
func is_mirrored() -> bool:
|
||||
return (flags & KeypointFrame.FLAG_MIRRORED) != 0
|
||||
|
||||
func is_upper_body_only() -> bool:
|
||||
return (flags & KeypointFrame.FLAG_UPPER_BODY_ONLY) != 0
|
||||
|
||||
func has_low_confidence() -> bool:
|
||||
return (flags & KeypointFrame.FLAG_LOW_CONFIDENCE) != 0
|
||||
|
||||
func confidence_at(index: int) -> float:
|
||||
if index < 0 or index >= confidence.size():
|
||||
return 0.0
|
||||
return confidence[index]
|
||||
|
||||
func is_reliable(index: int) -> bool:
|
||||
return confidence_at(index) >= KeypointFrame.CONFIDENCE_THRESHOLD
|
||||
|
||||
func point(index: int) -> Vector3:
|
||||
if index < 0 or index >= points.size():
|
||||
return Vector3.ZERO
|
||||
return points[index]
|
||||
|
||||
## 已镜像的帧需要翻转 x 才能与本端坐标系一致（契约 skeleton-topology.md §4）
|
||||
func unmirrored_points() -> PackedVector3Array:
|
||||
if not is_mirrored():
|
||||
return points
|
||||
var out := PackedVector3Array()
|
||||
out.resize(points.size())
|
||||
for i in points.size():
|
||||
var p := points[i]
|
||||
out[i] = Vector3(1.0 - p.x, p.y, p.z)
|
||||
return out
|
||||
|
||||
|
||||
## 校验帧字节流；合法返回空字符串，否则返回可读原因
|
||||
static func validate(buf: PackedByteArray) -> String:
|
||||
if buf.size() != BINARY_SIZE:
|
||||
return "帧长度必须为 %d 字节，实际 %d" % [BINARY_SIZE, buf.size()]
|
||||
var magic := buf.decode_u16(0)
|
||||
if magic != MAGIC:
|
||||
return "magic 校验失败：期望 0x%04X，实际 0x%04X" % [MAGIC, magic]
|
||||
var version := buf.decode_u8(2)
|
||||
if version != PROTOCOL_VERSION:
|
||||
return "不支持的协议版本 %d（本端支持 %d）" % [version, PROTOCOL_VERSION]
|
||||
return ""
|
||||
|
||||
|
||||
## 解码 239 字节帧；失败返回 null 并 push_error
|
||||
static func decode(buf: PackedByteArray) -> PoseFrame:
|
||||
var problem := validate(buf)
|
||||
if problem != "":
|
||||
push_error("[KeypointFrame] 解码失败：%s" % problem)
|
||||
return null
|
||||
|
||||
var frame := PoseFrame.new()
|
||||
frame.flags = buf.decode_u8(3)
|
||||
frame.frame_id = buf.decode_u32(4)
|
||||
|
||||
frame.points.resize(KEYPOINT_COUNT)
|
||||
frame.confidence.resize(KEYPOINT_COUNT)
|
||||
|
||||
for i in KEYPOINT_COUNT:
|
||||
var offset := HEADER_SIZE + i * POINT_BYTES
|
||||
frame.points[i] = Vector3(
|
||||
buf.decode_s16(offset) / SCALE,
|
||||
buf.decode_s16(offset + 2) / SCALE,
|
||||
buf.decode_s16(offset + 4) / SCALE
|
||||
)
|
||||
frame.confidence[i] = buf.decode_u8(offset + 6) / 255.0
|
||||
|
||||
return frame
|
||||
|
||||
|
||||
## 编码为 239 字节帧（本端为接收方，此函数用于自测与协议回归）
|
||||
static func encode(frame_id: int, points: PackedVector3Array, confidence: PackedFloat32Array, flags: int = 0) -> PackedByteArray:
|
||||
assert(points.size() == KEYPOINT_COUNT, "points 必须为 33 项")
|
||||
assert(confidence.size() == KEYPOINT_COUNT, "confidence 必须为 33 项")
|
||||
|
||||
var buf := PackedByteArray()
|
||||
buf.resize(BINARY_SIZE)
|
||||
buf.encode_u16(0, MAGIC)
|
||||
buf.encode_u8(2, PROTOCOL_VERSION)
|
||||
buf.encode_u8(3, flags & 0xFF)
|
||||
buf.encode_u32(4, frame_id)
|
||||
|
||||
for i in KEYPOINT_COUNT:
|
||||
var offset := HEADER_SIZE + i * POINT_BYTES
|
||||
var p := points[i]
|
||||
buf.encode_s16(offset, clampi(roundi(p.x * SCALE), -32768, 32767))
|
||||
buf.encode_s16(offset + 2, clampi(roundi(p.y * SCALE), -32768, 32767))
|
||||
buf.encode_s16(offset + 4, clampi(roundi(p.z * SCALE), -32768, 32767))
|
||||
buf.encode_u8(offset + 6, clampi(roundi(confidence[i] * 255.0), 0, 255))
|
||||
|
||||
return buf
|
||||
|
||||
|
||||
## 由 JSON 调试帧构造（契约 §4，仅联调用）
|
||||
static func from_json_dict(data: Dictionary) -> PoseFrame:
|
||||
var raw_points: Array = data.get("points", [])
|
||||
if raw_points.size() != KEYPOINT_COUNT:
|
||||
push_error("[KeypointFrame] JSON 帧 points 必须为 33 项")
|
||||
return null
|
||||
|
||||
var frame := PoseFrame.new()
|
||||
frame.frame_id = int(data.get("frameId", 0))
|
||||
var flags: Dictionary = data.get("flags", {})
|
||||
if bool(flags.get("mirrored", false)):
|
||||
frame.flags |= FLAG_MIRRORED
|
||||
if bool(flags.get("upperBodyOnly", false)):
|
||||
frame.flags |= FLAG_UPPER_BODY_ONLY
|
||||
|
||||
frame.points.resize(KEYPOINT_COUNT)
|
||||
frame.confidence.resize(KEYPOINT_COUNT)
|
||||
for i in KEYPOINT_COUNT:
|
||||
var entry: Array = raw_points[i]
|
||||
if entry.size() != 4:
|
||||
push_error("[KeypointFrame] JSON 帧第 %d 个点必须为 [x, y, z, conf]" % i)
|
||||
return null
|
||||
frame.points[i] = Vector3(float(entry[0]), float(entry[1]), float(entry[2]))
|
||||
frame.confidence[i] = float(entry[3])
|
||||
return frame
|
||||
|
||||
|
||||
## 该帧是否含低于阈值的点，用于设置 FLAG_LOW_CONFIDENCE
|
||||
static func compute_low_confidence(points: PackedVector3Array, confidence: PackedFloat32Array) -> bool:
|
||||
for i in confidence.size():
|
||||
if confidence[i] < CONFIDENCE_THRESHOLD:
|
||||
return true
|
||||
return false
|
||||
Reference in New Issue
Block a user