From 2237be77a4ea44f92cf78cfa53e28c73fdf6981d Mon Sep 17 00:00:00 2001 From: "lichun.qu" <16975270+zzqlc@user.noreply.gitee.com> Date: Mon, 10 Aug 2026 13:26:32 +0800 Subject: [PATCH] =?UTF-8?q?=E6=94=AF=E6=8C=81=20HI13/H32=20=E4=B8=BB?= =?UTF-8?q?=E6=9C=BA=20UTC=20=E6=A1=A5=E6=8E=A5=E5=AF=B9=E9=BD=90=E3=80=81?= =?UTF-8?q?=E5=A4=9A=E4=BC=9A=E8=AF=9D=E8=81=94=E5=90=88=E6=A0=87=E5=AE=9A?= =?UTF-8?q?=E4=B8=8E=20CAD=20=E5=B9=B3=E7=A7=BB=E5=85=88=E9=AA=8C=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Cursor --- README.md | 9 +- config/vehicle_hi13_h32_20260808.yaml | 80 +++++ docs/V1_数据格式.md | 40 ++- imu_lidar/CHANGELOG.md | 14 + imu_lidar/cli.py | 60 +++- imu_lidar/joint_optimizer.py | 108 +++++-- imu_lidar/pipeline.py | 251 +++++++++------ imu_lidar/time_offset.py | 12 +- tests/test_h32_dlog_export.py | 16 +- tests/test_hi13_imu.py | 79 +++++ tools/export_rscap_to_v1.py | 275 ++++++++++++---- tools/export_usable_20260808_windows.py | 125 ++++++++ tools/h32_dlog/__init__.py | 8 +- tools/h32_dlog/dobject.py | 364 +++++++++++++++++----- tools/h32_dlog/load_session.py | 155 +++++---- tools/h32_dlog/timeutil.py | 56 ++++ tools/rscap_v2/h32_msop.py | 47 ++- tools/rscap_v2/hi13_imu.py | 136 ++++++++ tools/run_joint_three_windows_full_se3.py | 50 +++ tools/run_priority_windows_calibration.py | 292 +++++++++++++++++ 20 files changed, 1830 insertions(+), 347 deletions(-) create mode 100644 config/vehicle_hi13_h32_20260808.yaml create mode 100644 tests/test_hi13_imu.py create mode 100644 tools/export_usable_20260808_windows.py create mode 100644 tools/h32_dlog/timeutil.py create mode 100644 tools/rscap_v2/hi13_imu.py create mode 100644 tools/run_joint_three_windows_full_se3.py create mode 100644 tools/run_priority_windows_calibration.py diff --git a/README.md b/README.md index 1f7623e..ac2fa51 100644 --- a/README.md +++ b/README.md @@ -44,12 +44,15 @@ p_IMU = T_IMU_lidar · p_lidar | `summary.json` | 状态、残差、可观性 | -新车原始数据导出（H32 dlog + N300 rscap）： +新车原始数据导出（H32 dlog/zip + HI13 rscap）： ```powershell python tools\export_rscap_to_v1.py ` - --imu-rscap path\to\n300.rscap ` - --lidar-dlog path\to\session_or_dlog ` + --imu-rscap path\to\hi13r4-imu.rscap ` + --imu-kind hi13 ` + --lidar-dlog path\to\session_or_recovered.zip ` + --host-start 2026-08-08T17:40:05 ` + --host-end 2026-08-08T17:45:15 ` --out path\to\session_v1 ` --require-difop ``` diff --git a/config/vehicle_hi13_h32_20260808.yaml b/config/vehicle_hi13_h32_20260808.yaml new file mode 100644 index 0000000..aa1e27f --- /dev/null +++ b/config/vehicle_hi13_h32_20260808.yaml @@ -0,0 +1,80 @@ +schema_version: 1 + +vehicle: + vehicle_id: "outdoor_usable_20260808" + body_frame: + name: "base_link" + # CAD / 后轮轴中心测量系（与安装图 dX/dY/dZ 一致） + axes: "X forward, Y left, Z up" + unit: m + reference_point: "rear_axle_center" + +installation: + installation_id: "20260808_priority_windows" + installed_at: "2026-08-08" + notes: > + HI13R4 + H32 DLogCapture. CAD mounts are origins vs rear axle center only + (translation). IMU axes confirmed on vehicle as HI13R4 manual §2.4 RFU + (X right, Y forward, Z up). LiDAR Cartesian assumed body-aligned. + +sensors: + imu: + model: "HI13R4" + raw_frame: + # HI13R4 用户手册 2.4：右-前-上 (RFU) + axes: "X right, Y forward, Z up (RFU)" + driver_axis_remapped: false + mount_in_body: + # CAD 原点相对后轮轴中心（body: X fwd, Y left, Z up），单位 m + translation_m: [2.574126255, 0.0365, 0.8925] + # 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" + + lidar: + model: "RSLidarH32" + points_field: points + raw_frame: + axes: "X forward, Y left, Z up (Cartesian metres in NPZ points)" + driver_axis_remapped: false + mount_in_body: + translation_m: [2.522276859, 0.000020526, 1.637499879] + # 假设雷达系与车体 CAD 轴一致（导出 XYZ 已按此约定） + 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 vs rear axle; attitude assumed = body" + + rtk: + frame_definition: "" + reference_point: "" + existing_T_RTK_LIDAR_file: "" + +time: + imu_timestamp_source: "hi13_device_timestamp_ms_seconds" + lidar_timestamp_source: "h32_msop_device_timestamp_seconds" + lidar_frame_time_definition: "t_start/t_end in frames_index.csv; pipeline uses midpoint" + host_bridge: "MSOP HostReceiveUtcTicks + IMU receive_utc_ticks" + +# Derived prior for p_IMU = R_IMU_lidar * p_lidar + t_IMU_lidar +# t_body = t_lidar_body - t_imu_body +# 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.744999879] + t_lidar_from_imu_in_body_m: [-0.051849396, -0.036479474, 0.744999879] + notes: > + Rotation prior is ~90 deg yaw between body/lidar (X-fwd) and IMU RFU (Y-fwd). + Translation prior from CAD only; use for full_se3 / sanity, not as hard lock + for rotation_only. + +initialization: + translation_prior: + enabled: true + sigma_m: [0.05, 0.05, 0.05] + t_IMU_lidar_m: [0.036479474, -0.051849396, 0.744999879] + rotation_prior: + enabled: true + sigma_deg: 15.0 + R_IMU_lidar: [[0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0]] diff --git a/docs/V1_数据格式.md b/docs/V1_数据格式.md index 2f2da4b..62289ea 100644 --- a/docs/V1_数据格式.md +++ b/docs/V1_数据格式.md @@ -4,20 +4,24 @@ ## 从原始数据导出 -**推荐（新 H32 插件 `RSLidarH32_3D_DLogCaptureNet48`）：** N300 `.rscap` + 雷达 Medulla dlog（含 raw MSOP / DIFOP）。 +**推荐（新 H32 + HI13）：** HI13 `.rscap` + 雷达 Medulla dlog / recovered zip（raw MSOP + DIFOP）。 ```powershell python tools\export_rscap_to_v1.py ` - --imu-rscap path\to\n300.rscap ` - --lidar-dlog path\to\session_or_dlog ` + --imu-rscap path\to\hi13r4-imu.rscap ` + --imu-kind hi13 ` + --lidar-dlog path\to\session_or_dlog_or_recovered.zip ` + --host-start 2026-08-08T17:40:05 ` + --host-end 2026-08-08T17:45:15 ` --out path\to\session_v1 ` - --frame-stride 1 ` + --frame-stride 5 ` --require-difop ``` -`--lidar-dlog` 指向含 `dobject/` + `dobject_recording/` 的目录（或其上级含 `dlog/` 子目录亦可）。 -默认 DObject：`frontlidar-msop-raw`、`frontlidar-difop-raw`（可用 `--msop-object` / `--difop-object` 覆盖）。 -有 DIFOP 时用设备通道角做 XYZ；`--require-difop` 在缺少有效 DIFOP 时直接失败。 +`--lidar-dlog` 可为：标准 `dobject/`+`dobject_recording/` 目录，或 recovered zip（`indices.log` + `data.bin`）。 +`--imu-kind`：`hi13` / `n300` / `auto`（默认按文件名推断）。 +`--host-start/end`：按本地墙钟切窗（仅裁剪；标定主轴仍是设备时间）。 +默认 DObject：`frontlidar-msop-raw`、`frontlidar-difop-raw`。 **兼容旧 MSOP-only `.rscap`：** @@ -30,7 +34,7 @@ python tools\export_rscap_to_v1.py ` ``` 产出：`imu.csv`、`lidar/`（含 `frames_index.csv`）、`export_summary.json`。 -时间轴为**设备时间**：N300 `device_timestamp_us`→秒；H32 MSOP 设备时间戳→秒。主机接收时间不写入标定主轴。 +标定主轴仍是**设备时间**；同时写出**主机 UTC 接收时间**，用于把雷达帧桥接到 IMU 设备钟（禁止把两边设备时间第一帧强行重合）。 ## IMU @@ -39,16 +43,18 @@ python tools\export_rscap_to_v1.py ` ### CSV ```text -t,gx,gy,gz,ax,ay,az -0.000000000,0.01,-0.02,0.00,0.05,-0.03,9.81 +t,gx,gy,gz,ax,ay,az,t_host_utc_s,receive_utc_ticks +0.000000000,0.01,-0.02,0.00,0.05,-0.03,9.81,1754646005.123,6389... ... ``` | 列 | 含义 | 单位 | |---|---|---| -| t | IMU 时钟时间 | s | +| t | IMU 设备时钟时间 | s | | gx,gy,gz | 角速度 | rad/s | | ax,ay,az | 比力/加速度 | m/s² | +| t_host_utc_s | 主机 UTC 接收时间（Unix） | s | +| receive_utc_ticks | 同上，.NET UTC ticks | — | ### NPZ @@ -72,12 +78,16 @@ lidar_session/ ### frames_index.csv ```text -frame_id,filename,t_start,t_end -0,frames/frame_00000.npz,10.000,10.100 -1,frames/frame_00001.npz,10.100,10.200 +frame_id,filename,t_start,t_end,host_receive_utc_ticks,t_host_utc_s,host_receive_utc_end_ticks,t_host_utc_end_s +0,frames/frame_00000.npz,10.000,10.100,6389...,1754646005.12,6389...,1754646005.22 ``` -也兼容旧列名 `file`（NumPy 读取时可能变成 `file_`）。 +| 列 | 含义 | +|---|---| +| t_start / t_end | H32 MSOP **设备时间**（秒） | +| t_host_utc_s / t_host_utc_end_s | 帧首/末包 **HostReceiveUtcTicks** → Unix 秒 | + +也兼容旧列名 `file`。对齐脚本用主机 UTC 把 `t_*` 重写到 IMU 设备钟后再跑标定。 ### 每帧 NPZ diff --git a/imu_lidar/CHANGELOG.md b/imu_lidar/CHANGELOG.md index 8ee4535..c2ba2a5 100644 --- a/imu_lidar/CHANGELOG.md +++ b/imu_lidar/CHANGELOG.md @@ -5,6 +5,20 @@ --- +## 2026-08-09 14:30 (UTC+8) + +### 导出：HI13 IMU + recovered dlog zip + 墙钟切窗 + +- **原本**：IMU 只解 N300 FDILink；dlog 只认标准 `*.dorec`；无法按图上时段切窗。 +- **改成**： + - 新增 `tools/rscap_v2/hi13_imu.py`（HI91：g→m/s²、°/s→rad/s、设备 ms）。 + - `h32_dlog` 支持 recovered zip（`indices.log` + `data.bin`），ZIP_STORED 成员按文件绝对 offset 直读。 + - `export_rscap_to_v1.py`：`--imu-kind hi13|n300|auto`、多段 `--imu-rscap`、`--host-start/end` 切窗。 + - 辅助脚本 `tools/export_usable_20260808_windows.py` 导出优先运动段。 +- **未推送**（按用户要求本地改完即可）。 + +--- + ## 2026-08-05 09:00 (UTC+8) ### 导出：支持 H32 DLogCapture（MSOP+DIFOP）→ V1 diff --git a/imu_lidar/cli.py b/imu_lidar/cli.py index e5011ff..7fa0bbd 100644 --- a/imu_lidar/cli.py +++ b/imu_lidar/cli.py @@ -21,10 +21,28 @@ def build_parser() -> argparse.ArgumentParser: 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 = subcommands.add_parser( + "run", + help="执行 V1 标定流水线（可重复 --imu/--lidar/--session-id 做多会话联合）", + ) + run.add_argument( + "--session-id", + action="append", + default=None, + help="会话 ID（可重复；与 --imu/--lidar 一一对应）", + ) + run.add_argument( + "--imu", + action="append", + required=True, + help="IMU CSV/NPZ 路径（可重复）", + ) + run.add_argument( + "--lidar", + action="append", + required=True, + help="LiDAR 会话目录（可重复）", + ) run.add_argument("--vehicle-config", required=True, help="车辆配置 YAML") run.add_argument("--output", required=True, help="输出目录") run.add_argument( @@ -39,6 +57,25 @@ def build_parser() -> argparse.ArgumentParser: return parser +def _build_sessions(args: argparse.Namespace) -> tuple[SessionInput, ...]: + imus = [Path(p) for p in args.imu] + lidars = [Path(p) for p in args.lidar] + if len(imus) != len(lidars): + raise SystemExit(f"--imu count ({len(imus)}) must match --lidar count ({len(lidars)})") + if args.session_id is None: + session_ids = [f"session{i}" for i in range(len(imus))] + else: + session_ids = list(args.session_id) + if len(session_ids) != len(imus): + raise SystemExit( + f"--session-id count ({len(session_ids)}) must match --imu/--lidar ({len(imus)})" + ) + return tuple( + SessionInput(session_id=sid, imu_source=imu, lidar_source=lidar) + for sid, imu, lidar in zip(session_ids, imus, lidars) + ) + + def main(argv: list[str] | None = None) -> int: parser = build_parser() args = parser.parse_args(argv) @@ -55,15 +92,10 @@ def main(argv: list[str] | None = None) -> int: return 0 if args.command == "run": + sessions = _build_sessions(args) 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), - ), - ), + sessions=sessions, requested_mode=CalibrationMode(args.mode), output_directory=Path(args.output), max_iterations=args.max_iterations, @@ -75,7 +107,11 @@ def main(argv: list[str] | None = None) -> int: 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}") + print(f"time_offset_s (first session; t_imu = t_lidar + dt): {result.time_offset_s:.6f}") + joint = (result.details or {}).get("joint") or {} + if joint: + print(f"merged_pair_count: {joint.get('merged_pair_count')}") + print(f"pair_counts_per_session: {joint.get('pair_counts_per_session')}") if result.T_IMU_lidar is not None: print("T_IMU_lidar:") print(result.T_IMU_lidar) diff --git a/imu_lidar/joint_optimizer.py b/imu_lidar/joint_optimizer.py index 966dab5..3cbfafa 100644 --- a/imu_lidar/joint_optimizer.py +++ b/imu_lidar/joint_optimizer.py @@ -152,21 +152,21 @@ def _build_nav_rotations( r_x: np.ndarray, t_x: np.ndarray, ) -> list[np.ndarray]: - """Chain IMU orientations in the first-keyframe nav frame using LiDAR+extrinsic.""" + """Chain IMU orientations; restart at session/gap boundaries (no cross-link).""" + del id_to_idx 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] + # Missing link or new session: start a fresh nav chain. + rotations[k + 1] = np.eye(3) 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 @@ -178,6 +178,9 @@ def _solve_phase_c_se3( gravity_init: np.ndarray, sigma_bg_rw: float = 1.0e-5, sigma_ba_rw: float = 1.0e-3, + t_init: np.ndarray | None = None, + t_prior: np.ndarray | None = None, + t_prior_sigma_m: np.ndarray | float | None = None, ) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray, float, float, list[str]]: """Keyframe IMU factor optimization for full SE(3).""" @@ -185,21 +188,36 @@ def _solve_phase_c_se3( 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 + t0 = np.zeros(3) if t_init is None else np.asarray(t_init, dtype=float).reshape(3) + return r_x, t0, gravity_init, gyro_bias0, np.zeros(3), 1e9, 1e9, notes - # Unique keyframes sorted by IMU time. + # Keyframes: group by session, sort each session by IMU time (no cross-session chain). stamp: dict[int, float] = {} + kf_session: dict[int, str] = {} 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]) + kf_session[pair.i] = pair.session_id + kf_session[pair.j] = pair.session_id + session_ids = sorted(set(kf_session.values())) + keyframe_ids: list[int] = [] + for sid in session_ids: + local = [kid for kid, sess in kf_session.items() if sess == sid] + local.sort(key=lambda kid: stamp[kid]) + keyframe_ids.extend(local) 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 kf_session.get(pair.i) != kf_session.get(pair.j): + continue if id_to_idx[pair.j] == id_to_idx[pair.i] + 1: consecutive_pairs[(pair.i, pair.j)] = pair + notes.append( + f"phase-C multi-session graph: sessions={len(session_ids)}, " + f"keyframes={k_count}, consecutive_links={len(consecutive_pairs)}" + ) g0 = np.asarray(gravity_init, dtype=float).reshape(3) if np.linalg.norm(g0) < 1e-6: @@ -214,6 +232,15 @@ def _solve_phase_c_se3( n_b = 3 * k_count dim = 3 + 3 + 2 + n_v + n_b + n_b x0 = np.zeros(dim) + t0 = np.zeros(3) if t_init is None else np.asarray(t_init, dtype=float).reshape(3) + x0[3:6] = t0 + t_prior_vec = None if t_prior is None else np.asarray(t_prior, dtype=float).reshape(3) + if t_prior_sigma_m is None: + t_sigma = np.array([0.05, 0.05, 0.05], dtype=float) + else: + t_sigma = np.asarray(t_prior_sigma_m, dtype=float).reshape(-1) + if t_sigma.size == 1: + t_sigma = np.full(3, float(t_sigma[0]), dtype=float) # 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 @@ -269,18 +296,28 @@ def _solve_phase_c_se3( w = np.sqrt(_pair_weight(pair)) out.append(w * (whiten @ err)) - # Bias random-walk between consecutive keyframes. + # Bias random-walk between consecutive keyframes (same session only). for k in range(k_count - 1): - dt = max(stamp[keyframe_ids[k + 1]] - stamp[keyframe_ids[k]], 1e-3) + a = keyframe_ids[k] + b = keyframe_ids[k + 1] + if kf_session.get(a) != kf_session.get(b): + continue + dt = max(stamp[b] - stamp[a], 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 + # Weak priors: first keyframe of each session + CAD/installation translation. + for sid in session_ids: + first = next(kid for kid in keyframe_ids if kf_session[kid] == sid) + idx0 = id_to_idx[first] + out.append(50.0 * (bgs[idx0] - bg0)) + out.append(20.0 * bas[idx0]) + if t_prior_vec is not None: + out.append((t_opt - t_prior_vec) / np.maximum(t_sigma, 1e-3)) + else: + out.append(0.2 * t_opt) # soft |t|~0 prior when no CAD prior return np.concatenate(out) # Cap evaluations: Phase-C is high-dimensional; synthetic ICP already dominates runtime. @@ -331,6 +368,9 @@ def solve_joint_extrinsic( gravity_init_m_s2: np.ndarray | None = None, bias_prior_sigma_rad_s: float = 0.02, enable_phase_c: bool | None = None, + t_init_m: np.ndarray | None = None, + t_prior_m: np.ndarray | None = None, + t_prior_sigma_m: np.ndarray | float | None = None, ) -> JointExtrinsicResult: """Refine extrinsic using Phase-A whitened rotation factors, optional Phase-C SE(3).""" @@ -344,6 +384,7 @@ def solve_joint_extrinsic( 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) + t_seed = None if t_init_m is None else np.asarray(t_init_m, 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) @@ -389,7 +430,7 @@ def solve_joint_extrinsic( 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) + t = np.zeros(3) if t_seed is None else t_seed.copy() translation_accepted = False trans_rms = 1e9 gravity_out: np.ndarray | None = None @@ -400,6 +441,12 @@ def solve_joint_extrinsic( else: gravity_init = np.asarray(gravity_init_m_s2, dtype=float).reshape(3) + if t_prior_m is not None: + notes.append( + "using CAD/installation translation prior " + f"t={np.asarray(t_prior_m, dtype=float).reshape(3).tolist()}" + ) + if ( enable_phase_c and not force_rotation_only @@ -412,12 +459,23 @@ def solve_joint_extrinsic( r, gyro_bias0=bias_out, gravity_init=gravity_init, + t_init=t_seed if t_seed is not None else t_prior_m, + t_prior=t_prior_m, + t_prior_sigma_m=t_prior_sigma_m, ) 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) + # Prefer CAD prior over silent zero when motion SE3 is rejected. + if t_prior_m is not None: + t = np.asarray(t_prior_m, dtype=float).reshape(3) + translation_accepted = True + notes.append( + "phase-C translation residual/gate failed; keeping CAD translation prior" + ) + else: + 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 @@ -437,9 +495,19 @@ def solve_joint_extrinsic( 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)) + if t_prior_m is not None: + sigma = np.asarray(t_prior_sigma_m if t_prior_sigma_m is not None else 0.05, dtype=float) + if sigma.size == 1: + sigma = np.full(3, float(sigma), dtype=float) + residuals.append((t_opt - np.asarray(t_prior_m, dtype=float).reshape(3)) / np.maximum(sigma, 1e-3)) return np.concatenate(residuals) - opt_t = least_squares(residual_se3, np.zeros(6), loss="huber", f_scale=0.05, max_nfev=200) + x_se3 = np.zeros(6) + if t_seed is not None: + x_se3[3:] = t_seed + elif t_prior_m is not None: + x_se3[3:] = np.asarray(t_prior_m, dtype=float).reshape(3) + opt_t = least_squares(residual_se3, x_se3, 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 = [] @@ -452,11 +520,15 @@ def solve_joint_extrinsic( 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 + translation_accepted = trans_rms < 0.5 or t_prior_m is not None 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) + elif not force_rotation_only and t_prior_m is not None: + t = np.asarray(t_prior_m, dtype=float).reshape(3) + translation_accepted = True + notes.append("SE3 motion solve gated off; using CAD translation prior with refined rotation") else: notes.append("rotation-only extrinsic returned (phase-A; phase-C SE3 gated off)") diff --git a/imu_lidar/pipeline.py b/imu_lidar/pipeline.py index a7eef0e..94ee7ab 100644 --- a/imu_lidar/pipeline.py +++ b/imu_lidar/pipeline.py @@ -2,7 +2,7 @@ from __future__ import annotations -from dataclasses import asdict, dataclass +from dataclasses import asdict, dataclass, replace from pathlib import Path from typing import Any @@ -13,6 +13,7 @@ from .contracts import ( CalibrationRequest, CalibrationResult, CalibrationStatus, + MotionPair, SessionInput, ) from .finalize import finalize_result @@ -26,7 +27,10 @@ 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 +from .vehicle_config import load_vehicle_config, prior_enabled + +# Remap keyframe indices so multi-session Phase-C graphs do not collide. +_SESSION_INDEX_OFFSET = 1_000_000 def _merge_time_offset(previous: TimeOffsetResult, refined: TimeOffsetResult) -> TimeOffsetResult: @@ -49,11 +53,11 @@ 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("time_offset", "各会话独立粗估/精修 δt"), + PipelineStage("lidar_motion", "各会话关键帧、可选去畸变与 LiDAR 相对运动"), + PipelineStage("motion_pairs", "各会话构造运动对，再合并"), + PipelineStage("rotation_handeye", "用全部会话运动对联合求解旋转外参"), + PipelineStage("joint_optimizer", "用全部会话运动对联合精修；完整模式估平移"), PipelineStage("finalize", "写出结果与质量报告"), ) @@ -92,21 +96,34 @@ def _build_pairs_and_handeye( return keyframes, pair_set, handeye -def _session_details( +def _translation_prior_from_config( + vehicle_config: dict[str, Any] | None, +) -> tuple[np.ndarray | None, np.ndarray | float | None]: + if vehicle_config is None or not prior_enabled(vehicle_config, "translation_prior"): + return None, None + init_cfg = vehicle_config.get("initialization") or {} + tp = init_cfg.get("translation_prior") or {} + if tp.get("t_IMU_lidar_m") is None: + return None, None + return np.asarray(tp["t_IMU_lidar_m"], dtype=float).reshape(3), tp.get("sigma_m", [0.05, 0.05, 0.05]) + + +def _prepare_session_pairs( session: SessionInput, request: CalibrationRequest, - vehicle_config: dict[str, Any] | None, ) -> dict[str, Any]: + """Per-session: audit, δt, keyframes/pairs. No joint extrinsic yet.""" + 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)} + return {"ok": False, "stage": "timestamp_audit", "session_id": session.session_id, "report": asdict(ts)} imu_report = audit_imu(imu) if not imu_report.ok: - return {"ok": False, "stage": "imu_audit", "report": asdict(imu_report)} + return {"ok": False, "stage": "imu_audit", "session_id": session.session_id, "report": asdict(imu_report)} offset = estimate_time_offset( imu, @@ -115,7 +132,7 @@ def _session_details( search_s=request.time_offset_search_s, ) if not offset.ok: - return {"ok": False, "stage": "time_offset", "report": asdict(offset)} + return {"ok": False, "stage": "time_offset", "session_id": session.session_id, "report": asdict(offset)} working_frames = frames r_x = np.eye(3) @@ -124,7 +141,6 @@ def _session_details( 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: @@ -145,22 +161,21 @@ def _session_details( ) pairs_notes = list(pair_set.notes) pair_count = len(pair_set.pairs) - if handeye.pair_count < 3: + if pair_count < 3: return { "ok": False, - "stage": "rotation_handeye", + "stage": "motion_pairs", + "session_id": session.session_id, "iteration": iteration, "time_offset": asdict(offset), "imu_audit": asdict(imu_report), "timestamp_audit": asdict(ts), - "keyframes": len(keyframes.indices), + "keyframes": 0 if keyframes is None else 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, @@ -172,7 +187,6 @@ def _session_details( ) 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( @@ -185,70 +199,47 @@ def _session_details( ) pairs_notes = list(pair_set.notes) pair_count = len(pair_set.pairs) - if handeye.pair_count < 3: + if pair_count < 3: return { "ok": False, - "stage": "rotation_handeye", + "stage": "motion_pairs", + "session_id": session.session_id, "iteration": iteration, "time_offset": asdict(offset), "imu_audit": asdict(imu_report), "timestamp_audit": asdict(ts), - "keyframes": len(keyframes.indices), + "keyframes": 0 if keyframes is None else 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, + "pairs": tuple(pair_set.pairs), + "gyro_bias_rad_s": np.asarray(imu_report.gyro_bias_rad_s, dtype=float).reshape(3), + "gravity_init_m_s2": gravity_init, "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, + "time_offset": asdict(offset), + "time_offset_s": float(offset.delta_t_s), "keyframes": len(keyframes.indices), "pair_count": pair_count, "pair_notes": pairs_notes, - "handeye": { + "handeye_local": { "residual_rms_deg": handeye.residual_rms_deg, "residual_median_deg": handeye.residual_median_deg, "pair_count": handeye.pair_count, @@ -256,32 +247,30 @@ def _session_details( "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 _remap_pairs_for_joint(prepared: list[dict[str, Any]]) -> list[MotionPair]: + merged: list[MotionPair] = [] + for index, prep in enumerate(prepared): + id_offset = (index + 1) * _SESSION_INDEX_OFFSET + for pair in prep["pairs"]: + merged.append( + replace( + pair, + i=int(pair.i) + id_offset, + j=int(pair.j) + id_offset, + ) + ) + return merged + + def run_calibration(request: CalibrationRequest) -> CalibrationResult: - """Run the V1 calibration pipeline for one or more sessions.""" + """Run the V1 calibration pipeline for one or more sessions. + + Multi-session: each session estimates its own δt and builds motion pairs; + rotation hand-eye and joint SE3 are solved once on the merged pair set. + """ if not request.sessions: return finalize_result( @@ -303,38 +292,123 @@ def run_calibration(request: CalibrationRequest) -> CalibrationResult: output_directory=request.output_directory, ) - session_results = [] + prepared: list[dict[str, Any]] = [] for session in request.sessions: - session_results.append(_session_details(session, request, vehicle_config)) + prep = _prepare_session_pairs(session, request) + if not prep.get("ok"): + return finalize_result( + status=CalibrationStatus.BLOCKED, + message=f"blocked at stage {prep.get('stage')} ({prep.get('session_id')})", + details={"sessions": [prep]}, + output_directory=request.output_directory, + ) + prepared.append(prep) - primary = session_results[0] - if not primary.get("ok"): + all_pairs = _remap_pairs_for_joint(prepared) + handeye = solve_rotation_handeye(all_pairs) + if not handeye.ok: return finalize_result( status=CalibrationStatus.BLOCKED, - message=f"blocked at stage {primary.get('stage')}", - details={"sessions": session_results}, + message="blocked at stage rotation_handeye (joint)", + details={ + "sessions": [_public_session(p) for p in prepared], + "joint_handeye": asdict(handeye), + "merged_pair_count": len(all_pairs), + }, output_directory=request.output_directory, ) - T = np.asarray(primary["T_IMU_lidar"], dtype=float) - delta_t = float(primary["time_offset_s"]) + force_rotation_only = request.requested_mode == CalibrationMode.ROTATION_ONLY + t_prior, t_prior_sigma = _translation_prior_from_config(vehicle_config) + gyro_bias = np.mean(np.stack([p["gyro_bias_rad_s"] for p in prepared], axis=0), axis=0) + gravity_init = np.mean(np.stack([p["gravity_init_m_s2"] for p in prepared], axis=0), axis=0) + g_n = float(np.linalg.norm(gravity_init)) + if g_n > 1e-6: + gravity_init = gravity_init * (9.80665 / g_n) + + joint = solve_joint_extrinsic( + all_pairs, + handeye.R_IMU_lidar, + force_rotation_only=force_rotation_only, + imu=None, + delta_t_s=0.0, + gyro_bias_rad_s=gyro_bias, + gravity_init_m_s2=gravity_init, + enable_phase_c=not force_rotation_only, + t_init_m=t_prior, + t_prior_m=t_prior, + t_prior_sigma_m=t_prior_sigma, + ) + + session_results = [] + for prep in prepared: + session_results.append( + { + **_public_session(prep), + "vehicle_config_loaded": vehicle_config is not None, + "handeye": { + "residual_rms_deg": handeye.residual_rms_deg, + "residual_median_deg": handeye.residual_median_deg, + "pair_count": handeye.pair_count, + "ok": handeye.ok, + "notes": tuple(list(handeye.notes) + [f"joint over {len(request.sessions)} sessions"]), + "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(), + }, + "translation_accepted": joint.translation_accepted, + "rotation_ok": handeye.ok and joint.observability.rotation_observable, + } + ) + + T = np.asarray(joint.T_IMU_lidar, dtype=float) + # Report per-session δt list; keep first as scalar for backward-compatible field. + delta_t = float(prepared[0]["time_offset_s"]) if request.requested_mode == CalibrationMode.FULL_SE3: - if primary.get("translation_accepted"): + if joint.translation_accepted: status = CalibrationStatus.FULL_SE3_ACCEPTED - message = "full SE3 accepted" + message = f"full SE3 accepted (joint {len(prepared)} sessions, {len(all_pairs)} pairs)" else: status = CalibrationStatus.FULL_SE3_REJECTED - message = "rotation accepted; translation rejected by observability/residual gates" + message = ( + f"rotation accepted jointly ({len(prepared)} sessions); " + "translation rejected by observability/residual gates" + ) else: status = CalibrationStatus.ROTATION_ONLY_ACCEPTED - message = "rotation-only calibration accepted" + message = f"rotation-only calibration accepted (joint {len(prepared)} sessions, {len(all_pairs)} pairs)" 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]}, + details={ + "sessions": session_results, + "joint": { + "session_count": len(prepared), + "merged_pair_count": len(all_pairs), + "pair_counts_per_session": {p["session_id"]: p["pair_count"] for p in prepared}, + "time_offset_s_per_session": {p["session_id"]: p["time_offset_s"] for p in prepared}, + "handeye_rms_deg": handeye.residual_rms_deg, + "translation_accepted": joint.translation_accepted, + }, + }, T_IMU_lidar=T, time_offset_s=delta_t, output_directory=request.output_directory, @@ -344,4 +418,7 @@ def run_calibration(request: CalibrationRequest) -> CalibrationResult: def _public_session(session_result: dict[str, Any]) -> dict[str, Any]: payload = dict(session_result) payload.pop("T_IMU_lidar", None) + payload.pop("pairs", None) + payload.pop("gyro_bias_rad_s", None) + payload.pop("gravity_init_m_s2", None) return payload diff --git a/imu_lidar/time_offset.py b/imu_lidar/time_offset.py index d789a49..51e5ab6 100644 --- a/imu_lidar/time_offset.py +++ b/imu_lidar/time_offset.py @@ -73,7 +73,10 @@ def _correlate_offset( 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) + refined = float(best_delta + 0.5 * (y0 - y2) / denom * dt) + # Parabola can jump outside the searched window; keep it clamped. + if abs(refined) <= search_s + dt: + best_delta = refined best_peak = float(y1) return best_delta, best_peak @@ -99,9 +102,12 @@ def estimate_time_offset( 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) + # Use short consecutive (or near-consecutive) pairs. A large stride (e.g. + # len//20) averages over many seconds and destroys |ω| correlation even when + # host/device clocks are already aligned. + stride = 1 if len(frames) < 80 else 2 rotations, pair_times = estimate_frame_rotations(frames, stride=stride) - if len(rotations) < 4: + if len(rotations) < 8: 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) diff --git a/tests/test_h32_dlog_export.py b/tests/test_h32_dlog_export.py index fd031d1..b924f7d 100644 --- a/tests/test_h32_dlog_export.py +++ b/tests/test_h32_dlog_export.py @@ -8,7 +8,8 @@ from pathlib import Path import numpy as np from tools.h32_dlog.difop import CHANNELS, HORIZONTAL_START, VERTICAL_START, parse_difop_angles -from tools.h32_dlog.dobject import discover_records, iter_payloads, resolve_dlog_root +from tools.h32_dlog.dobject import RECORD_RE, discover_records, iter_payloads, resolve_dlog_root +from tools.h32_dlog.timeutil import local_wall_to_dotnet_ticks from tools.h32_dlog.load_session import load_h32_dlog_lidar from tools.h32_dlog.payload_v1 import ( MsopPacketItem, @@ -90,6 +91,19 @@ def _write_dorec_record( return start +def test_recovered_index_line_and_local_ticks(): + line = ( + ">DObject `frontlidar-msop-raw` post len=15532B, id:9CF1, " + "tic:639218060782100466, @data.bin:0" + ) + match = RECORD_RE.search(line) + assert match is not None + assert match.group("name") == "frontlidar-msop-raw" + assert match.group("file") == "data.bin" + assert int(match.group("offset")) == 0 + assert local_wall_to_dotnet_ticks("2026-08-08T17:14:38") == 639218060780000000 + + def test_parse_msop_and_difop_payload_roundtrip(): packet = _make_msop_packet(seconds=1700000000, microseconds=123456) item = MsopPacketItem( diff --git a/tests/test_hi13_imu.py b/tests/test_hi13_imu.py new file mode 100644 index 0000000..cef1798 --- /dev/null +++ b/tests/test_hi13_imu.py @@ -0,0 +1,79 @@ +"""Unit tests for HI13 / HI91 IMU decoding.""" + +from __future__ import annotations + +import struct + +from tools.rscap_v2.capture_format_v2 import CaptureFile, CaptureHeader, RawChunk +from tools.rscap_v2.hi13_imu import crc16_hi13, iter_hi13_imu_samples, parse_hi91_frame + + +def _hi91_frame( + *, + device_ms: int = 123456, + accel_g=(0.0, 0.0, 1.0), + gyro_dps=(1.0, -2.0, 3.0), +) -> bytes: + payload = bytearray(76) + payload[0] = 0x91 + struct.pack_into("> 8) & 0xFF + frame_wo_crc = bytes(header[:4]) + bytes(payload) + # crc over header[0:4] + payload + tmp = bytearray(6 + payload_length) + tmp[0:4] = header[0:4] + tmp[6:] = payload + crc = crc16_hi13(tmp, payload_length) + header[4] = crc & 0xFF + header[5] = (crc >> 8) & 0xFF + return bytes(header) + bytes(payload) + + +def test_parse_hi91_units(): + frame = _hi91_frame(device_ms=5000, accel_g=(0.0, 0.0, 1.0), gyro_dps=(57.2957795, 0.0, 0.0)) + parsed = parse_hi91_frame(frame) + assert parsed is not None + gyro, accel, device_ms = parsed + assert device_ms == 5000 + assert abs(accel[2] - 9.80665) < 1e-4 + assert abs(gyro[0] - 1.0) < 1e-5 + + +def test_iter_hi13_from_capture(): + frame = _hi91_frame(device_ms=42) + header = CaptureHeader( + sensor_kind="hi13r4-imu", + session_id="t", + session_start_utc_ticks=0, + session_start_monotonic_ticks=0, + monotonic_frequency=10_000_000, + port="COM1", + baud=115200, + file_start_utc_ticks=0, + ) + chunk = RawChunk( + sequence=1, + receive_utc_ticks=100, + receive_monotonic_ticks=1, + raw=frame, + record_file_offset=0, + raw_file_offset=0, + record_crc32=0, + crc_valid=True, + ) + capture = CaptureFile(path="mem", header=header, chunks=[chunk], footer=None) + samples = iter_hi13_imu_samples(capture) + assert len(samples) == 1 + assert samples[0].device_timestamp_us == 42_000 + assert abs(samples[0].t_s - 0.042) < 1e-12 diff --git a/tools/export_rscap_to_v1.py b/tools/export_rscap_to_v1.py index 85af4cb..cf2d3a6 100644 --- a/tools/export_rscap_to_v1.py +++ b/tools/export_rscap_to_v1.py @@ -1,13 +1,18 @@ #!/usr/bin/env python3 -"""Export N300 IMU + H32 LiDAR captures to Lidar-IMU V1 intermediate format. +"""Export IMU + H32 LiDAR captures to Lidar-IMU V1 intermediate format. -Supported LiDAR sources (exactly one required): +IMU sources: +- ``--imu-kind hi13`` (HI13R4 / HI91) or ``n300`` or ``auto`` +- one or more ``--imu-rscap`` files (concatenated) -- ``--lidar-dlog``: Medulla dlog from ``RSLidarH32_3D_DLogCaptureNet48`` - (raw MSOP + DIFOP DObjects; preferred for new recordings) -- ``--lidar-rscap``: legacy H32 MSOP V2 ``.rscap`` (MSOP-only defaults for angles) +LiDAR sources (exactly one): +- ``--lidar-dlog``: Medulla dlog dir **or recovered zip** (MSOP + DIFOP) +- ``--lidar-rscap``: legacy H32 MSOP V2 ``.rscap`` -Output layout under --out: +Optional host-time window (local wall clock, DateTime.Now.Ticks convention): +- ``--host-start`` / ``--host-end`` e.g. ``2026-08-08T17:40:05`` + +Output under ``--out``: imu.csv lidar/ @@ -15,8 +20,9 @@ Output layout under --out: frames/frame_XXXXX.npz export_summary.json -Timestamps written into the intermediate format are **device times** -(N300 device_timestamp_us, H32 MSOP device timestamp), not host receive time. +Device times stay in ``t`` / ``t_start``/``t_end``. Host UTC receive times are +also written so LiDAR–IMU alignment can bridge clocks without forcing first-frame +device coincidence. """ from __future__ import annotations @@ -34,26 +40,48 @@ if str(ROOT) not in sys.path: sys.path.insert(0, str(ROOT)) from tools.h32_dlog.load_session import load_h32_dlog_lidar +from tools.h32_dlog.timeutil import ( + local_wall_to_dotnet_ticks, + local_wall_to_utc_dotnet_ticks, + utc_dotnet_ticks_to_unix_s, +) from tools.rscap_v2.capture_format_v2 import file_summary, read_capture from tools.rscap_v2.h32_msop import iter_h32_frames, iter_h32_frames_from_packets -from tools.rscap_v2.n300_imu import iter_n300_imu_samples, samples_to_arrays +from tools.rscap_v2.hi13_imu import iter_hi13_imu_samples +from tools.rscap_v2.n300_imu import ImuSample, iter_n300_imu_samples, samples_to_arrays -def write_imu_csv(path: Path, t: np.ndarray, gyro: np.ndarray, accel: np.ndarray) -> None: +def write_imu_csv(path: Path, samples: list[ImuSample]) -> None: path.parent.mkdir(parents=True, exist_ok=True) with path.open("w", newline="", encoding="utf-8") as handle: writer = csv.writer(handle) - writer.writerow(["t", "gx", "gy", "gz", "ax", "ay", "az"]) - for index in range(t.shape[0]): + writer.writerow( + [ + "t", + "gx", + "gy", + "gz", + "ax", + "ay", + "az", + "t_host_utc_s", + "receive_utc_ticks", + ] + ) + for sample in samples: + ticks = int(sample.host_receive_utc_ticks) + t_host = utc_dotnet_ticks_to_unix_s(ticks) if ticks > 0 else float("nan") writer.writerow( [ - f"{t[index]:.9f}", - f"{gyro[index, 0]:.12g}", - f"{gyro[index, 1]:.12g}", - f"{gyro[index, 2]:.12g}", - f"{accel[index, 0]:.12g}", - f"{accel[index, 1]:.12g}", - f"{accel[index, 2]:.12g}", + f"{sample.t_s:.9f}", + f"{sample.gyro_rad_s[0]:.12g}", + f"{sample.gyro_rad_s[1]:.12g}", + f"{sample.gyro_rad_s[2]:.12g}", + f"{sample.accel_m_s2[0]:.12g}", + f"{sample.accel_m_s2[1]:.12g}", + f"{sample.accel_m_s2[2]:.12g}", + f"{t_host:.9f}" if ticks > 0 else "", + ticks, ] ) @@ -64,22 +92,45 @@ def write_lidar_session(root: Path, frames) -> dict: index_path = root / "frames_index.csv" with index_path.open("w", newline="", encoding="utf-8") as handle: writer = csv.writer(handle) - writer.writerow(["frame_id", "filename", "t_start", "t_end"]) + writer.writerow( + [ + "frame_id", + "filename", + "t_start", + "t_end", + "host_receive_utc_ticks", + "t_host_utc_s", + "host_receive_utc_end_ticks", + "t_host_utc_end_s", + ] + ) point_counts = [] + host_ok = 0 for index, frame in enumerate(frames): rel = f"frames/frame_{index:05d}.npz" np.savez_compressed(root / rel, points=np.asarray(frame.points_xyz, dtype=np.float32)) + h0 = int(getattr(frame, "host_receive_utc_ticks_start", 0) or 0) + h1 = int(getattr(frame, "host_receive_utc_ticks_end", 0) or 0) + t_host0 = utc_dotnet_ticks_to_unix_s(h0) if h0 > 0 else float("nan") + t_host1 = utc_dotnet_ticks_to_unix_s(h1) if h1 > 0 else float("nan") + if h0 > 0: + host_ok += 1 writer.writerow( [ index, rel, f"{frame.t_start_s:.9f}", f"{frame.t_end_s:.9f}", + h0, + f"{t_host0:.9f}" if h0 > 0 else "", + h1, + f"{t_host1:.9f}" if h1 > 0 else "", ] ) point_counts.append(int(frame.points_xyz.shape[0])) return { "frames": len(frames), + "frames_with_host_utc": host_ok, "points_min": int(min(point_counts)) if point_counts else 0, "points_max": int(max(point_counts)) if point_counts else 0, "points_mean": float(np.mean(point_counts)) if point_counts else 0.0, @@ -88,15 +139,63 @@ def write_lidar_session(root: Path, frames) -> dict: } +def detect_imu_kind(paths: list[Path], explicit: str) -> str: + if explicit != "auto": + return explicit + joined = " ".join(path.name.lower() for path in paths) + if "hi13" in joined or "hipnuc" in joined: + return "hi13" + if "n300" in joined or "wheeltec" in joined: + return "n300" + return "hi13" + + +def load_imu_samples( + paths: list[Path], + *, + kind: str, + host_ticks_min: int | None, + host_ticks_max: int | None, +) -> tuple[list[ImuSample], list[dict], str]: + samples: list[ImuSample] = [] + captures_meta: list[dict] = [] + for path in paths: + capture = read_capture(path) + captures_meta.append(file_summary(capture)) + if kind == "hi13": + part = iter_hi13_imu_samples( + capture, + host_utc_ticks_min=host_ticks_min, + host_utc_ticks_max=host_ticks_max, + ) + elif kind == "n300": + part = iter_n300_imu_samples(capture) + if host_ticks_min is not None or host_ticks_max is not None: + part = [ + sample + for sample in part + if (host_ticks_min is None or sample.host_receive_utc_ticks >= host_ticks_min) + and (host_ticks_max is None or sample.host_receive_utc_ticks <= host_ticks_max) + ] + else: + raise ValueError(f"unsupported imu kind: {kind}") + samples.extend(part) + samples.sort(key=lambda sample: (sample.t_s, sample.device_timestamp_us)) + return samples, captures_meta, kind + + def export_session( *, - imu_rscap: Path, + imu_rscap: list[Path] | Path, out: Path, lidar_rscap: Path | None = None, lidar_dlog: Path | None = None, + imu_kind: str = "auto", msop_object: str = "frontlidar-msop-raw", difop_object: str = "frontlidar-difop-raw", require_difop: bool = False, + host_start: str | None = None, + host_end: str | None = None, frame_stride: int = 1, max_points_per_frame: int | None = 80000, min_range_m: float = 0.3, @@ -106,24 +205,41 @@ def export_session( if (lidar_rscap is None) == (lidar_dlog is None): raise ValueError("provide exactly one of lidar_rscap or lidar_dlog") + imu_paths = [imu_rscap] if isinstance(imu_rscap, Path) else list(imu_rscap) + if not imu_paths: + raise ValueError("at least one --imu-rscap is required") + + # LiDAR DObject tic uses DateTime.Now; IMU/MSOP host fields use UTC. + lidar_ticks_min = local_wall_to_dotnet_ticks(host_start) if host_start else None + lidar_ticks_max = local_wall_to_dotnet_ticks(host_end) if host_end else None + imu_ticks_min = local_wall_to_utc_dotnet_ticks(host_start) if host_start else None + imu_ticks_max = local_wall_to_utc_dotnet_ticks(host_end) if host_end else None + kind = detect_imu_kind(imu_paths, imu_kind) + out.mkdir(parents=True, exist_ok=True) - imu_capture = read_capture(imu_rscap) - - samples = iter_n300_imu_samples(imu_capture) - t, gyro, accel = samples_to_arrays(samples) + samples, imu_captures, kind = load_imu_samples( + imu_paths, + kind=kind, + host_ticks_min=imu_ticks_min, + host_ticks_max=imu_ticks_max, + ) + t, _gyro, _accel = samples_to_arrays(samples) imu_csv = out / "imu.csv" - write_imu_csv(imu_csv, t, gyro, accel) + write_imu_csv(imu_csv, samples) + imu_host_ok = sum(1 for sample in samples if sample.host_receive_utc_ticks > 0) - lidar_meta: dict if lidar_dlog is not None: session = load_h32_dlog_lidar( lidar_dlog, msop_object=msop_object, difop_object=difop_object, require_difop=require_difop, + host_ticks_min=lidar_ticks_min, + host_ticks_max=lidar_ticks_max, ) frames = iter_h32_frames_from_packets( session.msop_packets, + host_utc_ticks=session.msop_host_utc_ticks, min_frame_points=min_frame_points, frame_stride=frame_stride, min_range_m=min_range_m, @@ -134,16 +250,20 @@ def export_session( ) lidar_meta = { "source": "dlog", - "lidar_dlog": str(session.dlog_root), + "lidar_dlog": session.dlog_root, "msop_object": session.msop_object, "difop_object": session.difop_object, "msop_packets": len(session.msop_packets), + "msop_packets_with_host_utc": sum(1 for ticks in session.msop_host_utc_ticks if ticks > 0), "msop_batches": session.msop_batch_count, "difop_records": session.difop_record_count, "session_id": session.session_id, "lidar_ip": session.lidar_ip, "angle_source": session.angle_source, - "timestamp_note": "h32_msop_device_timestamp -> seconds (from MSOP bytes)", + "timestamp_note": ( + "device: h32_msop_device_timestamp -> seconds; " + "host: MSOP HostReceiveUtcTicks -> unix seconds" + ), } else: assert lidar_rscap is not None @@ -161,25 +281,46 @@ def export_session( "lidar_rscap": str(lidar_rscap), "capture": file_summary(lidar_capture), "angle_source": "default_msop_only_vertical_-16_to_16_deg", - "timestamp_note": "h32_msop_device_timestamp_ms -> seconds", + "timestamp_note": ( + "device: h32_msop_device_timestamp_ms -> seconds; " + "host: rscap receive_utc_ticks -> unix seconds" + ), } lidar_dir = out / "lidar" lidar_stats = write_lidar_session(lidar_dir, frames) + imu_time_note = ( + "hi13_device_timestamp_ms -> seconds" + if kind == "hi13" + else "n300_device_timestamp_us -> seconds" + ) summary = { - "imu_rscap": str(imu_rscap), + "imu_rscap": [str(path) for path in imu_paths], + "imu_kind": kind, "out": str(out), + "host_window": { + "host_start": host_start, + "host_end": host_end, + "lidar_ticks_min": lidar_ticks_min, + "lidar_ticks_max": lidar_ticks_max, + "imu_ticks_min": imu_ticks_min, + "imu_ticks_max": imu_ticks_max, + "note": "local wall cut; lidar DObject tic=DateTime.Now, IMU/MSOP host=UTC", + }, "timestamp_policy": { - "imu": "n300_device_timestamp_us -> seconds", - "lidar": lidar_meta["timestamp_note"], - "host_utc": "not used as calibration timeline", + "imu_device": imu_time_note, + "imu_host": "rscap receive_utc_ticks -> t_host_utc_s", + "lidar_device": "MSOP device timestamp -> t_start/t_end", + "lidar_host": "MSOP HostReceiveUtcTicks -> t_host_utc_s", + "calibration_align": "bridge via host UTC; do not force first device samples to coincide", }, "imu": { "samples": int(t.shape[0]), + "samples_with_host_utc": imu_host_ok, "t_start": float(t[0]) if t.size else None, "t_end": float(t[-1]) if t.size else None, - "capture": file_summary(imu_capture), + "captures": imu_captures, }, "lidar": { **lidar_stats, @@ -201,41 +342,38 @@ def export_session( def main() -> int: parser = argparse.ArgumentParser(description=__doc__) - parser.add_argument("--imu-rscap", type=Path, required=True, help="N300 V2 .rscap") + parser.add_argument( + "--imu-rscap", + type=Path, + action="append", + required=True, + help="IMU V2 .rscap (repeatable)", + ) + parser.add_argument( + "--imu-kind", + choices=("auto", "hi13", "n300"), + default="auto", + help="IMU decoder (default: auto from filename)", + ) lidar = parser.add_mutually_exclusive_group(required=True) lidar.add_argument( "--lidar-dlog", type=Path, - help="H32 Medulla dlog root (dobject/ + dobject_recording/), preferred", + help="H32 dlog directory or recovered zip (indices.log + data.bin)", ) lidar.add_argument( "--lidar-rscap", type=Path, - help="Legacy H32 MSOP V2 .rscap (no DIFOP; default vertical angles)", - ) - parser.add_argument( - "--msop-object", - default="frontlidar-msop-raw", - help="DObject name for raw MSOP batches (dlog path)", - ) - parser.add_argument( - "--difop-object", - default="frontlidar-difop-raw", - help="DObject name for raw DIFOP packets (dlog path)", - ) - parser.add_argument( - "--require-difop", - action="store_true", - help="Fail if dlog has no valid DIFOP channel angles", - ) - parser.add_argument("--out", type=Path, required=True, help="Output session directory") - parser.add_argument("--frame-stride", type=int, default=1, help="Keep every N-th LiDAR frame") - parser.add_argument( - "--max-points-per-frame", - type=int, - default=80000, - help="Uniform downsample cap per frame; 0 disables", + help="Legacy H32 MSOP V2 .rscap", ) + parser.add_argument("--msop-object", default="frontlidar-msop-raw") + parser.add_argument("--difop-object", default="frontlidar-difop-raw") + parser.add_argument("--require-difop", action="store_true") + parser.add_argument("--host-start", type=str, default=None, help="Local wall start, e.g. 2026-08-08T17:40:05") + parser.add_argument("--host-end", type=str, default=None, help="Local wall end, e.g. 2026-08-08T17:45:15") + parser.add_argument("--out", type=Path, required=True) + parser.add_argument("--frame-stride", type=int, default=1) + parser.add_argument("--max-points-per-frame", type=int, default=80000) parser.add_argument("--min-range-m", type=float, default=0.3) parser.add_argument("--max-range-m", type=float, default=120.0) parser.add_argument("--min-frame-points", type=int, default=100) @@ -245,9 +383,12 @@ def main() -> int: imu_rscap=args.imu_rscap, lidar_rscap=args.lidar_rscap, lidar_dlog=args.lidar_dlog, + imu_kind=args.imu_kind, msop_object=args.msop_object, difop_object=args.difop_object, require_difop=args.require_difop, + host_start=args.host_start, + host_end=args.host_end, out=args.out, frame_stride=args.frame_stride, max_points_per_frame=max_points, @@ -258,10 +399,14 @@ def main() -> int: print( json.dumps( { + "imu_kind": summary["imu_kind"], "imu_samples": summary["imu"]["samples"], + "imu_host_utc": summary["imu"]["samples_with_host_utc"], "lidar_frames": summary["lidar"]["frames"], + "lidar_host_utc": summary["lidar"]["frames_with_host_utc"], "lidar_source": summary["lidar"]["source"], "angle_source": summary["lidar"]["angle_source"], + "host_window": summary["host_window"], "imu_csv": summary["outputs"]["imu_csv"], "lidar_session": summary["outputs"]["lidar_session"], "export_summary": str(Path(args.out) / "export_summary.json"), @@ -271,9 +416,13 @@ def main() -> int: ) ) if summary["imu"]["samples"] == 0: - raise SystemExit("no valid N300 IMU samples decoded") + raise SystemExit("no valid IMU samples decoded in window") if summary["lidar"]["frames"] == 0: - raise SystemExit("no valid H32 frames decoded") + raise SystemExit("no valid H32 frames decoded in window") + if summary["lidar"]["frames_with_host_utc"] == 0: + raise SystemExit("no LiDAR frames with MSOP HostReceiveUtcTicks; cannot host-bridge align") + if summary["imu"]["samples_with_host_utc"] == 0: + raise SystemExit("no IMU samples with host receive UTC; cannot host-bridge align") return 0 diff --git a/tools/export_usable_20260808_windows.py b/tools/export_usable_20260808_windows.py new file mode 100644 index 0000000..1906f3f --- /dev/null +++ b/tools/export_usable_20260808_windows.py @@ -0,0 +1,125 @@ +#!/usr/bin/env python3 +"""Export priority LiDAR–IMU windows from calibration_usable_20260808. + +Does not push anything; writes local V1 sessions under --out-root. +""" + +from __future__ import annotations + +import argparse +import json +import sys +from pathlib import Path + +ROOT = Path(__file__).resolve().parents[1] +if str(ROOT) not in sys.path: + sys.path.insert(0, str(ROOT)) + +from tools.export_rscap_to_v1 import export_session + +DEFAULT_DATA = Path(r"D:\data\calibration_usable_20260808") +LIDAR_ZIP = "lidar_dlog/dorec_recovered_20260808_171438_181422.zip" +IMU_MAIN = "imu_rscap/hi13r4-imu_20260808-092827.638_39783edb-e46e-4b28-a5e8-427b981c2fce.rscap" +IMU_TAIL = "imu_rscap/hi13r4-imu_20260808-101022.036_87ea5edc-cd3d-4192-809a-469fbc8cac01.rscap" + +# From usable-segment chart (local wall clock). +WINDOWS = [ + { + "name": "priority_174005_174515", + "host_start": "2026-08-08T17:40:05", + "host_end": "2026-08-08T17:45:15", + "imu": [IMU_MAIN], + "priority": True, + }, + { + "name": "priority_174905_175450", + "host_start": "2026-08-08T17:49:05", + "host_end": "2026-08-08T17:54:50", + "imu": [IMU_MAIN], + "priority": True, + }, + { + "name": "priority_175910_180530", + "host_start": "2026-08-08T17:59:10", + "host_end": "2026-08-08T18:05:30", + "imu": [IMU_MAIN], + "priority": True, + }, + { + "name": "usable_181035_181050", + "host_start": "2026-08-08T18:10:35", + "host_end": "2026-08-08T18:10:50", + "imu": [IMU_TAIL], + "priority": False, + }, + { + "name": "usable_181225_181300", + "host_start": "2026-08-08T18:12:25", + "host_end": "2026-08-08T18:13:00", + "imu": [IMU_TAIL], + "priority": False, + }, + { + "name": "usable_181350_181410", + "host_start": "2026-08-08T18:13:50", + "host_end": "2026-08-08T18:14:10", + "imu": [IMU_TAIL], + "priority": False, + }, +] + + +def main() -> int: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--data-root", type=Path, default=DEFAULT_DATA) + parser.add_argument( + "--out-root", + type=Path, + default=DEFAULT_DATA / "sessions_v1", + ) + parser.add_argument("--priority-only", action="store_true", default=True) + parser.add_argument("--all-windows", action="store_true") + parser.add_argument("--frame-stride", type=int, default=5) + parser.add_argument("--max-points-per-frame", type=int, default=40000) + args = parser.parse_args() + priority_only = not args.all_windows + lidar = args.data_root / LIDAR_ZIP + if not lidar.is_file(): + raise SystemExit(f"missing lidar zip: {lidar}") + + selected = [w for w in WINDOWS if (not priority_only) or w["priority"]] + results = [] + for window in selected: + out = args.out_root / window["name"] + imu_paths = [args.data_root / rel for rel in window["imu"]] + print(f"=== exporting {window['name']} ===", flush=True) + summary = export_session( + imu_rscap=imu_paths, + lidar_dlog=lidar, + imu_kind="hi13", + require_difop=True, + host_start=window["host_start"], + host_end=window["host_end"], + out=out, + frame_stride=args.frame_stride, + max_points_per_frame=args.max_points_per_frame, + ) + brief = { + "name": window["name"], + "imu_samples": summary["imu"]["samples"], + "lidar_frames": summary["lidar"]["frames"], + "angle_source": summary["lidar"]["angle_source"], + "out": str(out), + } + results.append(brief) + print(json.dumps(brief, ensure_ascii=False, indent=2), flush=True) + + manifest = args.out_root / "export_windows_manifest.json" + args.out_root.mkdir(parents=True, exist_ok=True) + manifest.write_text(json.dumps(results, ensure_ascii=False, indent=2) + "\n", encoding="utf-8") + print(f"manifest: {manifest}") + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/tools/h32_dlog/__init__.py b/tools/h32_dlog/__init__.py index a743f7e..4f3770f 100644 --- a/tools/h32_dlog/__init__.py +++ b/tools/h32_dlog/__init__.py @@ -1,12 +1,18 @@ """Medulla dlog readers for RSLidarH32_3D_DLogCaptureNet48 raw MSOP/DIFOP.""" from .difop import parse_difop_angles -from .dobject import discover_records, iter_payloads, resolve_dlog_root +from .dobject import discover_records, iter_payloads, open_dlog_source, resolve_dlog_root +from .load_session import H32DlogLidarSession, load_h32_dlog_lidar from .payload_v1 import parse_difop_payload, parse_msop_batch_payload +from .timeutil import local_wall_to_dotnet_ticks __all__ = [ + "H32DlogLidarSession", "discover_records", "iter_payloads", + "load_h32_dlog_lidar", + "local_wall_to_dotnet_ticks", + "open_dlog_source", "parse_difop_angles", "parse_difop_payload", "parse_msop_batch_payload", diff --git a/tools/h32_dlog/dobject.py b/tools/h32_dlog/dobject.py index 82416a0..8d4e7f7 100644 --- a/tools/h32_dlog/dobject.py +++ b/tools/h32_dlog/dobject.py @@ -1,16 +1,24 @@ -"""Index and read Medulla DObject recordings (dobject/ + dobject_recording/).""" +"""Index and read Medulla DObject recordings. + +Supports: + +- standard layout: ``dobject/**/*.log`` + ``dobject_recording/**/*.dorec`` +- recovered layout: ``dobject/all/indices.log`` + ``dobject_recording/data.bin`` +- either as an extracted directory or a zip containing those paths +""" from __future__ import annotations import re import struct +import zipfile from dataclasses import dataclass from pathlib import Path from typing import BinaryIO, Iterator RECORD_RE = re.compile( - r"^\[(?P[^]]+)\].*?DObject `(?P[^`]+)` post " + r"^(?:\[(?P[^]]+)\])?>?\s*DObject `(?P[^`]+)` post " r"len=(?P\d+)B, id:(?P[0-9A-Fa-f]+), tic:(?P\d+), " r"@(?P[^:]+):(?P\d+)" ) @@ -29,37 +37,225 @@ class RecordRef: dotnet_ticks: int -def resolve_dlog_root(value: Path | str) -> Path: - root = Path(value).expanduser().resolve() - if (root / "dobject").is_dir() and (root / "dobject_recording").is_dir(): - return root - child = root / "dlog" - if (child / "dobject").is_dir() and (child / "dobject_recording").is_dir(): - return child - raise FileNotFoundError(f"{root} does not contain dobject and dobject_recording") +class _ZipStoredMemberIO: + """Random-access reader for a ZIP_STORED member via the underlying zip file. + + ``ZipExtFile.seek`` on multi-GB members is far too slow for per-record reads. + """ + + def __init__(self, zip_path: Path, member_name: str, data_offset: int, data_size: int): + self._path = zip_path + self._member_name = member_name + self._data_offset = data_offset + self._data_size = data_size + self._fh = zip_path.open("rb") + self._pos = 0 + + def seek(self, offset: int, whence: int = 0) -> int: + if whence == 0: + self._pos = offset + elif whence == 1: + self._pos += offset + elif whence == 2: + self._pos = self._data_size + offset + else: + raise ValueError(f"invalid whence: {whence}") + if self._pos < 0: + raise ValueError("negative seek") + return self._pos + + def read(self, size: int = -1) -> bytes: + if size is None or size < 0: + size = self._data_size - self._pos + if size <= 0 or self._pos >= self._data_size: + return b"" + size = min(size, self._data_size - self._pos) + self._fh.seek(self._data_offset + self._pos) + data = self._fh.read(size) + self._pos += len(data) + return data + + def close(self) -> None: + self._fh.close() -def discover_records(dlog_root: Path, object_name: str) -> list[RecordRef]: - pending: list[tuple[str, str, str, int, int, str, int, str]] = [] - for log_path in sorted((dlog_root / "dobject").rglob("*.log")): - relative_log = log_path.relative_to(dlog_root).as_posix() - with log_path.open("r", encoding="utf-8", errors="replace") as stream: - for line in stream: - match = RECORD_RE.search(line) - if not match or match.group("name").casefold() != object_name.casefold(): - continue - pending.append( - ( - match.group("name"), - match.group("log_time"), - relative_log, - int(match.group("offset")), - int(match.group("len")), - match.group("id").upper(), - int(match.group("tic")), - match.group("file"), - ) +def _zip_stored_member_offset(zip_path: Path, info: zipfile.ZipInfo) -> int: + if info.compress_type != zipfile.ZIP_STORED: + raise RuntimeError( + f"member {info.filename!r} is compressed (type={info.compress_type}); " + "extract it first or store uncompressed" + ) + with zip_path.open("rb") as handle: + handle.seek(info.header_offset) + header = handle.read(30) + if len(header) != 30 or header[:4] != b"PK\x03\x04": + raise RuntimeError(f"bad local zip header for {info.filename!r}") + name_len, extra_len = struct.unpack(" None: + if self._zip is not None: + self._zip.close() + self._zip = None + + def __enter__(self) -> "DlogSource": + return self + + def __exit__(self, exc_type, exc, tb) -> None: + self.close() + + def iter_log_texts(self) -> Iterator[tuple[str, str]]: + if self.zip_path is not None: + assert self._zip is not None + if self._log_cache is None: + self._log_cache = {} + names = sorted( + name + for name in self._zip.namelist() + if name.replace("\\", "/").startswith("dobject/") + and name.replace("\\", "/").endswith(".log") ) + for name in names: + key = name.replace("\\", "/") + self._log_cache[key] = self._zip.read(name).decode("utf-8", errors="replace") + for name, text in self._log_cache.items(): + yield name, text + return + assert self.directory is not None + for log_path in sorted((self.directory / "dobject").rglob("*.log")): + relative = log_path.relative_to(self.directory).as_posix() + yield relative, log_path.read_text(encoding="utf-8", errors="replace") + + def open_recording(self, name: str) -> tuple[object, BinaryIO]: + """Return (owner, binary stream) supporting seek/read of one recording member.""" + + base = Path(name).name + if self.zip_path is not None: + assert self._zip is not None + candidates = [ + n + for n in self._zip.namelist() + if Path(n.replace("\\", "/")).name.casefold() == base.casefold() + and "dobject_recording/" in n.replace("\\", "/") + ] + if not candidates: + alt = name.replace("\\", "/") + if alt in self._zip.namelist(): + candidates = [alt] + elif f"dobject_recording/{base}" in self._zip.namelist(): + candidates = [f"dobject_recording/{base}"] + if not candidates: + raise FileNotFoundError(f"missing recording in zip: {name}") + if len(candidates) > 1: + raise RuntimeError(f"ambiguous recording in zip {name}: {candidates}") + member = candidates[0].replace("\\", "/") + if self._member_offsets is None: + self._member_offsets = {} + if member not in self._member_offsets: + info = self._zip.getinfo(member) + self._member_offsets[member] = ( + _zip_stored_member_offset(self.zip_path, info), + info.file_size, + ) + data_offset, data_size = self._member_offsets[member] + stream = _ZipStoredMemberIO(self.zip_path, member, data_offset, data_size) + return stream, stream + + assert self.directory is not None + index = index_dorec_files(self.directory) + if base.casefold() == "data.bin": + path = self.directory / "dobject_recording" / "data.bin" + if not path.is_file(): + matches = list((self.directory / "dobject_recording").rglob("data.bin")) + if not matches: + raise FileNotFoundError(f"missing recording file: {name}") + path = matches[0] + stream = path.open("rb") + return stream, stream + path = choose_dorec(index, name) + stream = path.open("rb") + return stream, stream + + +def open_dlog_source(value: Path | str) -> DlogSource: + path = Path(value).expanduser().resolve() + if path.is_file() and path.suffix.lower() == ".zip": + zf = zipfile.ZipFile(path, "r") + names = {n.replace("\\", "/") for n in zf.namelist()} + has_log = any(n.startswith("dobject/") and n.endswith(".log") for n in names) + has_rec = any(n.startswith("dobject_recording/") for n in names) + if not (has_log and has_rec): + zf.close() + raise FileNotFoundError(f"{path} is not a recovered/standard dlog zip") + return DlogSource(label=str(path), zip_path=path, _zip=zf) + + root = path + if not ((root / "dobject").is_dir() and (root / "dobject_recording").is_dir()): + child = root / "dlog" + if (child / "dobject").is_dir() and (child / "dobject_recording").is_dir(): + root = child + else: + raise FileNotFoundError(f"{path} does not contain dobject and dobject_recording") + return DlogSource(label=str(root), directory=root) + + +def resolve_dlog_root(value: Path | str) -> Path: + """Backward-compatible helper: directory roots only (not zip).""" + + source = open_dlog_source(value) + try: + if source.directory is None: + raise FileNotFoundError( + f"{value} is a zip; use open_dlog_source()/iter_payloads_from_source()" + ) + return source.directory + finally: + source.close() + + +def discover_records_from_source( + source: DlogSource, + object_name: str, + *, + host_ticks_min: int | None = None, + host_ticks_max: int | None = None, +) -> list[RecordRef]: + pending: list[tuple[str, str, str, int, int, str, int, str]] = [] + name_key = object_name.casefold() + for relative_log, text in source.iter_log_texts(): + for line in text.splitlines(): + match = RECORD_RE.search(line.strip()) + if not match or match.group("name").casefold() != name_key: + continue + ticks = int(match.group("tic")) + if host_ticks_min is not None and ticks < host_ticks_min: + continue + if host_ticks_max is not None and ticks > host_ticks_max: + continue + pending.append( + ( + match.group("name"), + match.group("log_time") or "", + relative_log, + int(match.group("offset")), + int(match.group("len")), + match.group("id").upper(), + ticks, + match.group("file"), + ) + ) pending.sort(key=lambda item: (item[6], item[7].casefold(), item[3])) seen: set[tuple[str, int, int]] = set() records: list[RecordRef] = [] @@ -84,10 +280,19 @@ def discover_records(dlog_root: Path, object_name: str) -> list[RecordRef]: return records +def discover_records(dlog_root: Path, object_name: str) -> list[RecordRef]: + with open_dlog_source(dlog_root) as source: + return discover_records_from_source(source, object_name) + + def index_dorec_files(dlog_root: Path) -> dict[str, list[Path]]: result: dict[str, list[Path]] = {} - for path in (dlog_root / "dobject_recording").rglob("*.dorec"): - result.setdefault(path.name.casefold(), []).append(path) + recording = dlog_root / "dobject_recording" + if not recording.is_dir(): + return result + for path in recording.rglob("*"): + if path.is_file() and path.suffix.lower() in {".dorec", ".bin"}: + result.setdefault(path.name.casefold(), []).append(path) return result @@ -107,17 +312,15 @@ def read_exact(stream: BinaryIO, size: int) -> bytes: return data -def read_record_payload(path: Path, record: RecordRef) -> bytes: - with path.open("rb") as stream: - stream.seek(record.source_offset) - name_length = read_exact(stream, 1)[0] - name = read_exact(stream, name_length).decode("ascii") - ticks = struct.unpack(" bytes: + stream.seek(record.source_offset) + name_length = read_exact(stream, 1)[0] + name = read_exact(stream, name_length).decode("ascii") + ticks = struct.unpack(" bytes: return payload -def iter_payloads(dlog_root: Path, object_name: str) -> Iterator[tuple[RecordRef, bytes]]: - root = resolve_dlog_root(dlog_root) - records = discover_records(root, object_name) +def iter_payloads_from_source( + source: DlogSource, + object_name: str, + *, + host_ticks_min: int | None = None, + host_ticks_max: int | None = None, +) -> Iterator[tuple[RecordRef, bytes]]: + records = discover_records_from_source( + source, + object_name, + host_ticks_min=host_ticks_min, + host_ticks_max=host_ticks_max, + ) if not records: return - dorec_index = index_dorec_files(root) - open_files: dict[str, tuple[Path, BinaryIO]] = {} + open_files: dict[str, BinaryIO] = {} try: for record in records: - key = record.source_dorec.casefold() - handle = open_files.get(key) - if handle is None: - path = choose_dorec(dorec_index, record.source_dorec) - handle = (path, path.open("rb")) - open_files[key] = handle - path, stream = handle - stream.seek(record.source_offset) - name_length = read_exact(stream, 1)[0] - name = read_exact(stream, name_length).decode("ascii") - ticks = struct.unpack(" Iterator[tuple[RecordRef, bytes]]: + with open_dlog_source(dlog_root) as source: + yield from iter_payloads_from_source( + source, + object_name, + host_ticks_min=host_ticks_min, + host_ticks_max=host_ticks_max, + ) diff --git a/tools/h32_dlog/load_session.py b/tools/h32_dlog/load_session.py index f4c6610..4918f9d 100644 --- a/tools/h32_dlog/load_session.py +++ b/tools/h32_dlog/load_session.py @@ -10,16 +10,21 @@ import numpy as np from tools.rscap_v2.h32_msop import default_horizontal_deg, default_vertical_deg from .difop import DifopAngles, parse_difop_angles -from .dobject import discover_records, iter_payloads, resolve_dlog_root +from .dobject import ( + discover_records_from_source, + iter_payloads_from_source, + open_dlog_source, +) from .payload_v1 import parse_difop_payload, parse_msop_batch_payload @dataclass class H32DlogLidarSession: - dlog_root: Path + dlog_root: str msop_object: str difop_object: str msop_packets: list[bytes] + msop_host_utc_ticks: list[int] msop_batch_count: int difop_record_count: int angle_source: str @@ -27,6 +32,8 @@ class H32DlogLidarSession: horizontal_deg: np.ndarray session_id: str | None = None lidar_ip: str | None = None + host_ticks_min: int | None = None + host_ticks_max: int | None = None def load_h32_dlog_lidar( @@ -35,65 +42,99 @@ def load_h32_dlog_lidar( msop_object: str = "frontlidar-msop-raw", difop_object: str = "frontlidar-difop-raw", require_difop: bool = False, + host_ticks_min: int | None = None, + host_ticks_max: int | None = None, ) -> H32DlogLidarSession: - root = resolve_dlog_root(dlog_root) - msop_packets: list[bytes] = [] - batch_count = 0 - session_id: str | None = None - lidar_ip: str | None = None + with open_dlog_source(dlog_root) as source: + # DIFOP angles: prefer packets inside the window, else any in the capture. + angles: DifopAngles | None = None + difop_count = 0 + session_id: str | None = None + lidar_ip: str | None = None + for _record, payload in iter_payloads_from_source( + source, + difop_object, + host_ticks_min=host_ticks_min, + host_ticks_max=host_ticks_max, + ): + difop = parse_difop_payload(payload) + difop_count += 1 + try: + angles = parse_difop_angles(difop.raw) + except ValueError: + continue + if session_id is None: + session_id = difop.session_id + lidar_ip = difop.lidar_ip - for _record, payload in iter_payloads(root, msop_object): - batch = parse_msop_batch_payload(payload) - batch_count += 1 - if session_id is None: - session_id = batch.session_id - lidar_ip = batch.lidar_ip - for item in batch.packets: - msop_packets.append(item.raw) + if angles is None: + for _record, payload in iter_payloads_from_source(source, difop_object): + difop = parse_difop_payload(payload) + difop_count += 1 + try: + angles = parse_difop_angles(difop.raw) + except ValueError: + continue + if session_id is None: + session_id = difop.session_id + lidar_ip = difop.lidar_ip + if angles is not None: + break - angles: DifopAngles | None = None - difop_count = 0 - for _record, payload in iter_payloads(root, difop_object): - difop = parse_difop_payload(payload) - difop_count += 1 - try: - angles = parse_difop_angles(difop.raw) - except ValueError: - continue - if session_id is None: - session_id = difop.session_id - lidar_ip = difop.lidar_ip + msop_packets: list[bytes] = [] + msop_host_utc_ticks: list[int] = [] + batch_count = 0 + for record, payload in iter_payloads_from_source( + source, + msop_object, + host_ticks_min=host_ticks_min, + host_ticks_max=host_ticks_max, + ): + batch = parse_msop_batch_payload(payload) + batch_count += 1 + if session_id is None: + session_id = batch.session_id + lidar_ip = batch.lidar_ip + for item in batch.packets: + msop_packets.append(item.raw) + # Per-packet UTC host receive from MSOP DLog payload only. + # Do NOT fall back to DObject tic (DateTime.Now / local). + msop_host_utc_ticks.append(int(item.host_receive_utc_ticks)) - if not msop_packets: - msop_records = discover_records(root, msop_object) - raise RuntimeError( - f"no MSOP packets from DObject {msop_object!r} under {root} " - f"(log records={len(msop_records)})" - ) - - if angles is None: - if require_difop: + if not msop_packets: + msop_records = discover_records_from_source(source, msop_object) raise RuntimeError( - f"no valid DIFOP calibration from DObject {difop_object!r} under {root}" + f"no MSOP packets from DObject {msop_object!r} under {source.label} " + f"(log records={len(msop_records)}, " + f"host_ticks=[{host_ticks_min}, {host_ticks_max}])" ) - vertical = default_vertical_deg() - horizontal = default_horizontal_deg() - angle_source = "default_msop_only_vertical_-16_to_16_deg" - else: - vertical = angles.vertical_deg - horizontal = angles.horizontal_deg - angle_source = "difop_channel_angles" - return H32DlogLidarSession( - dlog_root=root, - msop_object=msop_object, - difop_object=difop_object, - msop_packets=msop_packets, - msop_batch_count=batch_count, - difop_record_count=difop_count, - angle_source=angle_source, - vertical_deg=vertical, - horizontal_deg=horizontal, - session_id=session_id, - lidar_ip=lidar_ip, - ) + if angles is None: + if require_difop: + raise RuntimeError( + f"no valid DIFOP calibration from DObject {difop_object!r} under {source.label}" + ) + vertical = default_vertical_deg() + horizontal = default_horizontal_deg() + angle_source = "default_msop_only_vertical_-16_to_16_deg" + else: + vertical = angles.vertical_deg + horizontal = angles.horizontal_deg + angle_source = "difop_channel_angles" + + return H32DlogLidarSession( + dlog_root=source.label, + msop_object=msop_object, + difop_object=difop_object, + msop_packets=msop_packets, + msop_host_utc_ticks=msop_host_utc_ticks, + msop_batch_count=batch_count, + difop_record_count=difop_count, + angle_source=angle_source, + vertical_deg=vertical, + horizontal_deg=horizontal, + session_id=session_id, + lidar_ip=lidar_ip, + host_ticks_min=host_ticks_min, + host_ticks_max=host_ticks_max, + ) diff --git a/tools/h32_dlog/timeutil.py b/tools/h32_dlog/timeutil.py new file mode 100644 index 0000000..07fe60d --- /dev/null +++ b/tools/h32_dlog/timeutil.py @@ -0,0 +1,56 @@ +"""Wall-clock helpers for Medulla tick filtering. + +Two tick conventions appear in this dataset: + +- LiDAR DObject ``tic`` / recovered ``indices.log``: ``DateTime.Now.Ticks`` (local) +- IMU / MSOP payload host receive fields: UTC ``DateTime.UtcNow.Ticks`` +""" + +from __future__ import annotations + +from datetime import datetime, timedelta, timezone + +TICKS_PER_SECOND = 10_000_000 +DOTNET_UNIX_EPOCH_TICKS = 621355968000000000 + + +def _parse_local_wall(text: str) -> datetime: + normalized = text.strip().replace(" ", "T") + if normalized.endswith("Z"): + raise ValueError("expected local wall time without Z; got UTC marker") + if "+" in normalized[10:]: + idx = normalized.find("+", 10) + normalized = normalized[:idx] + elif normalized.count("-") > 2: + # timezone like -08:00 after the date + idx = normalized.find("-", 10) + if idx > 0 and ":" in normalized[idx + 1 :]: + normalized = normalized[:idx] + return datetime.fromisoformat(normalized).replace(tzinfo=None) + + +def local_wall_to_dotnet_ticks(text: str) -> int: + """Local wall time → ``DateTime.Now.Ticks`` (LiDAR DObject tic).""" + + dt = _parse_local_wall(text) + delta = dt - datetime(1, 1, 1) + return int(delta.total_seconds() * TICKS_PER_SECOND) + + +def local_wall_to_utc_dotnet_ticks(text: str, *, tz_hours: float = 8.0) -> int: + """Local wall time in ``tz_hours`` → UTC ``DateTime.UtcNow.Ticks`` (IMU host).""" + + dt = _parse_local_wall(text).replace(tzinfo=timezone(timedelta(hours=tz_hours))) + unix = dt.timestamp() + return int(round(unix * TICKS_PER_SECOND)) + DOTNET_UNIX_EPOCH_TICKS + + +def dotnet_ticks_to_local_iso(ticks: int) -> str: + dt = datetime(1, 1, 1) + timedelta(microseconds=ticks / 10.0) + return dt.isoformat(timespec="milliseconds") + + +def utc_dotnet_ticks_to_unix_s(ticks: int) -> float: + """UTC ``DateTime.UtcNow.Ticks`` → Unix seconds.""" + + return (float(ticks) - float(DOTNET_UNIX_EPOCH_TICKS)) / float(TICKS_PER_SECOND) diff --git a/tools/rscap_v2/h32_msop.py b/tools/rscap_v2/h32_msop.py index 598b262..52c8216 100644 --- a/tools/rscap_v2/h32_msop.py +++ b/tools/rscap_v2/h32_msop.py @@ -15,7 +15,7 @@ and horizontal channel offsets default to 0. from __future__ import annotations from dataclasses import dataclass -from typing import Iterable +from typing import Iterable, Sequence import numpy as np @@ -66,6 +66,8 @@ class LidarFrameExport: t_start_s: float t_end_s: float points_xyz: np.ndarray # (N, 3) metres + host_receive_utc_ticks_start: int = 0 + host_receive_utc_ticks_end: int = 0 def decode_packet_points( @@ -144,6 +146,7 @@ def _block_points( def iter_h32_frames_from_packets( packets: Iterable[bytes], *, + host_utc_ticks: Sequence[int] | None = None, min_frame_points: int = MIN_FRAME_POINTS_DEFAULT, frame_stride: int = 1, min_range_m: float = 0.3, @@ -152,31 +155,49 @@ def iter_h32_frames_from_packets( vertical_deg: np.ndarray | None = None, horizontal_deg: np.ndarray | None = None, ) -> list[LidarFrameExport]: - """Assemble raw MSOP packets into frames using the 270°→90° azimuth wrap.""" + """Assemble raw MSOP packets into frames using the 270°→90° azimuth wrap. + + ``host_utc_ticks`` is optional per-packet ``HostReceiveUtcTicks`` from the + MSOP DLog payload (UTC DateTime ticks). When provided, each emitted frame + carries host receive start/end ticks from the first/last contributing packet. + """ vertical = default_vertical_deg() if vertical_deg is None else np.asarray(vertical_deg, dtype=np.float64) horizontal = default_horizontal_deg() if horizontal_deg is None else np.asarray(horizontal_deg, dtype=np.float64) if vertical.shape != (CHANNELS,) or horizontal.shape != (CHANNELS,): raise ValueError(f"vertical/horizontal must have shape ({CHANNELS},)") + packet_list = list(packets) + host_list = list(host_utc_ticks) if host_utc_ticks is not None else None + if host_list is not None and len(host_list) != len(packet_list): + raise ValueError( + f"host_utc_ticks length {len(host_list)} != packets length {len(packet_list)}" + ) + frames: list[LidarFrameExport] = [] point_chunks: list[np.ndarray] = [] t_start: float | None = None t_end: float | None = None + host_start: int | None = None + host_end: int | None = None prev_az: float | None = None kept = 0 stride = max(1, int(frame_stride)) def emit() -> None: - nonlocal point_chunks, t_start, t_end, kept + nonlocal point_chunks, t_start, t_end, host_start, host_end, kept if not point_chunks or t_start is None or t_end is None: point_chunks = [] t_start = t_end = None + host_start = host_end = None return points = np.vstack(point_chunks) point_chunks = [] start_s, end_s = t_start, t_end + h0 = int(host_start or 0) + h1 = int(host_end or 0) t_start = t_end = None + host_start = host_end = None if points.shape[0] < min_frame_points: return if kept % stride != 0: @@ -188,12 +209,21 @@ def iter_h32_frames_from_packets( points = points[select] if end_s <= start_s: end_s = start_s + 0.1 - frames.append(LidarFrameExport(t_start_s=start_s, t_end_s=end_s, points_xyz=points)) + frames.append( + LidarFrameExport( + t_start_s=start_s, + t_end_s=end_s, + points_xyz=points, + host_receive_utc_ticks_start=h0, + host_receive_utc_ticks_end=h1, + ) + ) - for packet in packets: + for index, packet in enumerate(packet_list): if len(packet) != PACKET_LENGTH: continue packet_t = device_timestamp_ms(packet) * 1e-3 + packet_host = int(host_list[index]) if host_list is not None else 0 unit = distance_unit_mm(packet) idx = DATA_START for _block in range(BLOCKS): @@ -216,7 +246,9 @@ def iter_h32_frames_from_packets( if pts.shape[0]: if t_start is None: t_start = packet_t + host_start = packet_host t_end = packet_t + host_end = packet_host point_chunks.append(pts) idx += BLOCK_LENGTH @@ -237,8 +269,11 @@ def iter_h32_frames( ) -> list[LidarFrameExport]: """Assemble MSOP packets from a V2 .rscap capture into frames.""" + packets = [chunk.raw for chunk in capture.chunks] + host_ticks = [chunk.receive_utc_ticks for chunk in capture.chunks] return iter_h32_frames_from_packets( - (chunk.raw for chunk in capture.chunks), + packets, + host_utc_ticks=host_ticks, min_frame_points=min_frame_points, frame_stride=frame_stride, min_range_m=min_range_m, diff --git a/tools/rscap_v2/hi13_imu.py b/tools/rscap_v2/hi13_imu.py new file mode 100644 index 0000000..82689a8 --- /dev/null +++ b/tools/rscap_v2/hi13_imu.py @@ -0,0 +1,136 @@ +"""Decode Hipnuc / HI13 (HI91/HI92) IMU frames from a V2 .rscap capture. + +Matches ``EcarSensorMinimal/RawSerialImu/Hi13Protocol.cs``: +sync ``5A A5``, CRC16 over header[0:4]+payload, tag ``0x91`` / ``0x92``. + +HI91 (preferred for calibration): +- accel: float32 in g → m/s² (* 9.80665) +- gyro: float32 in deg/s → rad/s +- device time: uint32 ms at frame offset 14 → ``t_s = ms * 1e-3`` +""" + +from __future__ import annotations + +import struct + +import numpy as np + +from .capture_format_v2 import CaptureFile +from .n300_imu import ImuSample, samples_to_arrays + +G0 = 9.80665 +DEG2RAD = np.pi / 180.0 + + +def crc16_hi13(frame: bytes, payload_length: int) -> int: + crc = 0 + for value in frame[:4]: + crc = _update_crc16(crc, value) + for value in frame[6 : 6 + payload_length]: + crc = _update_crc16(crc, value) + return crc & 0xFFFF + + +def _update_crc16(crc: int, value: int) -> int: + crc ^= (value & 0xFF) << 8 + for _ in range(8): + if crc & 0x8000: + crc = ((crc << 1) ^ 0x1021) & 0xFFFF + else: + crc = (crc << 1) & 0xFFFF + return crc + + +def parse_hi91_frame(raw: bytes) -> tuple[tuple[float, float, float], tuple[float, float, float], int] | None: + """Return (gyro_rad_s, accel_m_s2, device_timestamp_ms) for a CRC-valid HI91 frame.""" + + if len(raw) < 6 + 76: + return None + payload_length = raw[2] | (raw[3] << 8) + if payload_length < 76 or len(raw) < 6 + payload_length: + return None + if raw[6] != 0x91: + return None + expected = raw[4] | (raw[5] << 8) + if crc16_hi13(raw, payload_length) != expected: + return None + device_ms = struct.unpack_from(" list[ImuSample]: + """Return CRC-valid HI91 samples sorted by device timestamp. + + Streams chunk-by-chunk (no giant join) and can skip whole chunks outside the + host UTC receive window before parsing. + """ + + samples: list[ImuSample] = [] + carry = b"" + for chunk in capture.chunks: + if host_utc_ticks_min is not None and chunk.receive_utc_ticks < host_utc_ticks_min: + carry = b"" + continue + if host_utc_ticks_max is not None and chunk.receive_utc_ticks > host_utc_ticks_max: + # chunks are time-ordered; remaining ones are later + if chunk.receive_utc_ticks > host_utc_ticks_max: + break + stream = carry + chunk.raw + cursor = 0 + while cursor + 6 < len(stream): + sync = stream.find(b"\x5A\xA5", cursor) + if sync < 0: + carry = b"" + break + if sync + 6 > len(stream): + carry = stream[sync:] + break + payload_length = stream[sync + 2] | (stream[sync + 3] << 8) + if payload_length < 1 or payload_length > 512: + cursor = sync + 1 + continue + end = sync + 6 + payload_length + if end > len(stream): + carry = stream[sync:] + break + parsed = parse_hi91_frame(stream[sync:end]) + cursor = end + if parsed is None: + continue + gyro, accel, device_ms = parsed + host_ticks = chunk.receive_utc_ticks + if host_utc_ticks_min is not None and host_ticks < host_utc_ticks_min: + continue + if host_utc_ticks_max is not None and host_ticks > host_utc_ticks_max: + continue + samples.append( + ImuSample( + t_s=float(device_ms) * 1e-3, + gyro_rad_s=gyro, + accel_m_s2=accel, + host_receive_utc_ticks=host_ticks, + device_timestamp_us=int(device_ms) * 1000, + ) + ) + else: + carry = b"" + samples.sort(key=lambda sample: (sample.t_s, sample.device_timestamp_us)) + return samples + + +__all__ = [ + "ImuSample", + "crc16_hi13", + "iter_hi13_imu_samples", + "parse_hi91_frame", + "samples_to_arrays", +] diff --git a/tools/run_joint_three_windows_full_se3.py b/tools/run_joint_three_windows_full_se3.py new file mode 100644 index 0000000..4350911 --- /dev/null +++ b/tools/run_joint_three_windows_full_se3.py @@ -0,0 +1,50 @@ +#!/usr/bin/env python3 +"""Joint full_se3 on the three host-aligned priority windows.""" + +from __future__ import annotations + +import sys +from pathlib import Path + +ROOT = Path(__file__).resolve().parents[1] +if str(ROOT) not in sys.path: + sys.path.insert(0, str(ROOT)) + +from imu_lidar.cli import main as cli_main + +ALIGNED = Path(r"D:\data\calibration_usable_20260808\sessions_v1_host_aligned") +SESSIONS = [ + "priority_174005_174515", + "priority_174905_175450", + "priority_175910_180530", +] + + +def main() -> int: + out = ALIGNED / "joint_full_se3" + argv = [ + "run", + "--vehicle-config", + str(ROOT / "config" / "vehicle_hi13_h32_20260808.yaml"), + "--output", + str(out), + "--mode", + "full_se3", + "--time-offset-search-s", + "0.5", + "--min-pair-rotation-deg", + "2.0", + ] + for name in SESSIONS: + session = ALIGNED / name + if not (session / "imu.csv").is_file() or not (session / "lidar" / "frames_index.csv").is_file(): + raise SystemExit(f"missing host-aligned session: {session}") + argv.extend(["--session-id", name]) + argv.extend(["--imu", str(session / "imu.csv")]) + argv.extend(["--lidar", str(session / "lidar")]) + print("argv:", " ".join(argv), flush=True) + return cli_main(argv) + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/tools/run_priority_windows_calibration.py b/tools/run_priority_windows_calibration.py new file mode 100644 index 0000000..5689c19 --- /dev/null +++ b/tools/run_priority_windows_calibration.py @@ -0,0 +1,292 @@ +#!/usr/bin/env python3 +"""Align HI13/H32 via host-UTC bridge, then run rotation_only. + +Device clocks (HI13 boot ms vs H32 absolute) must NOT be forced to share a +first-sample epoch. Instead map each LiDAR frame onto the IMU device timeline +by interpolating IMU device time at the frame's MSOP HostReceiveUtcTicks. +Optional |ω| correlation then refines residual host/path delay. +""" + +from __future__ import annotations + +import argparse +import csv +import json +import shutil +import sys +from pathlib import Path + +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.cli import main as cli_main +from imu_lidar.geometry import so3_log +from imu_lidar.imu_io import load_imu_samples +from imu_lidar.lidar_io import load_lidar_frames +from imu_lidar.registration import estimate_frame_rotations +from imu_lidar.time_offset import _correlate_offset, _magnitude_series + +SESSIONS = [ + "priority_174005_174515", + "priority_174905_175450", + "priority_175910_180530", +] + + +def _read_imu_host_table(imu_csv: Path) -> tuple[np.ndarray, np.ndarray]: + rows = list(csv.DictReader(imu_csv.open(encoding="utf-8"))) + if not rows: + raise RuntimeError(f"empty IMU csv: {imu_csv}") + if "t_host_utc_s" not in rows[0] or not rows[0].get("t_host_utc_s"): + raise RuntimeError( + f"{imu_csv} missing t_host_utc_s; re-export with HostReceiveUtcTicks support" + ) + t_dev = np.asarray([float(row["t"]) for row in rows], dtype=np.float64) + t_host = np.asarray([float(row["t_host_utc_s"]) for row in rows], dtype=np.float64) + order = np.argsort(t_host) + return t_host[order], t_dev[order] + + +def _imu_device_at_host(t_host_query: np.ndarray, imu_host: np.ndarray, imu_dev: np.ndarray) -> np.ndarray: + """Map host UTC seconds → IMU device seconds (linear interp, edge clamp).""" + + return np.interp(t_host_query, imu_host, imu_dev) + + +def rewrite_lidar_index_host_bridge( + src_index: Path, + dst_index: Path, + *, + imu_host: np.ndarray, + imu_dev: np.ndarray, + residual_delta_s: float = 0.0, +) -> dict: + """Rewrite LiDAR times onto IMU device clock via host UTC bridge. + + For each frame: + t_host_mid = mid of MSOP host receive window + t_imu_mid = interp(IMU device @ t_host_mid) + residual_delta + keep device duration: t_start/t_end centered on t_imu_mid + """ + + rows = list(csv.DictReader(src_index.open(encoding="utf-8"))) + if not rows: + raise RuntimeError(f"empty frames_index: {src_index}") + if "t_host_utc_s" not in rows[0]: + raise RuntimeError( + f"{src_index} missing t_host_utc_s; re-export DLog with MSOP HostReceiveUtcTicks" + ) + + dst_index.parent.mkdir(parents=True, exist_ok=True) + offsets: list[float] = [] + with dst_index.open("w", newline="", encoding="utf-8") as handle: + writer = csv.writer(handle) + writer.writerow(["frame_id", "filename", "t_start", "t_end"]) + for row in rows: + t0 = float(row["t_start"]) + t1 = float(row["t_end"]) + host0 = row.get("t_host_utc_s") or "" + host1 = row.get("t_host_utc_end_s") or "" + if not host0: + raise RuntimeError(f"frame {row.get('frame_id')} missing t_host_utc_s") + h0 = float(host0) + h1 = float(host1) if host1 else h0 + host_mid = 0.5 * (h0 + h1) + imu_mid = float(_imu_device_at_host(np.asarray([host_mid]), imu_host, imu_dev)[0]) + imu_mid += residual_delta_s + duration = max(t1 - t0, 1e-3) + new0 = imu_mid - 0.5 * duration + new1 = imu_mid + 0.5 * duration + offsets.append(imu_mid - 0.5 * (t0 + t1)) + writer.writerow( + [ + row["frame_id"], + row["filename"], + f"{new0:.9f}", + f"{new1:.9f}", + ] + ) + arr = np.asarray(offsets, dtype=np.float64) + return { + "frames": len(offsets), + "bridge_offset_median_s": float(np.median(arr)), + "bridge_offset_mean_s": float(np.mean(arr)), + "bridge_offset_std_s": float(np.std(arr)), + "bridge_offset_min_s": float(np.min(arr)), + "bridge_offset_max_s": float(np.max(arr)), + "residual_delta_s": float(residual_delta_s), + } + + +def estimate_residual_delta(session_dir: Path, *, search_s: float = 5.0) -> tuple[float, float]: + imu = load_imu_samples(session_dir / "imu.csv") + frames = load_lidar_frames(session_dir / "lidar") + # Short pairs only — large stride anti-correlates with IMU |gyro|. + stride = 1 if len(frames) < 80 else 2 + rotations, pair_times = estimate_frame_rotations(frames, stride=stride) + if len(rotations) < 8: + rotations, pair_times = estimate_frame_rotations(frames, stride=1) + 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) + imu_t, imu_mag = _magnitude_series(imu.t_s, imu.gyro_rad_s) + lidar_t_arr, lidar_mag = _magnitude_series(np.asarray(lidar_t), np.asarray(lidar_w)) + delta, peak = _correlate_offset( + imu_t, + imu_mag, + lidar_t_arr, + lidar_mag, + search_s=search_s, + sample_hz=20.0, + ) + return float(delta), float(peak) + + +def align_session(src: Path, dst: Path, *, residual_search_s: float = 5.0) -> dict: + if dst.exists(): + shutil.rmtree(dst) + dst.mkdir(parents=True) + shutil.copy2(src / "imu.csv", dst / "imu.csv") + shutil.copytree(src / "lidar" / "frames", dst / "lidar" / "frames") + + imu_host, imu_dev = _read_imu_host_table(src / "imu.csv") + bridge = rewrite_lidar_index_host_bridge( + src / "lidar" / "frames_index.csv", + dst / "lidar" / "frames_index.csv", + imu_host=imu_host, + imu_dev=imu_dev, + residual_delta_s=0.0, + ) + + residual_delta, residual_peak = estimate_residual_delta(dst, search_s=residual_search_s) + # Only apply residual when correlation is clearly positive; otherwise the + # host-UTC bridge alone is the trusted alignment (weak peaks are noise). + apply_residual = residual_peak >= 0.5 and abs(residual_delta) <= residual_search_s + applied = float(residual_delta) if apply_residual else 0.0 + if apply_residual: + bridge = rewrite_lidar_index_host_bridge( + src / "lidar" / "frames_index.csv", + dst / "lidar" / "frames_index.csv", + imu_host=imu_host, + imu_dev=imu_dev, + residual_delta_s=applied, + ) + + meta = { + "source": str(src), + "aligned": str(dst), + "method": "host_utc_bridge", + "imu_host_span_s": [float(imu_host[0]), float(imu_host[-1])], + "imu_device_span_s": [float(imu_dev[0]), float(imu_dev[-1])], + "bridge": bridge, + "residual_delta_s": residual_delta, + "residual_peak": residual_peak, + "residual_applied_s": applied, + "residual_applied": apply_residual, + "note": ( + "LiDAR t_* rewritten onto IMU device clock via MSOP/IMU HostReceiveUtc; " + "not first-device-sample coincidence. residual |omega| shift applied only if peak>=0.5." + ), + } + (dst / "align_meta.json").write_text( + json.dumps(meta, indent=2, ensure_ascii=False) + "\n", encoding="utf-8" + ) + return meta + + +def run_one(session_dir: Path, vehicle: Path, search_s: float) -> dict: + output = session_dir / "out" + if output.exists(): + shutil.rmtree(output) + argv = [ + "run", + "--session-id", + session_dir.name, + "--imu", + str(session_dir / "imu.csv"), + "--lidar", + str(session_dir / "lidar"), + "--vehicle-config", + str(vehicle), + "--output", + str(output), + "--mode", + "rotation_only", + "--time-offset-search-s", + str(search_s), + "--min-pair-rotation-deg", + "2.0", + ] + code = cli_main(argv) + summary_path = output / "summary.json" + summary = {} + if summary_path.is_file(): + summary = json.loads(summary_path.read_text(encoding="utf-8")) + t_block = summary.get("T_IMU_lidar") or {} + return { + "session": session_dir.name, + "exit_code": code, + "status": summary.get("status"), + "message": summary.get("message"), + "time_offset_s": summary.get("time_offset_s"), + "rotation_deg": t_block.get("rotation_deg") if isinstance(t_block, dict) else None, + "summary": str(summary_path) if summary_path.is_file() else None, + } + + +def main() -> int: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument( + "--sessions-root", + type=Path, + default=Path(r"D:\data\calibration_usable_20260808\sessions_v1"), + ) + parser.add_argument( + "--aligned-root", + type=Path, + default=Path(r"D:\data\calibration_usable_20260808\sessions_v1_host_aligned"), + ) + parser.add_argument( + "--vehicle-config", + type=Path, + default=ROOT / "config" / "vehicle_hi13_h32_20260808.yaml", + ) + parser.add_argument( + "--residual-search-s", + type=float, + default=5.0, + help="|ω| residual search after host bridge (seconds)", + ) + parser.add_argument("--time-offset-search-s", type=float, default=1.0) + args = parser.parse_args() + + results = [] + for name in SESSIONS: + src = args.sessions_root / name + if not src.is_dir(): + raise SystemExit(f"missing session: {src}") + aligned = args.aligned_root / name + print(f"=== align {name} ===", flush=True) + meta = align_session(src, aligned, residual_search_s=args.residual_search_s) + print(json.dumps(meta, ensure_ascii=False, indent=2), flush=True) + print(f"=== calibrate {name} ===", flush=True) + result = run_one(aligned, args.vehicle_config, args.time_offset_search_s) + results.append({"align": meta, **result}) + print(json.dumps(result, ensure_ascii=False, indent=2), flush=True) + + manifest = args.aligned_root / "calibration_manifest.json" + args.aligned_root.mkdir(parents=True, exist_ok=True) + manifest.write_text(json.dumps(results, ensure_ascii=False, indent=2) + "\n", encoding="utf-8") + print(f"manifest: {manifest}") + return 0 + + +if __name__ == "__main__": + raise SystemExit(main())