122 lines
3.8 KiB
Python
122 lines
3.8 KiB
Python
"""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<hz>\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())
|