fix: 修正ACT相机采样质量判定

xr_rm_teleop/test/test_act_episode_recorder.py

@@ -1,5 +1,6 @@
 import queue
 import threading
+import time
 from types import SimpleNamespace
 
 import numpy as np
@@ -29,6 +30,7 @@ from xr_rm_teleop.act_episode_recorder import (
     next_episode_index,
     publish_without_overwrite,
     recover_partial_files,
+    sample_frame_metrics,
     select_camera_pair,
     select_frame,
     validate_episode,
@@ -296,6 +298,34 @@ def test_camera_buffer_is_bounded_and_counts_dropped_frames():
     assert stats.fps == pytest.approx(800.0)
 
 
+def test_camera_buffer_counts_frame_number_regressions():
+    buffer = CameraBuffer(maxlen=4)
+    for frame_number in (100, 101, 1, 2):
+        buffer.push(
+            CameraFrame(
+                _image(frame_number),
+                frame_number,
+                float(frame_number),
+                time.monotonic_ns(),
+            )
+        )
+
+    stats = buffer.stats()
+
+    assert stats.frame_number_regression_count == 1
+    assert stats.dropped_frames == 0
+
+
+def test_sample_frame_metrics_separate_repeats_skips_and_regressions():
+    repeat, skip, regression = sample_frame_metrics(
+        np.asarray((986, 986, 988), dtype=np.uint64)
+    )
+
+    assert repeat == pytest.approx(0.5)
+    assert skip == pytest.approx(0.5)
+    assert regression == 0
+
+
 requires_h5py = pytest.mark.skipif(
     h5py is None,
     reason="h5py is not installed",
@@ -506,6 +536,8 @@ def _valid_episode(tmp_path):
         root.attrs["camera_right_wrist_fps"] = 30.0
         root.attrs["camera_high_drop_ratio"] = 0.0
         root.attrs["camera_right_wrist_drop_ratio"] = 0.0
+        root.attrs["camera_high_frame_number_regression_count"] = 0
+        root.attrs["camera_right_wrist_frame_number_regression_count"] = 0
     return path
 
 
@@ -518,6 +550,19 @@ def test_validate_episode_accepts_valid_file(tmp_path):
     assert report.metrics["control_hz"] == pytest.approx(30.0, rel=1e-5)
 
 
+@requires_h5py
+def test_validate_episode_accepts_async_camera_phase_drift(tmp_path):
+    path = _valid_episode(tmp_path)
+    with h5py.File(path, "r+") as root:
+        root["debug/cameras/cam_high_frame_number"][:] = (986, 986, 988)
+
+    report = validate_episode(path, _quality_limits())
+
+    assert report.accepted
+    assert report.metrics["cam_high_sample_repeat_ratio"] == pytest.approx(0.5)
+    assert report.metrics["cam_high_sample_skip_ratio"] == pytest.approx(0.5)
+
+
 def _mutate_episode(path, mutation):
     with h5py.File(path, "r+") as root:
         if mutation == "short_episode":
@@ -547,6 +592,8 @@ def _mutate_episode(path, mutation):
             root.attrs["camera_high_fps"] = 20.0
         elif mutation == "camera_drop":
             root.attrs["camera_right_wrist_drop_ratio"] = 0.02
+        elif mutation == "camera_frame_regression":
+            root.attrs["camera_high_frame_number_regression_count"] = 1
         elif mutation == "camera_age":
             root["debug/timestamps/cam_high_age_ms"][1] = 60.0
         elif mutation == "camera_skew":
@@ -571,6 +618,7 @@ def _mutate_episode(path, mutation):
         ("control_fault", "control_fault"),
         ("camera_fps", "camera_fps"),
         ("camera_drop", "camera_drop_ratio"),
+        ("camera_frame_regression", "camera_frame_number_regression"),
         ("camera_age", "camera_frame_too_old"),
         ("camera_skew", "camera_skew"),
         ("final_gripper_closed", "final_gripper_not_open"),

xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py

@@ -405,6 +405,32 @@ def _quality_failure(
     return QualityReport(False, reason, metrics)
 
 
+def sample_frame_metrics(
+    frame_numbers: np.ndarray,
+) -> tuple[float, float, int]:
+    diffs = np.diff(np.asarray(frame_numbers, dtype=np.int64))
+    denominator = max(1, len(diffs))
+    return (
+        float(np.count_nonzero(diffs == 0) / denominator),
+        float(np.count_nonzero(diffs > 1) / denominator),
+        int(np.count_nonzero(diffs < 0)),
+    )
+
+
+def episode_sample_frame_metrics(root: Any) -> dict[str, int | float]:
+    metrics: dict[str, int | float] = {}
+    for camera in ("cam_high", "cam_wrist"):
+        repeat, skip, regression = sample_frame_metrics(
+            root[f"debug/cameras/{camera}_frame_number"][:]
+        )
+        metrics[f"{camera}_sample_repeat_ratio"] = repeat
+        metrics[f"{camera}_sample_skip_ratio"] = skip
+        metrics[f"{camera}_sample_frame_number_regression_count"] = (
+            regression
+        )
+    return metrics
+
+
 def validate_episode(path: Path, limits: QualityLimits) -> QualityReport:
     metrics: dict[str, int | float] = {}
     try:
@@ -541,6 +567,20 @@ def validate_episode(path: Path, limits: QualityLimits) -> QualityReport:
             if reason == "camera_drop_ratio" and value > threshold:
                 return _quality_failure(reason, metrics)
 
+        for name in (
+            "camera_high_frame_number_regression_count",
+            "camera_right_wrist_frame_number_regression_count",
+        ):
+            if name not in root.attrs:
+                return _quality_failure("camera_stats_missing", metrics)
+            value = int(root.attrs[name])
+            metrics[name] = value
+            if value:
+                return _quality_failure(
+                    "camera_frame_number_regression",
+                    metrics,
+                )
+
         high_age_ms = datasets[
             "debug/timestamps/cam_high_age_ms"
         ][:]
@@ -564,15 +604,15 @@ def validate_episode(path: Path, limits: QualityLimits) -> QualityReport:
         ):
             return _quality_failure("camera_skew", metrics)
 
-        for camera in ("cam_high", "cam_wrist"):
+        sample_metrics = episode_sample_frame_metrics(root)
-            frame_numbers = datasets[
+        metrics.update(sample_metrics)
-                f"debug/cameras/{camera}_frame_number"
+        if any(
-            ][:].astype(np.int64)
+            sample_metrics[
-            discontinuities = int((np.diff(frame_numbers) != 1).sum())
+                f"{camera}_sample_frame_number_regression_count"
-            ratio = discontinuities / max(1, sample_count - 1)
+            ]
-            metrics[f"{camera}_sample_discontinuity_ratio"] = float(ratio)
+            for camera in ("cam_high", "cam_wrist")
-            if ratio > limits.max_drop_ratio:
+        ):
-                return _quality_failure("camera_sample_drop_ratio", metrics)
+            return _quality_failure("camera_frame_number_regression", metrics)
 
         if qpos[-1, 7] != 1.0:
             return _quality_failure("final_gripper_not_open", metrics)
@@ -595,6 +635,7 @@ class CameraFrame:
 class CameraStats:
     frame_count: int
     dropped_frames: int
+    frame_number_regression_count: int
     first_host_monotonic_ns: int | None
     last_host_monotonic_ns: int | None
 
@@ -623,19 +664,20 @@ class CameraBuffer:
         self._lock = threading.Lock()
         self._frame_count = 0
         self._dropped_frames = 0
+        self._frame_number_regression_count = 0
         self._first_host_monotonic_ns: int | None = None
         self._last_host_monotonic_ns: int | None = None
         self._last_frame_number: int | None = None
 
     def push(self, frame: CameraFrame) -> None:
         with self._lock:
-            if (
+            if self._last_frame_number is not None:
-                self._last_frame_number is not None
+                if frame.frame_number <= self._last_frame_number:
-                and frame.frame_number > self._last_frame_number + 1
+                    self._frame_number_regression_count += 1
-            ):
+                elif frame.frame_number > self._last_frame_number + 1:
-                self._dropped_frames += (
+                    self._dropped_frames += (
-                    frame.frame_number - self._last_frame_number - 1
+                        frame.frame_number - self._last_frame_number - 1
-                )
+                    )
             self._last_frame_number = frame.frame_number
             self._frame_count += 1
             if self._first_host_monotonic_ns is None:
@@ -652,6 +694,9 @@ class CameraBuffer:
             return CameraStats(
                 frame_count=self._frame_count,
                 dropped_frames=self._dropped_frames,
+                frame_number_regression_count=(
+                    self._frame_number_regression_count
+                ),
                 first_host_monotonic_ns=self._first_host_monotonic_ns,
                 last_host_monotonic_ns=self._last_host_monotonic_ns,
             )
@@ -1473,14 +1518,19 @@ class ActEpisodeRecorder(Node):
     def _interval_camera_metrics(
         baseline: CameraStats,
         current: CameraStats,
-    ) -> tuple[float, float]:
+    ) -> tuple[float, float, int]:
         frames = max(0, current.frame_count - baseline.frame_count)
         dropped = max(0, current.dropped_frames - baseline.dropped_frames)
+        regressions = max(
+            0,
+            current.frame_number_regression_count
+            - baseline.frame_number_regression_count,
+        )
         if (
             baseline.last_host_monotonic_ns is None
             or current.last_host_monotonic_ns is None
         ):
-            return 0.0, 1.0
+            return 0.0, 1.0, regressions
         elapsed_ns = (
             current.last_host_monotonic_ns
             - baseline.last_host_monotonic_ns
@@ -1488,7 +1538,7 @@ class ActEpisodeRecorder(Node):
         fps = frames * 1e9 / elapsed_ns if elapsed_ns > 0 else 0.0
         expected = frames + dropped
         drop_ratio = dropped / expected if expected else 1.0
-        return fps, drop_ratio
+        return fps, drop_ratio, regressions
 
     def _write_camera_metrics(self) -> None:
         assert self._store is not None
@@ -1505,8 +1555,12 @@ class ActEpisodeRecorder(Node):
             {
                 "camera_high_fps": high[0],
                 "camera_high_drop_ratio": high[1],
+                "camera_high_frame_number_regression_count": high[2],
                 "camera_right_wrist_fps": wrist[0],
                 "camera_right_wrist_drop_ratio": wrist[1],
+                "camera_right_wrist_frame_number_regression_count": (
+                    wrist[2]
+                ),
             }
         )
 
@@ -1574,6 +1628,13 @@ class ActEpisodeRecorder(Node):
             f"ACT数据已保存：{destination}"
             f"（{report.metrics['sample_count']} samples）"
         )
+        self.get_logger().info(
+            "ACT相机采样相位："
+            f"high重复={report.metrics['cam_high_sample_repeat_ratio']:.2%}, "
+            f"high跨帧={report.metrics['cam_high_sample_skip_ratio']:.2%}, "
+            f"wrist重复={report.metrics['cam_wrist_sample_repeat_ratio']:.2%}, "
+            f"wrist跨帧={report.metrics['cam_wrist_sample_skip_ratio']:.2%}"
+        )
         self._finish_result(RecordingState.SAVED)
 
     def _reject_closed_partial(
@@ -1587,6 +1648,8 @@ class ActEpisodeRecorder(Node):
             root.attrs["episode_status"] = "rejected"
             root.attrs["reject_reason"] = reason
             root.attrs["interrupted"] = np.bool_(interrupted)
+            for name, value in episode_sample_frame_metrics(root).items():
+                root.attrs[name] = value
             sample_count = int(root["action"].shape[0])
         rejected = self._task_dir / "rejected"
         rejected.mkdir(exist_ok=True)