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

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

imu_lidar/imu_audit.py

@@ -0,0 +1,86 @@
+"""IMU unit, axis, bias, and saturation audit."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+
+import numpy as np
+
+from .contracts import ImuSeries
+
+G = 9.80665
+
+
+@dataclass(frozen=True)
+class ImuAuditReport:
+    ok: bool
+    gyro_bias_rad_s: np.ndarray
+    static_acc_mean_m_s2: np.ndarray
+    static_acc_norm_m_s2: float
+    suggested_up_axis: int
+    suggested_up_sign: float
+    static_ratio: float
+    notes: tuple[str, ...] = ()
+
+
+def _static_mask(gyro: np.ndarray, acc: np.ndarray) -> np.ndarray:
+    gyro_norm = np.linalg.norm(gyro, axis=1)
+    acc_norm = np.linalg.norm(acc, axis=1)
+    gyro_thr = max(0.02, float(np.percentile(gyro_norm, 20)) * 1.5)
+    acc_thr_low = 0.7 * G
+    acc_thr_high = 1.3 * G
+    return (gyro_norm < gyro_thr) & (acc_norm > acc_thr_low) & (acc_norm < acc_thr_high)
+
+
+def audit_imu(imu: ImuSeries) -> ImuAuditReport:
+    """Audit normalized IMU samples and estimate a static gyro bias."""
+
+    notes: list[str] = []
+    mask = _static_mask(imu.gyro_rad_s, imu.acc_m_s2)
+    static_ratio = float(np.mean(mask)) if mask.size else 0.0
+    if static_ratio < 0.02:
+        # Fall back to lowest-gyro percentile window.
+        gyro_norm = np.linalg.norm(imu.gyro_rad_s, axis=1)
+        cutoff = float(np.percentile(gyro_norm, 10))
+        mask = gyro_norm <= cutoff
+        notes.append("few gravity-consistent static samples; using lowest-gyro percentile")
+        static_ratio = float(np.mean(mask))
+
+    if not np.any(mask):
+        notes.append("no static samples found")
+        bias = np.zeros(3)
+        acc_mean = np.zeros(3)
+        acc_norm = 0.0
+        up_axis = 2
+        up_sign = 1.0
+        ok = False
+    else:
+        bias = np.mean(imu.gyro_rad_s[mask], axis=0)
+        acc_mean = np.mean(imu.acc_m_s2[mask], axis=0)
+        acc_norm = float(np.linalg.norm(acc_mean))
+        up_axis = int(np.argmax(np.abs(acc_mean)))
+        up_sign = float(np.sign(acc_mean[up_axis]) or 1.0)
+        if abs(acc_norm - G) > 2.5:
+            notes.append(
+                f"static |acc|={acc_norm:.3f} differs from g={G}; check units (expect m/s^2)"
+            )
+        gyro_peak = float(np.max(np.linalg.norm(imu.gyro_rad_s, axis=1)))
+        if gyro_peak > 20.0:
+            notes.append(
+                f"peak |gyro|={gyro_peak:.1f} rad/s looks extreme; check whether data is deg/s"
+            )
+        ok = abs(acc_norm - G) < 3.5 or static_ratio > 0.05
+
+    notes.append(
+        f"suggested up axis index={up_axis} sign={up_sign:+.0f} (0=x,1=y,2=z)"
+    )
+    return ImuAuditReport(
+        ok=ok,
+        gyro_bias_rad_s=np.asarray(bias, dtype=float),
+        static_acc_mean_m_s2=np.asarray(acc_mean, dtype=float),
+        static_acc_norm_m_s2=float(acc_norm),
+        suggested_up_axis=up_axis,
+        suggested_up_sign=up_sign,
+        static_ratio=static_ratio,
+        notes=tuple(notes),
+    )