机器人视频质检（二）：跨模态摄像头配对校验

if min(cross_cells) > max(self_cells):
    crossed = True
    verdict = "CROSSED"
else:
    crossed = False
    verdict = "ALIGNED"

def camera_motion_series(mad_series, window=SMALL_W):
    """相机运动代理量：帧间平均绝对差。

    MAD 本身就是逐帧的运动度量，所以"相机运动序列"就是 MAD 本身。
    移动窗口把编码器噪声抹平，让相关性反映手臂的运动而不是编码器的。
    """
    return moving_window_motion(mad_series, window)

def joint_motion_series(joint_positions, window=SMALL_W):
    """手臂运动代理量：逐帧关节位移的 L2 范数。

    joint_positions 形状是 (n_frames, n_joints)。
    取相邻帧之差，平方求和，开方。
    """
    j = np.asarray(joint_positions, dtype=np.float64)
    if j.ndim != 2 or j.shape[0] < 2:
        return np.array([]), np.array([], dtype=bool)
    d = np.linalg.norm(np.diff(j, axis=0), axis=1)
    # 补回一帧，让下标和相机序列对齐
    d = np.concatenate([[0.0], d])
    return moving_window_motion(d, window)

SMALL_W = 64
"""运动信号的窗口宽度（采样点数）。

窗口要短到能捕捉一次伸手（双臂抓取大约几秒），
又要长到让相关性不被编码器噪声主导。
64 个采样点 @ 30fps 是 2.13 秒。

这个值是验证过的：在实测的 episode 上，
32、64、128 给出同样的判定，取中间的 64。
"""

import numpy as np
import pyarrow.parquet as pq

# 1. episode -> 视频里的时间区间
meta = pq.read_table(
    "data/.../meta/episodes/chunk-000/file-000.parquet",
    columns=["episode_index", "length",
             "videos/observation.images.left/from_timestamp",
             "videos/observation.images.left/to_timestamp",
             "videos/observation.images.left/file_index",
             "videos/observation.images.right/file_index"],
).to_pandas()

# 只取完整落在 file-000.mp4 里的 episode
meta = meta[(meta["videos/observation.images.left/file_index"] == 0) &
            (meta["videos/observation.images.right/file_index"] == 0)].reset_index(drop=True)

# 2. 状态
state = pd.concat([
    pq.read_table(f"data/.../data/chunk-000/file-{i:03d}.parquet",
                  columns=["episode_index", "frame_index", "observation.state"]).to_pandas()
    for i in range(4)
], ignore_index=True)

# 3. 每个 episode 的帧区间
f0 = int(round(row["videos/observation.images.left/from_timestamp"] * 30))
f1 = int(round(row["videos/observation.images.left/to_timestamp"] * 30))

cams = {"left": mad_l[f0:f0+n], "right": mad_r[f0:f0+n]}
swapped_cams = {"left": cams["right"], "right": cams["left"]}

a = stream_pairing(cams, arms)          # 原始数据
b = stream_pairing(swapped_cams, arms)  # 人为交换

def naive_pairing(cam_mad, arm_state, window=SMALL_W):
    """错误做法：把缺失的采样当作真实的零运动。"""
    out = {}
    for c in cam_mad:
        s = np.convolve(np.asarray(cam_mad[c], float), np.ones(window)/window, mode="same")
        for a in arm_state:
            js, _ = joint_motion_series(np.asarray(arm_state[a]), window)
            n_ = min(len(s), len(js))
            out[f"{c}_vs_{a}"] = _corr(s[:n_], js[:n_])
    return out

def moving_window_motion(values, window=SMALL_W):
    v = np.asarray(values, dtype=np.float64)
    n = len(v)
    if n == 0:
        return np.array([]), np.array([], dtype=bool)

    w = min(window, n)
    kernel = np.ones(w) / w
    smoothed = np.convolve(v, kernel, mode="same")

    valid = np.zeros(n, dtype=bool)
    if n >= w:
        # "same" 卷积在边缘是零填充的，那些采样不是真实的窗口均值，丢掉。
        valid[w // 2: n - (w - w // 2) if w > 1 else n] = True

    # 一个所有取值都（几乎）相同的窗口不携带运动信息。
    # 用窗口上的 max-min 得到它的跨度，代价很低。
    if n >= w:
        spread = np.zeros(n, dtype=np.float64)
        for i in range(n):
            lo = max(0, min(i - w // 2, n - w))
            seg = v[lo:lo + w]
            spread[i] = seg.max() - seg.min()
        valid &= spread > 1e-9

    return smoothed, valid

def _align(a, a_valid, b, b_valid):
    """在两个序列的公共有效采样上求交。"""
    n = min(len(a), len(b))
    a = a[:n]; b = b[:n]
    av = a_valid[:n]; bv = b_valid[:n]
    both = av & bv
    return a[both], b[both], both

def _corr(a, b):
    """Pearson 相关系数，不可定义时返回 None。

    守卫：
      n < 3       相关性至少需要几个点才有意义
      std < 1e-9  其中一个序列是常数 -> 相关性是 0/0
    """
    n = min(len(a), len(b))
    if n < 3:
        return None
    a = np.asarray(a[:n], dtype=np.float64)
    b = np.asarray(b[:n], dtype=np.float64)
    sa = a.std()
    sb = b.std()
    if sa < 1e-9 or sb < 1e-9:
        return None
    return float(np.corrcoef(a, b)[0, 1])


def stream_pairing(cam_mad, arm_state, window=SMALL_W):
    """判定相机流是否接到了正确的臂上。

    cam_mad   相机名 -> 每帧 MAD 序列
    arm_state 臂名   -> (n_frames, n_joints) 关节位置

    相机名和臂名必须对应：相机 "left" 应当在臂 "left" 上。
    """
    cameras = list(cam_mad.keys())
    arms = list(arm_state.keys())

    res = PairingResult()

    if len(cameras) != 2 or len(arms) != 2:
        res.verdict = "UNKNOWN"
        res.detail["reason"] = f"need exactly 2 cameras and 2 arms, got {cameras} and {arms}"
        return res

    cam_series, cam_valid = {}, {}
    for c in cameras:
        s, v = camera_motion_series(cam_mad[c], window)
        cam_series[c], cam_valid[c] = s, v

    arm_series, arm_valid = {}, {}
    for a in arms:
        s, v = joint_motion_series(np.asarray(arm_state[a]), window)
        arm_series[a], arm_valid[a] = s, v

    corrs = {}
    for c in cameras:
        for a in arms:
            xs, ys, both = _align(cam_series[c], cam_valid[c],
                                  arm_series[a], arm_valid[a])
            corrs[f"{c}_vs_{a}"] = _corr(xs, ys)
            res.n_samples = max(res.n_samples, len(xs))
    res.correlations = corrs
    res.n_valid = res.n_samples

    # 规则需要全部四格都有值。
    if any(v is None for v in corrs.values()):
        res.verdict = "UNKNOWN"
        res.detail["reason"] = "at least one correlation is undefined (constant or too-short series)"
        return res

    cam0, cam1 = cameras
    arm0, arm1 = arms
    self_cells = [corrs[f"{cam0}_vs_{arm0}"], corrs[f"{cam1}_vs_{arm1}"]]
    cross_cells = [corrs[f"{cam0}_vs_{arm1}"], corrs[f"{cam1}_vs_{arm0}"]]

    res.detail["min_cross"] = min(cross_cells)
    res.detail["max_self"] = max(self_cells)

    if min(cross_cells) > max(self_cells):
        res.crossed = True
        res.verdict = "CROSSED"
    else:
        res.crossed = False
        res.verdict = "ALIGNED"

    return res

def _episode_status(per_camera):
    """最差的那个相机决定 episode 的状态。"""
    order = [Status.FAIL, Status.INCOMPLETE, Status.UNKNOWN, Status.WARN, Status.PASS]
    present = {c["summary"].status for c in per_camera.values()}
    for s in order:
        if s in present:
            return s
    return Status.UNKNOWN

robot_video_qc/pairing.py     # 本文的全部实现