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

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

tests/README.md

@@ -0,0 +1,127 @@
+# 测试说明
+
+本目录说明两类试验的**边界**，避免把「合成通过」或「旧车 blocked」误读成实车外参已交付 / 算法不可用。
+
+总览见根目录 [`README.md`](../README.md) §0。
+
+| 试验 | 是否默认 pytest | 在证明什么 | 不在证明什么 |
+|---|---|---|---|
+| **合成数据** | 是 | 算法链路正确、能收回已知 yaw / δt | 实车安装精度、平移可交付 |
+| **旧车 S2** | 否（线下手工） | 主机时间旧数据上流水线能跑完；质量门会拒绝坏结果 | 外参真值；新车可用性 |
+
+---
+
+## 1. 自动化测试（合成数据 / `pytest`）
+
+入口：`tests/test_v1_pipeline.py`  
+命令：
+
+```powershell
+cd <仓库根目录>
+python -m pytest -q
+```
+
+
+| 测试                                                            | 输入           | 在测什么                | 期望结果                                       |
+| ------------------------------------------------------------- | ------------ | ------------------- | ------------------------------------------ |
+| `test_rotation_handeye_recovers_yaw`                          | 合成运动对（无点云）   | 旋转手眼能否收回已知 yaw      | 旋转误差 < 1°                                  |
+| `test_time_offset_on_synthetic`                               | 合成会话（故意加 δt） | 模长相关粗估时间偏置          | \|δt 误差\| < 0.05 s                           |
+| `test_signed_time_offset_refine_improves_or_keeps`            | 同上 + 真值 R    | 有符号三轴 δt 精修         | 不比粗估明显更差                                   |
+| `test_preintegration_bias_jacobian_matches_finite_difference` | 随机陀螺序列       | 旋转预积分 `J_bg`        | 与有限差分一致（松阈值）                               |
+| `test_imu_preintegration_recovers_constant_accel_translation` | 常值加速度        | 完整预积分 Δv/Δp         | 接近解析值                                      |
+| `test_imu_preintegration_bias_jacobian_finite_difference`     | 随机 IMU       | `J_bg`/`J_ba` 一阶修正  | 与重积分接近                                     |
+| `test_synthetic_pipeline_rotation_and_time_offset`            | 端到端合成会话      | `rotation_only` 全流程 | `rotation_only_accepted`；δt 准；手眼 RMS < 5°  |
+| `test_synthetic_pipeline_full_se3_smoke`                      | 同上           | `full_se3` 不崩溃      | 状态为 accepted / rejected / rotation_only 之一 |
+
+
+合成数据由 `tools/generate_synthetic_session.py` 生成（墙面点云 + 已知外参 yaw 与 δt）。  
+一键复现见根目录 README：`tools/reproduce_synthetic.py`。
+
+---
+
+
+
+## 2. 旧车 S2 线下试验（不在默认 pytest 里）
+
+
+
+### 用了什么数据
+
+
+| 项       | 内容                                                                   |
+| ------- | -------------------------------------------------------------------- |
+| 车辆 / 批次 | 旧 **S2** 验证集（主机时间时代的录制）                                              |
+| 典型路径    | `D:\IMU_calibration\work\S2_scheme1_validation\`（历史目录名；含 `imu.csv` + 雷达会话） |
+| IMU 时间  | **主机 UTC 接收时间**（串口块到达时刻），不是 IMU 设备时间                                 |
+| 雷达时间    | dlog 导出的主机侧 `unix_time_ns`，不是 MSOP 设备时间                              |
+| 帧率特征    | 雷达约 **1 Hz** 量级，关键帧间隔偏长                                              |
+| 配置烟测    | `config/s2_old_smoke.yaml`（仅声明为旧数据烟测，不当交付）                   |
+
+
+这些数据**只能用来验证流水线能否跑通**，不能当作新车外参真值来源。  
+`blocked` 是质量门的**预期结果**，不是「算法突然坏了」。
+
+### 做了什么测试
+
+对同一批 S2 中间格式多次跑 `cli run`，例如：
+
+- 预积分加强后的输出目录（本机历史名如 `out_scheme2_preint`）
+- 有符号 δt / 联合精修后的输出（本机历史名如 `out_scheme2_phaseA`）
+- 用 `tools/compare_s2_runs.py` 对比两次 `summary.json`
+
+命令形态（路径按本机实际修改；雷达会话目录若仍叫 `scheme2_session` 为历史命名）：
+
+```powershell
+python -m imu_lidar.cli run `
+  --vehicle-config config\s2_old_smoke.yaml `
+  --imu D:\IMU_calibration\work\S2_scheme1_validation\imu.csv `
+  --lidar D:\IMU_calibration\work\S2_scheme1_validation\scheme2_session `
+  --output path\to\out_s2 `
+  --mode rotation_only `
+  --time-offset-search-s 2.0
+
+python tools\compare_s2_runs.py path\to\out_old\summary.json path\to\out_new\summary.json
+```
+
+
+
+### 得到什么结果（记录摘要）
+
+
+| 指标       | 预积分加强一轮       | 有符号 δt / 精修一轮 |
+| -------- | ------------- | ------------------ |
+| `status` | `blocked`（预期） | `blocked`（预期）      |
+| 手眼 RMS   | 约 **15.0°**   | 约 **14.5°**        |
+| 手眼中位数    | —             | 约 **6.8°**         |
+| δt       | 约 **−2.0 s**  | 约 **−1.75 s**（有修正） |
+| 相关峰      | 很弱（约 0.18）    | 仍弱（约 0.13）         |
+| 结论       | 链路可跑          | 残差略降，但 **不当交付外参**  |
+
+
+原因归纳：
+
+1. 时间戳是**主机时间**，相关峰弱，δt / yaw / 零偏互相耦合；
+2. 雷达约 1 Hz，运动对间隔长，IMU 侧更易漂；
+3. 质量门主动 `blocked`，避免把坏结果当成安装参数。
+
+**正式标定**必须改用设备时间（IMU `device_timestamp`、雷达 MSOP 设备时）重新采集后再跑。
+
+---
+
+
+
+## 3. 配准结果怎么目视检查
+
+标定跑完后（合成或实车）：
+
+```powershell
+python tools\visualize_pair_3d.py `
+  --lidar examples\synthetic_session\lidar `
+  --imu examples\synthetic_session\imu.csv `
+  --summary examples\synthetic_session\out\summary.json `
+  --pair-index 0 `
+  --save-png examples\synthetic_session\out\pair0_overlay.png
+```
+
+交互窗口快捷键：`1`–`4` 切换叠点模式；`N`/`]` 下一运动对，`P`/`[` 上一运动对。  
+无显示器时加 `--no-gui --save-png ...` 只出俯视图 PNG。

tests/test_v1_pipeline.py

@@ -0,0 +1,206 @@
+"""Automated tests for imu_lidar (synthetic data).
+
+See ``tests/README.md`` for:
+- what each pytest covers;
+- offline S2 host-time experiments (not run in default pytest) and recorded outcomes.
+"""
+
+from __future__ import annotations
+
+from pathlib import Path
+
+import numpy as np
+
+from imu_lidar.contracts import CalibrationMode, CalibrationRequest, MotionPair, SessionInput
+from imu_lidar.geometry import so3_exp
+from imu_lidar.pipeline import run_calibration
+from imu_lidar.rotation_handeye import solve_rotation_handeye
+from imu_lidar.time_offset import estimate_time_offset
+from imu_lidar.imu_io import load_imu_samples
+from imu_lidar.lidar_io import load_lidar_frames
+from tools.generate_synthetic_session import generate_synthetic_session
+
+
+def test_rotation_handeye_recovers_yaw():
+    r_true = so3_exp(np.deg2rad(np.array([1.0, -2.0, 30.0])))
+    pairs = []
+    rng = np.random.default_rng(1)
+    for _ in range(20):
+        axis = rng.normal(size=3)
+        axis /= np.linalg.norm(axis)
+        angle = np.deg2rad(rng.uniform(8.0, 35.0))
+        r_b = so3_exp(axis * angle)
+        r_a = r_true @ r_b @ r_true.T
+        pairs.append(
+            MotionPair(
+                session_id="s",
+                i=0,
+                j=1,
+                t_i_s=0.0,
+                t_j_s=1.0,
+                R_A=r_a,
+                R_B=r_b,
+            )
+        )
+    result = solve_rotation_handeye(pairs)
+    assert result.ok
+    err = np.linalg.norm(_log(r_true.T @ result.R_IMU_lidar))
+    assert np.degrees(err) < 1.0
+
+
+def _log(rotation: np.ndarray) -> np.ndarray:
+    from imu_lidar.geometry import so3_log
+
+    return so3_log(rotation)
+
+
+def test_synthetic_pipeline_rotation_and_time_offset(tmp_path: Path):
+    meta = generate_synthetic_session(tmp_path, delta_t_s=0.17, yaw_extrinsic_deg=25.0)
+    config = Path(__file__).resolve().parents[1] / "config" / "vehicle_installation.template.yaml"
+    out = tmp_path / "out"
+    request = CalibrationRequest(
+        vehicle_config=config,
+        sessions=(
+            SessionInput(
+                session_id="synth",
+                imu_source=tmp_path / "imu.csv",
+                lidar_source=tmp_path / "lidar",
+            ),
+        ),
+        requested_mode=CalibrationMode.ROTATION_ONLY,
+        output_directory=out,
+        max_iterations=1,
+        time_offset_search_s=0.5,
+        min_pair_rotation_deg=2.0,
+        min_pair_translation_m=0.05,
+    )
+    result = run_calibration(request)
+    assert result.status.value == "rotation_only_accepted"
+    assert result.time_offset_s is not None
+    assert abs(result.time_offset_s - meta["delta_t_s"]) < 0.05
+    assert result.T_IMU_lidar is not None
+    # End-to-end uses approximate ICP; allow moderate absolute error but require consistency.
+    r_true = so3_exp(np.deg2rad(np.array([2.0, -1.5, meta["yaw_extrinsic_deg"]])))
+    err_deg = np.degrees(np.linalg.norm(_log(r_true.T @ result.T_IMU_lidar[:3, :3])))
+    assert err_deg < 15.0
+    session0 = result.details["sessions"][0]
+    assert session0["handeye"]["residual_rms_deg"] < 5.0
+
+
+def test_time_offset_on_synthetic(tmp_path: Path):
+    meta = generate_synthetic_session(tmp_path, delta_t_s=0.21, yaw_extrinsic_deg=15.0)
+    imu = load_imu_samples(tmp_path / "imu.csv")
+    frames = load_lidar_frames(tmp_path / "lidar")
+    offset = estimate_time_offset(imu, frames, search_s=0.5)
+    assert offset.ok
+    assert abs(offset.delta_t_s - meta["delta_t_s"]) < 0.05
+
+
+def test_preintegration_bias_jacobian_matches_finite_difference():
+    from imu_lidar.imu_preintegration import apply_bias_jacobian_correction, preintegrate_gyro
+    from imu_lidar.geometry import so3_log
+
+    rng = np.random.default_rng(0)
+    t = np.linspace(0.0, 1.0, 200)
+    gyro = rng.normal(scale=0.2, size=(t.size, 3))
+    bias0 = np.array([0.01, -0.02, 0.005])
+    preint = preintegrate_gyro(t, gyro, 0.1, 0.7, bias0)
+    db = np.array([1e-3, -2e-3, 5e-4])
+    approx = apply_bias_jacobian_correction(preint.delta_R, preint.J_bg, db)
+    exact = preintegrate_gyro(t, gyro, 0.1, 0.7, bias0 + db).delta_R
+    err = np.linalg.norm(so3_log(approx.T @ exact))
+    assert err < 2e-3
+
+
+def test_imu_preintegration_recovers_constant_accel_translation():
+    from imu_lidar.imu_preintegration import preintegrate_imu
+    from imu_lidar.geometry import so3_log
+
+    # Constant body accel (no gravity in preint body increments), zero gyro.
+    dt = 0.01
+    t = np.arange(0.0, 1.0 + 1e-9, dt)
+    gyro = np.zeros((t.size, 3))
+    acc = np.tile(np.array([0.5, -0.2, 0.1]), (t.size, 1))
+    preint = preintegrate_imu(t, gyro, acc, 0.0, 1.0, np.zeros(3), np.zeros(3))
+    assert np.linalg.norm(so3_log(preint.delta_R)) < 1e-9
+    # Δv ≈ a Δt, Δp ≈ 0.5 a Δt²
+    assert np.linalg.norm(preint.delta_v - acc[0] * 1.0) < 5e-3
+    assert np.linalg.norm(preint.delta_p - 0.5 * acc[0] * 1.0) < 1e-2
+    assert preint.cov.shape == (9, 9)
+    assert preint.J_bg.shape == (9, 3) and preint.J_ba.shape == (9, 3)
+
+
+def test_imu_preintegration_bias_jacobian_finite_difference():
+    from imu_lidar.imu_preintegration import apply_bias_correction_imu, preintegrate_imu
+
+    rng = np.random.default_rng(2)
+    t = np.linspace(0.0, 0.8, 160)
+    gyro = rng.normal(scale=0.15, size=(t.size, 3))
+    acc = rng.normal(scale=0.5, size=(t.size, 3)) + np.array([0.0, 0.0, 9.8])
+    bg0 = np.array([0.01, -0.01, 0.0])
+    ba0 = np.array([0.02, 0.0, -0.01])
+    base = preintegrate_imu(t, gyro, acc, 0.05, 0.55, bg0, ba0)
+    dbg = np.array([5e-4, -3e-4, 2e-4])
+    dba = np.array([1e-3, -5e-4, 0.0])
+    r_a, v_a, p_a = apply_bias_correction_imu(base, dbg, dba)
+    exact = preintegrate_imu(t, gyro, acc, 0.05, 0.55, bg0 + dbg, ba0 + dba)
+    from imu_lidar.geometry import so3_log
+
+    assert np.linalg.norm(so3_log(r_a.T @ exact.delta_R)) < 5e-3
+    assert np.linalg.norm(v_a - exact.delta_v) < 3e-2
+    assert np.linalg.norm(p_a - exact.delta_p) < 2e-2
+
+
+def test_synthetic_pipeline_full_se3_smoke(tmp_path: Path):
+    generate_synthetic_session(tmp_path, delta_t_s=0.12, yaw_extrinsic_deg=18.0)
+    config = Path(__file__).resolve().parents[1] / "config" / "vehicle_installation.template.yaml"
+    out = tmp_path / "out_se3"
+    request = CalibrationRequest(
+        vehicle_config=config,
+        sessions=(
+            SessionInput(
+                session_id="synth",
+                imu_source=tmp_path / "imu.csv",
+                lidar_source=tmp_path / "lidar",
+            ),
+        ),
+        requested_mode=CalibrationMode.FULL_SE3,
+        output_directory=out,
+        max_iterations=1,
+        time_offset_search_s=0.5,
+        min_pair_rotation_deg=2.0,
+        min_pair_translation_m=0.05,
+    )
+    result = run_calibration(request)
+    assert result.status.value in {
+        "full_se3_accepted",
+        "full_se3_rejected_due_to_observability",
+        "rotation_only_accepted",
+    }
+    assert result.T_IMU_lidar is not None
+    session0 = result.details["sessions"][0]
+    assert "delta_v" in session0.get("pair_notes", []) or session0.get("pair_count", 0) >= 0
+    # Phase-C fields appear when joint ran successfully on pairs.
+    if session0.get("ok"):
+        assert "gyro_bias_rad_s" in session0["joint"]
+
+
+def test_signed_time_offset_refine_improves_or_keeps(tmp_path: Path):
+    from imu_lidar.geometry import so3_exp
+    from imu_lidar.time_offset import refine_time_offset_signed
+
+    meta = generate_synthetic_session(tmp_path, delta_t_s=0.18, yaw_extrinsic_deg=20.0)
+    imu = load_imu_samples(tmp_path / "imu.csv")
+    frames = load_lidar_frames(tmp_path / "lidar")
+    coarse = estimate_time_offset(imu, frames, search_s=0.5)
+    r_true = so3_exp(np.deg2rad(np.array([2.0, -1.5, meta["yaw_extrinsic_deg"]])))
+    refined = refine_time_offset_signed(
+        imu,
+        frames,
+        delta_t_s=coarse.delta_t_s,
+        R_IMU_lidar=r_true,
+        search_s=0.08,
+    )
+    assert refined.ok
+    # Must not drift farther from truth than the coarse estimate by a large margin.
+    assert abs(refined.delta_t_s - meta["delta_t_s"]) <= abs(coarse.delta_t_s - meta["delta_t_s"]) + 0.01