Compare commits
2
Commits
30f7e66db3
...
c2f99b94a2
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
c2f99b94a2 | ||
|
|
2237be77a4 |
@@ -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
|
||||||
```
|
```
|
||||||
|
|||||||
@@ -0,0 +1,84 @@
|
|||||||
|
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
|
||||||
|
(translation only). LiDAR phase-center Z = CAD dZ + 63.5 mm.
|
||||||
|
IMU axes: 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 图二:后轮轴中心 → IMU,单位 m(mm/1000)
|
||||||
|
# dX=2574.126255, dY=36.5, dZ=892.5
|
||||||
|
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:
|
||||||
|
# CAD 图一:后轮轴中心 → 雷达安装点,再加相位中心 +63.5 mm(仅 Z)
|
||||||
|
# dX=2522.276859, dY=0.020526, dZ=1637.499879+63.5=1700.999879
|
||||||
|
translation_m: [2.522276859, 0.000020526, 1.700999879]
|
||||||
|
# 假设雷达系与车体 CAD 轴一致(导出 XYZ 已按此约定)
|
||||||
|
rotation_matrix_body_lidar: [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]
|
||||||
|
rotation_quaternion_xyzw: null
|
||||||
|
source: "CAD dX/dY/dZ + phase-center +63.5mm on Z; attitude assumed = body"
|
||||||
|
|
||||||
|
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.808499879]
|
||||||
|
t_lidar_from_imu_in_body_m: [-0.051849396, -0.036479474, 0.808499879]
|
||||||
|
notes: >
|
||||||
|
Rotation prior is ~90 deg yaw between body/lidar (X-fwd) and IMU RFU (Y-fwd).
|
||||||
|
Translation prior from CAD + LiDAR phase-center offset; use for full_se3 /
|
||||||
|
sanity, not as hard lock for rotation_only.
|
||||||
|
|
||||||
|
initialization:
|
||||||
|
translation_prior:
|
||||||
|
enabled: true
|
||||||
|
sigma_m: [0.05, 0.05, 0.05]
|
||||||
|
t_IMU_lidar_m: [0.036479474, -0.051849396, 0.808499879]
|
||||||
|
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
@@ -4,20 +4,24 @@
|
|||||||
|
|
||||||
## 从原始数据导出
|
## 从原始数据导出
|
||||||
|
|
||||||
**推荐(新 H32 插件 `RSLidarH32_3D_DLogCaptureNet48`):** N300 `.rscap` + 雷达 Medulla dlog(含 raw MSOP / DIFOP)。
|
**推荐(新 H32 + HI13):** HI13 `.rscap` + 雷达 Medulla dlog / recovered zip(raw MSOP + DIFOP)。
|
||||||
|
|
||||||
```powershell
|
```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
|
||||||
|
|
||||||
|
|||||||
@@ -5,6 +5,20 @@
|
|||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
|
## 2026-08-09 14:30 (UTC+8)
|
||||||
|
|
||||||
|
### 导出:HI13 IMU + recovered dlog zip + 墙钟切窗
|
||||||
|
|
||||||
|
- **原本**:IMU 只解 N300 FDILink;dlog 只认标准 `*.dorec`;无法按图上时段切窗。
|
||||||
|
- **改成**:
|
||||||
|
- 新增 `tools/rscap_v2/hi13_imu.py`(HI91:g→m/s²、°/s→rad/s、设备 ms)。
|
||||||
|
- `h32_dlog` 支持 recovered zip(`indices.log` + `data.bin`),ZIP_STORED 成员按文件绝对 offset 直读。
|
||||||
|
- `export_rscap_to_v1.py`:`--imu-kind hi13|n300|auto`、多段 `--imu-rscap`、`--host-start/end` 切窗。
|
||||||
|
- 辅助脚本 `tools/export_usable_20260808_windows.py` 导出优先运动段。
|
||||||
|
- **未推送**(按用户要求本地改完即可)。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
## 2026-08-05 09:00 (UTC+8)
|
## 2026-08-05 09:00 (UTC+8)
|
||||||
|
|
||||||
### 导出:支持 H32 DLogCapture(MSOP+DIFOP)→ V1
|
### 导出:支持 H32 DLogCapture(MSOP+DIFOP)→ V1
|
||||||
|
|||||||
+48
-12
@@ -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)
|
||||||
|
|||||||
@@ -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
@@ -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
|
||||||
|
|||||||
@@ -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)
|
||||||
@@ -131,8 +137,16 @@ def estimate_time_offset(
|
|||||||
f"LiDAR mean pair rotation {np.mean([rotation_angle_deg(r) for r in rotations]):.2f} deg"
|
f"LiDAR mean pair rotation {np.mean([rotation_angle_deg(r) for r in rotations]):.2f} deg"
|
||||||
)
|
)
|
||||||
notes.append(f"searched delta_t in ±{search_s:.3f}s by direct correlation")
|
notes.append(f"searched delta_t in ±{search_s:.3f}s by direct correlation")
|
||||||
ok = peak > 0.15
|
# Host-UTC-bridged sessions are already on one timeline; |ω| peak can stay
|
||||||
if not ok:
|
# weak even at the correct lag (ICP rate vs gyro scale). Accept near-zero δt.
|
||||||
|
near_zero = abs(float(delta)) <= min(0.05, 0.25 * float(search_s))
|
||||||
|
ok = peak > 0.15 or near_zero
|
||||||
|
if peak <= 0.15 and near_zero:
|
||||||
|
notes.append(
|
||||||
|
f"correlation peak weak ({peak:.3f}) but |delta_t|={abs(delta):.4f}s ~0; "
|
||||||
|
"accepting as already-aligned (e.g. host UTC bridge)"
|
||||||
|
)
|
||||||
|
elif not ok:
|
||||||
notes.append("correlation peak is weak; check overlapping motion and axis units")
|
notes.append("correlation peak is weak; check overlapping motion and axis units")
|
||||||
return TimeOffsetResult(
|
return TimeOffsetResult(
|
||||||
delta_t_s=delta,
|
delta_t_s=delta,
|
||||||
|
|||||||
@@ -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(
|
||||||
|
|||||||
@@ -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
@@ -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 LiDAR–IMU 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
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -0,0 +1,125 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
"""Export priority LiDAR–IMU windows from calibration_usable_20260808.
|
||||||
|
|
||||||
|
Does not push anything; writes local V1 sessions under --out-root.
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import argparse
|
||||||
|
import json
|
||||||
|
import sys
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
|
ROOT = Path(__file__).resolve().parents[1]
|
||||||
|
if str(ROOT) not in sys.path:
|
||||||
|
sys.path.insert(0, str(ROOT))
|
||||||
|
|
||||||
|
from tools.export_rscap_to_v1 import export_session
|
||||||
|
|
||||||
|
DEFAULT_DATA = Path(r"D:\data\calibration_usable_20260808")
|
||||||
|
LIDAR_ZIP = "lidar_dlog/dorec_recovered_20260808_171438_181422.zip"
|
||||||
|
IMU_MAIN = "imu_rscap/hi13r4-imu_20260808-092827.638_39783edb-e46e-4b28-a5e8-427b981c2fce.rscap"
|
||||||
|
IMU_TAIL = "imu_rscap/hi13r4-imu_20260808-101022.036_87ea5edc-cd3d-4192-809a-469fbc8cac01.rscap"
|
||||||
|
|
||||||
|
# From usable-segment chart (local wall clock).
|
||||||
|
WINDOWS = [
|
||||||
|
{
|
||||||
|
"name": "priority_174005_174515",
|
||||||
|
"host_start": "2026-08-08T17:40:05",
|
||||||
|
"host_end": "2026-08-08T17:45:15",
|
||||||
|
"imu": [IMU_MAIN],
|
||||||
|
"priority": True,
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"name": "priority_174905_175450",
|
||||||
|
"host_start": "2026-08-08T17:49:05",
|
||||||
|
"host_end": "2026-08-08T17:54:50",
|
||||||
|
"imu": [IMU_MAIN],
|
||||||
|
"priority": True,
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"name": "priority_175910_180530",
|
||||||
|
"host_start": "2026-08-08T17:59:10",
|
||||||
|
"host_end": "2026-08-08T18:05:30",
|
||||||
|
"imu": [IMU_MAIN],
|
||||||
|
"priority": True,
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"name": "usable_181035_181050",
|
||||||
|
"host_start": "2026-08-08T18:10:35",
|
||||||
|
"host_end": "2026-08-08T18:10:50",
|
||||||
|
"imu": [IMU_TAIL],
|
||||||
|
"priority": False,
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"name": "usable_181225_181300",
|
||||||
|
"host_start": "2026-08-08T18:12:25",
|
||||||
|
"host_end": "2026-08-08T18:13:00",
|
||||||
|
"imu": [IMU_TAIL],
|
||||||
|
"priority": False,
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"name": "usable_181350_181410",
|
||||||
|
"host_start": "2026-08-08T18:13:50",
|
||||||
|
"host_end": "2026-08-08T18:14:10",
|
||||||
|
"imu": [IMU_TAIL],
|
||||||
|
"priority": False,
|
||||||
|
},
|
||||||
|
]
|
||||||
|
|
||||||
|
|
||||||
|
def main() -> int:
|
||||||
|
parser = argparse.ArgumentParser(description=__doc__)
|
||||||
|
parser.add_argument("--data-root", type=Path, default=DEFAULT_DATA)
|
||||||
|
parser.add_argument(
|
||||||
|
"--out-root",
|
||||||
|
type=Path,
|
||||||
|
default=DEFAULT_DATA / "sessions_v1",
|
||||||
|
)
|
||||||
|
parser.add_argument("--priority-only", action="store_true", default=True)
|
||||||
|
parser.add_argument("--all-windows", action="store_true")
|
||||||
|
parser.add_argument("--frame-stride", type=int, default=5)
|
||||||
|
parser.add_argument("--max-points-per-frame", type=int, default=40000)
|
||||||
|
args = parser.parse_args()
|
||||||
|
priority_only = not args.all_windows
|
||||||
|
lidar = args.data_root / LIDAR_ZIP
|
||||||
|
if not lidar.is_file():
|
||||||
|
raise SystemExit(f"missing lidar zip: {lidar}")
|
||||||
|
|
||||||
|
selected = [w for w in WINDOWS if (not priority_only) or w["priority"]]
|
||||||
|
results = []
|
||||||
|
for window in selected:
|
||||||
|
out = args.out_root / window["name"]
|
||||||
|
imu_paths = [args.data_root / rel for rel in window["imu"]]
|
||||||
|
print(f"=== exporting {window['name']} ===", flush=True)
|
||||||
|
summary = export_session(
|
||||||
|
imu_rscap=imu_paths,
|
||||||
|
lidar_dlog=lidar,
|
||||||
|
imu_kind="hi13",
|
||||||
|
require_difop=True,
|
||||||
|
host_start=window["host_start"],
|
||||||
|
host_end=window["host_end"],
|
||||||
|
out=out,
|
||||||
|
frame_stride=args.frame_stride,
|
||||||
|
max_points_per_frame=args.max_points_per_frame,
|
||||||
|
)
|
||||||
|
brief = {
|
||||||
|
"name": window["name"],
|
||||||
|
"imu_samples": summary["imu"]["samples"],
|
||||||
|
"lidar_frames": summary["lidar"]["frames"],
|
||||||
|
"angle_source": summary["lidar"]["angle_source"],
|
||||||
|
"out": str(out),
|
||||||
|
}
|
||||||
|
results.append(brief)
|
||||||
|
print(json.dumps(brief, ensure_ascii=False, indent=2), flush=True)
|
||||||
|
|
||||||
|
manifest = args.out_root / "export_windows_manifest.json"
|
||||||
|
args.out_root.mkdir(parents=True, exist_ok=True)
|
||||||
|
manifest.write_text(json.dumps(results, ensure_ascii=False, indent=2) + "\n", encoding="utf-8")
|
||||||
|
print(f"manifest: {manifest}")
|
||||||
|
return 0
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
raise SystemExit(main())
|
||||||
@@ -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
@@ -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,
|
||||||
|
)
|
||||||
|
|||||||
@@ -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,
|
||||||
|
)
|
||||||
|
|||||||
@@ -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)
|
||||||
@@ -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,
|
||||||
|
|||||||
@@ -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",
|
||||||
|
]
|
||||||
@@ -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())
|
||||||
@@ -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())
|
||||||
Reference in New Issue
Block a user