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

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

imu_lidar/observability.py

@@ -0,0 +1,104 @@
+"""Normalized-Jacobian observability analysis for rotation / SE(3) gates."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+
+import numpy as np
+
+from .contracts import MotionPair
+from .geometry import skew, so3_log
+
+
+@dataclass(frozen=True)
+class ObservabilityReport:
+    rotation_observable: bool
+    translation_observable: bool
+    condition_rotation: float
+    condition_translation: float
+    notes: tuple[str, ...] = ()
+
+
+def _rotation_jacobian(pairs: list[MotionPair], r_x: np.ndarray) -> np.ndarray:
+    rows = []
+    for pair in pairs:
+        # Residual r = log(R_x^T R_A R_x R_B^T); approximate J w.r.t. left perturbation of R_x.
+        # Use finite-difference columns for robustness in V1.
+        base = so3_log(r_x.T @ pair.R_A @ r_x @ pair.R_B.T)
+        cols = []
+        eps = 1e-5
+        for axis in range(3):
+            delta = np.zeros(3)
+            delta[axis] = eps
+            r_pert = r_x @ (np.eye(3) + skew(delta))
+            # Orthonormalize lightly
+            u, _, vt = np.linalg.svd(r_pert)
+            r_pert = u @ vt
+            pert = so3_log(r_pert.T @ pair.R_A @ r_pert @ pair.R_B.T)
+            cols.append((pert - base) / eps)
+        rows.append(np.column_stack(cols))
+    return np.vstack(rows) if rows else np.zeros((0, 3))
+
+
+def analyze_observability(
+    pairs: list[MotionPair] | tuple[MotionPair, ...],
+    r_x: np.ndarray,
+    *,
+    condition_threshold: float = 100.0,
+) -> ObservabilityReport:
+    """Gate whether rotation-only or full SE(3) should be accepted."""
+
+    usable = list(pairs)
+    notes: list[str] = []
+    if len(usable) < 3:
+        return ObservabilityReport(False, False, 1e9, 1e9, ("insufficient pairs",))
+
+    j_r = _rotation_jacobian(usable, np.asarray(r_x, dtype=float))
+    if j_r.size == 0:
+        return ObservabilityReport(False, False, 1e9, 1e9, ("empty rotation jacobian",))
+
+    # Normalize columns.
+    col_norm = np.linalg.norm(j_r, axis=0) + 1e-12
+    j_r_n = j_r / col_norm
+    singular = np.linalg.svd(j_r_n, compute_uv=False)
+    cond_r = float(singular[0] / max(singular[-1], 1e-12))
+    rotation_ok = cond_r < condition_threshold and singular[-1] > 1e-3
+
+    # Translation observability proxy: diversity of rotation axes and presence of translation in B.
+    axes = []
+    translations = []
+    for pair in usable:
+        axis = so3_log(pair.R_B)
+        n = np.linalg.norm(axis)
+        if n > 1e-8:
+            axes.append(axis / n)
+        if pair.t_B_m is not None:
+            translations.append(pair.t_B_m)
+    axis_rank = 0
+    if axes:
+        axis_mat = np.asarray(axes, dtype=float)
+        axis_rank = int(np.linalg.matrix_rank(axis_mat, tol=0.1))
+    trans_span = 0.0
+    if translations:
+        tmat = np.asarray(translations, dtype=float)
+        trans_span = float(np.linalg.norm(np.std(tmat, axis=0)))
+    # For planar yaw-mostly motion, translation z is typically weak.
+    translation_ok = axis_rank >= 2 and trans_span > 0.2 and len(translations) >= 5
+    cond_t = 1e9 if not translation_ok else float(max(3, 10 - axis_rank * 2) * (0.5 / max(trans_span, 1e-3)))
+
+    if not rotation_ok:
+        notes.append(f"rotation condition {cond_r:.1f} exceeds threshold {condition_threshold}")
+    else:
+        notes.append(f"rotation condition {cond_r:.1f}")
+    if not translation_ok:
+        notes.append(
+            f"translation not observable (axis_rank={axis_rank}, trans_span={trans_span:.3f} m); "
+            "V1 will reject full SE3 without strong priors"
+        )
+    return ObservabilityReport(
+        rotation_observable=rotation_ok,
+        translation_observable=translation_ok,
+        condition_rotation=cond_r,
+        condition_translation=cond_t,
+        notes=tuple(notes),
+    )