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

Co-authored-by: Cursor <cursoragent@cursor.com>
2026-08-01 12:02:37 +08:00
commit cf1fad7594
40 changed files with 5502 additions and 0 deletions
@@ -0,0 +1,149 @@
+# `imu_lidar` 改动记录
+
+本文件专门记录 `imu_lidar` 目录内的实现改动。  
+每条包含：**时间戳**、**改动内容**（以「原本怎么做 → 改成怎么做」书写）。
+
+---
+
+## 2026-08-01 11:40 (UTC+8)
+
+### 文档：现状一览补充「合格数据」定义
+
+- **原本**：§0 只写「合格数据拿到后」怎么跑，未写清何为合格。
+- **改成**：根 [`README.md`](../README.md) §0 增加「什么叫合格数据」表（时间戳 / 会话 / 场景 / 格式 / 反例）及拿到后的模式与预期。
+
+---
+
+## 2026-08-01 11:30 (UTC+8)
+
+### 文档：现状一览 + 去掉「方案」二分表述
+
+- **原本**：对外说明仍偶发「方案二」等旧称呼；根 README 缺少一眼可读的阶段 / 合成 vs 旧车 / 合格数据预期；烟测配置与对比脚本文件名带 `scheme2`。
+- **改成**：
+  - 根 [`README.md`](../README.md) 增加 §0「现状一览」；明确仓库只有一条连续运动标定路径。
+  - [`tests/README.md`](../tests/README.md)、[`docs/IMU-LiDAR标定.md`](../docs/IMU-LiDAR标定.md)、本目录说明同步边界与阶段。
+  - `config/s2_old_smoke.yaml`、`tools/compare_s2_runs.py` 替换旧 `*scheme2*` 命名。
+
+---
+
+## 2026-07-31 18:10 (UTC+8)
+
+### 配准可视化工具 + tests 说明（含 S2 线下记录）
+
+- **原本**：无类似 RTK 仓库的运动对叠点 3D 查看；`tests/` 未说明合成 pytest 与 S2 旧数据线下试验的区别与结果。
+- **改成**：
+  - 新增 `tools/visualize_pair_3d.py` / `view_pair.ps1`（键 1–4：原始 / IMU(X=I) / 雷达 B / `X⁻¹AX`；可 `--save-png`）。
+  - 新增 [`tests/README.md`](../tests/README.md)：自动化用例表 + S2 主机时间数据做了什么、结果为何 `blocked`。
+
+---
+
+## 2026-07-31 17:20 (UTC+8)
+
+### 文档同步 + 合成数据一键复现
+
+- **原本**：`docs/标定流程与采集清单.md` 仍偏旧版「待写代码 / 因子图设想」；根 README 缺少清晰的一键复现入口与输入输出总表。
+- **改成**：
+  - 采集清单与现行流水线对齐（完整预积分、δt↔R 交替、可观时再估平移）。
+  - 新增 `tools/reproduce_synthetic.py` / `.ps1`、`tools/show_calibration_report.py`；合成生成写入 `meta.json`；根 README 增加「系统输入输出 + 一键复现」。
+
+---
+
+## 2026-07-31 16:30 (UTC+8)
+
+### 文档：移除已删除的静站路径表述，对外 README 重写
+
+- **原本**：根 README / `docs` / 包说明仍对照已删除的静站路径与内部阶段黑话；`pyproject` 仍声明已删除的 `static_station` 包。
+- **改成**：
+  - 删除旧静站文档；采集清单定为 [`docs/标定流程与采集清单.md`](../docs/标定流程与采集清单.md)。
+  - 根 [`README.md`](../README.md)、[`docs/IMU-LiDAR标定.md`](../docs/IMU-LiDAR标定.md)、本目录说明改为对外可读，只保留连续运动标定路径。
+  - `pyproject.toml` 仅保留 `imu_lidar` / `tools`。
+
+---
+
+## 2026-07-31 14:00 (UTC+8)
+
+### Phase-C：完整 IMU 预积分 + 重力/速度/动态零偏（full_se3）
+
+- **原本**：
+  - 运动对仅陀螺旋转预积分（`ΔR/Σ/J_bg`）；`t_A` 为空。
+  - 联合精修只估常值陀螺零偏修正；SE(3) 平移用经典手眼式 `(R_A-I)t ≈ R_X t_B`，无重力/速度/`b_a`。
+- **改成**：
+  - `imu_preintegration.preintegrate_imu`：中值法积分 `ΔR/Δv/Δp`，传播 15 维误差态后输出 9×9 `Σ`（含 bias RW 过程噪声）与 9×3 `J_bg/J_ba`；保留 `preintegrate_gyro`。
+  - `motion_pairs` 始终调用完整预积分，写入 `delta_v/delta_p/cov9/J_bg9/J_ba`，`t_A_m=Δp`。
+  - `joint_optimizer`：`rotation_only` 仍 Phase-A；`full_se3` 可观时 Phase-C 联合估 `R_X,t_X,g,v_k,b_g,k,b_a,k`（关键帧 RW 先验）。
+  - `pipeline` 用静止加速度推重力初值；`summary.joint` 增加 `gravity_m_s2` / `accel_bias_m_s2`。
+
+---
+
+## 2026-07-31 11:20 (UTC+8)
+
+### 文档维护约定 + README 与现行实现对齐
+
+- **原本**：根 README 与已删除的静站目录说明仍按「双路径并行」表述；部分模块说明未写明有符号 δt；改代码时 README 更新不完整。
+- **改成**：
+  - 对外说明统一为**唯一连续运动标定路径**；流水线描述对齐有符号 δt 与联合精修。
+  - 根 README 增加「文档维护」表：每次改代码必须同步涉及的 README / 本 CHANGELOG。
+
+---
+
+## 2026-07-31 09:40 (UTC+8)
+
+### 流水线：手眼未过门时仍尝试有符号 δt 精修
+
+- **原本**：`rotation_handeye.ok=false`（如 RMS>5°）时立即 `blocked` 返回，阶段 A 的有符号 δt 精修根本不会执行。
+- **改成**：只要可用运动对数 ≥3，即使用当前候选 `R` 做最多 2 轮有符号 δt 精修并重建运动对；精修后再按手眼门控决定是否 `blocked`。保证阶段 A 在困难数据上也能完整参与。
+
+---
+
+## 2026-07-31 09:20 (UTC+8)
+
+### 阶段 A：标准旋转预积分因子 + 精确时间边界 + 有符号 δt 精修
+
+- **原本**：
+  - 预积分只输出 `ΔR` 与启发式标量 weight/`σ`，区间端点用邻近 IMU 样本，无 `Σ`、无 `J_bg`。
+  - δt 仅靠角速度模长互相关粗估；手眼得到 `R` 后不再回头精修时间。
+  - 联合精修对零偏多用重积分或 `Exp(-δbΔt)` 近似，残差未按协方差白化，也无 `δb` 先验。
+- **改成**：
+  - `imu_preintegration.preintegrate_gyro`：区间端点 **线性插值** 到精确 `t0/t1`；离散中值更新同时传播 **`cov(Σ)`** 与 **`J_bg`**（`ΔR(b+δb)≈ΔR Exp(J_bg δb)`）；weight 由 `trace(Σ)` + 激励/时长构造。
+  - `motion_pairs` metadata 增加 `cov`、`J_bg`，modeling 标记为 `gyro_preintegration_factor_phase_a`。
+  - `time_offset.refine_time_offset_signed`：用当前 `R_IMU_lidar` 把 LiDAR 角速度变到 IMU 系，在粗 δt 邻域做 **三轴有符号 MSE 精修**；仅当 MSE 下降且 **模长相关不劣化** 时才接受，避免 ICP 噪声带偏；`pipeline` 在手眼后与构对交替最多 2 轮。
+  - `joint_optimizer`：残差按 `Σ` **信息白化**；零偏用 `J_bg` 一阶修正；增加弱 `δb` 先验。
+
+---
+
+## 2026-07-30 17:50 (UTC+8)
+
+### 第 1 步：帧间 IMU 轻量加强（加权预积分手眼）
+
+- **原本**：`motion_pairs` 用 `integrate_gyro_rotation` 直接得到 `R_A`，各运动对等权进入 `rotation_handeye`；手眼残差不区分长短间隔与激励强弱。
+- **改成**：
+  - 新增 `imu_preintegration.py`：对 `[t_i, t_j]` 做中值陀螺预积分，估计 `σ` 与 **pair weight**（偏短间隔、有角速度、低不确定度）。
+  - `motion_pairs` 改为调用 `preintegrate_gyro`，在 `metadata` 写入 `weight/duration_s/mean_gyro_norm/preint_sigma_rad/t_*_imu_s`，并增加 A/B 转角粗一致性过滤。
+  - `rotation_handeye` 改为 **√weight 加权** 的 Tsai 初值与 Huber 非线性精修；报告仍给未加权 RMS/中位数便于解读。
+
+### 第 2 步：预积分残差联合精修（外参 + 陀螺零偏）
+
+- **原本**：`joint_optimizer` 在手眼 `R_X` 基础上，仅在可观时用离散手眼平移式尝试 SE(3)；旋转侧不再用 IMU 过程模型，也不联合估零偏。
+- **改成**：
+  - `joint_optimizer.solve_joint_extrinsic` 增加预积分旋转残差：`log(ΔRᵀ · R_X R_B R_Xᵀ)`，按 weight 加权。
+  - 联合变量增加陀螺零偏修正 `δb`：有 `imu` 时按区间 **重预积分**；否则用一阶修正 `ΔR(b+δb)≈ΔR Exp(-δbΔt)`。
+  - `pipeline` 把 `imu`、静止零偏、`δt` 传入 joint；summary 增加 `gyro_bias_rad_s`。
+  - 平移仍受可观性门控；`rotation_only` 时不交付平移。
+
+### 文档
+
+- **原本**：`imu_lidar/README.md` 仅模块列表，无逐次改动史。
+- **改成**：新增本文件 `CHANGELOG.md`；模块说明中补充 `imu_preintegration.py` 与建模步骤描述。
+
+---
+
+## 模板（以后追加用）
+
+```markdown
+## YYYY-MM-DD HH:MM (UTC+8)
+
+### 标题
+
+- **原本**：...
+- **改成**：...
+```
@@ -0,0 +1,5 @@
+"""LiDAR–IMU calibration package (V1 runnable pipeline)."""
+
+from .contracts import CalibrationMode, CalibrationStatus, TransformConvention
+
+__all__ = ["CalibrationMode", "CalibrationStatus", "TransformConvention"]
@@ -0,0 +1,90 @@
+"""Command-line entry point for LiDAR–IMU calibration."""
+
+from __future__ import annotations
+
+import argparse
+from pathlib import Path
+
+from .contracts import CalibrationMode, CalibrationRequest, CalibrationStatus, SessionInput
+from .pipeline import describe_pipeline, run_calibration
+
+
+def build_parser() -> argparse.ArgumentParser:
+    parser = argparse.ArgumentParser(description="LiDAR–IMU extrinsic calibration (V1)")
+    subcommands = parser.add_subparsers(dest="command", required=True)
+
+    plan = subcommands.add_parser("plan", help="显示标定阶段，不读取数据")
+    plan.add_argument("--vehicle-config", help="车辆配置路径（仅展示，plan 不读取）")
+    plan.add_argument(
+        "--mode",
+        choices=[mode.value for mode in CalibrationMode],
+        default=CalibrationMode.ROTATION_ONLY.value,
+    )
+
+    run = subcommands.add_parser("run", help="执行 V1 标定流水线")
+    run.add_argument("--session-id", default="session0")
+    run.add_argument("--imu", required=True, help="IMU CSV/NPZ 路径")
+    run.add_argument("--lidar", required=True, help="LiDAR 会话目录（含 frames_index.csv）")
+    run.add_argument("--vehicle-config", required=True, help="车辆配置 YAML")
+    run.add_argument("--output", required=True, help="输出目录")
+    run.add_argument(
+        "--mode",
+        choices=[mode.value for mode in CalibrationMode],
+        default=CalibrationMode.ROTATION_ONLY.value,
+    )
+    run.add_argument("--max-iterations", type=int, default=2)
+    run.add_argument("--time-offset-search-s", type=float, default=1.0)
+    run.add_argument("--min-pair-rotation-deg", type=float, default=3.0)
+    run.add_argument("--min-pair-translation-m", type=float, default=0.3)
+    return parser
+
+
+def main(argv: list[str] | None = None) -> int:
+    parser = build_parser()
+    args = parser.parse_args(argv)
+
+    if args.command == "plan":
+        request = CalibrationRequest(
+            vehicle_config=Path(args.vehicle_config) if args.vehicle_config else None,
+            requested_mode=CalibrationMode(args.mode),
+        )
+        print("LiDAR–IMU calibration stages:")
+        print(f"requested mode: {request.requested_mode.value}")
+        for index, stage in enumerate(describe_pipeline(request), start=1):
+            print(f"{index}. {stage.name}: {stage.responsibility}")
+        return 0
+
+    if args.command == "run":
+        request = CalibrationRequest(
+            vehicle_config=Path(args.vehicle_config),
+            sessions=(
+                SessionInput(
+                    session_id=args.session_id,
+                    imu_source=Path(args.imu),
+                    lidar_source=Path(args.lidar),
+                ),
+            ),
+            requested_mode=CalibrationMode(args.mode),
+            output_directory=Path(args.output),
+            max_iterations=args.max_iterations,
+            min_pair_rotation_deg=args.min_pair_rotation_deg,
+            min_pair_translation_m=args.min_pair_translation_m,
+            time_offset_search_s=args.time_offset_search_s,
+        )
+        result = run_calibration(request)
+        print(f"status: {result.status.value}")
+        print(f"message: {result.message}")
+        if result.time_offset_s is not None:
+            print(f"time_offset_s (t_imu = t_lidar + dt): {result.time_offset_s:.6f}")
+        if result.T_IMU_lidar is not None:
+            print("T_IMU_lidar:")
+            print(result.T_IMU_lidar)
+        print(f"report directory: {args.output}")
+        return 0 if result.status != CalibrationStatus.BLOCKED else 2
+
+    parser.error(f"unknown command {args.command}")
+    return 2
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())
@@ -0,0 +1,117 @@
+"""Shared contracts for the LiDAR–IMU calibration pipeline."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+from enum import Enum
+from pathlib import Path
+from typing import Any
+
+import numpy as np
+
+
+class TransformConvention(str, Enum):
+    """The only transform convention used by this project."""
+
+    T_A_B = "T_A_B maps points from frame B into frame A"
+
+
+class CalibrationMode(str, Enum):
+    ROTATION_ONLY = "rotation_only"
+    FULL_SE3 = "full_se3"
+
+
+class CalibrationStatus(str, Enum):
+    NOT_RUN = "not_run"
+    BLOCKED = "blocked"
+    ROTATION_ONLY_ACCEPTED = "rotation_only_accepted"
+    FULL_SE3_ACCEPTED = "full_se3_accepted"
+    FULL_SE3_REJECTED = "full_se3_rejected_due_to_observability"
+
+
+@dataclass(frozen=True)
+class SessionInput:
+    """Input paths for one independently recorded session."""
+
+    session_id: str
+    imu_source: Path
+    lidar_source: Path
+    board_configuration_id: str | None = None
+
+
+@dataclass(frozen=True)
+class CalibrationRequest:
+    """Top-level calibration request."""
+
+    vehicle_config: Path | None
+    sessions: tuple[SessionInput, ...] = ()
+    requested_mode: CalibrationMode = CalibrationMode.ROTATION_ONLY
+    output_directory: Path | None = None
+    max_iterations: int = 2
+    min_pair_rotation_deg: float = 3.0
+    min_pair_translation_m: float = 0.3
+    time_offset_search_s: float = 1.0
+
+
+@dataclass
+class CalibrationResult:
+    """Result envelope written by finalize after pipeline gates."""
+
+    status: CalibrationStatus = CalibrationStatus.NOT_RUN
+    message: str = "Calibration has not been executed."
+    details: dict[str, Any] = field(default_factory=dict)
+    T_IMU_lidar: np.ndarray | None = None
+    time_offset_s: float | None = None
+
+
+@dataclass(frozen=True)
+class ImuSeries:
+    """Normalized IMU samples.
+
+    ``t_s`` is the native IMU clock in seconds (need not match LiDAR epoch).
+    Gyro must be rad/s; accelerometer must be m/s^2.
+    """
+
+    t_s: np.ndarray
+    gyro_rad_s: np.ndarray
+    acc_m_s2: np.ndarray
+
+    def __post_init__(self) -> None:
+        object.__setattr__(self, "t_s", np.asarray(self.t_s, dtype=float).reshape(-1))
+        object.__setattr__(self, "gyro_rad_s", np.asarray(self.gyro_rad_s, dtype=float).reshape(-1, 3))
+        object.__setattr__(self, "acc_m_s2", np.asarray(self.acc_m_s2, dtype=float).reshape(-1, 3))
+        n = self.t_s.size
+        if self.gyro_rad_s.shape != (n, 3) or self.acc_m_s2.shape != (n, 3):
+            raise ValueError("IMU arrays must share the same length and have shape (N, 3)")
+
+
+@dataclass(frozen=True)
+class LidarFrame:
+    """One LiDAR sweep in Cartesian sensor coordinates."""
+
+    frame_id: str
+    t_start_s: float
+    t_end_s: float
+    points_xyz: np.ndarray
+    path: Path | None = None
+
+    @property
+    def t_mid_s(self) -> float:
+        return 0.5 * (self.t_start_s + self.t_end_s)
+
+
+@dataclass(frozen=True)
+class MotionPair:
+    """One relative-motion observation between keyframes i and j."""
+
+    session_id: str
+    i: int
+    j: int
+    t_i_s: float
+    t_j_s: float
+    R_A: np.ndarray
+    R_B: np.ndarray
+    t_A_m: np.ndarray | None = None
+    t_B_m: np.ndarray | None = None
+    fitness: float = 0.0
+    metadata: dict[str, Any] = field(default_factory=dict)
@@ -0,0 +1,76 @@
+"""Package calibration outputs as JSON-friendly artifacts."""
+
+from __future__ import annotations
+
+import json
+from pathlib import Path
+from typing import Any
+
+import numpy as np
+
+from .contracts import CalibrationResult, CalibrationStatus
+from .geometry import rotation_matrix_to_quaternion_xyzw, rpy_deg_xyz
+
+
+def _to_serializable(value: Any) -> Any:
+    if isinstance(value, np.ndarray):
+        return value.tolist()
+    if isinstance(value, (np.floating, np.integer, np.bool_)):
+        return value.item()
+    if isinstance(value, Path):
+        return str(value)
+    if isinstance(value, dict):
+        return {str(k): _to_serializable(v) for k, v in value.items()}
+    if isinstance(value, (list, tuple)):
+        return [_to_serializable(v) for v in value]
+    return value
+
+
+def finalize_result(
+    *,
+    status: CalibrationStatus,
+    message: str,
+    details: dict[str, Any],
+    T_IMU_lidar: np.ndarray | None = None,
+    time_offset_s: float | None = None,
+    output_directory: Path | None = None,
+) -> CalibrationResult:
+    """Build the result envelope and optionally write report files."""
+
+    result = CalibrationResult(
+        status=status,
+        message=message,
+        details=_to_serializable(details),
+        T_IMU_lidar=None if T_IMU_lidar is None else np.asarray(T_IMU_lidar, dtype=float),
+        time_offset_s=time_offset_s,
+    )
+
+    if output_directory is not None:
+        output_directory = Path(output_directory)
+        output_directory.mkdir(parents=True, exist_ok=True)
+        summary = {
+            "status": status.value,
+            "message": message,
+            "time_offset_s": time_offset_s,
+            "details": result.details,
+        }
+        if result.T_IMU_lidar is not None:
+            t = result.T_IMU_lidar
+            summary["T_IMU_lidar"] = {
+                "matrix": t.tolist(),
+                "translation_m": t[:3, 3].tolist(),
+                "rotation_quaternion_xyzw": rotation_matrix_to_quaternion_xyzw(t[:3, :3]).tolist(),
+                "rpy_deg_xyz": rpy_deg_xyz(t[:3, :3]).tolist(),
+                "convention": "p_IMU = T_IMU_lidar * p_lidar",
+            }
+            (output_directory / "T_IMU_lidar.json").write_text(
+                json.dumps(summary["T_IMU_lidar"], indent=2),
+                encoding="utf-8",
+            )
+        if time_offset_s is not None:
+            (output_directory / "time_offset.json").write_text(
+                json.dumps({"delta_t_s": time_offset_s, "definition": "t_imu = t_lidar + delta_t"}, indent=2),
+                encoding="utf-8",
+            )
+        (output_directory / "summary.json").write_text(json.dumps(summary, indent=2), encoding="utf-8")
+    return result
@@ -0,0 +1,207 @@
+"""SE(3)/SO(3) utilities for LiDAR–IMU calibration."""
+
+from __future__ import annotations
+
+import math
+
+import numpy as np
+
+
+def skew(vector: np.ndarray) -> np.ndarray:
+    """Return the skew-symmetric matrix such that ``skew(v) @ w == v x w``."""
+
+    x, y, z = np.asarray(vector, dtype=float).reshape(3)
+    return np.array([[0.0, -z, y], [z, 0.0, -x], [-y, x, 0.0]], dtype=float)
+
+
+def so3_exp(rotation_vector: np.ndarray) -> np.ndarray:
+    """Map a rotation vector in radians onto SO(3)."""
+
+    vector = np.asarray(rotation_vector, dtype=float).reshape(3)
+    angle = float(np.linalg.norm(vector))
+    if angle < 1e-12:
+        return np.eye(3) + skew(vector)
+    axis_cross = skew(vector / angle)
+    return np.eye(3) + math.sin(angle) * axis_cross + (1.0 - math.cos(angle)) * axis_cross @ axis_cross
+
+
+def so3_log(rotation: np.ndarray) -> np.ndarray:
+    """Map an SO(3) matrix to a rotation vector in radians."""
+
+    rotation = np.asarray(rotation, dtype=float).reshape(3, 3)
+    cos_angle = float(np.clip((np.trace(rotation) - 1.0) * 0.5, -1.0, 1.0))
+    angle = math.acos(cos_angle)
+    if angle < 1e-12:
+        return 0.5 * np.array(
+            [
+                rotation[2, 1] - rotation[1, 2],
+                rotation[0, 2] - rotation[2, 0],
+                rotation[1, 0] - rotation[0, 1],
+            ],
+            dtype=float,
+        )
+    if abs(angle - math.pi) < 1e-6:
+        # Near 180°: use eigenvector of the +1 eigenvalue.
+        eigvals, eigvecs = np.linalg.eigh(0.5 * (rotation + rotation.T))
+        axis = eigvecs[:, int(np.argmax(eigvals))]
+        return axis * angle
+    return (
+        0.5
+        * angle
+        / math.sin(angle)
+        * np.array(
+            [
+                rotation[2, 1] - rotation[1, 2],
+                rotation[0, 2] - rotation[2, 0],
+                rotation[1, 0] - rotation[0, 1],
+            ],
+            dtype=float,
+        )
+    )
+
+
+def rotation_angle_deg(rotation: np.ndarray) -> float:
+    """Return the rotation angle in degrees."""
+
+    return float(np.degrees(np.linalg.norm(so3_log(rotation))))
+
+
+def inverse_transform(transform: np.ndarray) -> np.ndarray:
+    """Return the inverse of a rigid 4x4 transform."""
+
+    transform = np.asarray(transform, dtype=float)
+    if transform.shape != (4, 4):
+        raise ValueError("a rigid transform must have shape (4, 4)")
+    result = np.eye(4)
+    result[:3, :3] = transform[:3, :3].T
+    result[:3, 3] = -result[:3, :3] @ transform[:3, 3]
+    return result
+
+
+def make_transform(translation_m: np.ndarray, rotation: np.ndarray) -> np.ndarray:
+    """Build ``T_A_B`` from its translation and rotation components."""
+
+    translation_m = np.asarray(translation_m, dtype=float).reshape(3)
+    rotation = np.asarray(rotation, dtype=float)
+    if rotation.shape != (3, 3):
+        raise ValueError("a rotation matrix must have shape (3, 3)")
+    result = np.eye(4)
+    result[:3, :3] = rotation
+    result[:3, 3] = translation_m
+    return result
+
+
+def transform_points(points: np.ndarray, transform: np.ndarray) -> np.ndarray:
+    """Apply ``T_A_B`` to an ``(N, 3)`` point array expressed in frame B."""
+
+    points = np.asarray(points, dtype=float)
+    if points.ndim != 2 or points.shape[1] != 3:
+        raise ValueError("points must have shape (N, 3)")
+    return points @ transform[:3, :3].T + transform[:3, 3]
+
+
+def orthonormalize_rotation(rotation: np.ndarray) -> np.ndarray:
+    """Project a near-rotation matrix onto SO(3)."""
+
+    u, _, vt = np.linalg.svd(np.asarray(rotation, dtype=float).reshape(3, 3))
+    result = u @ vt
+    if np.linalg.det(result) < 0:
+        u[:, -1] *= -1
+        result = u @ vt
+    return result
+
+
+def integrate_gyro_rotation(
+    times_s: np.ndarray,
+    gyro_rad_s: np.ndarray,
+    t0: float,
+    t1: float,
+    bias_rad_s: np.ndarray | None = None,
+) -> np.ndarray:
+    """Integrate gyroscope samples on ``[t0, t1]`` and return ``R(t0<-t1)`` wait.
+
+    Returns ``R_i_j`` that maps vectors from the IMU frame at ``t1`` into the
+    IMU frame at ``t0`` using right-invariant discrete integration:
+
+        R <- R @ Exp(omega * dt)
+    """
+
+    times_s = np.asarray(times_s, dtype=float).reshape(-1)
+    gyro_rad_s = np.asarray(gyro_rad_s, dtype=float).reshape(-1, 3)
+    if times_s.size < 2:
+        return np.eye(3)
+    bias = np.zeros(3) if bias_rad_s is None else np.asarray(bias_rad_s, dtype=float).reshape(3)
+
+    if t1 < t0:
+        raise ValueError("t1 must be >= t0")
+
+    # Include one sample before t0 and after t1 when possible for interpolation.
+    left = int(np.searchsorted(times_s, t0, side="left") - 1)
+    right = int(np.searchsorted(times_s, t1, side="right"))
+    left = max(left, 0)
+    right = min(right, times_s.size - 1)
+    if right <= left:
+        return np.eye(3)
+
+    rotation = np.eye(3)
+    for index in range(left, right):
+        t_a = float(times_s[index])
+        t_b = float(times_s[index + 1])
+        if t_b <= t0 or t_a >= t1:
+            continue
+        seg0 = max(t_a, t0)
+        seg1 = min(t_b, t1)
+        dt = seg1 - seg0
+        if dt <= 0:
+            continue
+        omega = 0.5 * (gyro_rad_s[index] + gyro_rad_s[index + 1]) - bias
+        rotation = rotation @ so3_exp(omega * dt)
+    return orthonormalize_rotation(rotation)
+
+
+def rotation_matrix_to_quaternion_xyzw(rotation: np.ndarray) -> np.ndarray:
+    """Convert SO(3) to quaternion ``[x, y, z, w]``."""
+
+    rotation = orthonormalize_rotation(rotation)
+    trace = float(np.trace(rotation))
+    if trace > 0:
+        s = math.sqrt(trace + 1.0) * 2.0
+        w = 0.25 * s
+        x = (rotation[2, 1] - rotation[1, 2]) / s
+        y = (rotation[0, 2] - rotation[2, 0]) / s
+        z = (rotation[1, 0] - rotation[0, 1]) / s
+    elif rotation[0, 0] > rotation[1, 1] and rotation[0, 0] > rotation[2, 2]:
+        s = math.sqrt(1.0 + rotation[0, 0] - rotation[1, 1] - rotation[2, 2]) * 2.0
+        w = (rotation[2, 1] - rotation[1, 2]) / s
+        x = 0.25 * s
+        y = (rotation[0, 1] + rotation[1, 0]) / s
+        z = (rotation[0, 2] + rotation[2, 0]) / s
+    elif rotation[1, 1] > rotation[2, 2]:
+        s = math.sqrt(1.0 + rotation[1, 1] - rotation[0, 0] - rotation[2, 2]) * 2.0
+        w = (rotation[0, 2] - rotation[2, 0]) / s
+        x = (rotation[0, 1] + rotation[1, 0]) / s
+        y = 0.25 * s
+        z = (rotation[1, 2] + rotation[2, 1]) / s
+    else:
+        s = math.sqrt(1.0 + rotation[2, 2] - rotation[0, 0] - rotation[1, 1]) * 2.0
+        w = (rotation[1, 0] - rotation[0, 1]) / s
+        x = (rotation[0, 2] + rotation[2, 0]) / s
+        y = (rotation[1, 2] + rotation[2, 1]) / s
+        z = 0.25 * s
+    return np.array([x, y, z, w], dtype=float)
+
+
+def rpy_deg_xyz(rotation: np.ndarray) -> np.ndarray:
+    """Intrinsic XYZ Euler angles in degrees from a rotation matrix."""
+
+    rotation = orthonormalize_rotation(rotation)
+    sy = math.sqrt(rotation[0, 0] ** 2 + rotation[1, 0] ** 2)
+    if sy > 1e-8:
+        roll = math.atan2(rotation[2, 1], rotation[2, 2])
+        pitch = math.atan2(-rotation[2, 0], sy)
+        yaw = math.atan2(rotation[1, 0], rotation[0, 0])
+    else:
+        roll = math.atan2(-rotation[1, 2], rotation[1, 1])
+        pitch = math.atan2(-rotation[2, 0], sy)
+        yaw = 0.0
+    return np.degrees(np.array([roll, pitch, yaw], dtype=float))
@@ -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),
+    )
@@ -0,0 +1,72 @@
+"""IMU adapters for the V1 standard intermediate format.
+
+Accepted inputs
+---------------
+1. CSV with header:
+   t,gx,gy,gz,ax,ay,az
+   - ``t`` in seconds on the IMU clock
+   - gyro in rad/s
+   - accel in m/s^2
+
+2. NPZ with arrays:
+   t, gyro, acc
+   shapes: (N,), (N,3), (N,3)
+"""
+
+from __future__ import annotations
+
+from pathlib import Path
+
+import numpy as np
+
+from .contracts import ImuSeries
+
+
+def load_imu_samples(path: Path | str) -> ImuSeries:
+    """Load normalized IMU samples from CSV or NPZ."""
+
+    source = Path(path)
+    if not source.exists():
+        raise FileNotFoundError(source)
+    if source.suffix.lower() == ".csv":
+        return _load_imu_csv(source)
+    if source.suffix.lower() == ".npz":
+        return _load_imu_npz(source)
+    raise ValueError(f"unsupported IMU format '{source.suffix}' (use .csv or .npz)")
+
+
+def _load_imu_csv(path: Path) -> ImuSeries:
+    data = np.genfromtxt(path, delimiter=",", names=True, dtype=float)
+    if data.ndim == 0:
+        data = np.array([data])
+    names = set(data.dtype.names or ())
+    required = {"t", "gx", "gy", "gz", "ax", "ay", "az"}
+    if not required.issubset(names):
+        raise ValueError(f"IMU CSV must contain columns {sorted(required)}, got {sorted(names)}")
+    t = np.asarray(data["t"], dtype=float).reshape(-1)
+    gyro = np.column_stack([data["gx"], data["gy"], data["gz"]]).astype(float)
+    acc = np.column_stack([data["ax"], data["ay"], data["az"]]).astype(float)
+    order = np.argsort(t)
+    return ImuSeries(t_s=t[order], gyro_rad_s=gyro[order], acc_m_s2=acc[order])
+
+
+def _load_imu_npz(path: Path) -> ImuSeries:
+    with np.load(path) as payload:
+        keys = set(payload.files)
+        if not {"t", "gyro", "acc"}.issubset(keys):
+            raise ValueError(f"IMU NPZ must contain t, gyro, acc; got {sorted(keys)}")
+        t = np.asarray(payload["t"], dtype=float).reshape(-1)
+        gyro = np.asarray(payload["gyro"], dtype=float).reshape(-1, 3)
+        acc = np.asarray(payload["acc"], dtype=float).reshape(-1, 3)
+    order = np.argsort(t)
+    return ImuSeries(t_s=t[order], gyro_rad_s=gyro[order], acc_m_s2=acc[order])
+
+
+def save_imu_csv(path: Path | str, imu: ImuSeries) -> None:
+    """Write IMU samples to the standard CSV format."""
+
+    destination = Path(path)
+    destination.parent.mkdir(parents=True, exist_ok=True)
+    array = np.column_stack([imu.t_s, imu.gyro_rad_s, imu.acc_m_s2])
+    header = "t,gx,gy,gz,ax,ay,az"
+    np.savetxt(destination, array, delimiter=",", header=header, comments="")
@@ -0,0 +1,452 @@
+"""Frame-to-frame IMU preintegration (Phase-A rotation + Phase-C full factor).
+
+Phase-A: ``ΔR``, 3×3 ``Σ``, ``J_bg``.
+Phase-C: ``ΔR/Δv/Δp``, 9×9 ``Σ`` (with bias RW process noise), ``J_bg``/``J_ba``.
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+
+import numpy as np
+
+from .geometry import orthonormalize_rotation, so3_exp, so3_log, skew
+
+
+@dataclass(frozen=True)
+class GyroPreintegration:
+    """Rotation-only preintegration on ``[t0, t1]`` (IMU clock)."""
+
+    delta_R: np.ndarray
+    duration_s: float
+    mean_gyro_norm: float
+    sigma_rad: float
+    weight: float
+    bias_rad_s: np.ndarray
+    cov: np.ndarray
+    J_bg: np.ndarray
+
+
+@dataclass(frozen=True)
+class ImuPreintegration:
+    """Full IMU preintegration on ``[t0, t1]`` (IMU clock).
+
+    ``delta_R`` maps vectors from IMU frame at ``t1`` into IMU frame at ``t0``.
+    ``delta_v`` / ``delta_p`` are body-frame increments (no gravity).
+    Error-state order in ``cov`` / Jacobians: ``[δθ, δv, δp]`` (9).
+    ``J_bg`` / ``J_ba`` are 9×3: first-order correction w.r.t. constant bias deltas.
+    """
+
+    delta_R: np.ndarray
+    delta_v: np.ndarray
+    delta_p: np.ndarray
+    duration_s: float
+    mean_gyro_norm: float
+    sigma_rad: float
+    weight: float
+    gyro_bias_rad_s: np.ndarray
+    acc_bias_m_s2: np.ndarray
+    cov: np.ndarray
+    J_bg: np.ndarray
+    J_ba: np.ndarray
+
+
+def _right_jacobian(phi: np.ndarray) -> np.ndarray:
+    """SO(3) right Jacobian ``Jr(φ)`` with ``Exp(φ+δ)≈Exp(φ)Exp(Jr δ)``."""
+
+    phi = np.asarray(phi, dtype=float).reshape(3)
+    angle = float(np.linalg.norm(phi))
+    if angle < 1e-8:
+        return np.eye(3) - 0.5 * skew(phi)
+    axis = phi / angle
+    s = skew(axis)
+    return (
+        np.eye(3)
+        - ((1.0 - np.cos(angle)) / angle) * s
+        + ((angle - np.sin(angle)) / angle) * (s @ s)
+    )
+
+
+def _interp_vec(times_s: np.ndarray, values: np.ndarray, t: float) -> np.ndarray:
+    """Linear interpolate a 3-vector series at an exact time."""
+
+    return np.array(
+        [float(np.interp(t, times_s, values[:, axis])) for axis in range(3)],
+        dtype=float,
+    )
+
+
+def _interp_gyro(times_s: np.ndarray, gyro_rad_s: np.ndarray, t: float) -> np.ndarray:
+    """Linear interpolate gyro at an exact time."""
+
+    return _interp_vec(times_s, gyro_rad_s, t)
+
+
+def _pair_weight(duration_s: float, mean_gyro_norm: float, cov_trace: float) -> float:
+    """Larger weight for short, excited, low-covariance intervals."""
+
+    duration_term = 1.0 / max(duration_s, 0.05)
+    excite_term = min(max(mean_gyro_norm, 1e-3), 1.0)
+    avg_var = max(cov_trace / 3.0, 1e-8)
+    return float(duration_term * excite_term / avg_var)
+
+
+def preintegrate_gyro(
+    times_s: np.ndarray,
+    gyro_rad_s: np.ndarray,
+    t0: float,
+    t1: float,
+    bias_rad_s: np.ndarray | None = None,
+    *,
+    sigma_g_rad_s_sqrt_hz: float = 1.5e-3,
+) -> GyroPreintegration:
+    """Discrete mid-point gyro preintegration with exact endpoints.
+
+    ``delta_R`` maps vectors from IMU frame at ``t1`` into IMU frame at ``t0``
+    via right-invariant updates ``ΔR ← ΔR Exp((ω-b) dt)``.
+
+    Also returns:
+    - ``cov``: 3×3 covariance of the right tangent noise on ``ΔR``
+    - ``J_bg``: ``ΔR(b+δb) ≈ ΔR Exp(J_bg δb)``
+    """
+
+    times_s = np.asarray(times_s, dtype=float).reshape(-1)
+    gyro_rad_s = np.asarray(gyro_rad_s, dtype=float).reshape(-1, 3)
+    bias = np.zeros(3) if bias_rad_s is None else np.asarray(bias_rad_s, dtype=float).reshape(3)
+    duration = float(max(t1 - t0, 0.0))
+
+    empty = GyroPreintegration(
+        delta_R=np.eye(3),
+        duration_s=0.0,
+        mean_gyro_norm=0.0,
+        sigma_rad=1e3,
+        weight=1e-6,
+        bias_rad_s=bias.copy(),
+        cov=np.eye(3) * 1e6,
+        J_bg=np.zeros((3, 3)),
+    )
+    if times_s.size < 2 or duration <= 0:
+        return empty
+
+    t0 = float(np.clip(t0, times_s[0], times_s[-1]))
+    t1 = float(np.clip(t1, times_s[0], times_s[-1]))
+    duration = float(max(t1 - t0, 0.0))
+    if duration <= 0:
+        return empty
+
+    left = int(np.searchsorted(times_s, t0, side="left") - 1)
+    right = int(np.searchsorted(times_s, t1, side="right"))
+    left = max(left, 0)
+    right = min(right, times_s.size - 1)
+    if right <= left:
+        return empty
+
+    delta_r = np.eye(3)
+    j_bg = np.zeros((3, 3))
+    cov = np.zeros((3, 3))
+    sigma2 = float(sigma_g_rad_s_sqrt_hz) ** 2
+    gyro_norms: list[float] = []
+
+    for index in range(left, right):
+        t_a = float(times_s[index])
+        t_b = float(times_s[index + 1])
+        if t_b <= t0 or t_a >= t1:
+            continue
+        seg0 = max(t_a, t0)
+        seg1 = min(t_b, t1)
+        dt = seg1 - seg0
+        if dt <= 0:
+            continue
+
+        # Exact endpoint gyro via linear interpolation inside the sample interval.
+        g_a = _interp_gyro(times_s, gyro_rad_s, seg0)
+        g_b = _interp_gyro(times_s, gyro_rad_s, seg1)
+        omega = 0.5 * (g_a + g_b) - bias
+        gyro_norms.append(float(np.linalg.norm(omega)))
+
+        theta = omega * dt
+        jr = _right_jacobian(theta)
+        a_mat = so3_exp(-theta)
+        j_bg = a_mat @ j_bg - jr * dt
+        cov = a_mat @ cov @ a_mat.T + jr @ (sigma2 * dt * np.eye(3)) @ jr.T
+        delta_r = delta_r @ so3_exp(theta)
+
+    delta_r = orthonormalize_rotation(delta_r)
+    mean_gyro_norm = float(np.mean(gyro_norms)) if gyro_norms else 0.0
+    cov = 0.5 * (cov + cov.T)
+    cov = cov + np.eye(3) * 1e-12
+    if mean_gyro_norm < 0.02:
+        cov = cov * 4.0
+    cov_trace = float(np.trace(cov))
+    sigma_rad = float(np.sqrt(max(cov_trace / 3.0, 1e-12)))
+    weight = _pair_weight(duration, mean_gyro_norm, cov_trace)
+    return GyroPreintegration(
+        delta_R=delta_r,
+        duration_s=duration,
+        mean_gyro_norm=mean_gyro_norm,
+        sigma_rad=sigma_rad,
+        weight=weight,
+        bias_rad_s=bias.copy(),
+        cov=cov,
+        J_bg=np.asarray(j_bg, dtype=float),
+    )
+
+
+def preintegrate_imu(
+    times_s: np.ndarray,
+    gyro_rad_s: np.ndarray,
+    acc_m_s2: np.ndarray,
+    t0: float,
+    t1: float,
+    gyro_bias_rad_s: np.ndarray | None = None,
+    acc_bias_m_s2: np.ndarray | None = None,
+    *,
+    sigma_g_rad_s_sqrt_hz: float = 1.5e-3,
+    sigma_a_m_s2_sqrt_hz: float = 2.0e-2,
+    sigma_bg_rw_rad_s_sqrt_hz: float = 1.0e-5,
+    sigma_ba_rw_m_s2_sqrt_hz: float = 1.0e-3,
+) -> ImuPreintegration:
+    """Mid-point IMU preintegration with exact endpoints and bias-RW noise.
+
+    Discrete updates (right-invariant)::
+
+        ΔR ← ΔR Exp((ω-bg) dt)
+        Δv ← Δv + ΔR (a-ba) dt
+        Δp ← Δp + Δv_old dt + 0.5 ΔR (a-ba) dt²
+
+    Propagates a 15-DoF error state ``[δθ, δv, δp, δbg, δba]`` then returns the
+    top-left 9×9 covariance (bias RW already folded in) and 9×3 Jacobians.
+    """
+
+    times_s = np.asarray(times_s, dtype=float).reshape(-1)
+    gyro_rad_s = np.asarray(gyro_rad_s, dtype=float).reshape(-1, 3)
+    acc_m_s2 = np.asarray(acc_m_s2, dtype=float).reshape(-1, 3)
+    bg = np.zeros(3) if gyro_bias_rad_s is None else np.asarray(gyro_bias_rad_s, dtype=float).reshape(3)
+    ba = np.zeros(3) if acc_bias_m_s2 is None else np.asarray(acc_bias_m_s2, dtype=float).reshape(3)
+
+    empty = ImuPreintegration(
+        delta_R=np.eye(3),
+        delta_v=np.zeros(3),
+        delta_p=np.zeros(3),
+        duration_s=0.0,
+        mean_gyro_norm=0.0,
+        sigma_rad=1e3,
+        weight=1e-6,
+        gyro_bias_rad_s=bg.copy(),
+        acc_bias_m_s2=ba.copy(),
+        cov=np.eye(9) * 1e6,
+        J_bg=np.zeros((9, 3)),
+        J_ba=np.zeros((9, 3)),
+    )
+    if times_s.size < 2 or acc_m_s2.shape != gyro_rad_s.shape:
+        return empty
+
+    t0 = float(np.clip(t0, times_s[0], times_s[-1]))
+    t1 = float(np.clip(t1, times_s[0], times_s[-1]))
+    duration = float(max(t1 - t0, 0.0))
+    if duration <= 0:
+        return empty
+
+    left = int(np.searchsorted(times_s, t0, side="left") - 1)
+    right = int(np.searchsorted(times_s, t1, side="right"))
+    left = max(left, 0)
+    right = min(right, times_s.size - 1)
+    if right <= left:
+        return empty
+
+    delta_r = np.eye(3)
+    delta_v = np.zeros(3)
+    delta_p = np.zeros(3)
+    # Jacobians of [δθ, δv, δp] w.r.t. constant bias (accumulated analytically).
+    j_bg = np.zeros((9, 3))
+    j_ba = np.zeros((9, 3))
+    # 15×15 covariance: [θ, v, p, bg, ba]
+    cov15 = np.zeros((15, 15))
+    sg2 = float(sigma_g_rad_s_sqrt_hz) ** 2
+    sa2 = float(sigma_a_m_s2_sqrt_hz) ** 2
+    sbg2 = float(sigma_bg_rw_rad_s_sqrt_hz) ** 2
+    sba2 = float(sigma_ba_rw_m_s2_sqrt_hz) ** 2
+    gyro_norms: list[float] = []
+
+    for index in range(left, right):
+        t_a = float(times_s[index])
+        t_b = float(times_s[index + 1])
+        if t_b <= t0 or t_a >= t1:
+            continue
+        seg0 = max(t_a, t0)
+        seg1 = min(t_b, t1)
+        dt = seg1 - seg0
+        if dt <= 0:
+            continue
+
+        g_a = _interp_vec(times_s, gyro_rad_s, seg0)
+        g_b = _interp_vec(times_s, gyro_rad_s, seg1)
+        a_a = _interp_vec(times_s, acc_m_s2, seg0)
+        a_b = _interp_vec(times_s, acc_m_s2, seg1)
+        omega = 0.5 * (g_a + g_b) - bg
+        acc = 0.5 * (a_a + a_b) - ba
+        gyro_norms.append(float(np.linalg.norm(omega)))
+
+        theta = omega * dt
+        jr = _right_jacobian(theta)
+        r_dt = so3_exp(theta)
+        r_mid = delta_r  # rotate body accel into i0 frame before update
+
+        # Bias Jacobians (Forster-style first-order recursion).
+        j_r_bg = j_bg[0:3]
+        j_v_bg = j_bg[3:6]
+        j_p_bg = j_bg[6:9]
+        j_r_ba = j_ba[0:3]
+        j_v_ba = j_ba[3:6]
+        j_p_ba = j_ba[6:9]
+
+        acc_skew = skew(acc)
+        j_p_bg_new = j_p_bg + j_v_bg * dt - 0.5 * r_mid @ acc_skew @ j_r_bg * (dt**2)
+        j_v_bg_new = j_v_bg - r_mid @ acc_skew @ j_r_bg * dt
+        j_r_bg_new = r_dt.T @ j_r_bg - jr * dt
+
+        j_p_ba_new = j_p_ba + j_v_ba * dt - 0.5 * r_mid * (dt**2)
+        j_v_ba_new = j_v_ba - r_mid * dt
+        j_r_ba_new = r_dt.T @ j_r_ba
+
+        j_bg = np.vstack([j_r_bg_new, j_v_bg_new, j_p_bg_new])
+        j_ba = np.vstack([j_r_ba_new, j_v_ba_new, j_p_ba_new])
+
+        # Nominal state update (use pre-update Δv in position).
+        delta_p = delta_p + delta_v * dt + 0.5 * r_mid @ acc * (dt**2)
+        delta_v = delta_v + r_mid @ acc * dt
+        delta_r = orthonormalize_rotation(delta_r @ r_dt)
+
+        # Linearized error-state transition (15×15).
+        f = np.eye(15)
+        a_mat = so3_exp(-theta)
+        f[0:3, 0:3] = a_mat
+        f[0:3, 9:12] = -jr * dt
+        f[3:6, 0:3] = -r_mid @ acc_skew * dt
+        f[3:6, 12:15] = -r_mid * dt
+        f[6:9, 0:3] = -0.5 * r_mid @ acc_skew * (dt**2)
+        f[6:9, 3:6] = np.eye(3) * dt
+        f[6:9, 12:15] = -0.5 * r_mid * (dt**2)
+
+        # Noise: continuous densities σ²; Var(∫n dt)=σ² dt. Columns: n_g, n_a, n_bg, n_ba.
+        g_mat = np.zeros((15, 12))
+        g_mat[0:3, 0:3] = jr
+        g_mat[3:6, 3:6] = r_mid
+        g_mat[6:9, 3:6] = 0.5 * r_mid * dt
+        g_mat[9:12, 6:9] = np.eye(3)
+        g_mat[12:15, 9:12] = np.eye(3)
+        q = np.zeros((12, 12))
+        q[0:3, 0:3] = sg2 * dt * np.eye(3)
+        q[3:6, 3:6] = sa2 * dt * np.eye(3)
+        q[6:9, 6:9] = sbg2 * dt * np.eye(3)
+        q[9:12, 9:12] = sba2 * dt * np.eye(3)
+        cov15 = f @ cov15 @ f.T + g_mat @ q @ g_mat.T
+
+    delta_r = orthonormalize_rotation(delta_r)
+    mean_gyro_norm = float(np.mean(gyro_norms)) if gyro_norms else 0.0
+    cov9 = cov15[0:9, 0:9]
+    cov9 = 0.5 * (cov9 + cov9.T) + np.eye(9) * 1e-12
+    if mean_gyro_norm < 0.02:
+        cov9 = cov9.copy()
+        cov9[0:3, 0:3] = cov9[0:3, 0:3] * 4.0
+    cov_trace = float(np.trace(cov9[0:3, 0:3]))
+    sigma_rad = float(np.sqrt(max(cov_trace / 3.0, 1e-12)))
+    weight = _pair_weight(duration, mean_gyro_norm, cov_trace)
+    return ImuPreintegration(
+        delta_R=delta_r,
+        delta_v=np.asarray(delta_v, dtype=float),
+        delta_p=np.asarray(delta_p, dtype=float),
+        duration_s=duration,
+        mean_gyro_norm=mean_gyro_norm,
+        sigma_rad=sigma_rad,
+        weight=weight,
+        gyro_bias_rad_s=bg.copy(),
+        acc_bias_m_s2=ba.copy(),
+        cov=np.asarray(cov9, dtype=float),
+        J_bg=np.asarray(j_bg, dtype=float),
+        J_ba=np.asarray(j_ba, dtype=float),
+    )
+
+
+def apply_bias_correction_imu(
+    preint: ImuPreintegration,
+    delta_gyro_bias: np.ndarray | None = None,
+    delta_acc_bias: np.ndarray | None = None,
+) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
+    """First-order bias correction of ``ΔR/Δv/Δp``.
+
+    Returns ``(delta_R, delta_v, delta_p)``.
+    """
+
+    dbg = np.zeros(3) if delta_gyro_bias is None else np.asarray(delta_gyro_bias, dtype=float).reshape(3)
+    dba = np.zeros(3) if delta_acc_bias is None else np.asarray(delta_acc_bias, dtype=float).reshape(3)
+    j_bg = np.asarray(preint.J_bg, dtype=float).reshape(9, 3)
+    j_ba = np.asarray(preint.J_ba, dtype=float).reshape(9, 3)
+    delta_r = orthonormalize_rotation(preint.delta_R @ so3_exp(j_bg[0:3] @ dbg))
+    delta_v = preint.delta_v + j_bg[3:6] @ dbg + j_ba[3:6] @ dba
+    delta_p = preint.delta_p + j_bg[6:9] @ dbg + j_ba[6:9] @ dba
+    return delta_r, np.asarray(delta_v, dtype=float), np.asarray(delta_p, dtype=float)
+
+
+def relative_rotation_from_lidar(R_X: np.ndarray, R_B: np.ndarray) -> np.ndarray:
+    """Map LiDAR relative rotation into IMU frame: ``R_X R_B R_X^T``."""
+
+    r_x = orthonormalize_rotation(R_X)
+    r_b = orthonormalize_rotation(R_B)
+    return orthonormalize_rotation(r_x @ r_b @ r_x.T)
+
+
+def preintegration_rotation_residual(
+    delta_R: np.ndarray,
+    R_X: np.ndarray,
+    R_B: np.ndarray,
+) -> np.ndarray:
+    """``log( delta_R^T * R_X R_B R_X^T )`` in so(3)."""
+
+    predicted = relative_rotation_from_lidar(R_X, R_B)
+    return so3_log(delta_R.T @ predicted)
+
+
+def apply_bias_jacobian_correction(
+    delta_R: np.ndarray,
+    J_bg: np.ndarray,
+    delta_bias_rad_s: np.ndarray,
+) -> np.ndarray:
+    """First-order update ``ΔR(b+δb) ≈ ΔR Exp(J_bg δb)``."""
+
+    db = np.asarray(delta_bias_rad_s, dtype=float).reshape(3)
+    j_bg = np.asarray(J_bg, dtype=float).reshape(3, 3)
+    return orthonormalize_rotation(delta_R @ so3_exp(j_bg @ db))
+
+
+def apply_constant_bias_correction(
+    delta_R: np.ndarray,
+    duration_s: float,
+    delta_bias_rad_s: np.ndarray,
+) -> np.ndarray:
+    """Legacy first-order correction when ``J_bg`` is unavailable.
+
+    ``ΔR(b+δb) ≈ ΔR Exp(-δb Δt)`` (identity Jacobian approximation).
+    """
+
+    db = np.asarray(delta_bias_rad_s, dtype=float).reshape(3)
+    return orthonormalize_rotation(delta_R @ so3_exp(-db * float(duration_s)))
+
+
+def residual_whiten_matrix(cov: np.ndarray) -> np.ndarray:
+    """Return ``W`` such that ``W @ e`` is approximately information-whitened.
+
+    Accepts square ``n×n`` covariances (3×3 rotation or 9×9 full IMU).
+    """
+
+    matrix = np.asarray(cov, dtype=float)
+    if matrix.ndim != 2 or matrix.shape[0] != matrix.shape[1]:
+        raise ValueError("cov must be square")
+    n = matrix.shape[0]
+    matrix = 0.5 * (matrix + matrix.T) + np.eye(n) * 1e-10
+    try:
+        info = np.linalg.inv(matrix)
+        return np.linalg.cholesky(info).T
+    except np.linalg.LinAlgError:
+        scale = 1.0 / max(float(np.sqrt(np.trace(matrix) / n)), 1e-6)
+        return np.eye(n) * scale
@@ -0,0 +1,473 @@
+"""Joint extrinsic refinement: Phase-A rotation factors + Phase-C SE(3) IMU factors."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+
+import numpy as np
+from scipy.optimize import least_squares
+
+from .contracts import ImuSeries, MotionPair
+from .geometry import make_transform, orthonormalize_rotation, so3_exp, so3_log
+from .imu_preintegration import (
+    apply_bias_jacobian_correction,
+    apply_constant_bias_correction,
+    preintegrate_gyro,
+    preintegration_rotation_residual,
+    residual_whiten_matrix,
+)
+from .observability import ObservabilityReport, analyze_observability
+
+G_NORM = 9.80665
+
+
+@dataclass(frozen=True)
+class JointExtrinsicResult:
+    T_IMU_lidar: np.ndarray
+    translation_accepted: bool
+    residual_rms_rot_deg: float
+    residual_rms_trans_m: float
+    observability: ObservabilityReport
+    gyro_bias_rad_s: np.ndarray | None = None
+    accel_bias_m_s2: np.ndarray | None = None
+    gravity_m_s2: np.ndarray | None = None
+    notes: tuple[str, ...] = ()
+
+
+def _pair_weight(pair: MotionPair) -> float:
+    weight = float(pair.metadata.get("weight", 1.0))
+    if not np.isfinite(weight) or weight <= 0:
+        return 1.0
+    return weight
+
+
+def _pair_j_bg(pair: MotionPair) -> np.ndarray | None:
+    raw = pair.metadata.get("J_bg")
+    if raw is None:
+        return None
+    return np.asarray(raw, dtype=float).reshape(3, 3)
+
+
+def _pair_cov(pair: MotionPair) -> np.ndarray:
+    raw = pair.metadata.get("cov")
+    if raw is None:
+        sigma = float(pair.metadata.get("preint_sigma_rad", 1e-2))
+        return np.eye(3) * max(sigma, 1e-4) ** 2
+    return np.asarray(raw, dtype=float).reshape(3, 3)
+
+
+def _corrected_delta_r(
+    pair: MotionPair,
+    delta_bias: np.ndarray,
+    *,
+    imu: ImuSeries | None,
+    bias0: np.ndarray,
+) -> np.ndarray:
+    j_bg = _pair_j_bg(pair)
+    if j_bg is not None:
+        return apply_bias_jacobian_correction(pair.R_A, j_bg, delta_bias)
+    if imu is not None and "t_i_imu_s" in pair.metadata and "t_j_imu_s" in pair.metadata:
+        preint = preintegrate_gyro(
+            imu.t_s,
+            imu.gyro_rad_s,
+            float(pair.metadata["t_i_imu_s"]),
+            float(pair.metadata["t_j_imu_s"]),
+            bias0 + delta_bias,
+        )
+        return preint.delta_R
+    duration = float(pair.metadata.get("duration_s", max(pair.t_j_s - pair.t_i_s, 1e-3)))
+    return apply_constant_bias_correction(pair.R_A, duration, delta_bias)
+
+
+def _gravity_basis(g0: np.ndarray) -> np.ndarray:
+    """Return 3×2 orthonormal basis spanning the plane orthogonal to ``g0``."""
+
+    g = np.asarray(g0, dtype=float).reshape(3)
+    n = np.linalg.norm(g)
+    if n < 1e-9:
+        g = np.array([0.0, 0.0, -G_NORM])
+        n = G_NORM
+    g = g / n
+    axis = np.array([1.0, 0.0, 0.0]) if abs(g[0]) < 0.9 else np.array([0.0, 1.0, 0.0])
+    e1 = np.cross(g, axis)
+    e1 /= max(np.linalg.norm(e1), 1e-12)
+    e2 = np.cross(g, e1)
+    return np.column_stack([e1, e2])
+
+
+def _gravity_from_params(xy: np.ndarray, g0: np.ndarray, basis: np.ndarray) -> np.ndarray:
+    raw = np.asarray(g0, dtype=float).reshape(3) + basis @ np.asarray(xy, dtype=float).reshape(2)
+    n = float(np.linalg.norm(raw))
+    if n < 1e-9:
+        return np.asarray(g0, dtype=float).reshape(3)
+    return raw * (G_NORM / n)
+
+
+def _lidar_to_imu_relative(r_x: np.ndarray, t_x: np.ndarray, r_b: np.ndarray, t_b: np.ndarray):
+    """Map LiDAR relative pose to IMU: ``T_A = T_X T_B T_X^{-1}``."""
+
+    r_a = orthonormalize_rotation(r_x @ r_b @ r_x.T)
+    t_a = (np.eye(3) - r_a) @ t_x + r_x @ t_b
+    return r_a, t_a
+
+
+def _corrected_preint_quantities(
+    pair: MotionPair,
+    bg_i: np.ndarray,
+    ba_i: np.ndarray,
+    bg0: np.ndarray,
+    ba0: np.ndarray,
+) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
+    """First-order correct ΔR/Δv/Δp for keyframe biases vs preintegration biases."""
+
+    dbg = np.asarray(bg_i, dtype=float).reshape(3) - np.asarray(bg0, dtype=float).reshape(3)
+    dba = np.asarray(ba_i, dtype=float).reshape(3) - np.asarray(ba0, dtype=float).reshape(3)
+    j_bg = pair.metadata.get("J_bg9")
+    j_ba = pair.metadata.get("J_ba")
+    delta_v0 = np.asarray(pair.metadata.get("delta_v", [0.0, 0.0, 0.0]), dtype=float).reshape(3)
+    delta_p0 = (
+        np.asarray(pair.t_A_m, dtype=float).reshape(3)
+        if pair.t_A_m is not None
+        else np.asarray(pair.metadata.get("delta_p", [0.0, 0.0, 0.0]), dtype=float).reshape(3)
+    )
+    if j_bg is None or j_ba is None:
+        delta_r = apply_bias_jacobian_correction(
+            pair.R_A,
+            _pair_j_bg(pair) if _pair_j_bg(pair) is not None else np.zeros((3, 3)),
+            dbg,
+        )
+        return delta_r, delta_v0, delta_p0
+    j_bg_m = np.asarray(j_bg, dtype=float).reshape(9, 3)
+    j_ba_m = np.asarray(j_ba, dtype=float).reshape(9, 3)
+    delta_r = orthonormalize_rotation(pair.R_A @ so3_exp(j_bg_m[0:3] @ dbg))
+    delta_v = delta_v0 + j_bg_m[3:6] @ dbg + j_ba_m[3:6] @ dba
+    delta_p = delta_p0 + j_bg_m[6:9] @ dbg + j_ba_m[6:9] @ dba
+    return delta_r, delta_v, delta_p
+
+
+def _build_nav_rotations(
+    keyframe_ids: list[int],
+    id_to_idx: dict[int, int],
+    consecutive_pairs: dict[tuple[int, int], MotionPair],
+    r_x: np.ndarray,
+    t_x: np.ndarray,
+) -> list[np.ndarray]:
+    """Chain IMU orientations in the first-keyframe nav frame using LiDAR+extrinsic."""
+
+    rotations = [np.eye(3) for _ in keyframe_ids]
+    for k in range(len(keyframe_ids) - 1):
+        a = keyframe_ids[k]
+        b = keyframe_ids[k + 1]
+        pair = consecutive_pairs.get((a, b))
+        if pair is None:
+            rotations[k + 1] = rotations[k]
+            continue
+        t_b = np.zeros(3) if pair.t_B_m is None else np.asarray(pair.t_B_m, dtype=float)
+        r_meas, _ = _lidar_to_imu_relative(r_x, t_x, pair.R_B, t_b)
+        rotations[k + 1] = orthonormalize_rotation(rotations[k] @ r_meas)
+    # Ensure list indexed by id_to_idx
+    del id_to_idx
+    return rotations
+
+
+def _solve_phase_c_se3(
+    pairs: list[MotionPair],
+    r_x: np.ndarray,
+    *,
+    gyro_bias0: np.ndarray,
+    gravity_init: np.ndarray,
+    sigma_bg_rw: float = 1.0e-5,
+    sigma_ba_rw: float = 1.0e-3,
+) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray, float, float, list[str]]:
+    """Keyframe IMU factor optimization for full SE(3)."""
+
+    notes: list[str] = []
+    usable = [pair for pair in pairs if pair.t_B_m is not None and "delta_v" in pair.metadata]
+    if len(usable) < 3:
+        notes.append("phase-C skipped: need pairs with full preintegration metadata")
+        return r_x, np.zeros(3), gravity_init, gyro_bias0, np.zeros(3), 1e9, 1e9, notes
+
+    # Unique keyframes sorted by IMU time.
+    stamp: dict[int, float] = {}
+    for pair in usable:
+        stamp[pair.i] = float(pair.metadata.get("t_i_imu_s", pair.t_i_s))
+        stamp[pair.j] = float(pair.metadata.get("t_j_imu_s", pair.t_j_s))
+    keyframe_ids = sorted(stamp.keys(), key=lambda kid: stamp[kid])
+    k_count = len(keyframe_ids)
+    id_to_idx = {kid: idx for idx, kid in enumerate(keyframe_ids)}
+
+    consecutive_pairs: dict[tuple[int, int], MotionPair] = {}
+    for pair in usable:
+        if id_to_idx[pair.j] == id_to_idx[pair.i] + 1:
+            consecutive_pairs[(pair.i, pair.j)] = pair
+
+    g0 = np.asarray(gravity_init, dtype=float).reshape(3)
+    if np.linalg.norm(g0) < 1e-6:
+        g0 = np.array([0.0, 0.0, -G_NORM])
+    g0 = g0 * (G_NORM / max(np.linalg.norm(g0), 1e-9))
+    basis = _gravity_basis(g0)
+    ba0 = np.zeros(3)
+    bg0 = np.asarray(gyro_bias0, dtype=float).reshape(3)
+
+    # State: dθ(3), t(3), g_xy(2), v(3K), bg(3K), ba(3K)
+    n_v = 3 * k_count
+    n_b = 3 * k_count
+    dim = 3 + 3 + 2 + n_v + n_b + n_b
+    x0 = np.zeros(dim)
+    # velocities start at 0; biases at prior
+    for idx in range(k_count):
+        x0[8 + n_v + 3 * idx : 8 + n_v + 3 * idx + 3] = bg0
+
+    whitened = []
+    for pair in usable:
+        cov9 = pair.metadata.get("cov9")
+        if cov9 is None:
+            cov = _pair_cov(pair)
+            cov9_m = np.eye(9)
+            cov9_m[0:3, 0:3] = cov
+            cov9_m[3:6, 3:6] = np.eye(3) * 0.25
+            cov9_m[6:9, 6:9] = np.eye(3) * 1.0
+        else:
+            cov9_m = np.asarray(cov9, dtype=float).reshape(9, 9)
+        whitened.append(residual_whiten_matrix(cov9_m))
+
+    def unpack(vec: np.ndarray):
+        r_opt = orthonormalize_rotation(so3_exp(vec[0:3]) @ r_x)
+        t_opt = vec[3:6]
+        g_opt = _gravity_from_params(vec[6:8], g0, basis)
+        base = 8
+        vels = vec[base : base + n_v].reshape(k_count, 3)
+        base += n_v
+        bgs = vec[base : base + n_b].reshape(k_count, 3)
+        base += n_b
+        bas = vec[base : base + n_b].reshape(k_count, 3)
+        return r_opt, t_opt, g_opt, vels, bgs, bas
+
+    def residuals(vec: np.ndarray) -> np.ndarray:
+        r_opt, t_opt, g_opt, vels, bgs, bas = unpack(vec)
+        nav_r = _build_nav_rotations(keyframe_ids, id_to_idx, consecutive_pairs, r_opt, t_opt)
+        out: list[np.ndarray] = []
+
+        for pair, whiten in zip(usable, whitened):
+            i_idx = id_to_idx[pair.i]
+            j_idx = id_to_idx[pair.j]
+            dt = float(pair.metadata.get("duration_s", pair.t_j_s - pair.t_i_s))
+            dt = max(dt, 1e-3)
+            delta_r, delta_v, delta_p = _corrected_preint_quantities(
+                pair, bgs[i_idx], bas[i_idx], bg0, ba0
+            )
+            t_b = np.asarray(pair.t_B_m, dtype=float).reshape(3)
+            r_meas, t_meas = _lidar_to_imu_relative(r_opt, t_opt, pair.R_B, t_b)
+            r_i = nav_r[i_idx]
+            v_i = vels[i_idx]
+            v_j = vels[j_idx]
+
+            err_r = so3_log(delta_r.T @ r_meas)
+            err_v = v_j - v_i - g_opt * dt - r_i @ delta_v
+            err_p = r_i @ (t_meas - delta_p) - v_i * dt - 0.5 * g_opt * (dt**2)
+            err = np.concatenate([err_r, err_v, err_p])
+            w = np.sqrt(_pair_weight(pair))
+            out.append(w * (whiten @ err))
+
+        # Bias random-walk between consecutive keyframes.
+        for k in range(k_count - 1):
+            dt = max(stamp[keyframe_ids[k + 1]] - stamp[keyframe_ids[k]], 1e-3)
+            scale_g = 1.0 / (max(sigma_bg_rw, 1e-8) * np.sqrt(dt))
+            scale_a = 1.0 / (max(sigma_ba_rw, 1e-8) * np.sqrt(dt))
+            out.append(scale_g * (bgs[k + 1] - bgs[k]))
+            out.append(scale_a * (bas[k + 1] - bas[k]))
+
+        # Weak priors: first-keyframe biases and translation magnitude.
+        out.append(50.0 * (bgs[0] - bg0))
+        out.append(20.0 * bas[0])
+        out.append(0.2 * t_opt)  # soft |t| prior ~ meters
+        return np.concatenate(out)
+
+    # Cap evaluations: Phase-C is high-dimensional; synthetic ICP already dominates runtime.
+    opt = least_squares(residuals, x0, loss="huber", f_scale=0.05, max_nfev=80)
+    r_opt, t_opt, g_opt, vels, bgs, bas = unpack(opt.x)
+
+    rot_errs = []
+    trans_errs = []
+    nav_r = _build_nav_rotations(keyframe_ids, id_to_idx, consecutive_pairs, r_opt, t_opt)
+    for pair in usable:
+        i_idx = id_to_idx[pair.i]
+        j_idx = id_to_idx[pair.j]
+        dt = max(float(pair.metadata.get("duration_s", pair.t_j_s - pair.t_i_s)), 1e-3)
+        delta_r, delta_v, delta_p = _corrected_preint_quantities(
+            pair, bgs[i_idx], bas[i_idx], bg0, ba0
+        )
+        t_b = np.asarray(pair.t_B_m, dtype=float).reshape(3)
+        r_meas, t_meas = _lidar_to_imu_relative(r_opt, t_opt, pair.R_B, t_b)
+        r_i = nav_r[i_idx]
+        err_r = so3_log(delta_r.T @ r_meas)
+        err_p = r_i @ (t_meas - delta_p) - vels[i_idx] * dt - 0.5 * g_opt * (dt**2)
+        rot_errs.append(np.degrees(np.linalg.norm(err_r)))
+        trans_errs.append(float(np.linalg.norm(err_p)))
+        del delta_v, j_idx
+
+    rot_rms = float(np.sqrt(np.mean(np.square(rot_errs)))) if rot_errs else 1e9
+    trans_rms = float(np.sqrt(np.mean(np.square(trans_errs)))) if trans_errs else 1e9
+    bg_mean = np.mean(bgs, axis=0)
+    ba_mean = np.mean(bas, axis=0)
+    notes.append(
+        "phase-C SE3 (Δv/Δp + g + keyframe v/bias RW): "
+        f"keyframes={k_count}, pairs={len(usable)}, "
+        f"|t|={float(np.linalg.norm(t_opt)):.3f} m, "
+        f"|g|={float(np.linalg.norm(g_opt)):.3f}, "
+        f"trans_rms={trans_rms:.3f} m"
+    )
+    return r_opt, t_opt, g_opt, bg_mean, ba_mean, rot_rms, trans_rms, notes
+
+
+def solve_joint_extrinsic(
+    pairs: list[MotionPair] | tuple[MotionPair, ...],
+    r_x: np.ndarray,
+    *,
+    force_rotation_only: bool = False,
+    imu: ImuSeries | None = None,
+    delta_t_s: float = 0.0,
+    gyro_bias_rad_s: np.ndarray | None = None,
+    gravity_init_m_s2: np.ndarray | None = None,
+    bias_prior_sigma_rad_s: float = 0.02,
+    enable_phase_c: bool | None = None,
+) -> JointExtrinsicResult:
+    """Refine extrinsic using Phase-A whitened rotation factors, optional Phase-C SE(3)."""
+
+    del delta_t_s  # reserved for future SE(3) time coupling
+    if enable_phase_c is None:
+        enable_phase_c = not force_rotation_only
+
+    usable = [pair for pair in pairs if pair.t_B_m is not None]
+    observability = analyze_observability(usable, r_x)
+    notes = list(observability.notes)
+
+    r = orthonormalize_rotation(np.asarray(r_x, dtype=float))
+    bias0 = np.zeros(3) if gyro_bias_rad_s is None else np.asarray(gyro_bias_rad_s, dtype=float).reshape(3)
+    weights = np.asarray([_pair_weight(pair) for pair in usable], dtype=float)
+    whitens = [residual_whiten_matrix(_pair_cov(pair)) for pair in usable]
+    prior_w = 1.0 / max(bias_prior_sigma_rad_s, 1e-4)
+
+    def rotation_residuals(r_opt: np.ndarray, delta_bias: np.ndarray) -> np.ndarray:
+        residuals = []
+        for pair, weight, whiten in zip(usable, weights, whitens):
+            delta_r = _corrected_delta_r(pair, delta_bias, imu=imu, bias0=bias0)
+            err = preintegration_rotation_residual(delta_r, r_opt, pair.R_B)
+            residuals.append(np.sqrt(weight) * (whiten @ err))
+        residuals.append(prior_w * delta_bias)
+        return np.concatenate(residuals) if residuals else np.zeros(0)
+
+    def residual_rot_bias(vec: np.ndarray) -> np.ndarray:
+        r_opt = orthonormalize_rotation(so3_exp(vec[:3]) @ r)
+        return rotation_residuals(r_opt, vec[3:])
+
+    if usable:
+        opt = least_squares(
+            residual_rot_bias,
+            np.zeros(6),
+            loss="huber",
+            f_scale=np.deg2rad(1.0),
+            max_nfev=200,
+        )
+        r = orthonormalize_rotation(so3_exp(opt.x[:3]) @ r)
+        delta_bias = opt.x[3:]
+        bias_out = bias0 + delta_bias
+        notes.append(
+            "phase-A joint refine (Σ-whitened + J_bg): "
+            f"|δb|={float(np.linalg.norm(delta_bias)):.3e} rad/s, "
+            f"weighted pairs={len(usable)}"
+        )
+    else:
+        bias_out = bias0
+        delta_bias = np.zeros(3)
+        notes.append("no pairs for joint refine")
+
+    rot_errs = []
+    for pair in usable:
+        delta_r = _corrected_delta_r(pair, delta_bias, imu=imu, bias0=bias0)
+        err = preintegration_rotation_residual(delta_r, r, pair.R_B)
+        rot_errs.append(np.degrees(np.linalg.norm(err)))
+    rot_rms = float(np.sqrt(np.mean(np.square(rot_errs)))) if rot_errs else 1e9
+
+    t = np.zeros(3)
+    translation_accepted = False
+    trans_rms = 1e9
+    gravity_out: np.ndarray | None = None
+    accel_bias_out: np.ndarray | None = None
+
+    if gravity_init_m_s2 is None:
+        gravity_init = np.array([0.0, 0.0, -G_NORM])
+    else:
+        gravity_init = np.asarray(gravity_init_m_s2, dtype=float).reshape(3)
+
+    if (
+        enable_phase_c
+        and not force_rotation_only
+        and observability.translation_observable
+        and observability.rotation_observable
+        and len(usable) >= 5
+    ):
+        r, t, gravity_out, bias_out, accel_bias_out, rot_rms, trans_rms, c_notes = _solve_phase_c_se3(
+            usable,
+            r,
+            gyro_bias0=bias_out,
+            gravity_init=gravity_init,
+        )
+        notes.extend(c_notes)
+        translation_accepted = bool(trans_rms < 0.75 and np.linalg.norm(t) > 1e-4)
+        if not translation_accepted:
+            notes.append("phase-C translation residual/gate failed; keeping translation at zero")
+            t = np.zeros(3)
+    elif (
+        not force_rotation_only
+        and observability.translation_observable
+        and observability.rotation_observable
+        and len(usable) >= 5
+    ):
+        # Legacy hand-eye translation fallback when Phase-C metadata missing.
+        def residual_se3(vec: np.ndarray) -> np.ndarray:
+            r_opt = orthonormalize_rotation(so3_exp(vec[:3]) @ r)
+            t_opt = vec[3:]
+            residuals = []
+            for pair, weight, whiten in zip(usable, weights, whitens):
+                delta_r = _corrected_delta_r(pair, delta_bias, imu=imu, bias0=bias0)
+                residuals.append(
+                    np.sqrt(weight) * (whiten @ preintegration_rotation_residual(delta_r, r_opt, pair.R_B))
+                )
+                pred = (pair.R_A - np.eye(3)) @ t_opt
+                meas = r_opt @ np.asarray(pair.t_B_m, dtype=float)
+                residuals.append(np.sqrt(weight) * (pred - meas))
+            return np.concatenate(residuals)
+
+        opt_t = least_squares(residual_se3, np.zeros(6), loss="huber", f_scale=0.05, max_nfev=200)
+        r = orthonormalize_rotation(so3_exp(opt_t.x[:3]) @ r)
+        t = opt_t.x[3:]
+        rot_errs = []
+        trans_errs = []
+        for pair in usable:
+            delta_r = _corrected_delta_r(pair, delta_bias, imu=imu, bias0=bias0)
+            rot_errs.append(np.degrees(np.linalg.norm(preintegration_rotation_residual(delta_r, r, pair.R_B))))
+            pred = (pair.R_A - np.eye(3)) @ t
+            meas = r @ np.asarray(pair.t_B_m, dtype=float)
+            trans_errs.append(np.linalg.norm(pred - meas))
+        rot_rms = float(np.sqrt(np.mean(np.square(rot_errs))))
+        trans_rms = float(np.sqrt(np.mean(np.square(trans_errs))))
+        translation_accepted = trans_rms < 0.5
+        notes.append(f"legacy translation refine rms={trans_rms:.3f} m")
+        if not translation_accepted:
+            notes.append("translation residual too large; keeping translation at zero")
+            t = np.zeros(3)
+    else:
+        notes.append("rotation-only extrinsic returned (phase-A; phase-C SE3 gated off)")
+
+    return JointExtrinsicResult(
+        T_IMU_lidar=make_transform(t, r),
+        translation_accepted=bool(translation_accepted and np.linalg.norm(t) > 0),
+        residual_rms_rot_deg=rot_rms,
+        residual_rms_trans_m=0.0 if not translation_accepted else trans_rms,
+        observability=observability,
+        gyro_bias_rad_s=np.asarray(bias_out, dtype=float),
+        accel_bias_m_s2=None if accel_bias_out is None else np.asarray(accel_bias_out, dtype=float),
+        gravity_m_s2=None if gravity_out is None else np.asarray(gravity_out, dtype=float),
+        notes=tuple(notes),
+    )
@@ -0,0 +1,49 @@
+"""LiDAR keyframe selection."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+
+import numpy as np
+
+from .contracts import LidarFrame
+from .registration import register_lidar_pair
+
+
+@dataclass(frozen=True)
+class KeyframeSet:
+    indices: tuple[int, ...]
+    frames: tuple[LidarFrame, ...]
+
+
+def build_keyframes(
+    frames: list[LidarFrame],
+    *,
+    min_translation_m: float = 0.3,
+    min_rotation_deg: float = 3.0,
+    max_frame_gap: int = 8,
+) -> KeyframeSet:
+    """Select keyframes with enough relative motion for hand-eye pairs."""
+
+    if not frames:
+        return KeyframeSet((), ())
+
+    selected = [0]
+    last = 0
+    for index in range(1, len(frames)):
+        if index - last > max_frame_gap:
+            selected.append(index)
+            last = index
+            continue
+        result = register_lidar_pair(frames[index].points_xyz, frames[last].points_xyz)
+        if not result.ok:
+            continue
+        if result.translation_m >= min_translation_m or result.rotation_deg >= min_rotation_deg:
+            selected.append(index)
+            last = index
+
+    if selected[-1] != len(frames) - 1 and len(frames) > 1:
+        selected.append(len(frames) - 1)
+
+    unique = tuple(dict.fromkeys(selected))
+    return KeyframeSet(indices=unique, frames=tuple(frames[i] for i in unique))
@@ -0,0 +1,59 @@
+"""Coarse LiDAR deskew using a constant body rate over the sweep."""
+
+from __future__ import annotations
+
+import numpy as np
+
+from .contracts import ImuSeries, LidarFrame
+from .geometry import so3_exp
+from .time_offset import lidar_time_to_imu_time
+
+
+def deskew_lidar_frames(
+    frames: list[LidarFrame],
+    imu: ImuSeries,
+    *,
+    delta_t_s: float,
+    R_IMU_lidar: np.ndarray | None = None,
+    gyro_bias_rad_s: np.ndarray | None = None,
+) -> list[LidarFrame]:
+    """Return deskewed copies when extrinsic is known; otherwise return originals."""
+
+    if R_IMU_lidar is None:
+        return frames
+
+    bias = np.zeros(3) if gyro_bias_rad_s is None else np.asarray(gyro_bias_rad_s, dtype=float)
+    r_li = np.asarray(R_IMU_lidar, dtype=float).reshape(3, 3).T
+    output: list[LidarFrame] = []
+
+    for frame in frames:
+        n = frame.points_xyz.shape[0]
+        if n < 10:
+            output.append(frame)
+            continue
+        t_mid_imu = lidar_time_to_imu_time(frame.t_mid_s, delta_t_s)
+        index = int(np.clip(np.searchsorted(imu.t_s, t_mid_imu), 1, imu.t_s.size - 1))
+        omega_lidar = r_li @ (imu.gyro_rad_s[index] - bias)
+        duration = max(frame.t_end_s - frame.t_start_s, 1e-3)
+        rel = np.linspace(-0.5, 0.5, n) * duration
+        deskewed = np.empty_like(frame.points_xyz)
+        # Piecewise-constant rotation over a few time bins.
+        bins = 12
+        edges = np.linspace(-0.5 * duration, 0.5 * duration, bins + 1)
+        for b in range(bins):
+            mask = (rel >= edges[b]) & (rel <= edges[b + 1] if b == bins - 1 else rel < edges[b + 1])
+            if not np.any(mask):
+                continue
+            tau = 0.5 * (edges[b] + edges[b + 1])
+            rot = so3_exp(omega_lidar * float(tau))
+            deskewed[mask] = frame.points_xyz[mask] @ rot.T
+        output.append(
+            LidarFrame(
+                frame_id=frame.frame_id,
+                t_start_s=frame.t_start_s,
+                t_end_s=frame.t_end_s,
+                points_xyz=deskewed,
+                path=frame.path,
+            )
+        )
+    return output
@@ -0,0 +1,82 @@
+"""LiDAR adapters for the V1 standard intermediate format.
+
+Accepted input: a directory containing ``frames_index.csv`` and per-frame NPZ files.
+
+frames_index.csv
+----------------
+frame_id,file,t_start,t_end
+
+Each NPZ referenced by ``file`` must contain:
+- points: float array shaped (N, 3) in LiDAR Cartesian coordinates (metres)
+"""
+
+from __future__ import annotations
+
+from pathlib import Path
+
+import numpy as np
+
+from .contracts import LidarFrame
+
+
+def load_lidar_frames(path: Path | str) -> list[LidarFrame]:
+    """Load all LiDAR frames listed by ``frames_index.csv`` under ``path``."""
+
+    root = Path(path)
+    index_path = root / "frames_index.csv"
+    if not index_path.exists():
+        raise FileNotFoundError(f"missing frames_index.csv under {root}")
+
+    rows = np.genfromtxt(index_path, delimiter=",", names=True, dtype=None, encoding="utf-8")
+    if rows.ndim == 0:
+        rows = np.array([rows])
+    names = set(rows.dtype.names or ())
+    # NumPy may rename reserved name ``file`` to ``file_``.
+    file_key = "filename" if "filename" in names else ("file_" if "file_" in names else "file")
+    required = {"frame_id", "t_start", "t_end"}
+    if not required.issubset(names) or file_key not in names:
+        raise ValueError(
+            f"frames_index.csv must contain frame_id,{file_key}/filename,t_start,t_end; got {sorted(names)}"
+        )
+
+    frames: list[LidarFrame] = []
+    for row in rows:
+        frame_id = str(row["frame_id"])
+        rel = str(row[file_key])
+        npz_path = root / rel
+        with np.load(npz_path) as payload:
+            if "points" not in payload.files:
+                raise ValueError(f"{npz_path} must contain array 'points'")
+            points = np.asarray(payload["points"], dtype=float)
+        if points.ndim != 2 or points.shape[1] < 3:
+            raise ValueError(f"{npz_path}: points must have shape (N, 3[+])")
+        frames.append(
+            LidarFrame(
+                frame_id=frame_id,
+                t_start_s=float(row["t_start"]),
+                t_end_s=float(row["t_end"]),
+                points_xyz=points[:, :3],
+                path=npz_path,
+            )
+        )
+    frames.sort(key=lambda frame: frame.t_mid_s)
+    return frames
+
+
+def save_lidar_session(
+    root: Path | str,
+    frames: list[LidarFrame],
+    *,
+    points_dirname: str = "frames",
+) -> None:
+    """Write a LiDAR session directory in the standard intermediate format."""
+
+    destination = Path(root)
+    frames_dir = destination / points_dirname
+    frames_dir.mkdir(parents=True, exist_ok=True)
+    index_rows: list[str] = ["frame_id,filename,t_start,t_end"]
+    for index, frame in enumerate(frames):
+        relative = f"{points_dirname}/frame_{index:05d}.npz"
+        np.savez_compressed(destination / relative, points=np.asarray(frame.points_xyz, dtype=float))
+        index_rows.append(f"{frame.frame_id},{relative},{frame.t_start_s:.9f},{frame.t_end_s:.9f}")
+    (destination / "frames_index.csv").write_text("\n".join(index_rows) + "\n", encoding="utf-8")
@@ -0,0 +1,136 @@
+"""Build IMU/LiDAR relative-motion pairs for hand-eye calibration."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+
+import numpy as np
+
+from .contracts import ImuSeries, LidarFrame, MotionPair
+from .geometry import make_transform, rotation_angle_deg
+from .imu_preintegration import preintegrate_imu
+from .registration import register_lidar_pair
+from .time_offset import lidar_time_to_imu_time
+
+
+@dataclass(frozen=True)
+class MotionPairSet:
+    pairs: tuple[MotionPair, ...]
+    notes: tuple[str, ...] = ()
+
+
+def build_motion_pairs(
+    *,
+    session_id: str,
+    keyframes: list[LidarFrame],
+    keyframe_indices: list[int] | tuple[int, ...],
+    imu: ImuSeries,
+    delta_t_s: float,
+    gyro_bias_rad_s: np.ndarray | None = None,
+    acc_bias_m_s2: np.ndarray | None = None,
+    min_rotation_deg: float = 3.0,
+    min_translation_m: float = 0.3,
+    max_index_span: int = 4,
+) -> MotionPairSet:
+    """Create A/B motion pairs between nearby keyframes.
+
+    IMU side uses full Phase-C preintegration (``ΔR/Δv/Δp``, ``Σ9``, ``J_bg/J_ba``).
+    Rotation hand-eye still consumes ``R_A = ΔR`` only.
+    """
+
+    notes: list[str] = []
+    pairs: list[MotionPair] = []
+    bias_g = np.zeros(3) if gyro_bias_rad_s is None else np.asarray(gyro_bias_rad_s, dtype=float)
+    bias_a = np.zeros(3) if acc_bias_m_s2 is None else np.asarray(acc_bias_m_s2, dtype=float)
+    n = len(keyframes)
+    if n < 2:
+        return MotionPairSet((), ("need at least two keyframes",))
+
+    for span in range(1, max_index_span + 1):
+        for start in range(0, n - span):
+            i = start
+            j = start + span
+            frame_i = keyframes[i]
+            frame_j = keyframes[j]
+            reg = register_lidar_pair(frame_j.points_xyz, frame_i.points_xyz)
+            if not reg.ok:
+                continue
+            if reg.rotation_deg < min_rotation_deg and reg.translation_m < min_translation_m:
+                continue
+
+            t_i_imu = lidar_time_to_imu_time(frame_i.t_mid_s, delta_t_s)
+            t_j_imu = lidar_time_to_imu_time(frame_j.t_mid_s, delta_t_s)
+            if t_j_imu <= t_i_imu:
+                continue
+            if t_i_imu < imu.t_s[0] or t_j_imu > imu.t_s[-1]:
+                continue
+
+            preint = preintegrate_imu(
+                imu.t_s,
+                imu.gyro_rad_s,
+                imu.acc_m_s2,
+                t_i_imu,
+                t_j_imu,
+                bias_g,
+                bias_a,
+            )
+            r_a = preint.delta_R
+            r_b = reg.transform[:3, :3]
+            t_b = reg.transform[:3, 3]
+            rot_a = rotation_angle_deg(r_a)
+            if abs(rot_a - reg.rotation_deg) > max(15.0, 1.0 * max(rot_a, reg.rotation_deg)):
+                continue
+
+            pairs.append(
+                MotionPair(
+                    session_id=session_id,
+                    i=int(keyframe_indices[i]),
+                    j=int(keyframe_indices[j]),
+                    t_i_s=frame_i.t_mid_s,
+                    t_j_s=frame_j.t_mid_s,
+                    R_A=r_a,
+                    R_B=r_b,
+                    t_A_m=np.asarray(preint.delta_p, dtype=float),
+                    t_B_m=np.asarray(t_b, dtype=float),
+                    fitness=reg.fitness,
+                    metadata={
+                        "backend": reg.backend,
+                        "rotation_deg_B": reg.rotation_deg,
+                        "translation_m_B": reg.translation_m,
+                        "rotation_deg_A": rot_a,
+                        "weight": preint.weight,
+                        "duration_s": preint.duration_s,
+                        "mean_gyro_norm": preint.mean_gyro_norm,
+                        "preint_sigma_rad": preint.sigma_rad,
+                        "cov": preint.cov[0:3, 0:3].tolist(),
+                        "cov9": preint.cov.tolist(),
+                        "J_bg": preint.J_bg[0:3, 0:3].tolist(),
+                        "J_bg9": preint.J_bg.tolist(),
+                        "J_ba": preint.J_ba.tolist(),
+                        "delta_v": preint.delta_v.tolist(),
+                        "delta_p": preint.delta_p.tolist(),
+                        "t_i_imu_s": t_i_imu,
+                        "t_j_imu_s": t_j_imu,
+                        "modeling": "imu_preintegration_factor_phase_c",
+                    },
+                )
+            )
+
+    notes.append(
+        f"built {len(pairs)} motion pairs (Phase-C preintegration: ΔR/Δv/Δp, Σ9, J_bg/J_ba)"
+    )
+    return MotionPairSet(pairs=tuple(pairs), notes=tuple(notes))
+
+
+def pairs_to_transforms(pairs: tuple[MotionPair, ...]) -> tuple[list[np.ndarray], list[np.ndarray]]:
+    """Helper returning SE(3) lists when translations are present."""
+
+    a_list: list[np.ndarray] = []
+    b_list: list[np.ndarray] = []
+    for pair in pairs:
+        if pair.t_B_m is None:
+            continue
+        t_a = np.zeros(3) if pair.t_A_m is None else pair.t_A_m
+        a_list.append(make_transform(t_a, pair.R_A))
+        b_list.append(make_transform(pair.t_B_m, pair.R_B))
+    return a_list, b_list
@@ -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),
+    )
@@ -0,0 +1,347 @@
+"""Executable LiDAR–IMU calibration pipeline (V1)."""
+
+from __future__ import annotations
+
+from dataclasses import asdict, dataclass
+from pathlib import Path
+from typing import Any
+
+import numpy as np
+
+from .contracts import (
+    CalibrationMode,
+    CalibrationRequest,
+    CalibrationResult,
+    CalibrationStatus,
+    SessionInput,
+)
+from .finalize import finalize_result
+from .imu_audit import audit_imu
+from .imu_io import load_imu_samples
+from .joint_optimizer import solve_joint_extrinsic
+from .keyframes import build_keyframes
+from .lidar_deskew import deskew_lidar_frames
+from .lidar_io import load_lidar_frames
+from .motion_pairs import build_motion_pairs
+from .rotation_handeye import solve_rotation_handeye
+from .time_offset import TimeOffsetResult, estimate_time_offset, refine_time_offset_signed
+from .timestamp_audit import audit_timestamps
+from .vehicle_config import load_vehicle_config
+
+
+def _merge_time_offset(previous: TimeOffsetResult, refined: TimeOffsetResult) -> TimeOffsetResult:
+    return TimeOffsetResult(
+        delta_t_s=refined.delta_t_s,
+        correlation_peak=refined.correlation_peak,
+        search_s=previous.search_s,
+        notes=tuple(list(previous.notes) + list(refined.notes)),
+        ok=True,
+    )
+
+
+@dataclass(frozen=True)
+class PipelineStage:
+    name: str
+    responsibility: str
+
+
+STAGES = (
+    PipelineStage("vehicle_config", "加载并校验当前车辆安装配置"),
+    PipelineStage("timestamp_audit", "审查 IMU 与 LiDAR 时间域"),
+    PipelineStage("imu_audit", "审查单位、轴向启发与静止零偏"),
+    PipelineStage("time_offset", "粗估 δt，并用 R 做有符号三轴精修"),
+    PipelineStage("lidar_motion", "关键帧、可选去畸变与 LiDAR 相对运动"),
+    PipelineStage("motion_pairs", "IMU 预积分与雷达配准，构造相对运动对"),
+    PipelineStage("rotation_handeye", "加权求解旋转外参"),
+    PipelineStage("joint_optimizer", "联合精修；完整模式下可估计平移"),
+    PipelineStage("finalize", "写出结果与质量报告"),
+)
+
+
+def describe_pipeline(_: CalibrationRequest) -> tuple[PipelineStage, ...]:
+    """Return the planned stages."""
+
+    return STAGES
+
+
+def _build_pairs_and_handeye(
+    *,
+    session_id: str,
+    working_frames,
+    imu,
+    delta_t_s: float,
+    gyro_bias_rad_s: np.ndarray,
+    request: CalibrationRequest,
+):
+    keyframes = build_keyframes(
+        working_frames,
+        min_translation_m=request.min_pair_translation_m,
+        min_rotation_deg=request.min_pair_rotation_deg,
+    )
+    pair_set = build_motion_pairs(
+        session_id=session_id,
+        keyframes=list(keyframes.frames),
+        keyframe_indices=keyframes.indices,
+        imu=imu,
+        delta_t_s=delta_t_s,
+        gyro_bias_rad_s=gyro_bias_rad_s,
+        min_rotation_deg=request.min_pair_rotation_deg,
+        min_translation_m=request.min_pair_translation_m,
+    )
+    handeye = solve_rotation_handeye(pair_set.pairs)
+    return keyframes, pair_set, handeye
+
+
+def _session_details(
+    session: SessionInput,
+    request: CalibrationRequest,
+    vehicle_config: dict[str, Any] | None,
+) -> dict[str, Any]:
+    imu = load_imu_samples(session.imu_source)
+    frames = load_lidar_frames(session.lidar_source)
+
+    ts = audit_timestamps(imu, frames)
+    if not ts.ok:
+        return {"ok": False, "stage": "timestamp_audit", "report": asdict(ts)}
+
+    imu_report = audit_imu(imu)
+    if not imu_report.ok:
+        return {"ok": False, "stage": "imu_audit", "report": asdict(imu_report)}
+
+    offset = estimate_time_offset(
+        imu,
+        frames,
+        gyro_bias_rad_s=imu_report.gyro_bias_rad_s,
+        search_s=request.time_offset_search_s,
+    )
+    if not offset.ok:
+        return {"ok": False, "stage": "time_offset", "report": asdict(offset)}
+
+    working_frames = frames
+    r_x = np.eye(3)
+    handeye = None
+    pair_set = None
+    keyframes = None
+    pairs_notes: list[str] = []
+    pair_count = 0
+    time_offset_notes = list(offset.notes)
+
+    for iteration in range(max(1, request.max_iterations)):
+        if iteration > 0:
+            working_frames = deskew_lidar_frames(
+                frames,
+                imu,
+                delta_t_s=offset.delta_t_s,
+                R_IMU_lidar=r_x,
+                gyro_bias_rad_s=imu_report.gyro_bias_rad_s,
+            )
+        keyframes, pair_set, handeye = _build_pairs_and_handeye(
+            session_id=session.session_id,
+            working_frames=working_frames,
+            imu=imu,
+            delta_t_s=offset.delta_t_s,
+            gyro_bias_rad_s=imu_report.gyro_bias_rad_s,
+            request=request,
+        )
+        pairs_notes = list(pair_set.notes)
+        pair_count = len(pair_set.pairs)
+        if handeye.pair_count < 3:
+            return {
+                "ok": False,
+                "stage": "rotation_handeye",
+                "iteration": iteration,
+                "time_offset": asdict(offset),
+                "imu_audit": asdict(imu_report),
+                "timestamp_audit": asdict(ts),
+                "keyframes": len(keyframes.indices),
+                "pair_notes": pairs_notes,
+                "handeye": asdict(handeye),
+            }
+        # Use candidate R even if RMS gate failed, so signed δt refine can still run.
+        r_x = handeye.R_IMU_lidar
+
+        # Phase-A: alternate signed δt refine with current R (up to 2 rounds).
+        for _ in range(2):
+            refined = refine_time_offset_signed(
+                imu,
+                frames,
+                delta_t_s=offset.delta_t_s,
+                R_IMU_lidar=r_x,
+                gyro_bias_rad_s=imu_report.gyro_bias_rad_s,
+                search_s=min(0.12, max(0.04, 0.25 * request.time_offset_search_s)),
+            )
+            delta_shift = abs(refined.delta_t_s - offset.delta_t_s)
+            offset = _merge_time_offset(offset, refined)
+            time_offset_notes = list(offset.notes)
+            if delta_shift < 1e-3:
+                break
+            keyframes, pair_set, handeye = _build_pairs_and_handeye(
+                session_id=session.session_id,
+                working_frames=working_frames,
+                imu=imu,
+                delta_t_s=offset.delta_t_s,
+                gyro_bias_rad_s=imu_report.gyro_bias_rad_s,
+                request=request,
+            )
+            pairs_notes = list(pair_set.notes)
+            pair_count = len(pair_set.pairs)
+            if handeye.pair_count < 3:
+                return {
+                    "ok": False,
+                    "stage": "rotation_handeye",
+                    "iteration": iteration,
+                    "time_offset": asdict(offset),
+                    "imu_audit": asdict(imu_report),
+                    "timestamp_audit": asdict(ts),
+                    "keyframes": len(keyframes.indices),
+                    "pair_notes": pairs_notes,
+                    "handeye": asdict(handeye),
+                }
+            r_x = handeye.R_IMU_lidar
+
+        if not handeye.ok:
+            return {
+                "ok": False,
+                "stage": "rotation_handeye",
+                "iteration": iteration,
+                "time_offset": asdict(offset),
+                "imu_audit": asdict(imu_report),
+                "timestamp_audit": asdict(ts),
+                "keyframes": len(keyframes.indices),
+                "pair_notes": pairs_notes,
+                "handeye": asdict(handeye),
+            }
+
+    assert handeye is not None and pair_set is not None and keyframes is not None
+    force_rotation_only = request.requested_mode == CalibrationMode.ROTATION_ONLY
+    # Specific force opposing measured specific force ≈ −g in the static IMU frame.
+    acc_mean = np.asarray(imu_report.static_acc_mean_m_s2, dtype=float).reshape(3)
+    acc_n = float(np.linalg.norm(acc_mean))
+    if acc_n > 1e-6:
+        gravity_init = -acc_mean * (9.80665 / acc_n)
+    else:
+        gravity_init = np.array([0.0, 0.0, -9.80665])
+    joint = solve_joint_extrinsic(
+        pair_set.pairs,
+        r_x,
+        force_rotation_only=force_rotation_only,
+        imu=imu,
+        delta_t_s=offset.delta_t_s,
+        gyro_bias_rad_s=imu_report.gyro_bias_rad_s,
+        gravity_init_m_s2=gravity_init,
+        enable_phase_c=not force_rotation_only,
+    )
+
+    offset_payload = asdict(offset)
+
+    return {
+        "ok": True,
+        "session_id": session.session_id,
+        "vehicle_config_loaded": vehicle_config is not None,
+        "timestamp_audit": asdict(ts),
+        "imu_audit": {
+            **asdict(imu_report),
+            "gyro_bias_rad_s": imu_report.gyro_bias_rad_s.tolist(),
+            "static_acc_mean_m_s2": imu_report.static_acc_mean_m_s2.tolist(),
+        },
+        "time_offset": offset_payload,
+        "keyframes": len(keyframes.indices),
+        "pair_count": pair_count,
+        "pair_notes": pairs_notes,
+        "handeye": {
+            "residual_rms_deg": handeye.residual_rms_deg,
+            "residual_median_deg": handeye.residual_median_deg,
+            "pair_count": handeye.pair_count,
+            "ok": handeye.ok,
+            "notes": handeye.notes,
+            "R_IMU_lidar": handeye.R_IMU_lidar.tolist(),
+        },
+        "joint": {
+            "translation_accepted": joint.translation_accepted,
+            "residual_rms_rot_deg": joint.residual_rms_rot_deg,
+            "residual_rms_trans_m": joint.residual_rms_trans_m,
+            "observability": asdict(joint.observability),
+            "notes": joint.notes,
+            "T_IMU_lidar": joint.T_IMU_lidar.tolist(),
+            "gyro_bias_rad_s": None
+            if joint.gyro_bias_rad_s is None
+            else np.asarray(joint.gyro_bias_rad_s, dtype=float).tolist(),
+            "accel_bias_m_s2": None
+            if joint.accel_bias_m_s2 is None
+            else np.asarray(joint.accel_bias_m_s2, dtype=float).tolist(),
+            "gravity_m_s2": None
+            if joint.gravity_m_s2 is None
+            else np.asarray(joint.gravity_m_s2, dtype=float).tolist(),
+        },
+        "T_IMU_lidar": joint.T_IMU_lidar,
+        "time_offset_s": offset.delta_t_s,
+        "translation_accepted": joint.translation_accepted,
+        "rotation_ok": handeye.ok and joint.observability.rotation_observable,
+    }
+
+
+def run_calibration(request: CalibrationRequest) -> CalibrationResult:
+    """Run the V1 calibration pipeline for one or more sessions."""
+
+    if not request.sessions:
+        return finalize_result(
+            status=CalibrationStatus.BLOCKED,
+            message="no sessions provided",
+            details={},
+            output_directory=request.output_directory,
+        )
+
+    vehicle_config = None
+    if request.vehicle_config is not None:
+        try:
+            vehicle_config = load_vehicle_config(request.vehicle_config)
+        except Exception as exc:  # noqa: BLE001 - surface config problems as blocked
+            return finalize_result(
+                status=CalibrationStatus.BLOCKED,
+                message=f"vehicle config failed: {exc}",
+                details={},
+                output_directory=request.output_directory,
+            )
+
+    session_results = []
+    for session in request.sessions:
+        session_results.append(_session_details(session, request, vehicle_config))
+
+    primary = session_results[0]
+    if not primary.get("ok"):
+        return finalize_result(
+            status=CalibrationStatus.BLOCKED,
+            message=f"blocked at stage {primary.get('stage')}",
+            details={"sessions": session_results},
+            output_directory=request.output_directory,
+        )
+
+    T = np.asarray(primary["T_IMU_lidar"], dtype=float)
+    delta_t = float(primary["time_offset_s"])
+    if request.requested_mode == CalibrationMode.FULL_SE3:
+        if primary.get("translation_accepted"):
+            status = CalibrationStatus.FULL_SE3_ACCEPTED
+            message = "full SE3 accepted"
+        else:
+            status = CalibrationStatus.FULL_SE3_REJECTED
+            message = "rotation accepted; translation rejected by observability/residual gates"
+    else:
+        status = CalibrationStatus.ROTATION_ONLY_ACCEPTED
+        message = "rotation-only calibration accepted"
+        T = T.copy()
+        T[:3, 3] = 0.0
+
+    return finalize_result(
+        status=status,
+        message=message,
+        details={"sessions": [_public_session(s) for s in session_results]},
+        T_IMU_lidar=T,
+        time_offset_s=delta_t,
+        output_directory=request.output_directory,
+    )
+
+
+def _public_session(session_result: dict[str, Any]) -> dict[str, Any]:
+    payload = dict(session_result)
+    payload.pop("T_IMU_lidar", None)
+    return payload
@@ -0,0 +1,159 @@
+"""LiDAR relative-motion registration.
+
+Uses Open3D Generalized ICP when available; otherwise a NumPy point-to-point ICP.
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+
+import numpy as np
+
+from .contracts import LidarFrame
+from .geometry import make_transform, orthonormalize_rotation, rotation_angle_deg, so3_log
+
+
+@dataclass(frozen=True)
+class RegistrationResult:
+    transform: np.ndarray
+    fitness: float
+    rotation_deg: float
+    translation_m: float
+    backend: str
+    ok: bool
+
+
+def _voxel_downsample(points: np.ndarray, voxel: float) -> np.ndarray:
+    if points.shape[0] == 0:
+        return points
+    quantized = np.floor(points / voxel).astype(np.int64)
+    _, unique_indices = np.unique(quantized, axis=0, return_index=True)
+    return points[np.sort(unique_indices)]
+
+
+def _numpy_icp(
+    source: np.ndarray,
+    target: np.ndarray,
+    *,
+    max_iterations: int = 30,
+    max_correspondence: float = 1.0,
+) -> RegistrationResult:
+    src = _voxel_downsample(source, 0.2)
+    tgt = _voxel_downsample(target, 0.2)
+    if src.shape[0] < 50 or tgt.shape[0] < 50:
+        return RegistrationResult(np.eye(4), 0.0, 0.0, 0.0, "numpy_icp", False)
+
+    # Subsample for speed.
+    rng = np.random.default_rng(0)
+    if src.shape[0] > 4000:
+        src = src[rng.choice(src.shape[0], 4000, replace=False)]
+    if tgt.shape[0] > 8000:
+        tgt = tgt[rng.choice(tgt.shape[0], 8000, replace=False)]
+
+    r = np.eye(3)
+    t = np.zeros(3)
+    last_error = 1e9
+    inlier_ratio = 0.0
+    for _ in range(max_iterations):
+        transformed = src @ r.T + t
+        # Nearest neighbour in target via brute force on chunks.
+        diff = transformed[:, None, :] - tgt[None, :, :]
+        dist2 = np.sum(diff * diff, axis=2)
+        nn = np.argmin(dist2, axis=1)
+        dist = np.sqrt(dist2[np.arange(src.shape[0]), nn])
+        mask = dist < max_correspondence
+        inlier_ratio = float(np.mean(mask))
+        if np.count_nonzero(mask) < 30:
+            break
+        p = transformed[mask]
+        q = tgt[nn[mask]]
+        mu_p = p.mean(axis=0)
+        mu_q = q.mean(axis=0)
+        h = (p - mu_p).T @ (q - mu_q)
+        u, _, vt = np.linalg.svd(h)
+        r_delta = vt.T @ u.T
+        if np.linalg.det(r_delta) < 0:
+            vt[-1, :] *= -1
+            r_delta = vt.T @ u.T
+        t_delta = mu_q - r_delta @ mu_p
+        # Update global transform: x' = r_delta (r x + t) + t_delta
+        r = orthonormalize_rotation(r_delta @ r)
+        t = r_delta @ t + t_delta
+        mean_err = float(np.mean(dist[mask]))
+        if abs(last_error - mean_err) < 1e-4:
+            break
+        last_error = mean_err
+
+    transform = make_transform(t, r)
+    return RegistrationResult(
+        transform=transform,
+        fitness=inlier_ratio,
+        rotation_deg=rotation_angle_deg(r),
+        translation_m=float(np.linalg.norm(t)),
+        backend="numpy_icp",
+        ok=inlier_ratio > 0.15,
+    )
+
+
+def _open3d_gicp(source: np.ndarray, target: np.ndarray) -> RegistrationResult | None:
+    try:
+        import open3d as o3d
+    except ImportError:
+        return None
+
+    src = o3d.geometry.PointCloud(o3d.utility.Vector3dVector(source))
+    tgt = o3d.geometry.PointCloud(o3d.utility.Vector3dVector(target))
+    src = src.voxel_down_sample(0.2)
+    tgt = tgt.voxel_down_sample(0.2)
+    if len(src.points) < 50 or len(tgt.points) < 50:
+        return RegistrationResult(np.eye(4), 0.0, 0.0, 0.0, "open3d_gicp", False)
+    src.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=1.0, max_nn=30))
+    tgt.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=1.0, max_nn=30))
+    result = o3d.pipelines.registration.registration_generalized_icp(
+        src,
+        tgt,
+        1.0,
+        np.eye(4),
+        o3d.pipelines.registration.TransformationEstimationForGeneralizedICP(),
+        o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=50),
+    )
+    transform = np.asarray(result.transformation, dtype=float)
+    return RegistrationResult(
+        transform=transform,
+        fitness=float(result.fitness),
+        rotation_deg=rotation_angle_deg(transform[:3, :3]),
+        translation_m=float(np.linalg.norm(transform[:3, 3])),
+        backend="open3d_gicp",
+        ok=float(result.fitness) > 0.15,
+    )
+
+
+def register_lidar_pair(source_points: np.ndarray, target_points: np.ndarray) -> RegistrationResult:
+    """Register source -> target and return ``T_target_source``."""
+
+    source = np.asarray(source_points, dtype=float).reshape(-1, 3)
+    target = np.asarray(target_points, dtype=float).reshape(-1, 3)
+    open3d_result = _open3d_gicp(source, target)
+    if open3d_result is not None:
+        return open3d_result
+    return _numpy_icp(source, target)
+
+
+def estimate_frame_rotations(
+    frames: list[LidarFrame],
+    *,
+    stride: int = 1,
+) -> tuple[list[np.ndarray], list[tuple[float, float]]]:
+    """Estimate consecutive (or strided) LiDAR relative rotations for time sync."""
+
+    rotations: list[np.ndarray] = []
+    pair_times: list[tuple[float, float]] = []
+    for index in range(0, len(frames) - stride, max(stride, 1)):
+        a = frames[index]
+        b = frames[index + stride]
+        result = register_lidar_pair(b.points_xyz, a.points_xyz)
+        if not result.ok:
+            continue
+        rotations.append(result.transform[:3, :3])
+        pair_times.append((a.t_mid_s, b.t_mid_s))
+    return rotations, pair_times
@@ -0,0 +1,113 @@
+"""SO(3) rotation hand-eye solver for ``R_A R_X = R_X R_B``."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+
+import numpy as np
+from scipy.optimize import least_squares
+
+from .contracts import MotionPair
+from .geometry import orthonormalize_rotation, rotation_angle_deg, skew, so3_exp, so3_log
+
+
+@dataclass(frozen=True)
+class RotationHandeyeResult:
+    R_IMU_lidar: np.ndarray
+    residual_rms_deg: float
+    residual_median_deg: float
+    pair_count: int
+    ok: bool
+    notes: tuple[str, ...] = ()
+
+
+def _pair_weight(pair: MotionPair) -> float:
+    weight = float(pair.metadata.get("weight", 1.0))
+    if not np.isfinite(weight) or weight <= 0:
+        return 1.0
+    return weight
+
+
+def _tsai_rotation_initial(pairs: list[MotionPair]) -> np.ndarray:
+    """Closed-form rotation hand-eye initial guess (Tsai-style linear solve)."""
+
+    rows: list[np.ndarray] = []
+    rhs: list[np.ndarray] = []
+    for pair in pairs:
+        alpha = so3_log(pair.R_A)
+        beta = so3_log(pair.R_B)
+        if np.linalg.norm(alpha) < 1e-6 or np.linalg.norm(beta) < 1e-6:
+            continue
+        w = np.sqrt(_pair_weight(pair))
+        rows.append(w * skew(alpha + beta))
+        rhs.append(w * (beta - alpha))
+    if len(rows) < 2:
+        return np.eye(3)
+    a = np.vstack(rows)
+    b = np.concatenate(rhs)
+    try:
+        rotvec, *_ = np.linalg.lstsq(a, b, rcond=None)
+    except np.linalg.LinAlgError:
+        return np.eye(3)
+    return orthonormalize_rotation(so3_exp(rotvec))
+
+
+def _pair_residual_deg(r_x: np.ndarray, pair: MotionPair) -> float:
+    err = so3_log(r_x.T @ pair.R_A @ r_x @ pair.R_B.T)
+    return float(np.degrees(np.linalg.norm(err)))
+
+
+def solve_rotation_handeye(pairs: list[MotionPair] | tuple[MotionPair, ...]) -> RotationHandeyeResult:
+    """Solve ``R_A R_X = R_X R_B`` with weighted robust nonlinear refinement."""
+
+    usable = [pair for pair in pairs if rotation_angle_deg(pair.R_A) > 1.0 and rotation_angle_deg(pair.R_B) > 1.0]
+    notes: list[str] = []
+    if len(usable) < 3:
+        return RotationHandeyeResult(
+            R_IMU_lidar=np.eye(3),
+            residual_rms_deg=1e9,
+            residual_median_deg=1e9,
+            pair_count=len(usable),
+            ok=False,
+            notes=("need at least 3 motion pairs with meaningful rotation",),
+        )
+
+    r0 = _tsai_rotation_initial(usable)
+    weights = np.asarray([_pair_weight(pair) for pair in usable], dtype=float)
+    notes.append(
+        f"weighted hand-eye: weight median={float(np.median(weights)):.3g}, "
+        f"min={float(np.min(weights)):.3g}, max={float(np.max(weights)):.3g}"
+    )
+
+    def pack(r: np.ndarray) -> np.ndarray:
+        return so3_log(r)
+
+    def unpack(vec: np.ndarray) -> np.ndarray:
+        return orthonormalize_rotation(so3_exp(vec))
+
+    def residual(vec: np.ndarray) -> np.ndarray:
+        r_x = unpack(vec)
+        residuals = []
+        for pair, weight in zip(usable, weights):
+            err = so3_log(r_x.T @ pair.R_A @ r_x @ pair.R_B.T)
+            residuals.append(np.sqrt(weight) * err)
+        return np.concatenate(residuals)
+
+    opt = least_squares(residual, pack(r0), loss="huber", f_scale=np.deg2rad(1.0), max_nfev=200)
+    r_x = unpack(opt.x)
+    errs = np.asarray([_pair_residual_deg(r_x, pair) for pair in usable], dtype=float)
+    # Report unweighted RMS/median for interpretability.
+    rms = float(np.sqrt(np.mean(errs**2)))
+    med = float(np.median(errs))
+    notes.append(f"optimized over {len(usable)} pairs")
+    ok = rms < 5.0 and len(usable) >= 3
+    if not ok:
+        notes.append("rotation residual RMS too high or too few pairs")
+    return RotationHandeyeResult(
+        R_IMU_lidar=r_x,
+        residual_rms_deg=rms,
+        residual_median_deg=med,
+        pair_count=len(usable),
+        ok=ok,
+        notes=tuple(notes),
+    )
@@ -0,0 +1,303 @@
+"""Constant IMU-to-LiDAR clock-offset estimation via angular-rate correlation."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+
+import numpy as np
+from scipy import signal
+
+from .contracts import ImuSeries, LidarFrame
+from .geometry import rotation_angle_deg, so3_log
+from .registration import estimate_frame_rotations
+
+
+@dataclass(frozen=True)
+class TimeOffsetResult:
+    delta_t_s: float
+    correlation_peak: float
+    search_s: float
+    notes: tuple[str, ...] = ()
+    ok: bool = True
+
+
+def _magnitude_series(times: np.ndarray, values: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
+    mag = np.linalg.norm(values, axis=1) if values.ndim == 2 else np.asarray(values, dtype=float)
+    return np.asarray(times, dtype=float), np.asarray(mag, dtype=float)
+
+
+def _correlate_offset(
+    imu_t: np.ndarray,
+    imu_mag: np.ndarray,
+    lidar_t: np.ndarray,
+    lidar_mag: np.ndarray,
+    *,
+    search_s: float,
+    sample_hz: float,
+) -> tuple[float, float]:
+    """Return ``(delta_t, peak)`` for ``t_imu = t_lidar + delta_t``.
+
+    Implementation: resample both on LiDAR-relative grid, shift IMU by candidate
+    offsets, maximize normalized correlation. This avoids ambiguous lag signs.
+    """
+
+    t_start = float(lidar_t[0])
+    t_end = float(lidar_t[-1])
+    if t_end - t_start < 0.5:
+        return 0.0, 0.0
+    dt = 1.0 / sample_hz
+    grid = np.arange(t_start, t_end, dt)
+    lidar_sig = np.interp(grid, lidar_t, lidar_mag, left=0.0, right=0.0)
+    lidar_sig = lidar_sig - np.mean(lidar_sig)
+    lidar_norm = float(np.linalg.norm(lidar_sig)) + 1e-12
+
+    best_delta = 0.0
+    best_peak = -1.0
+    for delta in np.arange(-search_s, search_s + 1e-12, dt):
+        imu_sig = np.interp(grid + delta, imu_t, imu_mag, left=0.0, right=0.0)
+        imu_sig = imu_sig - np.mean(imu_sig)
+        denom = lidar_norm * (float(np.linalg.norm(imu_sig)) + 1e-12)
+        peak = float(np.dot(imu_sig, lidar_sig) / denom)
+        if peak > best_peak:
+            best_peak = peak
+            best_delta = float(delta)
+
+    # Local parabolic refinement.
+    deltas = np.array([best_delta - dt, best_delta, best_delta + dt], dtype=float)
+    peaks = []
+    for delta in deltas:
+        imu_sig = np.interp(grid + delta, imu_t, imu_mag, left=0.0, right=0.0)
+        imu_sig = imu_sig - np.mean(imu_sig)
+        denom = lidar_norm * (float(np.linalg.norm(imu_sig)) + 1e-12)
+        peaks.append(float(np.dot(imu_sig, lidar_sig) / denom))
+    y0, y1, y2 = peaks
+    denom = y0 - 2 * y1 + y2
+    if abs(denom) > 1e-12:
+        best_delta = float(best_delta + 0.5 * (y0 - y2) / denom * dt)
+        best_peak = float(y1)
+    return best_delta, best_peak
+
+
+def estimate_time_offset(
+    imu: ImuSeries,
+    frames: list[LidarFrame],
+    *,
+    gyro_bias_rad_s: np.ndarray | None = None,
+    search_s: float = 1.0,
+    sample_hz: float = 50.0,
+) -> TimeOffsetResult:
+    """Estimate ``t_imu = t_lidar + delta_t``.
+
+    Positive ``delta_t`` means the IMU clock reading is ahead of the LiDAR clock
+    for the same physical instant (IMU timestamps are larger).
+    """
+
+    notes: list[str] = []
+    if len(frames) < 5:
+        return TimeOffsetResult(0.0, 0.0, search_s, ("not enough LiDAR frames",), False)
+
+    bias = np.zeros(3) if gyro_bias_rad_s is None else np.asarray(gyro_bias_rad_s, dtype=float)
+    gyro = imu.gyro_rad_s - bias
+
+    stride = max(1, len(frames) // 20)
+    rotations, pair_times = estimate_frame_rotations(frames, stride=stride)
+    if len(rotations) < 4:
+        rotations, pair_times = estimate_frame_rotations(frames, stride=1)
+    if len(rotations) < 4:
+        return TimeOffsetResult(0.0, 0.0, search_s, ("not enough LiDAR relative rotations",), False)
+
+    lidar_t = []
+    lidar_w = []
+    for (t_a, t_b), rotation in zip(pair_times, rotations):
+        dt_pair = max(t_b - t_a, 1e-3)
+        omega = so3_log(rotation) / dt_pair
+        lidar_t.append(0.5 * (t_a + t_b))
+        lidar_w.append(omega)
+    lidar_t_arr = np.asarray(lidar_t, dtype=float)
+    lidar_w_arr = np.asarray(lidar_w, dtype=float)
+
+    imu_t, imu_mag = _magnitude_series(imu.t_s, gyro)
+    lidar_t_mag, lidar_mag = _magnitude_series(lidar_t_arr, lidar_w_arr)
+    delta, peak = _correlate_offset(
+        imu_t,
+        imu_mag,
+        lidar_t_mag,
+        lidar_mag,
+        search_s=search_s,
+        sample_hz=sample_hz,
+    )
+
+    notes.append(
+        f"LiDAR mean pair rotation {np.mean([rotation_angle_deg(r) for r in rotations]):.2f} deg"
+    )
+    notes.append(f"searched delta_t in ±{search_s:.3f}s by direct correlation")
+    ok = peak > 0.15
+    if not ok:
+        notes.append("correlation peak is weak; check overlapping motion and axis units")
+    return TimeOffsetResult(
+        delta_t_s=delta,
+        correlation_peak=peak,
+        search_s=search_s,
+        notes=tuple(notes),
+        ok=ok,
+    )
+
+
+def lidar_time_to_imu_time(t_lidar_s: float, delta_t_s: float) -> float:
+    """Convert a LiDAR timestamp to the IMU clock using ``t_imu = t_lidar + delta_t``."""
+
+    return float(t_lidar_s + delta_t_s)
+
+
+def _lidar_omega_series(
+    frames: list[LidarFrame],
+    *,
+    stride: int,
+) -> tuple[np.ndarray, np.ndarray]:
+    rotations, pair_times = estimate_frame_rotations(frames, stride=stride)
+    if len(rotations) < 4:
+        rotations, pair_times = estimate_frame_rotations(frames, stride=1)
+    lidar_t: list[float] = []
+    lidar_w: list[np.ndarray] = []
+    for (t_a, t_b), rotation in zip(pair_times, rotations):
+        dt_pair = max(t_b - t_a, 1e-3)
+        omega = so3_log(rotation) / dt_pair
+        lidar_t.append(0.5 * (t_a + t_b))
+        lidar_w.append(omega)
+    return np.asarray(lidar_t, dtype=float), np.asarray(lidar_w, dtype=float)
+
+
+def refine_time_offset_signed(
+    imu: ImuSeries,
+    frames: list[LidarFrame],
+    *,
+    delta_t_s: float,
+    R_IMU_lidar: np.ndarray,
+    gyro_bias_rad_s: np.ndarray | None = None,
+    search_s: float = 0.08,
+    sample_hz: float = 50.0,
+) -> TimeOffsetResult:
+    """Refine ``δt`` with signed 3-axis rates using a known ``R_IMU_lidar``.
+
+    Cost: mean squared error between ``gyro_imu(t_lidar+δt)`` and
+    ``R_IMU_lidar @ omega_lidar(t_lidar)`` on a common grid around the coarse ``δt``.
+    """
+
+    notes: list[str] = [f"signed refine around coarse delta_t={delta_t_s:.6f}s"]
+    if len(frames) < 5:
+        return TimeOffsetResult(delta_t_s, 0.0, search_s, ("not enough LiDAR frames",), False)
+
+    bias = np.zeros(3) if gyro_bias_rad_s is None else np.asarray(gyro_bias_rad_s, dtype=float)
+    gyro = imu.gyro_rad_s - bias
+    r_x = np.asarray(R_IMU_lidar, dtype=float).reshape(3, 3)
+
+    stride = max(1, len(frames) // 20)
+    lidar_t, lidar_w = _lidar_omega_series(frames, stride=stride)
+    if lidar_t.size < 4:
+        return TimeOffsetResult(delta_t_s, 0.0, search_s, ("not enough LiDAR omega samples",), False)
+
+    # Predicted IMU-frame angular rate from LiDAR relative rotations.
+    pred = (r_x @ lidar_w.T).T
+    t_start = float(lidar_t[0])
+    t_end = float(lidar_t[-1])
+    if t_end - t_start < 0.5:
+        return TimeOffsetResult(delta_t_s, 0.0, search_s, ("LiDAR span too short for signed refine",), False)
+
+    dt = 1.0 / sample_hz
+    grid = np.arange(t_start, t_end, dt)
+    pred_grid = np.column_stack(
+        [np.interp(grid, lidar_t, pred[:, axis], left=np.nan, right=np.nan) for axis in range(3)]
+    )
+
+    def _cost_and_corr(delta: float) -> tuple[float, float]:
+        meas = np.column_stack(
+            [
+                np.interp(grid + delta, imu.t_s, gyro[:, axis], left=np.nan, right=np.nan)
+                for axis in range(3)
+            ]
+        )
+        mask = np.isfinite(pred_grid).all(axis=1) & np.isfinite(meas).all(axis=1)
+        if int(np.count_nonzero(mask)) < 10:
+            return float("inf"), -1.0
+        err = meas[mask] - pred_grid[mask]
+        cost = float(np.mean(np.sum(err * err, axis=1)))
+        a = meas[mask].reshape(-1)
+        b = pred_grid[mask].reshape(-1)
+        a = a - np.mean(a)
+        b = b - np.mean(b)
+        corr = float(np.dot(a, b) / ((np.linalg.norm(a) + 1e-12) * (np.linalg.norm(b) + 1e-12)))
+        return cost, corr
+
+    coarse_cost, coarse_corr = _cost_and_corr(float(delta_t_s))
+    best_delta = float(delta_t_s)
+    best_cost = coarse_cost
+    best_corr = coarse_corr
+    half = abs(float(search_s))
+    for delta in np.arange(delta_t_s - half, delta_t_s + half + 1e-12, dt):
+        cost, corr = _cost_and_corr(float(delta))
+        if cost < best_cost:
+            best_cost = cost
+            best_delta = float(delta)
+            best_corr = corr
+
+    # Parabolic refine on cost around the best discrete delta.
+    samples = []
+    for delta in (best_delta - dt, best_delta, best_delta + dt):
+        cost, _ = _cost_and_corr(float(delta))
+        samples.append(cost if np.isfinite(cost) else best_cost)
+    y0, y1, y2 = samples
+    denom = y0 - 2 * y1 + y2
+    if abs(denom) > 1e-12 and y1 <= y0 and y1 <= y2:
+        candidate = float(best_delta + 0.5 * (y0 - y2) / denom * dt)
+        cand_cost, cand_corr = _cost_and_corr(candidate)
+        if cand_cost < best_cost:
+            best_delta = candidate
+            best_cost = cand_cost
+            best_corr = cand_corr
+
+    # Guard with magnitude correlation so ICP-biased signed minima cannot wander.
+    imu_t, imu_mag = _magnitude_series(imu.t_s, gyro)
+    lidar_t_mag, lidar_mag = _magnitude_series(lidar_t, lidar_w)
+
+    def _mag_score(delta: float) -> float:
+        t_start_l = float(lidar_t_mag[0])
+        t_end_l = float(lidar_t_mag[-1])
+        grid_m = np.arange(t_start_l, t_end_l, dt)
+        lidar_sig = np.interp(grid_m, lidar_t_mag, lidar_mag, left=0.0, right=0.0)
+        lidar_sig = lidar_sig - np.mean(lidar_sig)
+        imu_sig = np.interp(grid_m + delta, imu_t, imu_mag, left=0.0, right=0.0)
+        imu_sig = imu_sig - np.mean(imu_sig)
+        denom = (float(np.linalg.norm(lidar_sig)) + 1e-12) * (float(np.linalg.norm(imu_sig)) + 1e-12)
+        return float(np.dot(imu_sig, lidar_sig) / denom)
+
+    mag_at_coarse = _mag_score(float(delta_t_s))
+    mag_at_best = _mag_score(best_delta)
+
+    notes.append(
+        f"signed 3-axis refine: delta_t={best_delta:.6f}s, "
+        f"mse={best_cost:.4g} (coarse_mse={coarse_cost:.4g}), "
+        f"corr={best_corr:.3f}, mag_corr={mag_at_best:.3f} (coarse_mag={mag_at_coarse:.3f}), "
+        f"search=±{half:.3f}s"
+    )
+    improved = (
+        np.isfinite(best_cost)
+        and best_cost < coarse_cost * 0.999
+        # Do not sacrifice the more reliable magnitude alignment for a noisy signed MSE gain.
+        and mag_at_best + 1e-4 >= mag_at_coarse
+    )
+    if not improved:
+        notes.append("signed refine rejected by MSE/mag-consistency; keeping previous delta_t")
+        return TimeOffsetResult(
+            delta_t_s=float(delta_t_s),
+            correlation_peak=mag_at_coarse if mag_at_coarse > 0 else best_corr,
+            search_s=search_s,
+            notes=tuple(notes),
+            ok=True,
+        )
+    return TimeOffsetResult(
+        delta_t_s=best_delta,
+        correlation_peak=mag_at_best,
+        search_s=search_s,
+        notes=tuple(notes),
+        ok=True,
+    )
@@ -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,
+    )
@@ -0,0 +1,83 @@
+"""Vehicle-installation configuration loading and light validation."""
+
+from __future__ import annotations
+
+from collections.abc import Mapping
+from pathlib import Path
+from typing import Any
+
+REQUIRED_TOP_LEVEL_KEYS = frozenset({"schema_version", "vehicle", "installation", "sensors", "time"})
+
+
+def validate_config_shape(config: Mapping[str, object]) -> list[str]:
+    """Return missing top-level keys without inventing default values."""
+
+    return sorted(REQUIRED_TOP_LEVEL_KEYS.difference(config))
+
+
+def validate_config_semantics(config: Mapping[str, Any]) -> list[str]:
+    """Return semantic issues that block calibration interpretation."""
+
+    issues: list[str] = []
+    sensors = config.get("sensors")
+    if not isinstance(sensors, Mapping):
+        return ["sensors must be a mapping"]
+    imu = sensors.get("imu")
+    lidar = sensors.get("lidar")
+    if not isinstance(imu, Mapping):
+        issues.append("sensors.imu missing")
+    else:
+        axes = ((imu.get("raw_frame") or {}) if isinstance(imu.get("raw_frame"), Mapping) else {}).get("axes")
+        if not axes:
+            issues.append("sensors.imu.raw_frame.axes is empty (declare axis meaning even if approximate)")
+    if not isinstance(lidar, Mapping):
+        issues.append("sensors.lidar missing")
+    else:
+        axes = ((lidar.get("raw_frame") or {}) if isinstance(lidar.get("raw_frame"), Mapping) else {}).get("axes")
+        if not axes:
+            issues.append("sensors.lidar.raw_frame.axes is empty (declare axis meaning even if approximate)")
+
+    time_cfg = config.get("time")
+    if not isinstance(time_cfg, Mapping):
+        issues.append("time missing")
+    else:
+        for key in ("imu_timestamp_source", "lidar_timestamp_source", "lidar_frame_time_definition"):
+            if not time_cfg.get(key):
+                issues.append(f"time.{key} is empty")
+    return issues
+
+
+def load_vehicle_config(path: str | Path) -> dict[str, Any]:
+    """Load and lightly validate a YAML vehicle configuration."""
+
+    try:
+        import yaml
+    except ImportError as exc:  # pragma: no cover
+        raise ImportError("PyYAML is required to load vehicle configuration files") from exc
+
+    config_path = Path(path)
+    with config_path.open("r", encoding="utf-8") as handle:
+        loaded = yaml.safe_load(handle)
+    if not isinstance(loaded, dict):
+        raise ValueError(f"vehicle config must be a mapping: {config_path}")
+
+    missing = validate_config_shape(loaded)
+    if missing:
+        raise ValueError(f"vehicle config missing keys {missing}: {config_path}")
+
+    semantic = validate_config_semantics(loaded)
+    if semantic:
+        raise ValueError("vehicle config semantic issues:\n- " + "\n- ".join(semantic))
+    return loaded
+
+
+def prior_enabled(config: Mapping[str, Any], name: str) -> bool:
+    """Return whether an optional prior is enabled."""
+
+    init = config.get("initialization")
+    if not isinstance(init, Mapping):
+        return False
+    prior = init.get(name)
+    if not isinstance(prior, Mapping):
+        return False
+    return bool(prior.get("enabled", False))
@@ -0,0 +1,86 @@
+# `imu_lidar` 模块说明
+
+本包实现 LiDAR–IMU 外参标定：在连续行驶数据上选取关键帧，用 IMU 预积分与雷达配准构造相对运动对，求解安装外参。
+
+```text
+A ≈ 关键帧间 IMU 相对运动（预积分：旋转 / 速度增量 / 位移增量）
+B ≈ 关键帧间雷达配准
+解 R_A R_X = R_X R_B  →  旋转外参（手眼阶段只用旋转）
+再精修旋转与陀螺零偏；在完整六自由度模式下，可观时再估计平移等
+```
+
+入口：
+
+```powershell
+python -m imu_lidar.cli plan
+python -m imu_lidar.cli run --vehicle-config ... --imu ... --lidar ... --output ...
+```
+
+整体流程由 `pipeline.py` 串联。  
+修改本目录代码时，请同步更新本说明，并在 [`CHANGELOG.md`](CHANGELOG.md) 追加「时间戳 + 原本 → 改成」。
+
+---
+
+## 流水线顺序与文件
+
+
+| 顺序  | 文件                      | 作用                            |
+| --- | ----------------------- | ----------------------------- |
+| 0   | `contracts.py`          | 公共数据类型与状态枚举                   |
+| 0   | `geometry.py`           | 刚体变换与旋转工具                     |
+| 0   | `vehicle_config.py`     | 读取并校验车辆 YAML                  |
+| 1   | `imu_io.py`             | 读标准 IMU 中间格式                  |
+| 1   | `lidar_io.py`           | 读标准雷达会话目录                     |
+| 2   | `timestamp_audit.py`    | 时间单调 / 频率 / 空洞检查              |
+| 3   | `imu_audit.py`          | 静止零偏、加速度模长检查、建议竖直轴            |
+| 4   | `time_offset.py`        | 粗估时间偏置 δt，并用旋转外参精修            |
+| 5   | `registration.py`       | 帧间点云配准                        |
+| 5   | `keyframes.py`          | 按运动量抽取关键帧                     |
+| 5   | `lidar_deskew.py`       | 可选点云去畸变（低速可关）                 |
+| 6   | `imu_preintegration.py` | IMU 预积分（旋转及速度/位移增量、协方差、零偏雅可比） |
+| 6   | `motion_pairs.py`       | 构造运动对；手眼使用其中的旋转               |
+| 7   | `rotation_handeye.py`   | 加权旋转手眼                        |
+| 8   | `observability.py`      | 旋转 / 平移可观性检查                  |
+| 8   | `joint_optimizer.py`    | 联合精修；完整模式下可估计平移、重力、速度与时变零偏    |
+| 9   | `finalize.py`           | 写出结果 JSON                     |
+| —   | `pipeline.py`           | 编排全流程                         |
+| —   | `cli.py`                | 命令行入口                         |
+| —   | `CHANGELOG.md`          | 改动记录                          |
+
+
+---
+
+
+
+## 运行模式要点
+
+- **运动对**始终计算完整预积分量（旋转、速度增量、位移增量及不确定度）。
+- `--mode rotation_only`：只精修旋转与常值陀螺零偏，交付旋转与时间偏置。
+- `--mode full_se3`：在可观时再估计重力、关键帧速度、时变零偏与平移；结果写入 `summary.json` 的 joint 字段。
+
+---
+
+
+
+## 输入格式
+
+```text
+imu.csv                         # t,gx,gy,gz,ax,ay,az（建议设备时间）
+lidar_session/
+  frames_index.csv              # frame_id,filename,t_start,t_end
+  frames/frame_XXXXX.npz        # points: (N,3) 米
+```
+
+原始录制格式不在本包内解析，需先导出为上述中间格式。详见 `[docs/V1_数据格式.md](../docs/V1_数据格式.md)`。
+
+---
+
+
+
+## 当前能力
+
+- 本包是仓库**唯一**标定路径：质检 → 时间偏置 → 关键帧配对 → 旋转手眼 → 联合精修 →（可选）完整六自由度 → 报告  
+- 点云去畸变：可选  
+- 阶段与试验边界见根目录 [`README.md`](../README.md) §0  
+- 改动史：[`CHANGELOG.md`](CHANGELOG.md)
+