添加 LiDAR-IMU 外参标定流水线与说明文档

Co-authored-by: Cursor <cursoragent@cursor.com>

imu_lidar/timestamp_audit.py

@@ -0,0 +1,78 @@
+"""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,
+    )