From 6b44a495fb70960195b748d74152e7e51c8030d2 Mon Sep 17 00:00:00 2001 From: "lichun.qu" <16975270+zzqlc@user.noreply.gitee.com> Date: Tue, 11 Aug 2026 13:28:28 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=AD=A3=E5=AE=89=E8=A3=85Z=E7=A6=BB?= =?UTF-8?q?=E5=9C=B0=E5=85=88=E9=AA=8C=EF=BC=8C=E5=B9=B6=E6=94=B9=E8=BF=9B?= =?UTF-8?q?=E6=97=8B=E8=BD=AC=E5=8F=AF=E8=A7=86=E5=8C=96=E6=A8=A1=E5=BC=8F?= =?UTF-8?q?4=E9=81=BF=E5=85=8D=E5=9D=8FIMU=E4=BD=8D=E7=A7=BB=E8=AF=AF?= =?UTF-8?q?=E5=AF=BC=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Cursor --- config/vehicle_hi13_h32_20260808.yaml | 40 +++--- docs/20260808_LiDAR-IMU标定现状与问题.md | 171 +++++++++++++++++++++++ tools/visualize_pair_3d.py | 122 ++++++++++++++-- 3 files changed, 298 insertions(+), 35 deletions(-) create mode 100644 docs/20260808_LiDAR-IMU标定现状与问题.md diff --git a/config/vehicle_hi13_h32_20260808.yaml b/config/vehicle_hi13_h32_20260808.yaml index 1d4b3f1..c1213d1 100644 --- a/config/vehicle_hi13_h32_20260808.yaml +++ b/config/vehicle_hi13_h32_20260808.yaml @@ -4,19 +4,23 @@ vehicle: vehicle_id: "outdoor_usable_20260808" body_frame: name: "base_link" - # CAD / 后轮轴中心测量系（与安装图 dX/dY/dZ 一致） + # 车体约定：后轮轴中心在地面投影为原点附近参考；X 前 / Y 左 / Z 上 + # translation_m 的 Z 使用「离地高度」；后轮轴中心离地 294 mm axes: "X forward, Y left, Z up" unit: m - reference_point: "rear_axle_center" + reference_point: "rear_axle_center_xy__z_above_ground" + rear_axle_height_above_ground_m: 0.294 installation: installation_id: "20260808_priority_windows" installed_at: "2026-08-08" notes: > - HI13R4 + H32 DLogCapture. CAD mounts are origins vs rear axle center - (translation only). LiDAR phase-center Z = CAD dZ + 63.5 mm. + HI13R4 + H32 DLogCapture. Body +X forward: LiDAR and IMU at positive X. + CAD sheet may draw +X rearward; numbers below are body-frame. + Z is height above ground = CAD height at axle + 0.294 m (axle AGL). + LiDAR phase-center AGL is the measured 1.9165 m (not CAD dZ+63.5). IMU axes: HI13R4 manual §2.4 RFU (X right, Y forward, Z up). - LiDAR Cartesian assumed body-aligned. + LiDAR Cartesian in NPZ assumed body-aligned (X forward). sensors: imu: @@ -26,14 +30,13 @@ sensors: axes: "X right, Y forward, Z up (RFU)" driver_axis_remapped: false mount_in_body: - # CAD 图二：后轮轴中心 → IMU，单位 m（mm/1000） - # dX=2574.126255, dY=36.5, dZ=892.5 - translation_m: [2.574126255, 0.0365, 0.8925] + # X/Y：后轮轴中心 → IMU；Z：离地 = CAD 0.8925 + 0.294 + translation_m: [2.574126255, 0.0365, 1.1865] # body <- imu : p_body = R_body_imu * p_imu # R_body_imu = [[0,1,0],[-1,0,0],[0,0,1]] (fwd=imu_y, left=-imu_x, up=imu_z) rotation_matrix_body_imu: [[0.0, 1.0, 0.0], [-1.0, 0.0, 0.0], [0.0, 0.0, 1.0]] rotation_quaternion_xyzw: null - source: "CAD dX/dY/dZ + HI13R4 manual RFU" + source: "CAD X/Y in body (+X forward); Z = CAD axle-height + 294mm AGL + HI13R4 RFU" lidar: model: "RSLidarH32" @@ -42,13 +45,11 @@ sensors: axes: "X forward, Y left, Z up (Cartesian metres in NPZ points)" driver_axis_remapped: false mount_in_body: - # CAD 图一：后轮轴中心 → 雷达安装点，再加相位中心 +63.5 mm（仅 Z） - # dX=2522.276859, dY=0.020526, dZ=1637.499879+63.5=1700.999879 - translation_m: [2.522276859, 0.000020526, 1.700999879] - # 假设雷达系与车体 CAD 轴一致（导出 XYZ 已按此约定） + # X/Y：后轮轴中心 → 雷达；Z：相位中心真实离地 1.9165 m + translation_m: [2.522276859, 0.000020526, 1.9165] rotation_matrix_body_lidar: [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]] rotation_quaternion_xyzw: null - source: "CAD dX/dY/dZ + phase-center +63.5mm on Z; attitude assumed = body" + source: "CAD X/Y in body (+X forward); Z = measured phase-center AGL 1.9165 m; attitude = body" rtk: frame_definition: "" @@ -66,18 +67,17 @@ time: # t_IMU_lidar = R_IMU_body * t_body, R_IMU_lidar = R_IMU_body * R_body_lidar derived_T_IMU_lidar_prior: R_IMU_lidar: [[0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0]] - t_IMU_lidar_m: [0.036479474, -0.051849396, 0.808499879] - t_lidar_from_imu_in_body_m: [-0.051849396, -0.036479474, 0.808499879] + t_IMU_lidar_m: [0.036479474, -0.051849396, 0.730] + t_lidar_from_imu_in_body_m: [-0.051849396, -0.036479474, 0.730] notes: > - Rotation prior is ~90 deg yaw between body/lidar (X-fwd) and IMU RFU (Y-fwd). - Translation prior from CAD + LiDAR phase-center offset; use for full_se3 / - sanity, not as hard lock for rotation_only. + Rotation prior ~90 deg yaw (body/lidar X-fwd vs IMU Y-fwd). + Relative Z = 1.9165 - 1.1865 = 0.730 m (was 0.8085 m with old CAD+63.5mm Z). initialization: translation_prior: enabled: true sigma_m: [0.05, 0.05, 0.05] - t_IMU_lidar_m: [0.036479474, -0.051849396, 0.808499879] + t_IMU_lidar_m: [0.036479474, -0.051849396, 0.730] rotation_prior: enabled: true sigma_deg: 15.0 diff --git a/docs/20260808_LiDAR-IMU标定现状与问题.md b/docs/20260808_LiDAR-IMU标定现状与问题.md new file mode 100644 index 0000000..db16b6e --- /dev/null +++ b/docs/20260808_LiDAR-IMU标定现状与问题.md @@ -0,0 +1,171 @@ +# 20260808 HI13 + H32：LiDAR–IMU 标定现状与问题 + +> 数据：`D:\data\calibration_usable_20260808` +> 可用会话：`sessions_v1_host_aligned`（三优先窗） +> 当前结果目录：各窗 `out_fixed_dt0/` +> 清单：`sessions_v1_host_aligned/calibration_manifest_fixed_dt0.json` +> 车辆配置：`config/vehicle_hi13_h32_20260808.yaml` +> 约定外参：`p_IMU = T_IMU_lidar · p_lidar` + +--- + +## 1. 一句话结论 + +**旋转 + 主机桥接时间对齐可以冻结；平移（full_se3）尚不可正式交付。** +三窗 `rotation_only`（δt=0）结果跨窗一致，**不必因平移先验 Z 修正而重跑旋转**。 + +--- + +## 2. 当前可用结果（`out_fixed_dt0`） + +| 窗 | 状态 | δt | yaw (°) | 手眼 RMS (°) | 手眼对数 | vs CAD prior | +|----|------|----|---------|--------------|----------|--------------| +| `priority_174005_174515` | `rotation_only_accepted` | 0 | ≈90.00 | 0.62 | 1374 | ≈0.41° | +| `priority_174905_175450` | 同上 | 0 | ≈90.00 | 0.29 | 1182 | ≈0.47° | +| `priority_175910_180530` | 同上 | 0 | ≈90.01 | 0.79 | 789 | ≈0.38° | + +- 跨窗旋转互差约 **0.15°–0.47°**（相对三窗均值 ≤0.26°）。 +- 交付矩阵中 **平移为 0**（`translation_accepted=false`）。 +- CAD/安装平移先验只用于后续 SE3 / 校验，不写入本轮交付 `T`。 + +### 相对历史失败轮次 + +| 轮次 | 问题 | 结果 | +|------|------|------| +| `sessions_v1_aligned` | 首帧强行对齐设备钟 | 三窗手眼失败，RMS ~9°–12° | +| 自由估 δt + signed refine | 窗3 δt 漂到 −0.48 s；窗2 yaw≈19° | 跨窗 yaw 矛盾（81°/19°/93°） | +| **本轮 fixed δt=0** | 主机桥接后冻结时间 | 三窗 yaw≈90°，可互证 | + +--- + +## 3. 已澄清并写入配置的坐标系 / 先验 + +### 3.1 车体与传感器 + +- 车体：X 前 / Y 左 / Z 上；雷达与 IMU 安装在 **X 正方向**（后轮轴前方）。 +- CAD 图纸可能画成 +X 朝后，那只是读图坐标系，**不是**车体真实轴。 +- IMU：HI13 RFU（X 右 / Y 前 / Z 上），原始数据不做轴向重映射。 +- 雷达 NPZ：假定与车体一致（X 前 / Y 左 / Z 上）。 + +### 3.2 安装量（`translation_m`） + +| 传感器 | X / Y（后轮轴中心） | Z（离地） | +|--------|---------------------|-----------| +| IMU | 2.574 / 0.0365 m | 0.8925 + 0.294 = **1.1865 m** | +| 雷达 | 2.522 / 0.00002 m | 相位中心实测 **1.9165 m** | + +- 后轮轴中心离地：**294 mm**（Z 用离地高时加在 CAD 轴心高上）。 +- 雷达相位中心以实测 1.9165 m 为准（不再用「CAD dZ + 63.5 mm」作为最终离地高）。 + +### 3.3 导出外参先验 + +- `R_IMU_lidar` ≈ yaw 90°：`[[0,-1,0],[1,0,0],[0,0,1]]`（软约束 σ=15°）。 +- `t_IMU_lidar` ≈ **`[0.0365, -0.0518, 0.730]` m**（相对 Z = 1.9165 − 1.1865）。 +- **旋转先验未因 Z 修正改变**；仅平移先验 Z 从旧值 0.8085 改为 0.730。 + +--- + +## 4. 现存问题清单 + +### P1. IMU 预积分平移 `Δp` 不可用（阻塞正式平移） + +- 现象：可视化模式 4 若用完整 `X⁻¹ A X`，橙/蓝点云常呈**上下错层**（Z 差米级～几十米）。 +- 根因：加速度预积分缺少可靠重力/零偏处理，`t_A` 尤其 Z 发散；**不是旋转外参错了**。 +- 旁证：相对 GICP 的旋转残差中位约 0.16°；`|t_A|` 中位却常 >1 m。 +- 影响：`full_se3` / 依赖 IMU 位移的平移估计不可信。 +- 缓解（已做）：`visualize_pair_3d.py` 对 `rotation_only` 默认模式 4 = **R 共轭 + GICP 的 t_B**（`--mode4-translation gicp|imu|auto`）。 + +### P2. 平面运动导致竖直平移弱可观 + +- 三优先窗以水平转弯为主，缺少缓坡/俯仰激励。 +- 流水线门控已给出 `translation_accepted=false`。 +- 即使打开平移先验（σ≈5 cm），弱激励下结果易变成**先验回显**，不宜当标定成功。 + +### P3. 时间偏移若再自由估计会被带偏（已规避，需保持） + +- 主机 UTC 桥接（MSOP/IMU `HostReceiveUtc`）后，两路已在同一时间轴，残差通常几十毫秒量级。 +- 若再做有符号 δt 精修，会与错误/未收敛的 R 耦合，窗3 曾从约 −0.12 s 走到 **−0.48 s**。 +- **现行做法**：桥接会话使用 `--fixed-time-offset-s 0 --no-signed-time-refine`。 + +### P4. 单窗低残差 ≠ 外参正确（历史教训） + +- 自由 δt 轮次中，窗2 手眼 RMS 最低（~0.3°）但 yaw≈19°，与 CAD/其他窗差 60°+。 +- 平面运动下 yaw 外参可出现多个能拟合 `R_A R_X ≈ R_X R_B` 的解。 +- **必须**做跨窗一致性 + 可视化叠点，不能只看单窗 RMS。 + +### P5. 旋转软先验尚未做无先验对照 + +- 当前 σ=15°；笔记显示 Tsai 初值本身已接近（约 0.3°–1.1° RMS），不像纯先验硬拽。 +- 仍缺一次：关闭先验或放大 `sigma_deg` 的对照，以排除「只是被拉到 90°」的疑虑。 + +### P6. 环境/导入陷阱（工程） + +- 本机若存在指向其他仓库的 editable 安装（如 `Lidar-IMU`），`python tools\...` 可能导入错误包。 +- `visualize_pair_3d.py` 已插入仓库根到 `sys.path`；长期仍建议在本仓库 `pip install -e .`。 + +### P7. 文档与操作约定未完全同步（工程） + +- README 需明确写清：host-bridge 后固定 δt=0、禁用 signed refine、rotation_only 可视化用法。 +- 交付物目前缺一版「冻结的联合/中位 R + 使用说明」JSON/报告（旋转可交，平移明确不交）。 + +--- + +## 5. 不该做 / 可以做 + +| 动作 | 建议 | +|------|------| +| 因 Z 先验修正重跑三窗 rotation_only | **不必**（R 未依赖新 t） | +| 正式交付 6-DOF / 信赖当前 `Δp` 估 t | **不要** | +| 试验性 `full_se3`（固定 R、δt=0、新 t 先验） | 可做，结果标「实验」 | +| 可视化验收模式 3 vs 4（gicp 平移） | **建议做** | +| 无先验 / 大 σ 旋转对照 | **建议做** | +| 冻结交付 `R` + `δt=0` 说明 | **建议做** | +| 补采缓坡或加强垂直尺寸约束后再估 t | 正式平移前需要 | + +--- + +## 6. 建议下一步顺序 + +1. **验收旋转**：三窗抽转弯运动对，模式 3/4 叠点；可选无先验对照。 +2. **定稿旋转**：三窗中位或联合手眼 → 交付 `R_IMU_lidar` +「δt=0（主机桥接）」说明；**明确不交 t**。 +3. **工程收尾**：README 主机桥接配方；需要时再整理联合标定脚本入口。 +4. **平移（靠后）**：改善 IMU 位移模型或改用更可靠的位移观测 + 竖直激励后，再用新 `t` 先验跑 SE3。 + +--- + +## 7. 常用路径与命令 + +```text +数据根： + D:\data\calibration_usable_20260808\sessions_v1_host_aligned\ + +结果： + ...\priority_XXXX\out_fixed_dt0\summary.json + ...\priority_XXXX\out_fixed_dt0\motion_pairs.json + ...\calibration_manifest_fixed_dt0.json +``` + +```powershell +# 可视化（rotation_only 默认模式4用 GICP 平移） +python tools\visualize_pair_3d.py ` + --lidar D:\data\calibration_usable_20260808\sessions_v1_host_aligned\priority_174005_174515\lidar ` + --summary D:\data\calibration_usable_20260808\sessions_v1_host_aligned\priority_174005_174515\out_fixed_dt0\summary.json ` + --pair-index 0 + +# 若要看「坏 Δp」导致的错层效果： +# --mode4-translation imu +``` + +--- + +## 8. 问题优先级（跟踪用） + +| ID | 严重度 | 状态 | 标题 | +|----|--------|------|------| +| P1 | 高 | 未解决 | IMU `Δp` 不可用，阻塞正式平移 | +| P2 | 高 | 未解决 | 平面运动，竖直 t 弱可观 | +| P3 | 高 | 已规避 | 自由 δt / signed refine 带偏（需保持冻结） | +| P4 | 中 | 已吸收教训 | 单窗低残差不可单独验收 | +| P5 | 中 | 待做 | 无旋转先验对照 | +| P6 | 低 | 部分修复 | 错误 `imu_lidar` 包导入 | +| P7 | 低 | 待做 | README/交付物同步 | diff --git a/tools/visualize_pair_3d.py b/tools/visualize_pair_3d.py index 74cb217..5bd0b18 100644 --- a/tools/visualize_pair_3d.py +++ b/tools/visualize_pair_3d.py @@ -9,6 +9,8 @@ Modes (keyboard): 2 IMU prediction with X=I (B_pred = A) 3 LiDAR registration B (reference) 4 calibrated prediction B_pred = X^{-1} A X + (rotation_only runs default to R conjug + t_B so bad IMU Δp + does not dominate the overlay) N / ] next motion pair P / [ previous motion pair Q / Esc exit @@ -20,10 +22,16 @@ from __future__ import annotations import argparse import json +import sys from pathlib import Path +from typing import Any import numpy as np +ROOT = Path(__file__).resolve().parents[1] +if str(ROOT) not in sys.path: + sys.path.insert(0, str(ROOT)) + from imu_lidar.geometry import ( inverse_transform, make_transform, @@ -80,24 +88,45 @@ MODE_NAMES = ( ) -def _load_extrinsic(summary_path: Path) -> tuple[np.ndarray, float, np.ndarray]: +def _load_extrinsic(summary_path: Path) -> tuple[np.ndarray, float, np.ndarray, dict[str, Any]]: summary = json.loads(summary_path.read_text(encoding="utf-8")) t_block = summary.get("T_IMU_lidar") + meta: dict[str, Any] = { + "rotation_only": False, + "translation_accepted": False, + "status": str(summary.get("status") or ""), + } if t_block is None: matrix = summary.get("matrix") if matrix is not None: - return np.asarray(matrix, dtype=float), 0.0, np.zeros(3) + return np.asarray(matrix, dtype=float), 0.0, np.zeros(3), meta raise ValueError(f"no T_IMU_lidar in {summary_path}") t_mat = np.asarray(t_block["matrix"], dtype=float) dt = float(summary.get("time_offset_s") or 0.0) session = (summary.get("details") or {}).get("sessions", [{}])[0] + joint = session.get("joint") or {} bias = np.asarray( (session.get("imu_audit") or {}).get("gyro_bias_rad_s") - or (session.get("joint") or {}).get("gyro_bias_rad_s") + or joint.get("gyro_bias_rad_s") or [0.0, 0.0, 0.0], dtype=float, ).reshape(3) - return t_mat, dt, bias + status = str(summary.get("status") or "") + translation_accepted = bool( + joint.get("translation_accepted") + or (summary.get("details") or {}).get("joint", {}).get("translation_accepted") + ) + rotation_only = ("rotation_only" in status) or ( + not translation_accepted and float(np.linalg.norm(t_mat[:3, 3])) < 1e-9 + ) + meta.update( + { + "rotation_only": rotation_only, + "translation_accepted": translation_accepted, + "status": status, + } + ) + return t_mat, dt, bias, meta def _delta_components(reference: np.ndarray, candidate: np.ndarray) -> dict: @@ -196,16 +225,42 @@ def _pair_from_indices( return frame_i, frame_j, a, reg.transform -def _transforms_for_pair(x: np.ndarray, a_ij: np.ndarray, b_gicp: np.ndarray) -> dict[str, np.ndarray]: +def _transforms_for_pair( + x: np.ndarray, + a_ij: np.ndarray, + b_gicp: np.ndarray, + *, + mode4_translation: str = "imu", +) -> dict[str, np.ndarray]: + """Build overlay transforms. + + ``mode4_translation``: + - ``imu``: full SE3 conjug ``X^{-1} A X`` (needs trustworthy IMU Δp) + - ``gicp``: rotation conjug only; translation taken from LiDAR B + (correct check for rotation_only calibrations) + """ + + calibrated = inverse_transform(x) @ a_ij @ x + if mode4_translation == "gicp": + calibrated = make_transform(b_gicp[:3, 3], calibrated[:3, :3]) + elif mode4_translation != "imu": + raise ValueError(f"unknown mode4_translation={mode4_translation!r}") return { MODE_NAMES[0]: np.eye(4), MODE_NAMES[1]: a_ij.copy(), MODE_NAMES[2]: b_gicp.copy(), - MODE_NAMES[3]: inverse_transform(x) @ a_ij @ x, + MODE_NAMES[3]: calibrated, } -def _resolve_pair(frames, pairs, pair_index: int, x: np.ndarray): +def _resolve_pair( + frames, + pairs, + pair_index: int, + x: np.ndarray, + *, + mode4_translation: str = "imu", +): pair = pairs[pair_index] frame_i = frames[pair.i] frame_j = frames[pair.j] @@ -217,7 +272,9 @@ def _resolve_pair(frames, pairs, pair_index: int, x: np.ndarray): pair.t_B_m if pair.t_B_m is not None else np.zeros(3), pair.R_B, ) - transforms = _transforms_for_pair(x, a_ij, b_gicp) + transforms = _transforms_for_pair( + x, a_ij, b_gicp, mode4_translation=mode4_translation + ) label = ( f"pair {pair_index + 1}/{len(pairs)} " f"frames {pair.i} <- {pair.j} " @@ -282,12 +339,15 @@ def _run_gui( start_index: int, voxel: float, fixed_single_pair: tuple | None, + mode4_translation: str = "imu", ) -> None: import open3d as o3d if fixed_single_pair is not None: frame_i, frame_j, a_ij, b_gicp = fixed_single_pair - transforms = _transforms_for_pair(x, a_ij, b_gicp) + transforms = _transforms_for_pair( + x, a_ij, b_gicp, mode4_translation=mode4_translation + ) label = f"fixed frames (no pair switching)" pair_index = 0 n_pairs = 1 @@ -297,7 +357,7 @@ def _run_gui( n_pairs = len(pairs) use_list = True frame_i, frame_j, a_ij, b_gicp, transforms, label = _resolve_pair( - frames, pairs, pair_index, x + frames, pairs, pair_index, x, mode4_translation=mode4_translation ) viewer = o3d.visualization.VisualizerWithKeyCallback() @@ -338,7 +398,7 @@ def _run_gui( return new_index = int(new_index) % n_pairs frame_i, frame_j, _a, b_gicp, transforms, label = _resolve_pair( - frames, pairs, new_index, x + frames, pairs, new_index, x, mode4_translation=mode4_translation ) state["pair_index"] = new_index state["transforms"] = transforms @@ -429,9 +489,22 @@ def main(argv: list[str] | None = None) -> int: action="store_true", help="Skip Open3D window (use with --save-png)", ) + parser.add_argument( + "--mode4-translation", + choices=("auto", "gicp", "imu"), + default="auto", + help=( + "Mode-4 translation source: gicp=R conjug + t_B (rotation check); " + "imu=full X^-1 A X; auto=gicp for rotation_only summaries" + ), + ) args = parser.parse_args(argv) - x, delta_t_s, gyro_bias = _load_extrinsic(args.summary) + x, delta_t_s, gyro_bias, extr_meta = _load_extrinsic(args.summary) + if args.mode4_translation == "auto": + mode4_translation = "gicp" if extr_meta.get("rotation_only") else "imu" + else: + mode4_translation = args.mode4_translation cache_path = args.motion_pairs or resolve_motion_pairs_path(args.summary) use_cache = (not args.rebuild_pairs) and cache_path is not None and args.frame_i is None @@ -456,7 +529,7 @@ def main(argv: list[str] | None = None) -> int: frames = _LazyFrameStore(args.lidar) pairs = tuple(pair_list) frame_i, frame_j, a_ij, b_gicp, transforms, label = _resolve_pair( - frames, pairs, args.pair_index, x + frames, pairs, args.pair_index, x, mode4_translation=mode4_translation ) print(f"loaded {len(pairs)} cached pairs from {cache_path}") else: @@ -479,7 +552,9 @@ def main(argv: list[str] | None = None) -> int: delta_t_s=delta_t_s, gyro_bias=gyro_bias, ) - transforms = _transforms_for_pair(x, a_ij, b_gicp) + transforms = _transforms_for_pair( + x, a_ij, b_gicp, mode4_translation=mode4_translation + ) label = f"frames {args.frame_i} <- {args.frame_j}" fixed_single_pair = (frame_i, frame_j, a_ij, b_gicp) pairs = () @@ -493,10 +568,26 @@ def main(argv: list[str] | None = None) -> int: ) pairs = pair_set.pairs frame_i, frame_j, a_ij, b_gicp, transforms, label = _resolve_pair( - frames, pairs, args.pair_index, x + frames, pairs, args.pair_index, x, mode4_translation=mode4_translation ) print(f"rebuilt {len(pairs)} pairs from {len(keyframes.indices)} keyframes") + print( + f"mode4 translation={mode4_translation} " + f"(status={extr_meta.get('status') or 'n/a'}, " + f"rotation_only={bool(extr_meta.get('rotation_only'))})" + ) + if mode4_translation == "gicp": + print( + "note: mode4 uses R conjug + t_B; IMU Δp is ignored " + "(typical for rotation_only — raw Δp often has large Z drift)." + ) + if mode4_translation == "imu": + print( + "note: mode4 uses full X^-1 A X. If clouds stack vertically, " + "IMU Δp is likely bad; retry with --mode4-translation gicp." + ) + if args.save_png is not None: _print_pair_header(label, b_gicp, transforms) _save_topdown_png(args.save_png, frame_i.points_xyz, frame_j.points_xyz, transforms) @@ -520,6 +611,7 @@ def main(argv: list[str] | None = None) -> int: start_index=args.pair_index, voxel=args.voxel, fixed_single_pair=fixed_single_pair, + mode4_translation=mode4_translation, ) return 0