YikaiFu-cart/acRealman_xr
1
1
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
YikaiFu-cart
2026-08-13 22:12:23 +08:00
parent 8329c6a44d
commit 676b33bfe4
2 changed files with 130 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
xr_rm_teleop/test/test_act_episode_recorder.py
+48
View File
@@ -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
+82 -19
View File
@@ -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"):
frame_numbers = datasets[
f"debug/cameras/{camera}_frame_number"
][:].astype(np.int64)
discontinuities = int((np.diff(frame_numbers) != 1).sum())
ratio = discontinuities / max(1, sample_count - 1)
metrics[f"{camera}_sample_discontinuity_ratio"] = float(ratio)
if ratio > limits.max_drop_ratio:
return _quality_failure("camera_sample_drop_ratio", metrics)
sample_metrics = episode_sample_frame_metrics(root)
metrics.update(sample_metrics)
if any(
sample_metrics[
f"{camera}_sample_frame_number_regression_count"
]
for camera in ("cam_high", "cam_wrist")
):
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 (
self._last_frame_number is not None
and frame.frame_number > self._last_frame_number + 1
):
self._dropped_frames += (
frame.frame_number - self._last_frame_number - 1
)
if self._last_frame_number is not None:
if frame.frame_number <= self._last_frame_number:
self._frame_number_regression_count += 1
elif frame.frame_number > self._last_frame_number + 1:
self._dropped_frames += (
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)