From f06e416a0ec113ae9575109bf7b1621247e585af Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E5=88=98=E6=B3=BD=E7=BE=A4?= Date: Thu, 16 Jul 2026 19:06:38 +0800 Subject: [PATCH] Add IMU frame loss analysis script --- scripts/analyze_imu_frame_loss.py | 121 +++++++++++++++++++++++++++ tests/test_analyze_imu_frame_loss.py | 58 +++++++++++++ 2 files changed, 179 insertions(+) create mode 100644 scripts/analyze_imu_frame_loss.py create mode 100644 tests/test_analyze_imu_frame_loss.py diff --git a/scripts/analyze_imu_frame_loss.py b/scripts/analyze_imu_frame_loss.py new file mode 100644 index 0000000..9e3742c --- /dev/null +++ b/scripts/analyze_imu_frame_loss.py @@ -0,0 +1,121 @@ +"""Estimate missing IMU frames from sensor uptime and CSV ODR metadata.""" + +from __future__ import annotations + +import argparse +from dataclasses import dataclass +import math +from pathlib import Path +import re + +from scripts.run_imu_ekf import read_imu_csv + + +RESTART_PREVIOUS_MIN_S = 1.0 +RESTART_CURRENT_MAX_S = 0.01 +ODR_HZ_PATTERN = re.compile(r"(?P\d+(?:\.\d+)?)\s*Hz\b", re.IGNORECASE) + + +@dataclass(frozen=True) +class FrameLossResult: + input_csv: Path + odr_hz: float + expected_period_s: float + received_frames: int + missing_frames: int + segment_count: int + + @property + def expected_frames(self) -> int: + return self.received_frames + self.missing_frames + + @property + def loss_rate(self) -> float: + return self.missing_frames / self.expected_frames + + +def analyze_file(path: Path) -> FrameLossResult: + path = Path(path) + metadata, rows = read_imu_csv(path) + odr_hz = _parse_odr_hz(metadata.get("odr", ""), path) + expected_period_s = 1.0 / odr_hz + tolerance_s = max(1.0e-12, expected_period_s * 1.0e-6) + + received_frames = 0 + missing_frames = 0 + segment_count = 0 + previous_time: float | None = None + + for data_row_index, row in enumerate(rows, start=1): + current_time = row.sensor_uptime_s + received_frames += 1 + if previous_time is None: + segment_count = 1 + previous_time = current_time + continue + + if current_time < previous_time: + if _is_device_restart(previous_time, current_time): + segment_count += 1 + previous_time = current_time + continue + raise ValueError( + f"{path} timestamp decreased at data row {data_row_index}: " + f"previous {previous_time}, current {current_time}" + ) + + delta_s = current_time - previous_time + period_count = round(delta_s / expected_period_s) + if period_count < 1 or abs(delta_s - period_count * expected_period_s) > tolerance_s: + raise ValueError( + f"{path} timestamp gap at data row {data_row_index} is not aligned to ODR: " + f"delta {delta_s}, expected period {expected_period_s}" + ) + missing_frames += period_count - 1 + previous_time = current_time + + if received_frames == 0: + raise ValueError(f"{path} has no IMU rows") + + return FrameLossResult( + input_csv=path, + odr_hz=odr_hz, + expected_period_s=expected_period_s, + received_frames=received_frames, + missing_frames=missing_frames, + segment_count=segment_count, + ) + + +def main(argv: list[str] | None = None) -> int: + parser = argparse.ArgumentParser(description="Estimate missing IMU frames from sensor_uptime_s.") + parser.add_argument("csv_files", nargs="+", type=Path) + args = parser.parse_args(argv) + + for path in args.csv_files: + result = analyze_file(path) + print( + f"{result.input_csv}: odr={result.odr_hz:g}Hz, " + f"received={result.received_frames}, missing={result.missing_frames}, " + f"expected={result.expected_frames}, loss_rate={result.loss_rate:.9%}, " + f"segments={result.segment_count}" + ) + return 0 + + +def _parse_odr_hz(value: str, path: Path) -> float: + match = ODR_HZ_PATTERN.search(value) + if match is None: + raise ValueError(f"{path} odr metadata must contain a frequency in Hz") + odr_hz = float(match.group("hz")) + if not math.isfinite(odr_hz) or odr_hz <= 0.0: + raise ValueError(f"{path} odr frequency must be finite and positive") + return odr_hz + + +def _is_device_restart(previous_time: float, current_time: float) -> bool: + return previous_time >= RESTART_PREVIOUS_MIN_S and current_time <= RESTART_CURRENT_MAX_S + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/tests/test_analyze_imu_frame_loss.py b/tests/test_analyze_imu_frame_loss.py new file mode 100644 index 0000000..940ef6b --- /dev/null +++ b/tests/test_analyze_imu_frame_loss.py @@ -0,0 +1,58 @@ +import tempfile +import unittest +from pathlib import Path + +from scripts import analyze_imu_frame_loss + + +class AnalyzeImuFrameLossTests(unittest.TestCase): + def _write_csv(self, path: Path, times: list[float], odr: str = "0x0F - 500 Hz"): + lines = [ + f"# odr={odr}", + "sensor_uptime_s,temp_c,acc_x_g,acc_y_g,acc_z_g,gyro_x_dps,gyro_y_dps,gyro_z_dps", + ] + lines.extend(f"{time},28,0,0,1,0,0,0" for time in times) + path.write_text("\n".join(lines), encoding="utf-8-sig") + + def test_counts_missing_frames_from_odr_period(self): + with tempfile.TemporaryDirectory() as tmp: + path = Path(tmp) / "imu.csv" + self._write_csv(path, [0.0, 0.002, 0.008]) + + result = analyze_imu_frame_loss.analyze_file(path) + + self.assertEqual(result.received_frames, 3) + self.assertEqual(result.missing_frames, 2) + self.assertEqual(result.expected_frames, 5) + self.assertAlmostEqual(result.loss_rate, 0.4) + self.assertEqual(result.segment_count, 1) + + def test_device_restart_starts_new_segment_without_loss(self): + with tempfile.TemporaryDirectory() as tmp: + path = Path(tmp) / "imu.csv" + self._write_csv(path, [1.0, 1.002, 0.0, 0.002]) + + result = analyze_imu_frame_loss.analyze_file(path) + + self.assertEqual(result.missing_frames, 0) + self.assertEqual(result.segment_count, 2) + + def test_non_integral_period_gap_reports_data_row(self): + with tempfile.TemporaryDirectory() as tmp: + path = Path(tmp) / "imu.csv" + self._write_csv(path, [0.0, 0.003]) + + with self.assertRaisesRegex(ValueError, "data row 2.*0.003.*0.002"): + analyze_imu_frame_loss.analyze_file(path) + + def test_missing_odr_metadata_fails_clearly(self): + with tempfile.TemporaryDirectory() as tmp: + path = Path(tmp) / "imu.csv" + self._write_csv(path, [0.0, 0.002], odr="unknown") + + with self.assertRaisesRegex(ValueError, "odr.*Hz"): + analyze_imu_frame_loss.analyze_file(path) + + +if __name__ == "__main__": + unittest.main()