Spaces:
Running
Running
File size: 7,209 Bytes
bb59211 | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 | """SLAM 轨迹连续性 —— 相邻位姿之间出现不可能的速度/角速度跳变。
为什么重要
----------
UMI 的 action 是从 SLAM 位姿**差分**出来的(见 ``app/umi_slam_action.py``)。
位姿一旦跳变,差分出来的动作就是一个巨大的假位移;模型会学到"突然瞬移"。
官方也承认 ORB_SLAM3 是整条流水线"最脆弱的部分"。
常见来源:视觉跟踪丢失后重定位、弱纹理/暗光、快速转手腕导致运动模糊。
判定分两档(这正是本项的设计,不是"只报告不判定")
--------------------------------------------------
方案明确警告:「SLAM 单帧跳了 113.9 毫米可以作为问题线索,但不能马上定成
全平台红线」。113.9mm 来自本平台实测(`umi_slam_action.py` 的 docstring 记录了
插值方案会横跨重定位跳变),它是**观测线索**不是阈值来源。
所以**单次跳变只判 WARN** —— 一次重定位不该废掉一整集(实测
UMIGripper_Action_AI_000001 就有一次真实的 6.8 m/s 跳变,属"该警告、不该拦")。
但跳变**达到 ``fail_jumps``(默认 10 处)判 FAIL**:那已经不是偶发重定位,而是
SLAM 全程没跟住,差分出来的动作整段都是假的 —— 这种数据确实不能训,按"数据不对
就拦回人工审核"处理。
★ 本文件的旧注释曾写成"默认判 WARN 而非 FAIL……只报告",与实现不符(实现一直
是分档的)。以本段为准。阈值本身仍须用人工确认过的好/坏数据校准后才固化。
"""
from __future__ import annotations
import math
from app.processing.cleaning.checks import (
CheckContext, CleaningCheck, register_check,
Finding, Range, PASS, WARN, FAIL,
)
from app.processing.cleaning.contract import pose_is_valid
def _norm3(vector) -> float:
return math.sqrt(sum(float(value) ** 2 for value in vector[:3]))
@register_check
class UmiSlamContinuityCheck(CleaningCheck):
slug = "umi.slam_continuity"
label = "SLAM Trajectory Continuity"
version = "1.0"
description = "Impossible linear or angular velocity between adjacent poses"
requires_channels = ("slam",)
# 属于哪些设备卡片 —— 前端设置面板按它分 tab
device_cards = ("gripper_device",)
# 单位:米/秒、弧度/秒。人手快速挥动约 2 m/s、6 rad/s 量级,
# 这里给的是"明显超出人类可达"的起点值。
#
# 判级用两个计数阈值,而不是"速度超过多少倍"—— 后者在默认值下会退化成
# "任何一次跳变都判失败"(一次跳变就废掉一整集太重了)。实测 episode 1
# 帧 293 有一次 6.8 m/s 的真实重定位跳变,属"该警告、但不该直接拦"。
default_params = {
"max_linear_mps": 5.0,
"max_angular_rps": 20.0,
"warn_jumps": 1, # 达到此数即警告
"fail_jumps": 10, # 达到此数判失败(说明 SLAM 全程不稳)
}
default_severity = WARN
def run(self, ctx: CheckContext) -> list[Finding]:
findings: list[Finding] = []
max_linear = float(self.param(ctx, "max_linear_mps", 5.0))
max_angular = float(self.param(ctx, "max_angular_rps", 20.0))
fail_jumps = int(self.param(ctx, "fail_jumps", 10))
for stream in ctx.streams_of("slam"):
poses = stream.series.get("pose") or ()
stamps = stream.series.get("t") or ()
if len(poses) < 2:
continue
fps = float(stream.fps or ctx.fps or 30.0)
dt_default = 1.0 / fps if fps > 0 else 1.0 / 30.0
jumps: list[Range] = []
peak_linear = 0.0
peak_angular = 0.0
for index in range(1, len(poses)):
prev, cur = poses[index - 1], poses[index]
# ★ 必须用 pose_is_valid,不能只判长度。
#
# 全零占位位姿长度也是 7,能过 len 检查,但四元数模为 0 ——
# 两个零四元数点积为 0,acos(0)=π/2 ⇒ 夹角 180° ⇒ 每帧
# 94.25 rad/s。实测 Test94 的 slam_pose 有 437/599 行是全零,
# 于是 599 帧报出 465 处"跳变",整集被假判 FAIL 并拦住批次。
if not pose_is_valid(prev) or not pose_is_valid(cur):
continue
try:
delta = [float(cur[i]) - float(prev[i]) for i in range(3)]
except (TypeError, ValueError):
continue
# 时间间隔取真实时间戳;缺失时回落到 fps
dt = dt_default
if len(stamps) > index and stamps[index] and stamps[index - 1]:
try:
span = float(stamps[index]) - float(stamps[index - 1])
if 0 < span < 1.0: # 超过 1 秒说明中间丢了整段
dt = span
except (TypeError, ValueError):
pass
linear = _norm3(delta) / dt
# 姿态差:四元数点积 → 夹角。|dot| 接近 1 表示几乎没转。
dot = sum(float(prev[3 + i]) * float(cur[3 + i]) for i in range(4))
angle = 2.0 * math.acos(max(-1.0, min(1.0, abs(dot))))
angular = angle / dt
peak_linear = max(peak_linear, linear)
peak_angular = max(peak_angular, angular)
if linear <= max_linear and angular <= max_angular:
continue
jumps.append(Range(
start_sec=index / fps, end_sec=(index + 1) / fps,
code=self.slug, severity=WARN,
stream=stream.key, channel="slam",
start_frame=index - 1, end_frame=index,
rule=self.slug,
message=f"pose jump at frame {index}: "
f"{_norm3(delta) * 1000:.1f}mm / {math.degrees(angle):.1f}°",
evidence={"linear_mps": round(linear, 3),
"angular_rps": round(angular, 3)},
))
metrics = {
"frames": len(poses),
"jumps": len(jumps),
"peak_linear_mps": round(peak_linear, 3),
"peak_angular_rps": round(peak_angular, 3),
}
if not jumps:
findings.append(self.finding(ctx, PASS, stream=stream.key,
metrics=metrics))
continue
findings.append(self.finding(
ctx,
FAIL if len(jumps) >= fail_jumps else WARN,
stream=stream.key,
message=f"{len(jumps)} pose jumps, peak "
f"{peak_linear:.1f} m/s / "
f"{math.degrees(peak_angular):.0f} deg/s "
f"(verify against the video)",
metrics=metrics,
ranges=jumps[:50], # 报告里最多带 50 个,避免 JSON 过大
))
return findings
|