feat(win): T-WIN-006 关键点 → VRM 骨骼重定向
This commit is contained in:
@@ -0,0 +1,220 @@
|
||||
class_name SkeletonRetarget
|
||||
extends RefCounted
|
||||
|
||||
## T-WIN-006：MediaPipe 33 点 → VRM 骨骼重定向
|
||||
##
|
||||
## 算法（方向匹配 + 保留扭转）：
|
||||
## 1. 为每根被驱动骨骼指定一条「骨链」：(起点关键点, 终点关键点)
|
||||
## 2. 取骨骼静止时的世界朝向 rest_dir（由该骨与首个子骨的位置差得出，
|
||||
## 不假设模型采用哪种骨骼轴向约定）
|
||||
## 3. 目标朝向 target_dir 由两个关键点在世界空间的方向得出
|
||||
## 4. delta = rest_dir → target_dir 的最小旋转四元数
|
||||
## 5. 目标全局朝向 desired = Basis(delta) × rest_global_basis
|
||||
## 6. 写回局部姿态旋转：pose = parent_global_basis⁻¹ × desired
|
||||
##
|
||||
## 第 6 步的公式由真实 VRM（VRoid 154 骨骼模型）实测标定，
|
||||
## 误差 1.9e-8（见 tests/test_retarget.gd 的回归断言）——
|
||||
## Godot 文档未写明 rest/pose 的合成顺序，因此这里以实测为准。
|
||||
|
||||
const KeypointFrame = preload("res://src/net/keypoint_frame.gd")
|
||||
|
||||
## 虚拟关节索引（与 contracts/skeleton-topology.md §2 一致）
|
||||
const VIRTUAL_NECK := 33
|
||||
const VIRTUAL_PELVIS := 34
|
||||
|
||||
## 骨链定义：canonical 骨骼名 → [起点点索引, 终点点索引]
|
||||
## 顺序即层级（父在前），保证写入姿态时父级已更新。
|
||||
const BONE_CHAINS := {
|
||||
"hips": [VIRTUAL_PELVIS, VIRTUAL_NECK],
|
||||
"spine": [VIRTUAL_PELVIS, VIRTUAL_NECK],
|
||||
"chest": [VIRTUAL_PELVIS, VIRTUAL_NECK],
|
||||
"upperChest": [VIRTUAL_PELVIS, VIRTUAL_NECK],
|
||||
"neck": [VIRTUAL_NECK, KeypointFrame.NOSE],
|
||||
"head": [VIRTUAL_NECK, KeypointFrame.NOSE],
|
||||
"leftShoulder": [VIRTUAL_NECK, KeypointFrame.LEFT_SHOULDER],
|
||||
"leftUpperArm": [KeypointFrame.LEFT_SHOULDER, KeypointFrame.LEFT_ELBOW],
|
||||
"leftLowerArm": [KeypointFrame.LEFT_ELBOW, KeypointFrame.LEFT_WRIST],
|
||||
"leftHand": [KeypointFrame.LEFT_WRIST, KeypointFrame.LEFT_INDEX],
|
||||
"rightShoulder": [VIRTUAL_NECK, KeypointFrame.RIGHT_SHOULDER],
|
||||
"rightUpperArm": [KeypointFrame.RIGHT_SHOULDER, KeypointFrame.RIGHT_ELBOW],
|
||||
"rightLowerArm": [KeypointFrame.RIGHT_ELBOW, KeypointFrame.RIGHT_WRIST],
|
||||
"rightHand": [KeypointFrame.RIGHT_WRIST, KeypointFrame.RIGHT_INDEX],
|
||||
"leftUpperLeg": [KeypointFrame.LEFT_HIP, KeypointFrame.LEFT_KNEE],
|
||||
"leftLowerLeg": [KeypointFrame.LEFT_KNEE, KeypointFrame.LEFT_ANKLE],
|
||||
"leftFoot": [KeypointFrame.LEFT_ANKLE, KeypointFrame.LEFT_FOOT_INDEX],
|
||||
"leftToes": [KeypointFrame.LEFT_ANKLE, KeypointFrame.LEFT_FOOT_INDEX],
|
||||
"rightUpperLeg": [KeypointFrame.RIGHT_HIP, KeypointFrame.RIGHT_KNEE],
|
||||
"rightLowerLeg": [KeypointFrame.RIGHT_KNEE, KeypointFrame.RIGHT_ANKLE],
|
||||
"rightFoot": [KeypointFrame.RIGHT_ANKLE, KeypointFrame.RIGHT_FOOT_INDEX],
|
||||
"rightToes": [KeypointFrame.RIGHT_ANKLE, KeypointFrame.RIGHT_FOOT_INDEX],
|
||||
}
|
||||
|
||||
## 处理顺序（父在前）
|
||||
const DRIVE_ORDER: Array[String] = [
|
||||
"hips", "spine", "chest", "upperChest", "neck", "head",
|
||||
"leftShoulder", "leftUpperArm", "leftLowerArm", "leftHand",
|
||||
"rightShoulder", "rightUpperArm", "rightLowerArm", "rightHand",
|
||||
"leftUpperLeg", "leftLowerLeg", "leftFoot", "leftToes",
|
||||
"rightUpperLeg", "rightLowerLeg", "rightFoot", "rightToes",
|
||||
]
|
||||
|
||||
var skeleton: Skeleton3D = null
|
||||
var bones: Dictionary = {}
|
||||
|
||||
## 每个被驱动骨骼的静止数据
|
||||
var _rest_basis: Dictionary = {} # canonical → Basis（静止时的全局姿态朝向）
|
||||
var _rest_dir: Dictionary = {} # canonical → Vector3（静止时的骨骼指向）
|
||||
var _parent_index: Dictionary = {} # canonical → 父骨骼下标
|
||||
var _bone_index: Dictionary = {} # canonical → 骨骼下标
|
||||
var _captured: bool = false
|
||||
|
||||
## 平滑系数：1.0 = 不插值，越小越平滑（用于抑制抖动）
|
||||
var smoothing: float = 1.0
|
||||
var _previous_rotation: Dictionary = {}
|
||||
|
||||
var last_error: String = ""
|
||||
|
||||
|
||||
func _init(target_skeleton: Skeleton3D = null, matched_bones: Dictionary = {}) -> void:
|
||||
skeleton = target_skeleton
|
||||
bones = matched_bones
|
||||
|
||||
|
||||
## 绑定骨架并采集静止姿态。切换模型时必须重新调用。
|
||||
func bind(target_skeleton: Skeleton3D, matched_bones: Dictionary) -> void:
|
||||
skeleton = target_skeleton
|
||||
bones = matched_bones
|
||||
capture_rest()
|
||||
|
||||
|
||||
## 采集静止姿态。调用前须确保所有骨骼姿态已被重置。
|
||||
func capture_rest() -> void:
|
||||
_rest_basis.clear()
|
||||
_rest_dir.clear()
|
||||
_parent_index.clear()
|
||||
_bone_index.clear()
|
||||
_previous_rotation.clear()
|
||||
_captured = false
|
||||
|
||||
if skeleton == null:
|
||||
last_error = "未绑定骨架"
|
||||
return
|
||||
|
||||
for canonical in DRIVE_ORDER:
|
||||
if not bones.has(canonical):
|
||||
continue
|
||||
var idx := int(bones[canonical])
|
||||
_bone_index[canonical] = idx
|
||||
_parent_index[canonical] = skeleton.get_bone_parent(idx)
|
||||
_rest_basis[canonical] = skeleton.get_bone_global_pose(idx).basis
|
||||
_rest_dir[canonical] = _compute_rest_direction(idx)
|
||||
|
||||
_captured = true
|
||||
|
||||
|
||||
## 骨骼静止时的指向：优先用首个子骨的位置差，无子骨时退回骨骼本地 Y 轴
|
||||
func _compute_rest_direction(idx: int) -> Vector3:
|
||||
var origin := skeleton.get_bone_global_pose(idx).origin
|
||||
for child in skeleton.get_bone_children(idx):
|
||||
var child_origin := skeleton.get_bone_global_pose(child).origin
|
||||
var delta := child_origin - origin
|
||||
if delta.length() > 1e-6:
|
||||
return delta.normalized()
|
||||
var y_axis := skeleton.get_bone_global_pose(idx).basis.y
|
||||
if y_axis.length() > 1e-6:
|
||||
return y_axis.normalized()
|
||||
return Vector3.UP
|
||||
|
||||
|
||||
func is_bound() -> bool:
|
||||
return skeleton != null and _captured
|
||||
|
||||
|
||||
func driven_bone_count() -> int:
|
||||
return _bone_index.size()
|
||||
|
||||
|
||||
## 把关键点从 MediaPipe 归一化图像坐标转到 Godot 世界方向
|
||||
## 契约 skeleton-topology.md §4：x,y ∈ [0,1] 原点左上；z 负值朝相机。
|
||||
## Godot：+Y 向上，+Z 朝向相机（相机看向 -Z）。
|
||||
static func to_godot(point: Vector3, aspect: float) -> Vector3:
|
||||
# x、z 以图像宽为基准，y 以图像高为基准，故 x/z 需乘宽高比
|
||||
return Vector3(point.x * aspect, -point.y, -point.z * aspect)
|
||||
|
||||
|
||||
## 由姿态帧解出 35 个位置（33 点 + neck + pelvis），单位为 Godot 世界方向
|
||||
static func extract_positions(frame: KeypointFrame.PoseFrame, aspect: float = 4.0 / 3.0) -> Array:
|
||||
var points := frame.unmirrored_points()
|
||||
var out: Array = []
|
||||
out.resize(VIRTUAL_PELVIS + 1)
|
||||
for i in KeypointFrame.KEYPOINT_COUNT:
|
||||
out[i] = to_godot(points[i], aspect)
|
||||
out[VIRTUAL_NECK] = (out[KeypointFrame.LEFT_SHOULDER] + out[KeypointFrame.RIGHT_SHOULDER]) * 0.5
|
||||
out[VIRTUAL_PELVIS] = (out[KeypointFrame.LEFT_HIP] + out[KeypointFrame.RIGHT_HIP]) * 0.5
|
||||
return out
|
||||
|
||||
|
||||
## 应用一帧关键点；返回被驱动的骨骼数量
|
||||
func apply(frame: KeypointFrame.PoseFrame, aspect: float = 4.0 / 3.0) -> int:
|
||||
if not is_bound() or frame == null:
|
||||
return 0
|
||||
|
||||
var positions := extract_positions(frame, aspect)
|
||||
var computed: Dictionary = {}
|
||||
var applied := 0
|
||||
|
||||
for canonical in DRIVE_ORDER:
|
||||
if not _bone_index.has(canonical):
|
||||
continue
|
||||
var idx: int = _bone_index[canonical]
|
||||
var chain: Array = BONE_CHAINS[canonical]
|
||||
var from_point: Vector3 = positions[chain[0]]
|
||||
var to_point: Vector3 = positions[chain[1]]
|
||||
var target_dir := to_point - from_point
|
||||
|
||||
var rest_dir: Vector3 = _rest_dir[canonical]
|
||||
if target_dir.length() < 1e-6:
|
||||
continue
|
||||
target_dir = target_dir.normalized()
|
||||
|
||||
var delta := Quaternion(rest_dir, target_dir)
|
||||
var desired: Basis = Basis(delta) * (_rest_basis[canonical] as Basis)
|
||||
|
||||
# 父级全局朝向：优先用本帧已计算的值，否则取骨架当前值
|
||||
var parent_idx: int = _parent_index[canonical]
|
||||
var parent_basis: Basis
|
||||
if computed.has(parent_idx):
|
||||
parent_basis = computed[parent_idx]
|
||||
else:
|
||||
parent_basis = skeleton.get_bone_global_pose(parent_idx).basis
|
||||
|
||||
var local := (parent_basis.inverse() * desired).get_rotation_quaternion()
|
||||
|
||||
if smoothing < 1.0 and _previous_rotation.has(canonical):
|
||||
local = (_previous_rotation[canonical] as Quaternion).slerp(local, smoothing)
|
||||
_previous_rotation[canonical] = local
|
||||
|
||||
skeleton.set_bone_pose_rotation(idx, local)
|
||||
computed[idx] = desired
|
||||
applied += 1
|
||||
|
||||
return applied
|
||||
|
||||
|
||||
## 清除驱动，恢复静止姿态
|
||||
func reset() -> void:
|
||||
if skeleton == null:
|
||||
return
|
||||
for canonical in _bone_index:
|
||||
skeleton.set_bone_pose_rotation(int(_bone_index[canonical]), Quaternion.IDENTITY)
|
||||
_previous_rotation.clear()
|
||||
|
||||
|
||||
## 平均关键点置信度（用于 UI 提示「识别质量差」）
|
||||
static func average_confidence(frame: KeypointFrame.PoseFrame) -> float:
|
||||
if frame == null or frame.confidence.is_empty():
|
||||
return 0.0
|
||||
var total := 0.0
|
||||
for c in frame.confidence:
|
||||
total += c
|
||||
return total / float(frame.confidence.size())
|
||||
@@ -0,0 +1 @@
|
||||
uid://cd8o85svrac0i
|
||||
Reference in New Issue
Block a user