支持 HI13/H32 主机 UTC 桥接对齐、多会话联合标定与 CAD 平移先验。

Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
lichun.qu
2026-08-10 13:26:32 +08:00
co-authored by Cursor
parent 30f7e66db3
commit 2237be77a4
20 changed files with 1830 additions and 347 deletions
+6 -3
View File
@@ -44,12 +44,15 @@ p_IMU = T_IMU_lidar · p_lidar
| `summary.json` | 状态、残差、可观性 | | `summary.json` | 状态、残差、可观性 |
新车原始数据导出(H32 dlog + N300 rscap): 新车原始数据导出(H32 dlog/zip + HI13 rscap):
```powershell ```powershell
python tools\export_rscap_to_v1.py ` python tools\export_rscap_to_v1.py `
--imu-rscap path\to\n300.rscap ` --imu-rscap path\to\hi13r4-imu.rscap `
--lidar-dlog path\to\session_or_dlog ` --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 ` --out path\to\session_v1 `
--require-difop --require-difop
``` ```
+80
View File
@@ -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]]
+25 -15
View File
@@ -4,20 +4,24 @@
## 从原始数据导出 ## 从原始数据导出
**推荐(新 H32 插件 `RSLidarH32_3D_DLogCaptureNet48`):** N300 `.rscap` + 雷达 Medulla dlograw MSOP / DIFOP)。 **推荐(新 H32 + HI13):** HI13 `.rscap` + 雷达 Medulla dlog / recovered zipraw MSOP + DIFOP)。
```powershell ```powershell
python tools\export_rscap_to_v1.py ` python tools\export_rscap_to_v1.py `
--imu-rscap path\to\n300.rscap ` --imu-rscap path\to\hi13r4-imu.rscap `
--lidar-dlog path\to\session_or_dlog ` --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 ` --out path\to\session_v1 `
--frame-stride 1 ` --frame-stride 5 `
--require-difop --require-difop
``` ```
`--lidar-dlog` 指向含 `dobject/` + `dobject_recording/` 目录(或其上级含 `dlog/` 子目录亦可)。 `--lidar-dlog` 可为:标准 `dobject/`+`dobject_recording/` 目录,或 recovered zip`indices.log` + `data.bin`)。
默认 DObject`frontlidar-msop-raw``frontlidar-difop-raw`(可用 `--msop-object` / `--difop-object` 覆盖)。 `--imu-kind``hi13` / `n300` / `auto`(默认按文件名推断)。
有 DIFOP 时用设备通道角做 XYZ;`--require-difop` 在缺少有效 DIFOP 时直接失败。 `--host-start/end`:按本地墙钟切窗(仅裁剪;标定主轴仍是设备时间)。
默认 DObject`frontlidar-msop-raw``frontlidar-difop-raw`
**兼容旧 MSOP-only `.rscap`** **兼容旧 MSOP-only `.rscap`**
@@ -30,7 +34,7 @@ python tools\export_rscap_to_v1.py `
``` ```
产出:`imu.csv``lidar/`(含 `frames_index.csv`)、`export_summary.json` 产出:`imu.csv``lidar/`(含 `frames_index.csv`)、`export_summary.json`
时间轴为**设备时间**N300 `device_timestamp_us`→秒;H32 MSOP 设备时间戳→秒。主机接收时间不写入标定主轴 标定主轴仍是**设备时间**;同时写出**主机 UTC 接收时间**,用于把雷达帧桥接到 IMU 设备钟(禁止把两边设备时间第一帧强行重合)
## IMU ## IMU
@@ -39,16 +43,18 @@ python tools\export_rscap_to_v1.py `
### CSV ### CSV
```text ```text
t,gx,gy,gz,ax,ay,az 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 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 | | gx,gy,gz | 角速度 | rad/s |
| ax,ay,az | 比力/加速度 | m/s² | | ax,ay,az | 比力/加速度 | m/s² |
| t_host_utc_s | 主机 UTC 接收时间(Unix | s |
| receive_utc_ticks | 同上,.NET UTC ticks | — |
### NPZ ### NPZ
@@ -72,12 +78,16 @@ lidar_session/
### frames_index.csv ### frames_index.csv
```text ```text
frame_id,filename,t_start,t_end 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 0,frames/frame_00000.npz,10.000,10.100,6389...,1754646005.12,6389...,1754646005.22
1,frames/frame_00001.npz,10.100,10.200
``` ```
也兼容旧列名 `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 ### 每帧 NPZ
+14
View File
@@ -5,6 +5,20 @@
--- ---
## 2026-08-09 14:30 (UTC+8)
### 导出:HI13 IMU + recovered dlog zip + 墙钟切窗
- **原本**IMU 只解 N300 FDILinkdlog 只认标准 `*.dorec`;无法按图上时段切窗。
- **改成**
- 新增 `tools/rscap_v2/hi13_imu.py`HI91g→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) ## 2026-08-05 09:00 (UTC+8)
### 导出:支持 H32 DLogCaptureMSOP+DIFOP)→ V1 ### 导出:支持 H32 DLogCaptureMSOP+DIFOP)→ V1
+48 -12
View File
@@ -21,10 +21,28 @@ def build_parser() -> argparse.ArgumentParser:
default=CalibrationMode.ROTATION_ONLY.value, default=CalibrationMode.ROTATION_ONLY.value,
) )
run = subcommands.add_parser("run", help="执行 V1 标定流水线") run = subcommands.add_parser(
run.add_argument("--session-id", default="session0") "run",
run.add_argument("--imu", required=True, help="IMU CSV/NPZ 路径") help="执行 V1 标定流水线(可重复 --imu/--lidar/--session-id 做多会话联合)",
run.add_argument("--lidar", required=True, help="LiDAR 会话目录(含 frames_index.csv") )
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("--vehicle-config", required=True, help="车辆配置 YAML")
run.add_argument("--output", required=True, help="输出目录") run.add_argument("--output", required=True, help="输出目录")
run.add_argument( run.add_argument(
@@ -39,6 +57,25 @@ def build_parser() -> argparse.ArgumentParser:
return parser 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: def main(argv: list[str] | None = None) -> int:
parser = build_parser() parser = build_parser()
args = parser.parse_args(argv) args = parser.parse_args(argv)
@@ -55,15 +92,10 @@ def main(argv: list[str] | None = None) -> int:
return 0 return 0
if args.command == "run": if args.command == "run":
sessions = _build_sessions(args)
request = CalibrationRequest( request = CalibrationRequest(
vehicle_config=Path(args.vehicle_config), vehicle_config=Path(args.vehicle_config),
sessions=( sessions=sessions,
SessionInput(
session_id=args.session_id,
imu_source=Path(args.imu),
lidar_source=Path(args.lidar),
),
),
requested_mode=CalibrationMode(args.mode), requested_mode=CalibrationMode(args.mode),
output_directory=Path(args.output), output_directory=Path(args.output),
max_iterations=args.max_iterations, 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"status: {result.status.value}")
print(f"message: {result.message}") print(f"message: {result.message}")
if result.time_offset_s is not None: 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: if result.T_IMU_lidar is not None:
print("T_IMU_lidar:") print("T_IMU_lidar:")
print(result.T_IMU_lidar) print(result.T_IMU_lidar)
+90 -18
View File
@@ -152,21 +152,21 @@ def _build_nav_rotations(
r_x: np.ndarray, r_x: np.ndarray,
t_x: np.ndarray, t_x: np.ndarray,
) -> list[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] rotations = [np.eye(3) for _ in keyframe_ids]
for k in range(len(keyframe_ids) - 1): for k in range(len(keyframe_ids) - 1):
a = keyframe_ids[k] a = keyframe_ids[k]
b = keyframe_ids[k + 1] b = keyframe_ids[k + 1]
pair = consecutive_pairs.get((a, b)) pair = consecutive_pairs.get((a, b))
if pair is None: 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 continue
t_b = np.zeros(3) if pair.t_B_m is None else np.asarray(pair.t_B_m, dtype=float) 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) r_meas, _ = _lidar_to_imu_relative(r_x, t_x, pair.R_B, t_b)
rotations[k + 1] = orthonormalize_rotation(rotations[k] @ r_meas) rotations[k + 1] = orthonormalize_rotation(rotations[k] @ r_meas)
# Ensure list indexed by id_to_idx
del id_to_idx
return rotations return rotations
@@ -178,6 +178,9 @@ def _solve_phase_c_se3(
gravity_init: np.ndarray, gravity_init: np.ndarray,
sigma_bg_rw: float = 1.0e-5, sigma_bg_rw: float = 1.0e-5,
sigma_ba_rw: float = 1.0e-3, 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]]: ) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray, float, float, list[str]]:
"""Keyframe IMU factor optimization for full SE(3).""" """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] usable = [pair for pair in pairs if pair.t_B_m is not None and "delta_v" in pair.metadata]
if len(usable) < 3: if len(usable) < 3:
notes.append("phase-C skipped: need pairs with full preintegration metadata") 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] = {} stamp: dict[int, float] = {}
kf_session: dict[int, str] = {}
for pair in usable: for pair in usable:
stamp[pair.i] = float(pair.metadata.get("t_i_imu_s", pair.t_i_s)) 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)) 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) k_count = len(keyframe_ids)
id_to_idx = {kid: idx for idx, kid in enumerate(keyframe_ids)} id_to_idx = {kid: idx for idx, kid in enumerate(keyframe_ids)}
consecutive_pairs: dict[tuple[int, int], MotionPair] = {} consecutive_pairs: dict[tuple[int, int], MotionPair] = {}
for pair in usable: 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: if id_to_idx[pair.j] == id_to_idx[pair.i] + 1:
consecutive_pairs[(pair.i, pair.j)] = pair 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) g0 = np.asarray(gravity_init, dtype=float).reshape(3)
if np.linalg.norm(g0) < 1e-6: if np.linalg.norm(g0) < 1e-6:
@@ -214,6 +232,15 @@ def _solve_phase_c_se3(
n_b = 3 * k_count n_b = 3 * k_count
dim = 3 + 3 + 2 + n_v + n_b + n_b dim = 3 + 3 + 2 + n_v + n_b + n_b
x0 = np.zeros(dim) 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 # velocities start at 0; biases at prior
for idx in range(k_count): for idx in range(k_count):
x0[8 + n_v + 3 * idx : 8 + n_v + 3 * idx + 3] = bg0 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)) w = np.sqrt(_pair_weight(pair))
out.append(w * (whiten @ err)) 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): 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_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)) 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_g * (bgs[k + 1] - bgs[k]))
out.append(scale_a * (bas[k + 1] - bas[k])) out.append(scale_a * (bas[k + 1] - bas[k]))
# Weak priors: first-keyframe biases and translation magnitude. # Weak priors: first keyframe of each session + CAD/installation translation.
out.append(50.0 * (bgs[0] - bg0)) for sid in session_ids:
out.append(20.0 * bas[0]) first = next(kid for kid in keyframe_ids if kf_session[kid] == sid)
out.append(0.2 * t_opt) # soft |t| prior ~ meters 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) return np.concatenate(out)
# Cap evaluations: Phase-C is high-dimensional; synthetic ICP already dominates runtime. # 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, gravity_init_m_s2: np.ndarray | None = None,
bias_prior_sigma_rad_s: float = 0.02, bias_prior_sigma_rad_s: float = 0.02,
enable_phase_c: bool | None = None, 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: ) -> JointExtrinsicResult:
"""Refine extrinsic using Phase-A whitened rotation factors, optional Phase-C SE(3).""" """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)) 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) 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) weights = np.asarray([_pair_weight(pair) for pair in usable], dtype=float)
whitens = [residual_whiten_matrix(_pair_cov(pair)) for pair in usable] whitens = [residual_whiten_matrix(_pair_cov(pair)) for pair in usable]
prior_w = 1.0 / max(bias_prior_sigma_rad_s, 1e-4) 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_errs.append(np.degrees(np.linalg.norm(err)))
rot_rms = float(np.sqrt(np.mean(np.square(rot_errs)))) if rot_errs else 1e9 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 translation_accepted = False
trans_rms = 1e9 trans_rms = 1e9
gravity_out: np.ndarray | None = None gravity_out: np.ndarray | None = None
@@ -400,6 +441,12 @@ def solve_joint_extrinsic(
else: else:
gravity_init = np.asarray(gravity_init_m_s2, dtype=float).reshape(3) 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 ( if (
enable_phase_c enable_phase_c
and not force_rotation_only and not force_rotation_only
@@ -412,12 +459,23 @@ def solve_joint_extrinsic(
r, r,
gyro_bias0=bias_out, gyro_bias0=bias_out,
gravity_init=gravity_init, 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) notes.extend(c_notes)
translation_accepted = bool(trans_rms < 0.75 and np.linalg.norm(t) > 1e-4) translation_accepted = bool(trans_rms < 0.75 and np.linalg.norm(t) > 1e-4)
if not translation_accepted: if not translation_accepted:
notes.append("phase-C translation residual/gate failed; keeping translation at zero") # Prefer CAD prior over silent zero when motion SE3 is rejected.
t = np.zeros(3) 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 ( elif (
not force_rotation_only not force_rotation_only
and observability.translation_observable and observability.translation_observable
@@ -437,9 +495,19 @@ def solve_joint_extrinsic(
pred = (pair.R_A - np.eye(3)) @ t_opt pred = (pair.R_A - np.eye(3)) @ t_opt
meas = r_opt @ np.asarray(pair.t_B_m, dtype=float) meas = r_opt @ np.asarray(pair.t_B_m, dtype=float)
residuals.append(np.sqrt(weight) * (pred - meas)) 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) 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) r = orthonormalize_rotation(so3_exp(opt_t.x[:3]) @ r)
t = opt_t.x[3:] t = opt_t.x[3:]
rot_errs = [] rot_errs = []
@@ -452,11 +520,15 @@ def solve_joint_extrinsic(
trans_errs.append(np.linalg.norm(pred - meas)) trans_errs.append(np.linalg.norm(pred - meas))
rot_rms = float(np.sqrt(np.mean(np.square(rot_errs)))) rot_rms = float(np.sqrt(np.mean(np.square(rot_errs))))
trans_rms = float(np.sqrt(np.mean(np.square(trans_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") notes.append(f"legacy translation refine rms={trans_rms:.3f} m")
if not translation_accepted: if not translation_accepted:
notes.append("translation residual too large; keeping translation at zero") notes.append("translation residual too large; keeping translation at zero")
t = np.zeros(3) 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: else:
notes.append("rotation-only extrinsic returned (phase-A; phase-C SE3 gated off)") notes.append("rotation-only extrinsic returned (phase-A; phase-C SE3 gated off)")
+164 -87
View File
@@ -2,7 +2,7 @@
from __future__ import annotations from __future__ import annotations
from dataclasses import asdict, dataclass from dataclasses import asdict, dataclass, replace
from pathlib import Path from pathlib import Path
from typing import Any from typing import Any
@@ -13,6 +13,7 @@ from .contracts import (
CalibrationRequest, CalibrationRequest,
CalibrationResult, CalibrationResult,
CalibrationStatus, CalibrationStatus,
MotionPair,
SessionInput, SessionInput,
) )
from .finalize import finalize_result from .finalize import finalize_result
@@ -26,7 +27,10 @@ from .motion_pairs import build_motion_pairs
from .rotation_handeye import solve_rotation_handeye from .rotation_handeye import solve_rotation_handeye
from .time_offset import TimeOffsetResult, estimate_time_offset, refine_time_offset_signed from .time_offset import TimeOffsetResult, estimate_time_offset, refine_time_offset_signed
from .timestamp_audit import audit_timestamps 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: def _merge_time_offset(previous: TimeOffsetResult, refined: TimeOffsetResult) -> TimeOffsetResult:
@@ -49,11 +53,11 @@ STAGES = (
PipelineStage("vehicle_config", "加载并校验当前车辆安装配置"), PipelineStage("vehicle_config", "加载并校验当前车辆安装配置"),
PipelineStage("timestamp_audit", "审查 IMU 与 LiDAR 时间域"), PipelineStage("timestamp_audit", "审查 IMU 与 LiDAR 时间域"),
PipelineStage("imu_audit", "审查单位、轴向启发与静止零偏"), PipelineStage("imu_audit", "审查单位、轴向启发与静止零偏"),
PipelineStage("time_offset", "粗估 δt,并用 R 做有符号三轴精修"), PipelineStage("time_offset", "各会话独立粗估/精修 δt"),
PipelineStage("lidar_motion", "关键帧、可选去畸变与 LiDAR 相对运动"), PipelineStage("lidar_motion", "各会话关键帧、可选去畸变与 LiDAR 相对运动"),
PipelineStage("motion_pairs", "IMU 预积分与雷达配准,构造相对运动对"), PipelineStage("motion_pairs", "各会话构造运动对,再合并"),
PipelineStage("rotation_handeye", "加权求解旋转外参"), PipelineStage("rotation_handeye", "用全部会话运动对联合求解旋转外参"),
PipelineStage("joint_optimizer", "联合精修;完整模式下可估计平移"), PipelineStage("joint_optimizer", "用全部会话运动对联合精修;完整模式平移"),
PipelineStage("finalize", "写出结果与质量报告"), PipelineStage("finalize", "写出结果与质量报告"),
) )
@@ -92,21 +96,34 @@ def _build_pairs_and_handeye(
return keyframes, pair_set, 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, session: SessionInput,
request: CalibrationRequest, request: CalibrationRequest,
vehicle_config: dict[str, Any] | None,
) -> dict[str, Any]: ) -> dict[str, Any]:
"""Per-session: audit, δt, keyframes/pairs. No joint extrinsic yet."""
imu = load_imu_samples(session.imu_source) imu = load_imu_samples(session.imu_source)
frames = load_lidar_frames(session.lidar_source) frames = load_lidar_frames(session.lidar_source)
ts = audit_timestamps(imu, frames) ts = audit_timestamps(imu, frames)
if not ts.ok: 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) imu_report = audit_imu(imu)
if not imu_report.ok: 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( offset = estimate_time_offset(
imu, imu,
@@ -115,7 +132,7 @@ def _session_details(
search_s=request.time_offset_search_s, search_s=request.time_offset_search_s,
) )
if not offset.ok: 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 working_frames = frames
r_x = np.eye(3) r_x = np.eye(3)
@@ -124,7 +141,6 @@ def _session_details(
keyframes = None keyframes = None
pairs_notes: list[str] = [] pairs_notes: list[str] = []
pair_count = 0 pair_count = 0
time_offset_notes = list(offset.notes)
for iteration in range(max(1, request.max_iterations)): for iteration in range(max(1, request.max_iterations)):
if iteration > 0: if iteration > 0:
@@ -145,22 +161,21 @@ def _session_details(
) )
pairs_notes = list(pair_set.notes) pairs_notes = list(pair_set.notes)
pair_count = len(pair_set.pairs) pair_count = len(pair_set.pairs)
if handeye.pair_count < 3: if pair_count < 3:
return { return {
"ok": False, "ok": False,
"stage": "rotation_handeye", "stage": "motion_pairs",
"session_id": session.session_id,
"iteration": iteration, "iteration": iteration,
"time_offset": asdict(offset), "time_offset": asdict(offset),
"imu_audit": asdict(imu_report), "imu_audit": asdict(imu_report),
"timestamp_audit": asdict(ts), "timestamp_audit": asdict(ts),
"keyframes": len(keyframes.indices), "keyframes": 0 if keyframes is None else len(keyframes.indices),
"pair_notes": pairs_notes, "pair_notes": pairs_notes,
"handeye": asdict(handeye), "handeye": asdict(handeye),
} }
# Use candidate R even if RMS gate failed, so signed δt refine can still run.
r_x = handeye.R_IMU_lidar r_x = handeye.R_IMU_lidar
# Phase-A: alternate signed δt refine with current R (up to 2 rounds).
for _ in range(2): for _ in range(2):
refined = refine_time_offset_signed( refined = refine_time_offset_signed(
imu, imu,
@@ -172,7 +187,6 @@ def _session_details(
) )
delta_shift = abs(refined.delta_t_s - offset.delta_t_s) delta_shift = abs(refined.delta_t_s - offset.delta_t_s)
offset = _merge_time_offset(offset, refined) offset = _merge_time_offset(offset, refined)
time_offset_notes = list(offset.notes)
if delta_shift < 1e-3: if delta_shift < 1e-3:
break break
keyframes, pair_set, handeye = _build_pairs_and_handeye( keyframes, pair_set, handeye = _build_pairs_and_handeye(
@@ -185,70 +199,47 @@ def _session_details(
) )
pairs_notes = list(pair_set.notes) pairs_notes = list(pair_set.notes)
pair_count = len(pair_set.pairs) pair_count = len(pair_set.pairs)
if handeye.pair_count < 3: if pair_count < 3:
return { return {
"ok": False, "ok": False,
"stage": "rotation_handeye", "stage": "motion_pairs",
"session_id": session.session_id,
"iteration": iteration, "iteration": iteration,
"time_offset": asdict(offset), "time_offset": asdict(offset),
"imu_audit": asdict(imu_report), "imu_audit": asdict(imu_report),
"timestamp_audit": asdict(ts), "timestamp_audit": asdict(ts),
"keyframes": len(keyframes.indices), "keyframes": 0 if keyframes is None else len(keyframes.indices),
"pair_notes": pairs_notes, "pair_notes": pairs_notes,
"handeye": asdict(handeye), "handeye": asdict(handeye),
} }
r_x = handeye.R_IMU_lidar 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 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_mean = np.asarray(imu_report.static_acc_mean_m_s2, dtype=float).reshape(3)
acc_n = float(np.linalg.norm(acc_mean)) acc_n = float(np.linalg.norm(acc_mean))
if acc_n > 1e-6: if acc_n > 1e-6:
gravity_init = -acc_mean * (9.80665 / acc_n) gravity_init = -acc_mean * (9.80665 / acc_n)
else: else:
gravity_init = np.array([0.0, 0.0, -9.80665]) 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 { return {
"ok": True, "ok": True,
"session_id": session.session_id, "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), "timestamp_audit": asdict(ts),
"imu_audit": { "imu_audit": {
**asdict(imu_report), **asdict(imu_report),
"gyro_bias_rad_s": imu_report.gyro_bias_rad_s.tolist(), "gyro_bias_rad_s": imu_report.gyro_bias_rad_s.tolist(),
"static_acc_mean_m_s2": imu_report.static_acc_mean_m_s2.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), "keyframes": len(keyframes.indices),
"pair_count": pair_count, "pair_count": pair_count,
"pair_notes": pairs_notes, "pair_notes": pairs_notes,
"handeye": { "handeye_local": {
"residual_rms_deg": handeye.residual_rms_deg, "residual_rms_deg": handeye.residual_rms_deg,
"residual_median_deg": handeye.residual_median_deg, "residual_median_deg": handeye.residual_median_deg,
"pair_count": handeye.pair_count, "pair_count": handeye.pair_count,
@@ -256,32 +247,30 @@ def _session_details(
"notes": handeye.notes, "notes": handeye.notes,
"R_IMU_lidar": handeye.R_IMU_lidar.tolist(), "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: 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: if not request.sessions:
return finalize_result( return finalize_result(
@@ -303,38 +292,123 @@ def run_calibration(request: CalibrationRequest) -> CalibrationResult:
output_directory=request.output_directory, output_directory=request.output_directory,
) )
session_results = [] prepared: list[dict[str, Any]] = []
for session in request.sessions: 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] all_pairs = _remap_pairs_for_joint(prepared)
if not primary.get("ok"): handeye = solve_rotation_handeye(all_pairs)
if not handeye.ok:
return finalize_result( return finalize_result(
status=CalibrationStatus.BLOCKED, status=CalibrationStatus.BLOCKED,
message=f"blocked at stage {primary.get('stage')}", message="blocked at stage rotation_handeye (joint)",
details={"sessions": session_results}, details={
"sessions": [_public_session(p) for p in prepared],
"joint_handeye": asdict(handeye),
"merged_pair_count": len(all_pairs),
},
output_directory=request.output_directory, output_directory=request.output_directory,
) )
T = np.asarray(primary["T_IMU_lidar"], dtype=float) force_rotation_only = request.requested_mode == CalibrationMode.ROTATION_ONLY
delta_t = float(primary["time_offset_s"]) 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 request.requested_mode == CalibrationMode.FULL_SE3:
if primary.get("translation_accepted"): if joint.translation_accepted:
status = CalibrationStatus.FULL_SE3_ACCEPTED status = CalibrationStatus.FULL_SE3_ACCEPTED
message = "full SE3 accepted" message = f"full SE3 accepted (joint {len(prepared)} sessions, {len(all_pairs)} pairs)"
else: else:
status = CalibrationStatus.FULL_SE3_REJECTED 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: else:
status = CalibrationStatus.ROTATION_ONLY_ACCEPTED 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 = T.copy()
T[:3, 3] = 0.0 T[:3, 3] = 0.0
return finalize_result( return finalize_result(
status=status, status=status,
message=message, 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, T_IMU_lidar=T,
time_offset_s=delta_t, time_offset_s=delta_t,
output_directory=request.output_directory, 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]: def _public_session(session_result: dict[str, Any]) -> dict[str, Any]:
payload = dict(session_result) payload = dict(session_result)
payload.pop("T_IMU_lidar", None) 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 return payload
+9 -3
View File
@@ -73,7 +73,10 @@ def _correlate_offset(
y0, y1, y2 = peaks y0, y1, y2 = peaks
denom = y0 - 2 * y1 + y2 denom = y0 - 2 * y1 + y2
if abs(denom) > 1e-12: 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) best_peak = float(y1)
return best_delta, best_peak 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) 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 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) 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) rotations, pair_times = estimate_frame_rotations(frames, stride=1)
if len(rotations) < 4: if len(rotations) < 4:
return TimeOffsetResult(0.0, 0.0, search_s, ("not enough LiDAR relative rotations",), False) return TimeOffsetResult(0.0, 0.0, search_s, ("not enough LiDAR relative rotations",), False)
+15 -1
View File
@@ -8,7 +8,8 @@ from pathlib import Path
import numpy as np import numpy as np
from tools.h32_dlog.difop import CHANNELS, HORIZONTAL_START, VERTICAL_START, parse_difop_angles 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.load_session import load_h32_dlog_lidar
from tools.h32_dlog.payload_v1 import ( from tools.h32_dlog.payload_v1 import (
MsopPacketItem, MsopPacketItem,
@@ -90,6 +91,19 @@ def _write_dorec_record(
return start 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(): def test_parse_msop_and_difop_payload_roundtrip():
packet = _make_msop_packet(seconds=1700000000, microseconds=123456) packet = _make_msop_packet(seconds=1700000000, microseconds=123456)
item = MsopPacketItem( item = MsopPacketItem(
+79
View File
@@ -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("<H", payload, 1, 0) # pps
payload[3] = 25 # temp
struct.pack_into("<f", payload, 4, 101325.0)
struct.pack_into("<I", payload, 8, device_ms)
struct.pack_into("<fff", payload, 12, *accel_g)
struct.pack_into("<fff", payload, 24, *gyro_dps)
# remaining mag/rpy/quat left zero
payload_length = len(payload)
header = bytearray(6)
header[0] = 0x5A
header[1] = 0xA5
header[2] = payload_length & 0xFF
header[3] = (payload_length >> 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
+212 -63
View File
@@ -1,13 +1,18 @@
#!/usr/bin/env python3 #!/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`` LiDAR sources (exactly one):
(raw MSOP + DIFOP DObjects; preferred for new recordings) - ``--lidar-dlog``: Medulla dlog dir **or recovered zip** (MSOP + DIFOP)
- ``--lidar-rscap``: legacy H32 MSOP V2 ``.rscap`` (MSOP-only defaults for angles) - ``--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 imu.csv
lidar/ lidar/
@@ -15,8 +20,9 @@ Output layout under --out:
frames/frame_XXXXX.npz frames/frame_XXXXX.npz
export_summary.json export_summary.json
Timestamps written into the intermediate format are **device times** Device times stay in ``t`` / ``t_start``/``t_end``. Host UTC receive times are
(N300 device_timestamp_us, H32 MSOP device timestamp), not host receive time. also written so LiDARIMU alignment can bridge clocks without forcing first-frame
device coincidence.
""" """
from __future__ import annotations from __future__ import annotations
@@ -34,26 +40,48 @@ if str(ROOT) not in sys.path:
sys.path.insert(0, str(ROOT)) sys.path.insert(0, str(ROOT))
from tools.h32_dlog.load_session import load_h32_dlog_lidar 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.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.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) path.parent.mkdir(parents=True, exist_ok=True)
with path.open("w", newline="", encoding="utf-8") as handle: with path.open("w", newline="", encoding="utf-8") as handle:
writer = csv.writer(handle) writer = csv.writer(handle)
writer.writerow(["t", "gx", "gy", "gz", "ax", "ay", "az"]) writer.writerow(
for index in range(t.shape[0]): [
"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( writer.writerow(
[ [
f"{t[index]:.9f}", f"{sample.t_s:.9f}",
f"{gyro[index, 0]:.12g}", f"{sample.gyro_rad_s[0]:.12g}",
f"{gyro[index, 1]:.12g}", f"{sample.gyro_rad_s[1]:.12g}",
f"{gyro[index, 2]:.12g}", f"{sample.gyro_rad_s[2]:.12g}",
f"{accel[index, 0]:.12g}", f"{sample.accel_m_s2[0]:.12g}",
f"{accel[index, 1]:.12g}", f"{sample.accel_m_s2[1]:.12g}",
f"{accel[index, 2]:.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" index_path = root / "frames_index.csv"
with index_path.open("w", newline="", encoding="utf-8") as handle: with index_path.open("w", newline="", encoding="utf-8") as handle:
writer = csv.writer(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 = [] point_counts = []
host_ok = 0
for index, frame in enumerate(frames): for index, frame in enumerate(frames):
rel = f"frames/frame_{index:05d}.npz" rel = f"frames/frame_{index:05d}.npz"
np.savez_compressed(root / rel, points=np.asarray(frame.points_xyz, dtype=np.float32)) 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( writer.writerow(
[ [
index, index,
rel, rel,
f"{frame.t_start_s:.9f}", f"{frame.t_start_s:.9f}",
f"{frame.t_end_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])) point_counts.append(int(frame.points_xyz.shape[0]))
return { return {
"frames": len(frames), "frames": len(frames),
"frames_with_host_utc": host_ok,
"points_min": int(min(point_counts)) if point_counts else 0, "points_min": int(min(point_counts)) if point_counts else 0,
"points_max": int(max(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, "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( def export_session(
*, *,
imu_rscap: Path, imu_rscap: list[Path] | Path,
out: Path, out: Path,
lidar_rscap: Path | None = None, lidar_rscap: Path | None = None,
lidar_dlog: Path | None = None, lidar_dlog: Path | None = None,
imu_kind: str = "auto",
msop_object: str = "frontlidar-msop-raw", msop_object: str = "frontlidar-msop-raw",
difop_object: str = "frontlidar-difop-raw", difop_object: str = "frontlidar-difop-raw",
require_difop: bool = False, require_difop: bool = False,
host_start: str | None = None,
host_end: str | None = None,
frame_stride: int = 1, frame_stride: int = 1,
max_points_per_frame: int | None = 80000, max_points_per_frame: int | None = 80000,
min_range_m: float = 0.3, min_range_m: float = 0.3,
@@ -106,24 +205,41 @@ def export_session(
if (lidar_rscap is None) == (lidar_dlog is None): if (lidar_rscap is None) == (lidar_dlog is None):
raise ValueError("provide exactly one of lidar_rscap or lidar_dlog") 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) out.mkdir(parents=True, exist_ok=True)
imu_capture = read_capture(imu_rscap) samples, imu_captures, kind = load_imu_samples(
imu_paths,
samples = iter_n300_imu_samples(imu_capture) kind=kind,
t, gyro, accel = samples_to_arrays(samples) host_ticks_min=imu_ticks_min,
host_ticks_max=imu_ticks_max,
)
t, _gyro, _accel = samples_to_arrays(samples)
imu_csv = out / "imu.csv" 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: if lidar_dlog is not None:
session = load_h32_dlog_lidar( session = load_h32_dlog_lidar(
lidar_dlog, lidar_dlog,
msop_object=msop_object, msop_object=msop_object,
difop_object=difop_object, difop_object=difop_object,
require_difop=require_difop, require_difop=require_difop,
host_ticks_min=lidar_ticks_min,
host_ticks_max=lidar_ticks_max,
) )
frames = iter_h32_frames_from_packets( frames = iter_h32_frames_from_packets(
session.msop_packets, session.msop_packets,
host_utc_ticks=session.msop_host_utc_ticks,
min_frame_points=min_frame_points, min_frame_points=min_frame_points,
frame_stride=frame_stride, frame_stride=frame_stride,
min_range_m=min_range_m, min_range_m=min_range_m,
@@ -134,16 +250,20 @@ def export_session(
) )
lidar_meta = { lidar_meta = {
"source": "dlog", "source": "dlog",
"lidar_dlog": str(session.dlog_root), "lidar_dlog": session.dlog_root,
"msop_object": session.msop_object, "msop_object": session.msop_object,
"difop_object": session.difop_object, "difop_object": session.difop_object,
"msop_packets": len(session.msop_packets), "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, "msop_batches": session.msop_batch_count,
"difop_records": session.difop_record_count, "difop_records": session.difop_record_count,
"session_id": session.session_id, "session_id": session.session_id,
"lidar_ip": session.lidar_ip, "lidar_ip": session.lidar_ip,
"angle_source": session.angle_source, "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: else:
assert lidar_rscap is not None assert lidar_rscap is not None
@@ -161,25 +281,46 @@ def export_session(
"lidar_rscap": str(lidar_rscap), "lidar_rscap": str(lidar_rscap),
"capture": file_summary(lidar_capture), "capture": file_summary(lidar_capture),
"angle_source": "default_msop_only_vertical_-16_to_16_deg", "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_dir = out / "lidar"
lidar_stats = write_lidar_session(lidar_dir, frames) 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 = { summary = {
"imu_rscap": str(imu_rscap), "imu_rscap": [str(path) for path in imu_paths],
"imu_kind": kind,
"out": str(out), "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": { "timestamp_policy": {
"imu": "n300_device_timestamp_us -> seconds", "imu_device": imu_time_note,
"lidar": lidar_meta["timestamp_note"], "imu_host": "rscap receive_utc_ticks -> t_host_utc_s",
"host_utc": "not used as calibration timeline", "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": { "imu": {
"samples": int(t.shape[0]), "samples": int(t.shape[0]),
"samples_with_host_utc": imu_host_ok,
"t_start": float(t[0]) if t.size else None, "t_start": float(t[0]) if t.size else None,
"t_end": float(t[-1]) 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": {
**lidar_stats, **lidar_stats,
@@ -201,41 +342,38 @@ def export_session(
def main() -> int: def main() -> int:
parser = argparse.ArgumentParser(description=__doc__) 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 = parser.add_mutually_exclusive_group(required=True)
lidar.add_argument( lidar.add_argument(
"--lidar-dlog", "--lidar-dlog",
type=Path, 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.add_argument(
"--lidar-rscap", "--lidar-rscap",
type=Path, type=Path,
help="Legacy H32 MSOP V2 .rscap (no DIFOP; default vertical angles)", help="Legacy H32 MSOP V2 .rscap",
)
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",
) )
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("--min-range-m", type=float, default=0.3)
parser.add_argument("--max-range-m", type=float, default=120.0) parser.add_argument("--max-range-m", type=float, default=120.0)
parser.add_argument("--min-frame-points", type=int, default=100) parser.add_argument("--min-frame-points", type=int, default=100)
@@ -245,9 +383,12 @@ def main() -> int:
imu_rscap=args.imu_rscap, imu_rscap=args.imu_rscap,
lidar_rscap=args.lidar_rscap, lidar_rscap=args.lidar_rscap,
lidar_dlog=args.lidar_dlog, lidar_dlog=args.lidar_dlog,
imu_kind=args.imu_kind,
msop_object=args.msop_object, msop_object=args.msop_object,
difop_object=args.difop_object, difop_object=args.difop_object,
require_difop=args.require_difop, require_difop=args.require_difop,
host_start=args.host_start,
host_end=args.host_end,
out=args.out, out=args.out,
frame_stride=args.frame_stride, frame_stride=args.frame_stride,
max_points_per_frame=max_points, max_points_per_frame=max_points,
@@ -258,10 +399,14 @@ def main() -> int:
print( print(
json.dumps( json.dumps(
{ {
"imu_kind": summary["imu_kind"],
"imu_samples": summary["imu"]["samples"], "imu_samples": summary["imu"]["samples"],
"imu_host_utc": summary["imu"]["samples_with_host_utc"],
"lidar_frames": summary["lidar"]["frames"], "lidar_frames": summary["lidar"]["frames"],
"lidar_host_utc": summary["lidar"]["frames_with_host_utc"],
"lidar_source": summary["lidar"]["source"], "lidar_source": summary["lidar"]["source"],
"angle_source": summary["lidar"]["angle_source"], "angle_source": summary["lidar"]["angle_source"],
"host_window": summary["host_window"],
"imu_csv": summary["outputs"]["imu_csv"], "imu_csv": summary["outputs"]["imu_csv"],
"lidar_session": summary["outputs"]["lidar_session"], "lidar_session": summary["outputs"]["lidar_session"],
"export_summary": str(Path(args.out) / "export_summary.json"), "export_summary": str(Path(args.out) / "export_summary.json"),
@@ -271,9 +416,13 @@ def main() -> int:
) )
) )
if summary["imu"]["samples"] == 0: 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: 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 return 0
+125
View File
@@ -0,0 +1,125 @@
#!/usr/bin/env python3
"""Export priority LiDARIMU 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())
+7 -1
View File
@@ -1,12 +1,18 @@
"""Medulla dlog readers for RSLidarH32_3D_DLogCaptureNet48 raw MSOP/DIFOP.""" """Medulla dlog readers for RSLidarH32_3D_DLogCaptureNet48 raw MSOP/DIFOP."""
from .difop import parse_difop_angles 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 .payload_v1 import parse_difop_payload, parse_msop_batch_payload
from .timeutil import local_wall_to_dotnet_ticks
__all__ = [ __all__ = [
"H32DlogLidarSession",
"discover_records", "discover_records",
"iter_payloads", "iter_payloads",
"load_h32_dlog_lidar",
"local_wall_to_dotnet_ticks",
"open_dlog_source",
"parse_difop_angles", "parse_difop_angles",
"parse_difop_payload", "parse_difop_payload",
"parse_msop_batch_payload", "parse_msop_batch_payload",
+283 -81
View File
@@ -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 from __future__ import annotations
import re import re
import struct import struct
import zipfile
from dataclasses import dataclass from dataclasses import dataclass
from pathlib import Path from pathlib import Path
from typing import BinaryIO, Iterator from typing import BinaryIO, Iterator
RECORD_RE = re.compile( RECORD_RE = re.compile(
r"^\[(?P<log_time>[^]]+)\].*?DObject `(?P<name>[^`]+)` post " r"^(?:\[(?P<log_time>[^]]+)\])?>?\s*DObject `(?P<name>[^`]+)` post "
r"len=(?P<len>\d+)B, id:(?P<id>[0-9A-Fa-f]+), tic:(?P<tic>\d+), " r"len=(?P<len>\d+)B, id:(?P<id>[0-9A-Fa-f]+), tic:(?P<tic>\d+), "
r"@(?P<file>[^:]+):(?P<offset>\d+)" r"@(?P<file>[^:]+):(?P<offset>\d+)"
) )
@@ -29,37 +37,225 @@ class RecordRef:
dotnet_ticks: int dotnet_ticks: int
def resolve_dlog_root(value: Path | str) -> Path: class _ZipStoredMemberIO:
root = Path(value).expanduser().resolve() """Random-access reader for a ZIP_STORED member via the underlying zip file.
if (root / "dobject").is_dir() and (root / "dobject_recording").is_dir():
return root ``ZipExtFile.seek`` on multi-GB members is far too slow for per-record reads.
child = root / "dlog" """
if (child / "dobject").is_dir() and (child / "dobject_recording").is_dir():
return child def __init__(self, zip_path: Path, member_name: str, data_offset: int, data_size: int):
raise FileNotFoundError(f"{root} does not contain dobject and dobject_recording") 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]: def _zip_stored_member_offset(zip_path: Path, info: zipfile.ZipInfo) -> int:
pending: list[tuple[str, str, str, int, int, str, int, str]] = [] if info.compress_type != zipfile.ZIP_STORED:
for log_path in sorted((dlog_root / "dobject").rglob("*.log")): raise RuntimeError(
relative_log = log_path.relative_to(dlog_root).as_posix() f"member {info.filename!r} is compressed (type={info.compress_type}); "
with log_path.open("r", encoding="utf-8", errors="replace") as stream: "extract it first or store uncompressed"
for line in stream: )
match = RECORD_RE.search(line) with zip_path.open("rb") as handle:
if not match or match.group("name").casefold() != object_name.casefold(): handle.seek(info.header_offset)
continue header = handle.read(30)
pending.append( if len(header) != 30 or header[:4] != b"PK\x03\x04":
( raise RuntimeError(f"bad local zip header for {info.filename!r}")
match.group("name"), name_len, extra_len = struct.unpack("<HH", header[26:30])
match.group("log_time"), return info.header_offset + 30 + name_len + extra_len
relative_log,
int(match.group("offset")),
int(match.group("len")), @dataclass
match.group("id").upper(), class DlogSource:
int(match.group("tic")), """Opened dlog directory or recovered zip."""
match.group("file"),
) label: str
directory: Path | None = None
zip_path: Path | None = None
_zip: zipfile.ZipFile | None = None
_log_cache: dict[str, str] | None = None
_member_offsets: dict[str, tuple[int, int]] | None = None
def close(self) -> 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])) pending.sort(key=lambda item: (item[6], item[7].casefold(), item[3]))
seen: set[tuple[str, int, int]] = set() seen: set[tuple[str, int, int]] = set()
records: list[RecordRef] = [] records: list[RecordRef] = []
@@ -84,10 +280,19 @@ def discover_records(dlog_root: Path, object_name: str) -> list[RecordRef]:
return records 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]]: def index_dorec_files(dlog_root: Path) -> dict[str, list[Path]]:
result: dict[str, list[Path]] = {} result: dict[str, list[Path]] = {}
for path in (dlog_root / "dobject_recording").rglob("*.dorec"): recording = dlog_root / "dobject_recording"
result.setdefault(path.name.casefold(), []).append(path) 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 return result
@@ -107,17 +312,15 @@ def read_exact(stream: BinaryIO, size: int) -> bytes:
return data return data
def read_record_payload(path: Path, record: RecordRef) -> bytes: def _read_payload_at(stream: BinaryIO, record: RecordRef) -> bytes:
with path.open("rb") as stream: stream.seek(record.source_offset)
stream.seek(record.source_offset) name_length = read_exact(stream, 1)[0]
name_length = read_exact(stream, 1)[0] name = read_exact(stream, name_length).decode("ascii")
name = read_exact(stream, name_length).decode("ascii") ticks = struct.unpack("<q", read_exact(stream, 8))[0]
ticks = struct.unpack("<q", read_exact(stream, 8))[0] id_length = read_exact(stream, 1)[0]
id_length = read_exact(stream, 1)[0] id_bytes = read_exact(stream, id_length)
id_bytes = read_exact(stream, id_length) payload_length = struct.unpack("<i", read_exact(stream, 4))[0]
payload_length = struct.unpack("<i", read_exact(stream, 4))[0] payload = read_exact(stream, payload_length)
payload = read_exact(stream, payload_length)
try: try:
record_id = id_bytes.decode("ascii") record_id = id_bytes.decode("ascii")
except UnicodeDecodeError: except UnicodeDecodeError:
@@ -133,47 +336,46 @@ def read_record_payload(path: Path, record: RecordRef) -> bytes:
return payload return payload
def iter_payloads(dlog_root: Path, object_name: str) -> Iterator[tuple[RecordRef, bytes]]: def iter_payloads_from_source(
root = resolve_dlog_root(dlog_root) source: DlogSource,
records = discover_records(root, object_name) 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: if not records:
return return
dorec_index = index_dorec_files(root) open_files: dict[str, BinaryIO] = {}
open_files: dict[str, tuple[Path, BinaryIO]] = {}
try: try:
for record in records: for record in records:
key = record.source_dorec.casefold() key = Path(record.source_dorec).name.casefold()
handle = open_files.get(key) stream = open_files.get(key)
if handle is None: if stream is None:
path = choose_dorec(dorec_index, record.source_dorec) _owner, stream = source.open_recording(record.source_dorec)
handle = (path, path.open("rb")) open_files[key] = stream
open_files[key] = handle yield record, _read_payload_at(stream, record)
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("<q", read_exact(stream, 8))[0]
id_length = read_exact(stream, 1)[0]
id_bytes = read_exact(stream, id_length)
payload_length = struct.unpack("<i", read_exact(stream, 4))[0]
payload = read_exact(stream, payload_length)
try:
record_id = id_bytes.decode("ascii")
except UnicodeDecodeError:
record_id = id_bytes.hex().upper()
if name != record.object_name:
raise ValueError(f"name mismatch: log={record.object_name}, dorec={name}")
if ticks != record.dotnet_ticks:
raise ValueError(f"tick mismatch: log={record.dotnet_ticks}, dorec={ticks}")
if payload_length != record.payload_length:
raise ValueError(
f"payload mismatch: log={record.payload_length}, dorec={payload_length}"
)
if record_id.upper() != record.log_record_id.upper():
raise ValueError(
f"record id mismatch: log={record.log_record_id}, dorec={record_id}"
)
yield record, payload
finally: finally:
for _path, stream in open_files.values(): for stream in open_files.values():
stream.close() stream.close()
def iter_payloads(
dlog_root: Path | str,
object_name: str,
*,
host_ticks_min: int | None = None,
host_ticks_max: int | None = None,
) -> 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,
)
+98 -57
View File
@@ -10,16 +10,21 @@ import numpy as np
from tools.rscap_v2.h32_msop import default_horizontal_deg, default_vertical_deg from tools.rscap_v2.h32_msop import default_horizontal_deg, default_vertical_deg
from .difop import DifopAngles, parse_difop_angles 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 from .payload_v1 import parse_difop_payload, parse_msop_batch_payload
@dataclass @dataclass
class H32DlogLidarSession: class H32DlogLidarSession:
dlog_root: Path dlog_root: str
msop_object: str msop_object: str
difop_object: str difop_object: str
msop_packets: list[bytes] msop_packets: list[bytes]
msop_host_utc_ticks: list[int]
msop_batch_count: int msop_batch_count: int
difop_record_count: int difop_record_count: int
angle_source: str angle_source: str
@@ -27,6 +32,8 @@ class H32DlogLidarSession:
horizontal_deg: np.ndarray horizontal_deg: np.ndarray
session_id: str | None = None session_id: str | None = None
lidar_ip: 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( def load_h32_dlog_lidar(
@@ -35,65 +42,99 @@ def load_h32_dlog_lidar(
msop_object: str = "frontlidar-msop-raw", msop_object: str = "frontlidar-msop-raw",
difop_object: str = "frontlidar-difop-raw", difop_object: str = "frontlidar-difop-raw",
require_difop: bool = False, require_difop: bool = False,
host_ticks_min: int | None = None,
host_ticks_max: int | None = None,
) -> H32DlogLidarSession: ) -> H32DlogLidarSession:
root = resolve_dlog_root(dlog_root) with open_dlog_source(dlog_root) as source:
msop_packets: list[bytes] = [] # DIFOP angles: prefer packets inside the window, else any in the capture.
batch_count = 0 angles: DifopAngles | None = None
session_id: str | None = None difop_count = 0
lidar_ip: str | None = None 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): if angles is None:
batch = parse_msop_batch_payload(payload) for _record, payload in iter_payloads_from_source(source, difop_object):
batch_count += 1 difop = parse_difop_payload(payload)
if session_id is None: difop_count += 1
session_id = batch.session_id try:
lidar_ip = batch.lidar_ip angles = parse_difop_angles(difop.raw)
for item in batch.packets: except ValueError:
msop_packets.append(item.raw) 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 msop_packets: list[bytes] = []
difop_count = 0 msop_host_utc_ticks: list[int] = []
for _record, payload in iter_payloads(root, difop_object): batch_count = 0
difop = parse_difop_payload(payload) for record, payload in iter_payloads_from_source(
difop_count += 1 source,
try: msop_object,
angles = parse_difop_angles(difop.raw) host_ticks_min=host_ticks_min,
except ValueError: host_ticks_max=host_ticks_max,
continue ):
if session_id is None: batch = parse_msop_batch_payload(payload)
session_id = difop.session_id batch_count += 1
lidar_ip = difop.lidar_ip 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: if not msop_packets:
msop_records = discover_records(root, msop_object) msop_records = discover_records_from_source(source, 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:
raise RuntimeError( 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( if angles is None:
dlog_root=root, if require_difop:
msop_object=msop_object, raise RuntimeError(
difop_object=difop_object, f"no valid DIFOP calibration from DObject {difop_object!r} under {source.label}"
msop_packets=msop_packets, )
msop_batch_count=batch_count, vertical = default_vertical_deg()
difop_record_count=difop_count, horizontal = default_horizontal_deg()
angle_source=angle_source, angle_source = "default_msop_only_vertical_-16_to_16_deg"
vertical_deg=vertical, else:
horizontal_deg=horizontal, vertical = angles.vertical_deg
session_id=session_id, horizontal = angles.horizontal_deg
lidar_ip=lidar_ip, 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,
)
+56
View File
@@ -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)
+41 -6
View File
@@ -15,7 +15,7 @@ and horizontal channel offsets default to 0.
from __future__ import annotations from __future__ import annotations
from dataclasses import dataclass from dataclasses import dataclass
from typing import Iterable from typing import Iterable, Sequence
import numpy as np import numpy as np
@@ -66,6 +66,8 @@ class LidarFrameExport:
t_start_s: float t_start_s: float
t_end_s: float t_end_s: float
points_xyz: np.ndarray # (N, 3) metres 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( def decode_packet_points(
@@ -144,6 +146,7 @@ def _block_points(
def iter_h32_frames_from_packets( def iter_h32_frames_from_packets(
packets: Iterable[bytes], packets: Iterable[bytes],
*, *,
host_utc_ticks: Sequence[int] | None = None,
min_frame_points: int = MIN_FRAME_POINTS_DEFAULT, min_frame_points: int = MIN_FRAME_POINTS_DEFAULT,
frame_stride: int = 1, frame_stride: int = 1,
min_range_m: float = 0.3, min_range_m: float = 0.3,
@@ -152,31 +155,49 @@ def iter_h32_frames_from_packets(
vertical_deg: np.ndarray | None = None, vertical_deg: np.ndarray | None = None,
horizontal_deg: np.ndarray | None = None, horizontal_deg: np.ndarray | None = None,
) -> list[LidarFrameExport]: ) -> 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) 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) 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,): if vertical.shape != (CHANNELS,) or horizontal.shape != (CHANNELS,):
raise ValueError(f"vertical/horizontal must have 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] = [] frames: list[LidarFrameExport] = []
point_chunks: list[np.ndarray] = [] point_chunks: list[np.ndarray] = []
t_start: float | None = None t_start: float | None = None
t_end: float | None = None t_end: float | None = None
host_start: int | None = None
host_end: int | None = None
prev_az: float | None = None prev_az: float | None = None
kept = 0 kept = 0
stride = max(1, int(frame_stride)) stride = max(1, int(frame_stride))
def emit() -> None: 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: if not point_chunks or t_start is None or t_end is None:
point_chunks = [] point_chunks = []
t_start = t_end = None t_start = t_end = None
host_start = host_end = None
return return
points = np.vstack(point_chunks) points = np.vstack(point_chunks)
point_chunks = [] point_chunks = []
start_s, end_s = t_start, t_end 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 t_start = t_end = None
host_start = host_end = None
if points.shape[0] < min_frame_points: if points.shape[0] < min_frame_points:
return return
if kept % stride != 0: if kept % stride != 0:
@@ -188,12 +209,21 @@ def iter_h32_frames_from_packets(
points = points[select] points = points[select]
if end_s <= start_s: if end_s <= start_s:
end_s = start_s + 0.1 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: if len(packet) != PACKET_LENGTH:
continue continue
packet_t = device_timestamp_ms(packet) * 1e-3 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) unit = distance_unit_mm(packet)
idx = DATA_START idx = DATA_START
for _block in range(BLOCKS): for _block in range(BLOCKS):
@@ -216,7 +246,9 @@ def iter_h32_frames_from_packets(
if pts.shape[0]: if pts.shape[0]:
if t_start is None: if t_start is None:
t_start = packet_t t_start = packet_t
host_start = packet_host
t_end = packet_t t_end = packet_t
host_end = packet_host
point_chunks.append(pts) point_chunks.append(pts)
idx += BLOCK_LENGTH idx += BLOCK_LENGTH
@@ -237,8 +269,11 @@ def iter_h32_frames(
) -> list[LidarFrameExport]: ) -> list[LidarFrameExport]:
"""Assemble MSOP packets from a V2 .rscap capture into frames.""" """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( 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, min_frame_points=min_frame_points,
frame_stride=frame_stride, frame_stride=frame_stride,
min_range_m=min_range_m, min_range_m=min_range_m,
+136
View File
@@ -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("<I", raw, 14)[0]
ax, ay, az = struct.unpack_from("<fff", raw, 18)
gx, gy, gz = struct.unpack_from("<fff", raw, 30)
gyro = (gx * DEG2RAD, gy * DEG2RAD, gz * DEG2RAD)
accel = (ax * G0, ay * G0, az * G0)
return gyro, accel, int(device_ms)
def iter_hi13_imu_samples(
capture: CaptureFile,
*,
host_utc_ticks_min: int | None = None,
host_utc_ticks_max: int | None = None,
) -> 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",
]
+50
View File
@@ -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())
+292
View File
@@ -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())