79 lines
2.4 KiB
Python
79 lines
2.4 KiB
Python
"""Timestamp audit for IMU and LiDAR streams."""
|
|
|
|
from __future__ import annotations
|
|
|
|
from dataclasses import dataclass
|
|
|
|
import numpy as np
|
|
|
|
from .contracts import ImuSeries, LidarFrame
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class TimestampAuditReport:
|
|
monotonic: bool
|
|
epoch_count: int
|
|
imu_rate_hz: float
|
|
lidar_rate_hz: float
|
|
imu_duration_s: float
|
|
lidar_duration_s: float
|
|
max_imu_gap_s: float
|
|
max_lidar_gap_s: float
|
|
notes: tuple[str, ...] = ()
|
|
ok: bool = True
|
|
|
|
|
|
def _rate_and_gaps(times: np.ndarray) -> tuple[float, float]:
|
|
if times.size < 2:
|
|
return 0.0, 0.0
|
|
dt = np.diff(times)
|
|
positive = dt[dt > 0]
|
|
if positive.size == 0:
|
|
return 0.0, float("inf")
|
|
rate = float(1.0 / np.median(positive))
|
|
return rate, float(np.max(dt))
|
|
|
|
|
|
def audit_timestamps(imu: ImuSeries, frames: list[LidarFrame]) -> TimestampAuditReport:
|
|
"""Audit native timestamps without assuming the two clocks share an epoch."""
|
|
|
|
notes: list[str] = []
|
|
imu_t = imu.t_s
|
|
lidar_t = np.asarray([frame.t_mid_s for frame in frames], dtype=float)
|
|
|
|
imu_mono = bool(np.all(np.diff(imu_t) >= 0)) if imu_t.size > 1 else False
|
|
lidar_mono = bool(np.all(np.diff(lidar_t) >= 0)) if lidar_t.size > 1 else False
|
|
if not imu_mono:
|
|
notes.append("IMU timestamps are not monotonic")
|
|
if not lidar_mono:
|
|
notes.append("LiDAR timestamps are not monotonic")
|
|
|
|
imu_rate, imu_gap = _rate_and_gaps(imu_t)
|
|
lidar_rate, lidar_gap = _rate_and_gaps(lidar_t)
|
|
if imu_t.size < 50:
|
|
notes.append(f"IMU sample count is low ({imu_t.size})")
|
|
if len(frames) < 5:
|
|
notes.append(f"LiDAR frame count is low ({len(frames)})")
|
|
if imu_gap > 0.05:
|
|
notes.append(f"large IMU gap detected: {imu_gap:.3f}s")
|
|
if lidar_gap > 1.0:
|
|
notes.append(f"large LiDAR gap detected: {lidar_gap:.3f}s")
|
|
|
|
notes.append(
|
|
"IMU and LiDAR clocks are treated as independent; constant offset is estimated later."
|
|
)
|
|
|
|
ok = imu_mono and lidar_mono and imu_t.size >= 50 and len(frames) >= 5
|
|
return TimestampAuditReport(
|
|
monotonic=imu_mono and lidar_mono,
|
|
epoch_count=2,
|
|
imu_rate_hz=imu_rate,
|
|
lidar_rate_hz=lidar_rate,
|
|
imu_duration_s=float(imu_t[-1] - imu_t[0]) if imu_t.size else 0.0,
|
|
lidar_duration_s=float(lidar_t[-1] - lidar_t[0]) if lidar_t.size else 0.0,
|
|
max_imu_gap_s=imu_gap,
|
|
max_lidar_gap_s=lidar_gap,
|
|
notes=tuple(notes),
|
|
ok=ok,
|
|
)
|