diff --git a/src/score/dtw.gd b/src/score/dtw.gd new file mode 100644 index 0000000..151436c --- /dev/null +++ b/src/score/dtw.gd @@ -0,0 +1,230 @@ +class_name DtwMatcher +extends RefCounted + +## DTW 评分引擎 —— T-WIN-007 +## +## 参考序列来自 Web 服务端预处理产出的关键点序列(契约 contracts/model-formats.md §2.1), +## 查询序列来自 Android 端实时推流。两侧结构同构,均为 PoseFeatures.FeatureVector。 +## +## 采用 Sakoe-Chiba 带状约束:|i - j| <= band。对舞蹈跟练场景, +## 玩家与参考基本同步开始,该约束既降低复杂度又避免病态对齐。 + +const PoseFeatures = preload("res://src/score/pose_features.gd") + +## 默认带宽(帧)。30 FPS 下 15 帧 ≈ 0.5 秒的抖动容忍。 +const DEFAULT_BAND := 15 + + +## 整段 DTW(离线/测试用) +## 返回 { distance, normalized, path, steps };distance 为路径累计代价。 +static func compute_full(query: Array, reference: Array, band: int = DEFAULT_BAND) -> Dictionary: + var n := query.size() + var m := reference.size() + var result := { + "distance": INF, + "normalized": INF, + "path": [], + "steps": 0, + } + if n == 0 or m == 0: + return result + + # 代价矩阵(完整存储,便于回溯);用 INF 表示不可达 + var cost: Array = [] + cost.resize(n) + for i in n: + var row := PackedFloat32Array() + row.resize(m) + row.fill(INF) + cost[i] = row + + for i in n: + var j_lo: int = maxi(0, i - band) + var j_hi: int = mini(m - 1, i + band) + for j in range(j_lo, j_hi + 1): + var step := PoseFeatures.weighted_distance(query[i], reference[j]) + if i == 0 and j == 0: + cost[i][j] = step + continue + var best := INF + if i > 0 and j_lo <= j and j <= mini(m - 1, i - 1 + band): + best = minf(best, cost[i - 1][j]) + if j > 0: + best = minf(best, cost[i][j - 1]) + if i > 0 and j > 0: + best = minf(best, cost[i - 1][j - 1]) + cost[i][j] = step + (best if best < INF else 0.0) + + # 端点可取最后一行的带内最小值(允许参考尾部未被走完) + var end_j := -1 + var end_cost := INF + for j in range(maxi(0, n - 1 - band), mini(m - 1, n - 1 + band) + 1): + if cost[n - 1][j] < end_cost: + end_cost = cost[n - 1][j] + end_j = j + if end_j < 0: + return result + + # 回溯 + var path: Array = [] + var i := n - 1 + var j := end_j + var guard := 0 + while i >= 0 and j >= 0 and guard < n + m + 8: + guard += 1 + path.append(Vector2i(i, j)) + if i == 0 and j == 0: + break + # cost 是无类型 Array,元素为 Variant;显式声明为 float 以做运行期转换 + var diag := INF + var up := INF + var left := INF + if i > 0 and j > 0: + diag = cost[i - 1][j - 1] + if i > 0: + up = cost[i - 1][j] + if j > 0: + left = cost[i][j - 1] + if diag <= up and diag <= left: + i -= 1 + j -= 1 + elif up <= left: + i -= 1 + else: + j -= 1 + path.reverse() + + result["distance"] = end_cost + result["steps"] = path.size() + result["normalized"] = end_cost / float(maxi(path.size(), 1)) + result["path"] = path + return result + + +## 流式 DTW:逐帧推送查询向量,随时可读取当前评分 +class StreamingDtw extends RefCounted: + var _reference: Array = [] + var _band: int = DtwMatcher.DEFAULT_BAND + var _m: int = 0 + ## 上一行代价(仅带内),_prev_lo 为其对应的参考起始下标 + var _prev_costs := PackedFloat32Array() + var _prev_lo: int = 0 + var _cur_costs := PackedFloat32Array() + var _cur_lo: int = 0 + var _row: int = -1 + var _last_normalized: float = INF + var _last_cost: float = INF + var _last_j: int = -1 + var _finished: bool = false + + func _init(reference: Array, band: int = DtwMatcher.DEFAULT_BAND) -> void: + _reference = reference + _band = maxi(band, 0) + _m = reference.size() + + func reference_length() -> int: + return _m + + func frames_consumed() -> int: + return _row + 1 + + ## 参考序列是否已被走完 + func is_finished() -> bool: + return _finished + + ## 推进一帧;返回本次的 { cost, normalized, j, finished } + func push(query: PoseFeatures.FeatureVector) -> Dictionary: + if _m == 0: + _finished = true + return {"cost": INF, "normalized": INF, "j": -1, "finished": true} + + _row += 1 + var i := _row + var lo: int = maxi(0, i - _band) + var hi: int = mini(_m - 1, i + _band) + var width := hi - lo + 1 + + _cur_lo = lo + _cur_costs = PackedFloat32Array() + _cur_costs.resize(width) + _cur_costs.fill(INF) + + var row_best := INF + var row_best_j := -1 + + for j in range(lo, hi + 1): + var step := PoseFeatures.weighted_distance(query, _reference[j]) + var value := 0.0 + if i == 0 and j == 0: + value = step + else: + var best := INF + # 左邻(本行) + if j > lo: + best = minf(best, _cur_costs[j - 1 - lo]) + # 上邻(上一行同列) + var up := _prev_at(j) + if up < INF: + best = minf(best, up) + # 左上对角 + var diag := _prev_at(j - 1) + if diag < INF: + best = minf(best, diag) + value = step + (best if best < INF else 0.0) + + _cur_costs[j - lo] = value + if value < row_best: + row_best = value + row_best_j = j + + _prev_costs = _cur_costs + _prev_lo = lo + + _last_cost = row_best + _last_j = row_best_j + # 归一化:路径至少经过 max(i, j)+1 个格点 + var steps: int = maxi(i, row_best_j) + 1 + _last_normalized = row_best / float(maxi(steps, 1)) + + if row_best_j >= _m - 1: + _finished = true + + return { + "cost": _last_cost, + "normalized": _last_normalized, + "j": _last_j, + "finished": _finished, + } + + func _prev_at(j: int) -> float: + if _prev_costs.is_empty(): + return INF + var idx := j - _prev_lo + if idx < 0 or idx >= _prev_costs.size(): + return INF + return _prev_costs[idx] + + ## 当前平均每步代价(越小越贴合) + func normalized_cost() -> float: + return _last_normalized + + ## 对齐到的参考帧下标 + func reference_cursor() -> int: + return _last_j + + ## 进度 [0,1] + func progress() -> float: + if _m <= 0: + return 0.0 + return clampf(float(_last_j + 1) / float(_m), 0.0, 1.0) + + func reset() -> void: + _prev_costs = PackedFloat32Array() + _cur_costs = PackedFloat32Array() + _prev_lo = 0 + _cur_lo = 0 + _row = -1 + _last_normalized = INF + _last_cost = INF + _last_j = -1 + _finished = false diff --git a/src/score/dtw.gd.uid b/src/score/dtw.gd.uid new file mode 100644 index 0000000..d51f183 --- /dev/null +++ b/src/score/dtw.gd.uid @@ -0,0 +1 @@ +uid://c3dcx0hsgp221 diff --git a/src/score/pose_features.gd b/src/score/pose_features.gd new file mode 100644 index 0000000..f6cd586 --- /dev/null +++ b/src/score/pose_features.gd @@ -0,0 +1,172 @@ +class_name PoseFeatures +extends RefCounted + +## 关键点 → DTW 特征向量 —— 契约 contracts/skeleton-topology.md §5(冻结) +## +## 每帧特征 = [33 × (x, y, z), 9 个关节角余弦] = 108 维。 +## 低置信度点(conf < 0.5)对应的维度权重置 0,不参与代价计算。 +## +## 关节角三元组顺序冻结为: +## [shoulder_L, shoulder_R, elbow_L, elbow_R, hip_L, hip_R, knee_L, knee_R, spine] + +const KeypointFrame = preload("res://src/net/keypoint_frame.gd") + +const KEYPOINT_COUNT := 33 +const VIRTUAL_NECK := 33 +const VIRTUAL_PELVIS := 34 +const ANGLE_COUNT := 9 +const FEATURE_DIM := KEYPOINT_COUNT * 3 + ANGLE_COUNT # 108 + +## 关节角 (a, b, c) 三元组索引 +const ANGLE_TRIPLETS := [ + [KeypointFrame.LEFT_ELBOW, KeypointFrame.LEFT_SHOULDER, KeypointFrame.LEFT_HIP], + [KeypointFrame.RIGHT_ELBOW, KeypointFrame.RIGHT_SHOULDER, KeypointFrame.RIGHT_HIP], + [KeypointFrame.LEFT_SHOULDER, KeypointFrame.LEFT_ELBOW, KeypointFrame.LEFT_WRIST], + [KeypointFrame.RIGHT_SHOULDER, KeypointFrame.RIGHT_ELBOW, KeypointFrame.RIGHT_WRIST], + [KeypointFrame.LEFT_SHOULDER, KeypointFrame.LEFT_HIP, KeypointFrame.LEFT_KNEE], + [KeypointFrame.RIGHT_SHOULDER, KeypointFrame.RIGHT_HIP, KeypointFrame.RIGHT_KNEE], + [KeypointFrame.LEFT_HIP, KeypointFrame.LEFT_KNEE, KeypointFrame.LEFT_ANKLE], + [KeypointFrame.RIGHT_HIP, KeypointFrame.RIGHT_KNEE, KeypointFrame.RIGHT_ANKLE], + [KeypointFrame.NOSE, VIRTUAL_NECK, VIRTUAL_PELVIS], +] + +const ANGLE_NAMES := [ + "shoulder_L", "shoulder_R", "elbow_L", "elbow_R", + "hip_L", "hip_R", "knee_L", "knee_R", "spine", +] + + +## 一帧特征:108 维值 + 108 维权重 +class FeatureVector extends RefCounted: + var values: PackedFloat32Array = PackedFloat32Array() + var weights: PackedFloat32Array = PackedFloat32Array() + var time_ms: float = 0.0 + + func _init() -> void: + values.resize(PoseFeatures.FEATURE_DIM) + weights.resize(PoseFeatures.FEATURE_DIM) + values.fill(0.0) + weights.fill(1.0) + + func dimension() -> int: + return values.size() + + +## 由姿态帧构造特征向量 +static func build(frame: KeypointFrame.PoseFrame, use_unmirrored: bool = true) -> FeatureVector: + var out := FeatureVector.new() + if frame == null: + return out + + var points: PackedVector3Array = frame.unmirrored_points() if use_unmirrored else frame.points + var confidence: PackedFloat32Array = frame.confidence + out.time_ms = float(frame.received_at_ms) + + # ---- 33 × 3 坐标 ---- + for i in KEYPOINT_COUNT: + var p := points[i] if i < points.size() else Vector3.ZERO + var c := confidence[i] if i < confidence.size() else 0.0 + var w := 1.0 if c >= KeypointFrame.CONFIDENCE_THRESHOLD else 0.0 + out.values[i * 3] = p.x + out.values[i * 3 + 1] = p.y + out.values[i * 3 + 2] = p.z + out.weights[i * 3] = w + out.weights[i * 3 + 1] = w + out.weights[i * 3 + 2] = w + + # ---- 9 个关节角 ---- + var neck := _midpoint(points, KeypointFrame.LEFT_SHOULDER, KeypointFrame.RIGHT_SHOULDER) + var pelvis := _midpoint(points, KeypointFrame.LEFT_HIP, KeypointFrame.RIGHT_HIP) + var positions := _extended_positions(points, neck, pelvis) + + for a in ANGLE_COUNT: + var triple: Array = ANGLE_TRIPLETS[a] + var idx := KEYPOINT_COUNT * 3 + a + out.values[idx] = _angle_cos(positions[triple[0]], positions[triple[1]], positions[triple[2]]) + # 三个端点均可信时该角度才参与代价计算 + var trustworthy := ( + _weight_for_point(confidence, triple[0]) > 0.0 + and _weight_for_point(confidence, triple[1]) > 0.0 + and _weight_for_point(confidence, triple[2]) > 0.0 + ) + out.weights[idx] = 1.0 if trustworthy else 0.0 + + return out + + +## 33 点 + neck(33) + pelvis(34) +static func _extended_positions(points: PackedVector3Array, neck: Vector3, pelvis: Vector3) -> Array: + var out: Array = [] + out.resize(VIRTUAL_PELVIS + 1) + for i in KEYPOINT_COUNT: + out[i] = points[i] if i < points.size() else Vector3.ZERO + out[VIRTUAL_NECK] = neck + out[VIRTUAL_PELVIS] = pelvis + return out + + +static func _midpoint(points: PackedVector3Array, a: int, b: int) -> Vector3: + var pa: Vector3 = points[a] if a < points.size() else Vector3.ZERO + var pb: Vector3 = points[b] if b < points.size() else Vector3.ZERO + return (pa + pb) * 0.5 + + +static func _weight_for_point(confidence: PackedFloat32Array, index: int) -> float: + if index == VIRTUAL_NECK or index == VIRTUAL_PELVIS or index >= KeypointFrame.KEYPOINT_COUNT: + return 1.0 + if index >= confidence.size(): + return 0.0 + return 1.0 if confidence[index] >= KeypointFrame.CONFIDENCE_THRESHOLD else 0.0 + + +## 向量 ba 与 bc 夹角的余弦,落在 [-1, 1] +static func _angle_cos(a: Vector3, b: Vector3, c: Vector3) -> float: + var ba := a - b + var bc := c - b + var denom := ba.length() * bc.length() + if denom < 1e-9: + return 0.0 + return clampf(ba.dot(bc) / denom, -1.0, 1.0) + + +## 加权欧氏距离(逐维归一化,使不同维数可比) +static func weighted_distance(a: FeatureVector, b: FeatureVector) -> float: + var dim := mini(a.values.size(), b.values.size()) + if dim == 0: + return 0.0 + var sum := 0.0 + var weight_total := 0.0 + for i in dim: + var w: float = minf(a.weights[i], b.weights[i]) + if w <= 0.0: + continue + var d := a.values[i] - b.values[i] + sum += w * d * d + weight_total += w + if weight_total <= 0.0: + return 0.0 + return sqrt(sum / weight_total) + + +## 逐维绝对差(用于调试与可视化) +static func dimension_deltas(a: FeatureVector, b: FeatureVector) -> PackedFloat32Array: + var out := PackedFloat32Array() + out.resize(mini(a.values.size(), b.values.size())) + for i in out.size(): + out[i] = absf(a.values[i] - b.values[i]) + return out + + +## 关节角名称索引 → 中文标签(UI 展示用) +static func angle_label(index: int) -> String: + match index: + 0: return "左肩" + 1: return "右肩" + 2: return "左肘" + 3: return "右肘" + 4: return "左髋" + 5: return "右髋" + 6: return "左膝" + 7: return "右膝" + 8: return "躯干" + return "角度%d" % index diff --git a/src/score/pose_features.gd.uid b/src/score/pose_features.gd.uid new file mode 100644 index 0000000..e3638f5 --- /dev/null +++ b/src/score/pose_features.gd.uid @@ -0,0 +1 @@ +uid://cqbovuli7hl5p diff --git a/src/score/scoring.gd b/src/score/scoring.gd new file mode 100644 index 0000000..7f0764d --- /dev/null +++ b/src/score/scoring.gd @@ -0,0 +1,84 @@ +class_name Scoring +extends RefCounted + +## 评分规则(T-WIN-007 / T-WIN-008) +## +## DTW 给出的是「平均每步代价」c(越小越贴合)。本类把它映射成 0~100 分与等级, +## 并定义连击阈值。所有常量集中在此,便于调参与测试。 + +## 连击判定阈值:单帧得分 ≥ 该值即计入连击 +const COMBO_THRESHOLD := 70.0 + +## 等级分界(含下界) +const GRADE_BANDS := [ + {"min": 95.0, "grade": "S", "label": "完美"}, + {"min": 85.0, "grade": "A", "label": "优秀"}, + {"min": 70.0, "grade": "B", "label": "良好"}, + {"min": 55.0, "grade": "C", "label": "及格"}, + {"min": 0.0, "grade": "D", "label": "需努力"}, +] + + +## 由 DTW 平均代价换算单帧得分 [0,100] +## cost == 0 → 100 分;cost >= tolerance → 0 分 +static func score_from_cost(cost: float, tolerance: float) -> float: + if tolerance <= 0.0: + return 0.0 + if cost <= 0.0: + return 100.0 + if cost >= tolerance: + return 0.0 + return (1.0 - cost / tolerance) * 100.0 + + +## 指数滑动平均,抑制逐帧抖动 +static func smooth(previous: float, current: float, alpha: float) -> float: + var a := clampf(alpha, 0.0, 1.0) + if previous == 0.0 and current > 0.0: + return current + return previous * (1.0 - a) + current * a + + +static func grade(score: float) -> String: + for band in GRADE_BANDS: + if score >= float(band["min"]): + return str(band["grade"]) + return "D" + + +static func grade_label(score: float) -> String: + for band in GRADE_BANDS: + if score >= float(band["min"]): + return str(band["label"]) + return "需努力" + + +## 展示用颜色(与 UI 主题一致) +static func grade_color(score: float) -> Color: + match grade(score): + "S": return Color("ffd166") + "A": return Color("7ae582") + "B": return Color("4ea1ff") + "C": return Color("ffb703") + _: return Color("ff6b6b") + + +## 结算评语 +static func summary_comment(score: float) -> String: + match grade(score): + "S": return "动作几乎与示范完全同步,非常出色!" + "A": return "节奏与姿态都很到位,继续保持。" + "B": return "整体跟得上,注意手部与躯干的细节。" + "C": return "基本跟上节奏,幅度和时机还有提升空间。" + _: return "先放慢速度熟悉动作,再逐步跟上音乐节奏。" + + +## 由单帧结果拼装 UI 文案 +static func format_result(result: Dictionary) -> String: + if result.is_empty(): + return "等待姿态数据…" + return "得分 %d | 连击 %d | 进度 %d%%" % [ + int(round(float(result.get("score", 0.0)))), + int(result.get("combo", 0)), + int(round(float(result.get("progress", 0.0)) * 100.0)), + ] diff --git a/src/score/scoring.gd.uid b/src/score/scoring.gd.uid new file mode 100644 index 0000000..f7669d8 --- /dev/null +++ b/src/score/scoring.gd.uid @@ -0,0 +1 @@ +uid://b8vvp446pe8q1 diff --git a/tests/test_dtw.gd b/tests/test_dtw.gd new file mode 100644 index 0000000..08dc61d --- /dev/null +++ b/tests/test_dtw.gd @@ -0,0 +1,150 @@ +extends TestCase + +## T-WIN-007:DTW 评分引擎 + +const PoseFeatures = preload("res://src/score/pose_features.gd") +const DtwMatcher = preload("res://src/score/dtw.gd") + + +func _vector(seed_value: float, dim: int = 108) -> PoseFeatures.FeatureVector: + var v := PoseFeatures.FeatureVector.new() + for i in dim: + v.values[i] = sin(float(i) * 0.1 + seed_value) + v.weights[i] = 1.0 + return v + + +func _sequence(count: int, seed_value: float) -> Array: + var out: Array = [] + for i in count: + out.append(_vector(seed_value + float(i) * 0.01)) + return out + + +func test_identical_sequences_have_zero_distance() -> void: + var seq := _sequence(20, 0.0) + var result := DtwMatcher.compute_full(seq, seq) + almost(result["distance"], 0.0, 1e-5, "完全相同序列距离为 0") + almost(result["normalized"], 0.0, 1e-6, "归一化距离为 0") + eq(result["steps"], 20, "路径长度等于序列长度") + + +func test_different_sequences_have_positive_distance() -> void: + var a := _sequence(20, 0.0) + var b := _sequence(20, 5.0) + var result := DtwMatcher.compute_full(a, b) + ok(result["distance"] > 0.0, "不同序列距离大于 0", "实际 %.6f" % result["distance"]) + ok(result["normalized"] > 0.0, "归一化距离大于 0") + + +func test_empty_input_is_handled() -> void: + var empty: Array = [] + var result := DtwMatcher.compute_full(empty, _sequence(5, 0.0)) + eq(result["steps"], 0, "空查询序列返回空结果") + is_true(is_inf(result["distance"]), "空输入距离为 INF") + + var result2 := DtwMatcher.compute_full(_sequence(5, 0.0), empty) + eq(result2["steps"], 0, "空参考序列返回空结果") + + +func test_path_is_monotonic_and_starts_at_origin() -> void: + var a := _sequence(15, 0.0) + var b := _sequence(15, 1.0) + var result := DtwMatcher.compute_full(a, b) + var path: Array = result["path"] + ok(path.size() > 0, "路径非空") + eq(path[0], Vector2i(0, 0), "路径起点为 (0,0)") + for k in range(1, path.size()): + var prev: Vector2i = path[k - 1] + var cur: Vector2i = path[k] + var di := cur.x - prev.x + var dj := cur.y - prev.y + ok(di >= 0 and dj >= 0 and (di + dj) >= 1, "路径第 %d 步单调前进" % k) + + +func test_band_constraint_limits_warping() -> void: + var a := _sequence(30, 0.0) + var b := _sequence(30, 1.0) + var result := DtwMatcher.compute_full(a, b, 3) + for point in result["path"]: + ok(absi(point.x - point.y) <= 3, "路径点 (%d,%d) 在带宽 3 内" % [point.x, point.y]) + + +func test_time_warping_absorbs_local_stretch() -> void: + # 参考序列把 a 中每一帧重复两次;DTW 应以较小代价吸收这种时间伸缩 + var base := _sequence(12, 0.0) + var stretched: Array = [] + for v in base: + stretched.append(v) + stretched.append(v) + + var plain := DtwMatcher.compute_full(base, base) + var warped := DtwMatcher.compute_full(base, stretched) + ok( + warped["normalized"] < plain["normalized"] + 1e-5, + "时间伸缩后归一化代价不明显增大", + "原始 %.6f,伸缩后 %.6f" % [plain["normalized"], warped["normalized"]] + ) + + +func test_streaming_matches_offline_on_identical_input() -> void: + var seq := _sequence(25, 0.0) + var streaming := DtwMatcher.StreamingDtw.new(seq, DtwMatcher.DEFAULT_BAND) + var last: Dictionary = {} + for v in seq: + last = streaming.push(v) + + almost(last["cost"], 0.0, 1e-5, "流式 DTW 对相同序列代价为 0") + eq(streaming.frames_consumed(), 25, "流式 DTW 消费 25 帧") + is_true(streaming.is_finished(), "走完全部参考帧后标记完成") + almost(streaming.progress(), 1.0, 1e-6, "进度为 1.0") + + +func test_streaming_reports_progress_and_cursor() -> void: + var reference := _sequence(30, 0.0) + var streaming := DtwMatcher.StreamingDtw.new(reference, 5) + var query := _sequence(10, 0.0) + for v in query: + streaming.push(v) + + eq(streaming.frames_consumed(), 10, "已消费 10 帧") + ok(streaming.reference_cursor() >= 0, "存在参考游标", "实际 %d" % streaming.reference_cursor()) + ok(streaming.progress() > 0.0 and streaming.progress() < 1.0, "进度落在 (0,1)") + is_false(streaming.is_finished(), "参考未走完时不标记完成") + + +func test_streaming_penalizes_deviation_monotonically() -> void: + var reference := _sequence(20, 0.0) + + var close := DtwMatcher.StreamingDtw.new(reference, 5) + for v in _sequence(20, 0.05): + close.push(v) + + var far := DtwMatcher.StreamingDtw.new(reference, 5) + for v in _sequence(20, 3.0): + far.push(v) + + ok( + close.normalized_cost() < far.normalized_cost(), + "偏差小的序列代价更低", + "接近 %.6f,偏离 %.6f" % [close.normalized_cost(), far.normalized_cost()] + ) + + +func test_streaming_reset_clears_state() -> void: + var reference := _sequence(10, 0.0) + var streaming := DtwMatcher.StreamingDtw.new(reference, 3) + for v in _sequence(5, 0.0): + streaming.push(v) + streaming.reset() + + eq(streaming.frames_consumed(), 0, "重置后帧计数归零") + eq(streaming.reference_cursor(), -1, "重置后游标归零") + is_false(streaming.is_finished(), "重置后未完成") + + +func test_streaming_with_empty_reference() -> void: + var streaming := DtwMatcher.StreamingDtw.new([], 5) + var result := streaming.push(_vector(0.0)) + is_true(is_inf(result["cost"]), "空参考序列返回 INF 代价") + is_true(streaming.is_finished(), "空参考序列立即标记完成") diff --git a/tests/test_pose_features.gd b/tests/test_pose_features.gd new file mode 100644 index 0000000..cd23366 --- /dev/null +++ b/tests/test_pose_features.gd @@ -0,0 +1,144 @@ +extends TestCase + +## DTW 特征向量构造 —— 契约 contracts/skeleton-topology.md §5 +## 每帧 108 维:[33 × (x, y, z), 9 个关节角余弦] + +const KeypointFrame = preload("res://src/net/keypoint_frame.gd") +const PoseFeatures = preload("res://src/score/pose_features.gd") + + +## 构造一个站姿:肩宽 0.2、髋宽 0.1,四肢按解剖学方向摆放 +func _upright_frame(confidence_value: float = 1.0) -> KeypointFrame.PoseFrame: + var frame := KeypointFrame.PoseFrame.new() + frame.points.resize(33) + frame.confidence.resize(33) + frame.points.fill(Vector3(0.5, 0.5, 0.0)) + frame.confidence.fill(confidence_value) + + frame.points[KeypointFrame.NOSE] = Vector3(0.50, 0.18, 0.0) + frame.points[KeypointFrame.LEFT_SHOULDER] = Vector3(0.60, 0.30, 0.0) + frame.points[KeypointFrame.RIGHT_SHOULDER] = Vector3(0.40, 0.30, 0.0) + frame.points[KeypointFrame.LEFT_ELBOW] = Vector3(0.70, 0.45, 0.0) + frame.points[KeypointFrame.RIGHT_ELBOW] = Vector3(0.30, 0.45, 0.0) + frame.points[KeypointFrame.LEFT_WRIST] = Vector3(0.78, 0.60, 0.0) + frame.points[KeypointFrame.RIGHT_WRIST] = Vector3(0.22, 0.60, 0.0) + frame.points[KeypointFrame.LEFT_HIP] = Vector3(0.55, 0.62, 0.0) + frame.points[KeypointFrame.RIGHT_HIP] = Vector3(0.45, 0.62, 0.0) + frame.points[KeypointFrame.LEFT_KNEE] = Vector3(0.56, 0.78, 0.0) + frame.points[KeypointFrame.RIGHT_KNEE] = Vector3(0.44, 0.78, 0.0) + frame.points[KeypointFrame.LEFT_ANKLE] = Vector3(0.57, 0.94, 0.0) + frame.points[KeypointFrame.RIGHT_ANKLE] = Vector3(0.43, 0.94, 0.0) + return frame + + +func test_feature_dimension_is_108() -> void: + var feature := PoseFeatures.build(_upright_frame()) + eq(feature.dimension(), 108, "特征维度为 108(33×3 + 9)") + eq(PoseFeatures.FEATURE_DIM, 108, "FEATURE_DIM 常量正确") + eq(PoseFeatures.ANGLE_COUNT, 9, "关节角数量为 9") + + +func test_coordinates_are_copied_verbatim() -> void: + var frame := _upright_frame() + var feature := PoseFeatures.build(frame, false) + for i in 33: + almost(feature.values[i * 3], frame.points[i].x, 1e-6, "第 %d 点 x 复制正确" % i) + almost(feature.values[i * 3 + 1], frame.points[i].y, 1e-6, "第 %d 点 y 复制正确" % i) + almost(feature.values[i * 3 + 2], frame.points[i].z, 1e-6, "第 %d 点 z 复制正确" % i) + + +func test_all_weights_one_when_confidence_high() -> void: + var feature := PoseFeatures.build(_upright_frame(1.0)) + var all_one := true + for w in feature.weights: + if absf(w - 1.0) > 1e-6: + all_one = false + break + is_true(all_one, "全部可信时权重均为 1") + + +func test_low_confidence_dimensions_are_zero_weighted() -> void: + var frame := _upright_frame(1.0) + frame.confidence[KeypointFrame.LEFT_WRIST] = 0.2 + var feature := PoseFeatures.build(frame) + var base := KeypointFrame.LEFT_WRIST * 3 + almost(feature.weights[base], 0.0, 1e-6, "低置信度点 x 维权重为 0") + almost(feature.weights[base + 1], 0.0, 1e-6, "低置信度点 y 维权重为 0") + almost(feature.weights[base + 2], 0.0, 1e-6, "低置信度点 z 维权重为 0") + almost(feature.weights[KeypointFrame.RIGHT_WRIST * 3], 1.0, 1e-6, "其它点权重不受影响") + + +func test_angle_of_straight_limb_is_minus_one() -> void: + # 左肘角度:左肩(0.60,0.30) — 左肘(0.70,0.45) — 左腕(0.80,0.60) 共线 + var frame := _upright_frame() + frame.points[KeypointFrame.LEFT_WRIST] = Vector3(0.80, 0.60, 0.0) + var feature := PoseFeatures.build(frame) + var elbow_left_index := 33 * 3 + 2 + almost(feature.values[elbow_left_index], -1.0, 1e-4, "共线肢体关节角余弦为 -1") + almost(feature.weights[elbow_left_index], 1.0, 1e-6, "共线角度权重为 1") + + +func test_angle_of_right_angle_is_zero() -> void: + # 构造左肘为直角:肩(0.60,0.30) — 肘(0.70,0.30) — 腕(0.70,0.40) + var frame := _upright_frame() + frame.points[KeypointFrame.LEFT_ELBOW] = Vector3(0.70, 0.30, 0.0) + frame.points[KeypointFrame.LEFT_WRIST] = Vector3(0.70, 0.40, 0.0) + var feature := PoseFeatures.build(frame) + almost(feature.values[33 * 3 + 2], 0.0, 1e-4, "直角关节角余弦为 0") + + +func test_angles_with_untrusted_endpoint_are_zero_weighted() -> void: + var frame := _upright_frame() + frame.confidence[KeypointFrame.LEFT_HIP] = 0.1 + var feature := PoseFeatures.build(frame) + almost(feature.weights[33 * 3 + 0], 0.0, 1e-6, "左肩角(依赖左髋)权重归零") + almost(feature.weights[33 * 3 + 4], 0.0, 1e-6, "左髋角(依赖左肩/左髋)权重归零") + almost(feature.weights[33 * 3 + 1], 1.0, 1e-6, "右肩角不受影响") + + +func test_identical_frames_have_zero_distance() -> void: + var a := PoseFeatures.build(_upright_frame()) + var b := PoseFeatures.build(_upright_frame()) + almost(PoseFeatures.weighted_distance(a, b), 0.0, 1e-9, "相同帧距离为 0") + + +func test_distance_ignores_untrusted_dimensions() -> void: + var frame_a := _upright_frame() + var frame_b := _upright_frame() + # 让左腕不可信并在 b 中挪动它:距离应不受影响 + frame_a.confidence[KeypointFrame.LEFT_WRIST] = 0.1 + frame_b.confidence[KeypointFrame.LEFT_WRIST] = 0.1 + frame_b.points[KeypointFrame.LEFT_WRIST] = Vector3(0.95, 0.95, 0.5) + almost(PoseFeatures.weighted_distance(PoseFeatures.build(frame_a), PoseFeatures.build(frame_b)), 0.0, 1e-9, "不可信维度不参与距离") + + +func test_distance_grows_with_deviation() -> void: + var base := _upright_frame() + var near := _upright_frame() + near.points[KeypointFrame.LEFT_WRIST] = Vector3(0.79, 0.61, 0.0) + var far := _upright_frame() + far.points[KeypointFrame.LEFT_WRIST] = Vector3(0.95, 0.10, 0.0) + + var d_near := PoseFeatures.weighted_distance(PoseFeatures.build(base), PoseFeatures.build(near)) + var d_far := PoseFeatures.weighted_distance(PoseFeatures.build(base), PoseFeatures.build(far)) + ok(d_near < d_far, "偏离越大距离越大", "近 %.6f,远 %.6f" % [d_near, d_far]) + + +func test_mirrored_frame_is_normalized_before_feature_extraction() -> void: + var frame := _upright_frame() + frame.flags = KeypointFrame.FLAG_MIRRORED + var mirrored := PoseFeatures.build(frame, true) + var unmirrored := PoseFeatures.build(frame, false) + # 镜像归一化后左腕 x 应变为 1 - 0.78 + almost(mirrored.values[KeypointFrame.LEFT_WRIST * 3], 1.0 - 0.78, 1e-4, "镜像帧已翻转 x") + almost(unmirrored.values[KeypointFrame.LEFT_WRIST * 3], 0.78, 1e-4, "保留原始坐标时 x 不变") + ok(PoseFeatures.weighted_distance(mirrored, unmirrored) > 0.0, "两种解释产生不同特征") + + +func test_angle_labels_cover_all_nine() -> void: + var labels: Array[String] = [] + for i in 9: + labels.append(PoseFeatures.angle_label(i)) + eq(labels.size(), 9, "9 个关节角均有标签") + for label in labels: + ok(label != "", "标签非空:%s" % label)