From 6b7844a977588d25e700121541aaecc95b88d9fc Mon Sep 17 00:00:00 2001 From: "lichun.qu" <16975270+zzqlc@user.noreply.gitee.com> Date: Wed, 22 Jul 2026 17:09:20 +0800 Subject: [PATCH] =?UTF-8?q?=E6=9B=B4=E6=96=B0README=E5=B9=B6=E6=B7=BB?= =?UTF-8?q?=E5=8A=A0=E6=A0=87=E5=AE=9A=E5=B7=A5=E5=85=B7?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 173 ++- tools/frontlidar_dlog_export.py | 1963 ++++++++++++++++++++++++++++++ tools/prepare_station_dataset.py | 194 +++ 3 files changed, 2329 insertions(+), 1 deletion(-) create mode 100644 tools/frontlidar_dlog_export.py create mode 100644 tools/prepare_station_dataset.py diff --git a/README.md b/README.md index 56d6c08..0e9fd24 100644 --- a/README.md +++ b/README.md @@ -99,6 +99,19 @@ X=T_{body\leftarrow lidar} 这里的 Z Bootstrap 只描述当前地面模型条件下的内部稳定性，不代表机械高度的绝对精度。 重要限制：旋转 RMS 仍约 1°，所以这不是厘米级绝对精度认证。远距离点云仍可能看到角度错层；Bootstrap 也不包含 RTK 参考点、航向偏置或杆臂定义错误等系统误差。 +### `code/` 文件职责 + +`code/` 中的 Python 脚本按“生成 B → 独立筛选 B → 求解 X → 汇总/诊断”的顺序组织。通常应通过 `run/` 下的 PowerShell 脚本调度，而不是手工跳过中间筛选步骤。 + +| 文件 | 职责 | 主要输入 → 主要输出 | +|---|---|---| +| [`rigorous_calibration.py`](code/rigorous_calibration.py) | **主求解器**，实现坐标变换、Open3D GICP/small_gicp 配准、质量计算和手眼优化。`ground` 从 `points_raw` 做 RANSAC 地面平面；`pairs` 将 RTK 车体轨迹构造成 A、将点云配准构造成 B，并输出海森矩阵、留出点、正反向和多初值质量报告；`calibrate` 用 Huber 鲁棒最小二乘、地面约束、多初值和 Bootstrap 求 `T_body_lidar`；`validate` 只计算给定 X 在独立 B 集上的 AX 残差。 | prepared 的 `frames_all`、车体位姿 CSV、B/地面文件 → `.npz` B 集、质量 JSON/CSV、外参 JSON、辅助复核 JSON。 | +| [`refine_pairs.py`](code/refine_pairs.py) | **第二级 B 精筛器**。按留出点重叠率/RMSE、旋转共轭不变量、正反向一致性剔除差的 B；不读取 X，也不用 AX 残差选 B。 | 初始 B `.npz` + 配准质量 JSON → `*_refined.npz` + `*.refinement.json` 审计记录。 | +| [`cross_backend_filter.py`](code/cross_backend_filter.py) | **跨后端一致性门控**。只保留 Open3D 与 small_gicp 对同一运动对的 B 差异不超过默认 `0.05 m / 0.50°` 的边；最终 B 本体采用 Open3D，small_gicp 仅作为独立门控。 | 两套 refined B `.npz` → `consensus/B_batch*_consensus.npz` + `*.consensus.json`。 | +| [`finalize_consensus.py`](code/finalize_consensus.py) | **发布共识解**。读取共识 B 求得的外参、第一批辅助复核和两单后端外参，写出带选择依据的 `final_extrinsic_recommended.json` 与 `final_summary.json`。 | 共识外参/复核 JSON、两后端外参 JSON → 推荐外参和最终摘要 JSON。 | +| [`summarize_results.py`](code/summarize_results.py) | **单后端阶段汇总器**。比较 Open3D 与 small_gicp 的外参差异、第二批拟合残差和第一批辅助复核，生成 `comparison_summary.json`；完整流程随后由 `finalize_consensus.py` 以共识解覆盖推荐结果。 | 两单后端外参、质量、辅助复核 JSON → 对比摘要 JSON 与阶段性推荐 JSON。 | +| [`scan_extrinsic_sensitivity.py`](code/scan_extrinsic_sensitivity.py) | **局部灵敏度诊断器**。对 X 在车体系左乘小的 roll/pitch/yaw 修正，比较某一对与全部 B 对的残差、改善/变差数量、地面指标和 Z 可观性。只生成扫描 JSON/CSV，**绝不改写 X**。 | B `.npz`、外参 JSON、可选地面 CSV → `diagnostics/*_scan.json/.csv`。 | +| [`visualize_pair_3d.py`](code/visualize_pair_3d.py) | **交互式 3D 点云核查器**。蓝色为目标站 i、橙色为源站 j；按键 1/2/3/4 分别查看原始、RTK 初值 A、GICP B、外参预测 `X^-1 A X`，并打印 `B^-1(X^-1AX)` 的 cm/deg 增量。可选按键 5 显示临时 RPY 修正，修正不写入文件。 | 站点点云、B `.npz`、外参 JSON → Open3D 交互窗口与终端增量。 | ## 坐标与公式 @@ -149,6 +162,9 @@ run/ run_sensitivity_scan.ps1 view_open3d_result.ps1 view_small_gicp_result.ps1 +tools/ + frontlidar_dlog_export.py # 从单个原始 dlog 导出 LiDAR NPZ 与 RTK sidecar + prepare_station_dataset.py # 从逐站导出结果构建标定所需 prepared 数据集 results/ common/ open3d_gicp/ @@ -160,7 +176,162 @@ results/ final_summary.json ``` -## 安装与完整重跑 +## 从原始数据到最终外参 + +下面是从云盘中的**原始 dlog**到最终 LiDAR—车体外参的完整流程。第 2、3 步由本仓库的 tools/ 脚本完成；第 4 步及之后由 run/ 调度。不要把云盘中的原始数据直接传给 run_all.ps1。 + +```text +原始静止站点 dlog（两批） + → 逐站导出 LiDAR 原始点云 + RTK 旁路表（NPZ） + → 逐站质量筛选、选取静止帧、重建 RTK 车体位姿（prepared） + → Open3D / small_gicp 分别求 B + → X 无关的 B 精筛与跨后端共识 + → AX=XB + 地面平面约束求 X + → 第一批辅助复核、3D 点云核查、确认部署 JSON +``` + +### 1. 整理原始数据与标定元数据 + +每一个静止站点目录必须完整保留 LiDAR DObject 记录、对应 `.dorec` 文件和 RTK 日志。dlog 导出器要求站点目录至少具有： + +```text +/ + dobject/ # DObject 索引日志 + dobject_recording/ # 与索引对应的 .dorec 二进制记录 + GPS-POST-Z/ # RTK 文本记录；建议同时保留完整原始 N4 串口流 +``` + +两批数据要分开存放：第二批高频 RTK 用于主要求解，第一批仅用于辅助复核。与数据一同归档的标定元数据必须包括：LiDAR DObject 名称、时区、RTK 天线参考点、双天线基线方向、heading offset、天线到后轮轴中心的杆臂、车体系定义，以及 LiDAR/RTK/IMU 的时间基准。 + +对于后续新录数据，RTK 原始记录应保留 `gnss_week`、`gnss_tow_ms`、GGA UTC、`pitch_deg`、基线长度、解算状态/类型、标准差、HDOP 和完整原文。PC 接收时间只能作为延迟诊断，不能替代 GNSS 测量时间。 + +### 2. 将每个原始站点 dlog 导出为 NPZ + +本仓库的 [tools/frontlidar_dlog_export.py](tools/frontlidar_dlog_export.py) 从每个静止站点的原始 dlog 导出 LiDAR 原始点云与 RTK sidecar。对每个站点分别执行；必须开启 RTK sidecar 与审计报告： + +```powershell +$Repo = "D:\Outdoor Ackerman Cart Sensor Adaptation\LiDAR_RTK_Calibration_Rigorous_20260721" +$Exporter = "$Repo\tools\frontlidar_dlog_export.py" +$RawStation = "<某一个原始静止站点 dlog 目录>" +$StationOut = "<工作目录>\export\001" + +python $Exporter ` + --dlog "$RawStation" ` + --out "$StationOut" ` + --object frontlidar ` + --format npz ` + --timezone +08:00 ` + --rtk-sidecars ` + --write-reports +``` + +导出后的站点目录应包含： + +```text +/ + frames/ # 多帧 LiDAR NPZ；每帧保留 points_raw + rtk/ # gps_post_z.csv/jsonl/npz 等 RTK 旁路表 + reports/manifest.csv # 每帧导出状态、时间及 RTK 匹配信息 + reports/validation_report.json + reports/metadata.json +``` + +先检查每个站点的 `validation_report.json`、`manifest.csv` 和 `rtk/gps_post_z.csv`：LiDAR payload 必须可读；候选帧必须存在；RTK 位置/航向有效；并记录 LiDAR—RTK 时间差。若新插件已开始输出 GNSS 周/周内时间与 pitch，导出器也必须同步扩展并写出这些字段；旧导出器只保留旧插件字段时，不能伪称已完成严格时间同步。 + +### 3. 从导出 NPZ 构建 prepared 静止站点数据 + +对每个批次，将所有站点导出目录汇总后执行以下预处理逻辑，生成供本仓库使用的 `prepared` 目录： + +1. 每个站点只选择一帧**车辆已静止**且 RTK 质量合格的 LiDAR 帧；通常取有效候选帧中的中间帧，避免启动/停车边缘。 +2. 仅保留 `position_valid=true`、`heading_valid=true`、Fix 为 4 或 5，且航向标准差满足项目门限的 RTK 样本；记录每站的样本数、时间跨度、航向圆标准差与被剔除原因。 +3. 使用 GNSS 测量时间把位置、heading 和 pitch 配成同一时刻；将经纬高转换到同一 ENU 世界系；根据天线杆臂和双天线方向求后轮轴中心车体位姿 `T_WB`。双天线可提供 heading/pitch，但不能提供 roll；roll 在后续 LiDAR—IMU 流程中由 IMU 补充。 +4. 将选出的 LiDAR 帧原样复制到 `frames_all/`。标定程序从每帧的 `points_raw` 转为 LiDAR 传感器系 XYZ，不得使用已经变换到车体系的点。 +5. 输出时间戳车体位姿 CSV。当前主求解器要求列为： + +```text +time,x,y,z,qx,qy,qz,qw +``` + +最终目录必须类似： + +```text +/ + frames_all/ + station_01.npz + station_02.npz + ... + body_poses_rear_gga_raw_rear_to_front.csv + station_summary.csv # 推荐保留：站点质量和选择原因 + manifest.json # 推荐保留：参考点、杆臂、heading/pitch 定义与筛选配置 +``` + +本仓库的 [tools/prepare_station_dataset.py](tools/prepare_station_dataset.py) 完成上述固定站点筛选、点云复制和车体位姿重建。它要求每个站点均已由上一步使用 --rtk-sidecars --write-reports 导出。下面的参数仅用于复现本仓库历史结果：lat/lon/h 被假定为后天线相位中心，raw_heading_deg 被假定为后天线指向前天线；车体系为后轮轴中心、X 前 Y 左 Z 上。 + +~~~powershell +$Repo = "D:\Outdoor Ackerman Cart Sensor Adaptation\LiDAR_RTK_Calibration_Rigorous_20260721" +$Prepare = "$Repo\tools\prepare_station_dataset.py" + +# 第二批：主要求解数据。ExportBatch2 下为 38 个逐站导出目录。 +python $Prepare --export-root "" --output "" --expected-stations 38 --heading-std-limit-deg 0.5 --heading-offset-deg 21.226 --antenna-lever -0.320 -0.365 0.620 --pose-name rear_gga_raw_rear_to_front + +# 第一批：辅助复核数据。若航向质量较低，不应设置过严的 heading 标准差阈值。 +python $Prepare --export-root "" --output "" --expected-stations 38 --heading-offset-deg 21.226 --antenna-lever -0.320 -0.365 0.620 --pose-name rear_gga_raw_rear_to_front +~~~ + +命令完成后检查 /station_summary.csv：每行均应有有效 RTK 样本，且没有超出设定的航向离散度。manifest.json 记录实际采用的杆臂、航向偏移、ENU 原点和输入站点；它应与原始数据一同归档。 + +不要把上述 21.226° 和 [-0.320,-0.365,0.620] m 当成通用常数：它们是本车、后天线、后天线到前天线航向定义、以及后轮轴中心车体原点的历史配置。换车、换参考天线、改变车体原点或改变 rawHeading 定义后，必须先复核并替换这些参数，再重建 A。 + +当前导出器可复现本仓库历史日志，但历史 RTK 插件只记录工控机接收时间，并未保存 GNSS 周/周内时间、双天线 pitch、质量标准差及完整原始串口流。因此它不能把旧数据宣称为严格 GNSS 时间同步。插件升级后，应先扩展导出器以保存新字段，再以 GNSS 测量时间重建位置/航向（及可用的 pitch）轨迹。 +### 4. 安装标定依赖并运行第一阶段 + +```powershell +$Repo = "D:\Outdoor Ackerman Cart Sensor Adaptation\LiDAR_RTK_Calibration_Rigorous_20260721" +python -m pip install -r "$Repo\requirements.txt" +python -c "import numpy, scipy, open3d, small_gicp; print('dependencies OK')" + +powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_all.ps1" ` + -Batch1Prepared "<第一批 prepared 目录>" ` + -Batch2Prepared "<第二批 prepared 目录>" +``` + +`run_all.ps1` 完成：第二批地面拟合；Open3D 与 small_gicp 两套 B 生成和精筛；两套单后端 X 求解；第一批独立 B 与辅助复核；以及单后端对比摘要。第一阶段至少应生成： + +```text +results/common/ground_planes_batch2.csv +results/open3d_gicp/B_batch2_refined.npz +results/small_gicp/B_batch2_refined.npz +results/open3d_gicp/extrinsic_batch2_refined.json +results/small_gicp/extrinsic_batch2_refined.json +results/comparison_summary.json +``` + +### 5. 构造跨后端共识并生成算法结果 + +第一阶段成功后执行： + +```powershell +powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_consensus_finish.ps1" +``` + +该步骤以 `0.05 m / 0.50°` 门限对同一运动对的 Open3D 与 small_gicp B 做一致性门控，用共识 B 重求第二批外参，并将第一批只用于辅助复核。主要输出为： + +```text +results/consensus/B_batch2_consensus.npz +results/consensus/extrinsic_batch2_consensus.json +results/consensus/batch1_auxiliary_check.json +results/final_extrinsic_recommended.json +results/final_summary.json +``` + +### 6. 验证、可视化与部署确认 + +先检查 `results/final_summary.json`：当前历史数据的参考值是第二批共识 39 对、AX RMS 约 `0.07985 m / 0.96118°`。若明显偏离，应从第 2、3 步检查站点选择、RTK 时间/参考点/杆臂/heading 定义，而不是直接手调 X。 + +再使用 3D 可视化检查：模式 3 为点云配准 B，模式 4 为外参预测 `X^-1AX`；终端打印 `B^-1(X^-1AX)` 的平移与旋转增量。只有 B 质量、全体运动对统计、地面约束和可视化均合理时，才可确认外参。 + +`final_extrinsic_recommended.json` 是完整算法审计结果。`run_consensus_finish.ps1` 不会自动覆盖 `results/final_extrinsic_deployment.json`；部署 JSON 必须在人工确认坐标定义和高度约束后明确写入。 +## prepared 阶段的快速重跑 ```powershell $Repo = "D:\Outdoor Ackerman Cart Sensor Adaptation\LiDAR_RTK_Calibration_Rigorous_20260721" diff --git a/tools/frontlidar_dlog_export.py b/tools/frontlidar_dlog_export.py new file mode 100644 index 0000000..9bfd2fa --- /dev/null +++ b/tools/frontlidar_dlog_export.py @@ -0,0 +1,1963 @@ +#!/usr/bin/env python3 +"""Export Medulla DObject lidar + RTK recordings to sharded NumPy NPZ / pickle. + +The default NPZ path uses only the Python standard library; NumPy is required +only when consuming the exported files, not while exporting them. +""" + +from __future__ import annotations + +import argparse +import array +import bisect +import csv +import io +import json +import math +import os +import pickle +import re +import struct +import sys +import time +import zipfile +from contextlib import nullcontext +from dataclasses import asdict, dataclass +from datetime import datetime, timedelta, timezone +from pathlib import Path +from typing import BinaryIO, Iterable, Iterator, Sequence + + +RECORD_RE = re.compile( + r"^\[(?P[^]]+)\].*?DObject `(?P[^`]+)` post " + r"len=(?P\d+)B, id:(?P[0-9A-Fa-f]+), tic:(?P\d+), " + r"@(?P[^:]+):(?P\d+)" +) +GPS_TEXT_RE = re.compile( + r"^\[(?P[^]]+)\].*?\$GPS-POST-Z>text\)>(?P.+)$" +) +POINT_STRUCT = struct.Struct("<5f") +DOTNET_UNIX_EPOCH_TICKS = 621355968000000000 +TICKS_PER_SECOND = 10_000_000 +TICKS_PER_DAY = 864_000_000_000 +NS_PER_TICK = 100 +FORMAT_VERSION = "medulla-lidar3d-rtk-v2" +RTK_OBJECT_NAMES = ("GPS-POST-Z", "rtk", "GPS-POST") + + +@dataclass(frozen=True) +class RecordRef: + sequence: int + object_name: str + log_time: str + source_log: str + source_dorec: str + source_offset: int + payload_length: int + log_record_id: str + dotnet_ticks: int + + +@dataclass +class RtkSample: + index: int + source: str + object_name: str + dotnet_ticks: int + timestamp_iso_local: str + unix_time_ns: int + log_time: str + record_id: str + source_log: str + source_dorec: str + source_offset: int + payload_length: int + counter: int | None = None + lat: float | None = None + lon: float | None = None + alt_m: float | None = None + raw_heading_deg: float | None = None + vehicle_heading_deg: float | None = None + fix: int | None = None + sat: int | None = None + position_valid: bool | None = None + heading_valid: bool | None = None + heading_solution: str | None = None + position_time: str | None = None + heading_time: str | None = None + last_line: str | None = None + device_name: str | None = None + device_stamp_hex: str | None = None + raw_text: str | None = None + parse_error: str | None = None + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser( + description=( + "Export Medulla frontlidar dlog records with matched RTK/GPS-POST-Z " + "to per-frame NPZ or pickle files." + ) + ) + parser.add_argument("--dlog", required=True, help="Directory containing dobject/ and dobject_recording/.") + parser.add_argument("--out", required=True, help="Output dataset directory.") + parser.add_argument("--object", default="frontlidar", help="Lidar DObject name; default: frontlidar.") + parser.add_argument("--format", choices=("npz", "pickle"), default="npz") + parser.add_argument("--timezone", default="+08:00", help="Fixed offset used to interpret DateTime.Now.Ticks.") + parser.add_argument("--stride", type=int, default=1, help="Export every Nth lidar frame.") + parser.add_argument("--max-frames", type=int, default=0, help="0 exports all selected frames.") + parser.add_argument("--resume", action="store_true", help="Keep already exported frame files.") + parser.add_argument("--compress", action="store_true", help="Use ZIP deflate level 1 for NPZ files.") + parser.add_argument( + "--include-xyz", + choices=("none", "sensor", "cart", "both"), + default="none", + help="Optionally generate XYZ arrays. Raw N x 5 data is always exported.", + ) + parser.add_argument("--x", type=float, default=736.0, help="Lidar X in vehicle frame, mm.") + parser.add_argument("--y", type=float, default=0.0, help="Lidar Y in vehicle frame, mm.") + parser.add_argument("--z", type=float, default=0.0, help="Lidar Z in vehicle frame, mm.") + parser.add_argument("--yaw", type=float, default=0.0, help="Yaw in degrees.") + parser.add_argument("--pitch", type=float, default=0.0, help="Pitch in degrees.") + parser.add_argument("--roll", type=float, default=0.0, help="Roll in degrees.") + parser.add_argument( + "--rtk-max-dt-ms", + type=float, + default=1000.0, + help=( + "Fail rtk_time_alignment when an interior frame's nearest |dt| exceeds this many " + "milliseconds. Edge frames before the first / after the last RTK are reported " + "separately and do not fail the check." + ), + ) + parser.add_argument( + "--skip-rtk", + action="store_true", + help="Export lidar only (no per-frame RTK matching).", + ) + parser.add_argument( + "--rtk-sidecars", + action="store_true", + help=( + "Also write rtk/ sidecars (gps_post_z / rtk_binary / gps_post / text). " + "Default is off; per-frame NPZ still embeds matched RTK fields." + ), + ) + parser.add_argument( + "--write-reports", + action="store_true", + help=( + "Write audit files under reports/ (metadata.json, manifest.csv, " + "rtk_match.csv, validation_report.json). Default package only has " + "frames/ and README.md." + ), + ) + args = parser.parse_args() + if args.stride < 1: + parser.error("--stride must be >= 1") + if args.max_frames < 0: + parser.error("--max-frames must be >= 0") + if args.rtk_max_dt_ms < 0: + parser.error("--rtk-max-dt-ms must be >= 0") + if args.skip_rtk and args.rtk_sidecars: + parser.error("--rtk-sidecars cannot be used with --skip-rtk") + parse_timezone(args.timezone) + return args + + +def resolve_dlog_root(value: str) -> Path: + root = Path(value).expanduser().resolve() + if (root / "dobject").is_dir() and (root / "dobject_recording").is_dir(): + return root + child = root / "dlog" + if (child / "dobject").is_dir() and (child / "dobject_recording").is_dir(): + return child + raise FileNotFoundError(f"{root} does not contain dobject and dobject_recording") + + +def parse_timezone(text: str) -> timezone: + match = re.fullmatch(r"([+-])(\d{2}):(\d{2})", text.strip()) + if not match: + raise ValueError(f"invalid timezone offset: {text!r}") + minutes = int(match.group(2)) * 60 + int(match.group(3)) + if match.group(1) == "-": + minutes = -minutes + return timezone(timedelta(minutes=minutes)) + + +def timezone_minutes(tz: timezone) -> int: + return int(tz.utcoffset(None).total_seconds() // 60) + + +def dotnet_ticks_to_values(ticks: int, tz: timezone) -> tuple[str, int]: + days, remainder = divmod(ticks, TICKS_PER_DAY) + seconds, subsecond_ticks = divmod(remainder, TICKS_PER_SECOND) + local_dt = datetime(1, 1, 1) + timedelta(days=days, seconds=seconds) + fraction = f"{subsecond_ticks:07d}" + offset = tz.utcoffset(None) + sign = "+" if offset >= timedelta(0) else "-" + total_minutes = abs(int(offset.total_seconds() // 60)) + iso = f"{local_dt:%Y-%m-%dT%H:%M:%S}.{fraction}{sign}{total_minutes // 60:02d}:{total_minutes % 60:02d}" + offset_ticks = int(offset.total_seconds()) * TICKS_PER_SECOND + unix_ns = (ticks - DOTNET_UNIX_EPOCH_TICKS - offset_ticks) * NS_PER_TICK + return iso, unix_ns + + +def discover_records(dlog_root: Path, object_name: str) -> list[RecordRef]: + pending: list[tuple[str, str, str, int, int, str, int, str]] = [] + for log_path in sorted((dlog_root / "dobject").rglob("*.log")): + relative_log = log_path.relative_to(dlog_root).as_posix() + with log_path.open("r", encoding="utf-8", errors="replace") as stream: + for line in stream: + match = RECORD_RE.search(line) + if not match or match.group("name").casefold() != object_name.casefold(): + continue + pending.append( + ( + match.group("name"), + match.group("log_time"), + relative_log, + int(match.group("offset")), + int(match.group("len")), + match.group("id").upper(), + int(match.group("tic")), + match.group("file"), + ) + ) + pending.sort(key=lambda item: (item[6], item[7].casefold(), item[3])) + seen: set[tuple[str, int, int]] = set() + records: list[RecordRef] = [] + for item in pending: + key = (item[7].casefold(), item[3], item[6]) + if key in seen: + continue + seen.add(key) + records.append( + RecordRef( + sequence=len(records), + object_name=item[0], + log_time=item[1], + source_log=item[2], + source_dorec=item[7], + source_offset=item[3], + payload_length=item[4], + log_record_id=item[5], + dotnet_ticks=item[6], + ) + ) + return records + + +def index_dorec_files(dlog_root: Path) -> dict[str, list[Path]]: + result: dict[str, list[Path]] = {} + for path in (dlog_root / "dobject_recording").rglob("*.dorec"): + result.setdefault(path.name.casefold(), []).append(path) + return result + + +def choose_dorec(index: dict[str, list[Path]], name: str) -> Path: + matches = index.get(Path(name).name.casefold(), []) + if not matches: + raise FileNotFoundError(f"missing recording file: {name}") + if len(matches) > 1: + raise RuntimeError(f"ambiguous recording file {name}: {matches}") + return matches[0] + + +def read_exact(stream: BinaryIO, size: int) -> bytes: + data = stream.read(size) + if len(data) != size: + raise EOFError(f"expected {size} bytes, got {len(data)}") + return data + + +def read_record(path: Path, record: RecordRef) -> tuple[dict[str, object], bytes]: + with path.open("rb") as stream: + stream.seek(record.source_offset) + name_length = read_exact(stream, 1)[0] + name = read_exact(stream, name_length).decode("ascii") + ticks = struct.unpack(" tuple[int, int, bytes]: + if len(payload) < 8: + raise ValueError(f"payload too short: {len(payload)}") + frame_counter, point_count = struct.unpack_from(" tuple[str, int]: + if pos >= len(data): + raise ValueError("truncated length-prefixed string") + length = data[pos] + pos += 1 + end = pos + length + if end > len(data): + raise ValueError("truncated length-prefixed string body") + return data[pos:end].decode("ascii", errors="replace"), end + + +def parse_bool_text(value: str) -> bool | None: + lowered = value.strip().casefold() + if lowered in ("true", "1", "yes"): + return True + if lowered in ("false", "0", "no"): + return False + return None + + +def parse_kv_payload(text: str, separators: Sequence[str] = ("=", ":")) -> dict[str, str]: + result: dict[str, str] = {} + for part in text.replace("\r", "").replace("\n", ",").split(","): + part = part.strip() + if not part: + continue + key = value = None + for sep in separators: + if sep in part: + key, value = part.split(sep, 1) + break + if key is None or value is None: + continue + result[key.strip()] = value.strip() + return result + + +def coerce_int(value: str | None) -> int | None: + if value is None or value == "": + return None + try: + return int(float(value)) + except ValueError: + return None + + +def coerce_float(value: str | None) -> float | None: + if value is None or value == "": + return None + try: + return float(value) + except ValueError: + return None + + +def parse_gps_post_z_text(text: str) -> dict[str, object]: + fields = parse_kv_payload(text, separators=("=", ":")) + alt = fields.get("alt", fields.get("h")) + vehicle_heading = fields.get("vehicleHeading", fields.get("th")) + return { + "counter": coerce_int(fields.get("counter")), + "lat": coerce_float(fields.get("lat")), + "lon": coerce_float(fields.get("lon")), + "alt_m": coerce_float(alt), + "raw_heading_deg": coerce_float(fields.get("rawHeading")), + "vehicle_heading_deg": coerce_float(vehicle_heading), + "fix": coerce_int(fields.get("fix")), + "sat": coerce_int(fields.get("sat")), + "position_valid": parse_bool_text(fields.get("positionValid", "")), + "heading_valid": parse_bool_text(fields.get("headingValid", "")), + "heading_solution": fields.get("headingSolution"), + "position_time": fields.get("positionTime"), + "heading_time": fields.get("headingTime"), + "last_line": fields.get("lastLine"), + "time_field": fields.get("time"), + "raw_fields": fields, + } + + +def parse_rtk_binary_payload(payload: bytes) -> dict[str, object]: + pos = 0 + device_name, pos = read_len_prefixed_ascii(payload, pos) + version, counter, unknown0 = struct.unpack_from("= len(payload): + raise ValueError("rtk payload truncated before validity flags") + position_valid = bool(payload[pos]) + pos += 1 + raw_heading, vehicle_heading = struct.unpack_from("= len(payload): + raise ValueError("rtk payload truncated before heading validity") + heading_valid = bool(payload[pos]) + pos += 1 + heading_solution, pos = read_len_prefixed_ascii(payload, pos) + has_nmea = bool(payload[pos]) if pos < len(payload) else False + if pos < len(payload): + pos += 1 + last_line = None + if has_nmea and pos < len(payload): + last_line, pos = read_len_prefixed_ascii(payload, pos) + return { + "device_name": device_name, + "version": version, + "counter": counter, + "unknown0": unknown0, + "device_stamp_hex": device_stamp.hex().upper(), + "position_time": position_time, + "heading_time": heading_time, + "lat": lat, + "lon": lon, + "alt_m": alt, + "reserved0": reserved0, + "reserved1": reserved1, + "fix": fix, + "sat": sat, + "position_valid": position_valid, + "raw_heading_deg": raw_heading, + "vehicle_heading_deg": vehicle_heading, + "heading_valid": heading_valid, + "heading_solution": heading_solution, + "last_line": last_line, + "bytes_consumed": pos, + "payload_length": len(payload), + } + + +def parse_gps_post_payload(payload: bytes) -> dict[str, object]: + pos = 0 + name, pos = read_len_prefixed_ascii(payload, pos) + if pos + 20 > len(payload): + raise ValueError("GPS-POST payload too short") + counter, unknown0 = struct.unpack_from(" tuple[float, ...]: + yaw, pitch, roll = map(math.radians, (yaw_deg, pitch_deg, roll_deg)) + cy, sy = math.cos(yaw), math.sin(yaw) + cp, sp = math.cos(pitch), math.sin(pitch) + cr, sr = math.cos(roll), math.sin(roll) + return ( + cy * cp, + -sy * cr + cy * sp * sr, + sy * sr + cy * sp * cr, + sy * cp, + cy * cr + sy * sp * sr, + -cy * sr + sy * sp * cr, + -sp, + cp * sr, + cp * cr, + ) + + +def generate_xyz( + raw_points: bytes, + mode: str, + translation: tuple[float, float, float], + rotation: tuple[float, ...], +) -> tuple[bytes | None, bytes | None]: + need_sensor = mode in ("sensor", "both") + need_cart = mode in ("cart", "both") + sensor = array.array("f") if need_sensor else None + cart = array.array("f") if need_cart else None + tx, ty, tz = translation + r = rotation + for distance, azimuth, altitude, _intensity, _progression in POINT_STRUCT.iter_unpack(raw_points): + alt = math.radians(altitude) + azi = math.radians(azimuth) + cos_alt = math.cos(alt) + sx = distance * cos_alt * math.cos(azi) + sy = distance * cos_alt * math.sin(azi) + sz = distance * math.sin(alt) + if sensor is not None: + sensor.extend((sx, sy, sz)) + if cart is not None: + cart.extend( + ( + r[0] * sx + r[1] * sy + r[2] * sz + tx, + r[3] * sx + r[4] * sy + r[5] * sz + ty, + r[6] * sx + r[7] * sy + r[8] * sz + tz, + ) + ) + if sys.byteorder != "little": + if sensor is not None: + sensor.byteswap() + if cart is not None: + cart.byteswap() + return (sensor.tobytes() if sensor is not None else None, cart.tobytes() if cart is not None else None) + + +def numpy_header(descr: str, shape: Sequence[int]) -> bytes: + shape_text = "(" + ", ".join(str(value) for value in shape) + if len(shape) == 1: + shape_text += "," + shape_text += ")" + header = f"{{'descr': '{descr}', 'fortran_order': False, 'shape': {shape_text}, }}" + encoded = header.encode("latin-1") + padding = (-((10 + len(encoded) + 1) % 16)) % 16 + encoded += b" " * padding + b"\n" + if len(encoded) > 65535: + raise ValueError("NPY v1 header is too long") + return b"\x93NUMPY\x01\x00" + struct.pack(" None: + with archive.open(name + ".npy", "w", force_zip64=True) as entry: + entry.write(numpy_header(descr, shape)) + entry.write(data) + + +def scalar_bytes(fmt: str, value: int | float) -> bytes: + return struct.pack(fmt, value) + + +def open_npz_writer(path: Path, compress: bool) -> zipfile.ZipFile: + compression = zipfile.ZIP_DEFLATED if compress else zipfile.ZIP_STORED + kwargs: dict[str, object] = {"compression": compression, "allowZip64": True} + if compress: + kwargs["compresslevel"] = 1 + return zipfile.ZipFile(path, "w", **kwargs) + + +def pack_f64_array(values: Sequence[float]) -> bytes: + data = array.array("d", values) + if sys.byteorder != "little": + data.byteswap() + return data.tobytes() + + +def pack_i8_array(values: Sequence[int]) -> bytes: + data = array.array("q", values) + if sys.byteorder != "little": + data.byteswap() + return data.tobytes() + + +def pack_i4_array(values: Sequence[int]) -> bytes: + data = array.array("i", values) + if sys.byteorder != "little": + data.byteswap() + return data.tobytes() + + +def pack_u1_bools(values: Sequence[bool | None]) -> bytes: + return bytes(1 if value else 0 for value in values) + + +def export_npz( + path: Path, + metadata: dict[str, object], + raw_points: bytes, + point_count: int, + xyz_sensor: bytes | None, + xyz_cart: bytes | None, + compress: bool, + rtk_arrays: dict[str, tuple[str, Sequence[int], bytes]] | None = None, +) -> None: + temp = path.with_suffix(path.suffix + ".tmp") + with open_npz_writer(temp, compress) as archive: + write_npy_entry(archive, "points_raw", " None: + points = array.array("f") + points.frombytes(raw_points) + if sys.byteorder != "little": + points.byteswap() + frame: dict[str, object] = { + "metadata": metadata, + "point_columns": ("d_mm", "azimuth_deg", "altitude_deg", "intensity", "progression"), + "points_raw_flat_f32": points, + "points_raw_shape": (point_count, 5), + } + for key, raw in (("xyz_sensor_mm_flat_f32", xyz_sensor), ("xyz_cart_mm_flat_f32", xyz_cart)): + if raw is not None: + values = array.array("f") + values.frombytes(raw) + if sys.byteorder != "little": + values.byteswap() + frame[key] = values + frame[key.replace("_flat_f32", "_shape")] = (point_count, 3) + temp = path.with_suffix(path.suffix + ".tmp") + with temp.open("wb") as stream: + pickle.dump(frame, stream, protocol=5) + os.replace(temp, path) + + +def frame_filename(record: RecordRef, frame_counter: int, extension: str) -> str: + return f"{record.object_name}_{record.sequence:06d}_{record.dotnet_ticks}_frame{frame_counter:010d}.{extension}" + + +def manifest_fields(include_rtk: bool) -> list[str]: + fields = [ + "sequence", + "status", + "object_name", + "dotnet_ticks", + "timestamp_iso_local", + "unix_time_ns", + "log_time", + "record_id", + "record_id_bytes_hex", + "frame_counter", + "point_count", + "payload_length", + "source_log", + "source_dorec", + "source_offset", + "output_file", + "error", + ] + if include_rtk: + fields.extend( + [ + "rtk_matched", + "rtk_nearest_index", + "rtk_prev_index", + "rtk_next_index", + "rtk_dt_ns", + "rtk_prev_dt_ns", + "rtk_next_dt_ns", + "rtk_lat", + "rtk_lon", + "rtk_alt_m", + "rtk_vehicle_heading_deg", + "rtk_raw_heading_deg", + "rtk_fix", + "rtk_sat", + "rtk_position_valid", + "rtk_heading_valid", + "rtk_heading_solution", + ] + ) + return fields + + +def nan_if_none(value: float | None) -> float: + return float("nan") if value is None else float(value) + + +def int_or_sentinel(value: int | None, sentinel: int = -1) -> int: + return sentinel if value is None else int(value) + + +def bool_or_false(value: bool | None) -> bool: + return bool(value) + + +def sample_to_public_dict(sample: RtkSample) -> dict[str, object]: + data = asdict(sample) + return data + + +def write_jsonl(path: Path, rows: Iterable[dict[str, object]]) -> int: + count = 0 + with path.open("w", encoding="utf-8") as stream: + for row in rows: + stream.write(json.dumps(row, ensure_ascii=False, separators=(",", ":"))) + stream.write("\n") + count += 1 + return count + + +def write_csv_rows(path: Path, fieldnames: Sequence[str], rows: Iterable[dict[str, object]]) -> int: + count = 0 + with path.open("w", newline="", encoding="utf-8-sig") as stream: + writer = csv.DictWriter(stream, fieldnames=list(fieldnames), extrasaction="ignore") + writer.writeheader() + for row in rows: + writer.writerow({key: row.get(key, "") for key in fieldnames}) + count += 1 + return count + + +def export_rtk_table_npz(path: Path, samples: Sequence[RtkSample], compress: bool) -> None: + n = len(samples) + temp = path.with_suffix(path.suffix + ".tmp") + with open_npz_writer(temp, compress) as archive: + write_npy_entry(archive, "index", " tuple[list[RtkSample], dict[str, int]]: + records = discover_records(root, "GPS-POST-Z") + samples: list[RtkSample] = [] + stats = { + "discovered": len(records), + "parsed_ok": 0, + "parse_errors": 0, + "read_errors": 0, + } + for record in records: + timestamp_iso, unix_ns = dotnet_ticks_to_values(record.dotnet_ticks, tz) + sample = RtkSample( + index=len(samples), + source="dobject", + object_name=record.object_name, + dotnet_ticks=record.dotnet_ticks, + timestamp_iso_local=timestamp_iso, + unix_time_ns=unix_ns, + log_time=record.log_time, + record_id=record.log_record_id, + source_log=record.source_log, + source_dorec=record.source_dorec, + source_offset=record.source_offset, + payload_length=record.payload_length, + ) + try: + dorec_path = choose_dorec(dorec_index, record.source_dorec) + record_meta, payload = read_record(dorec_path, record) + sample.record_id = str(record_meta["record_id"]) + text = payload.decode("utf-8", errors="replace") + sample.raw_text = text + parsed = parse_gps_post_z_text(text) + sample.counter = parsed["counter"] # type: ignore[assignment] + sample.lat = parsed["lat"] # type: ignore[assignment] + sample.lon = parsed["lon"] # type: ignore[assignment] + sample.alt_m = parsed["alt_m"] # type: ignore[assignment] + sample.raw_heading_deg = parsed["raw_heading_deg"] # type: ignore[assignment] + sample.vehicle_heading_deg = parsed["vehicle_heading_deg"] # type: ignore[assignment] + sample.fix = parsed["fix"] # type: ignore[assignment] + sample.sat = parsed["sat"] # type: ignore[assignment] + sample.position_valid = parsed["position_valid"] # type: ignore[assignment] + sample.heading_valid = parsed["heading_valid"] # type: ignore[assignment] + sample.heading_solution = parsed["heading_solution"] # type: ignore[assignment] + sample.position_time = parsed["position_time"] # type: ignore[assignment] + sample.heading_time = parsed["heading_time"] # type: ignore[assignment] + sample.last_line = parsed["last_line"] # type: ignore[assignment] + stats["parsed_ok"] += 1 + except Exception as exc: + sample.parse_error = f"{type(exc).__name__}: {exc}" + if "read" in type(exc).__name__.casefold() or "mismatch" in str(exc).casefold(): + stats["read_errors"] += 1 + else: + stats["parse_errors"] += 1 + samples.append(sample) + _ = timezone_text + return samples, stats + + +def load_rtk_binary_samples( + root: Path, + dorec_index: dict[str, list[Path]], + tz: timezone, +) -> tuple[list[RtkSample], dict[str, int]]: + records = discover_records(root, "rtk") + samples: list[RtkSample] = [] + stats = {"discovered": len(records), "parsed_ok": 0, "parse_errors": 0, "read_errors": 0} + for record in records: + timestamp_iso, unix_ns = dotnet_ticks_to_values(record.dotnet_ticks, tz) + sample = RtkSample( + index=len(samples), + source="dobject", + object_name=record.object_name, + dotnet_ticks=record.dotnet_ticks, + timestamp_iso_local=timestamp_iso, + unix_time_ns=unix_ns, + log_time=record.log_time, + record_id=record.log_record_id, + source_log=record.source_log, + source_dorec=record.source_dorec, + source_offset=record.source_offset, + payload_length=record.payload_length, + ) + try: + dorec_path = choose_dorec(dorec_index, record.source_dorec) + record_meta, payload = read_record(dorec_path, record) + sample.record_id = str(record_meta["record_id"]) + parsed = parse_rtk_binary_payload(payload) + sample.device_name = str(parsed["device_name"]) + sample.device_stamp_hex = str(parsed["device_stamp_hex"]) + sample.counter = int(parsed["counter"]) # type: ignore[arg-type] + sample.lat = float(parsed["lat"]) # type: ignore[arg-type] + sample.lon = float(parsed["lon"]) # type: ignore[arg-type] + sample.alt_m = float(parsed["alt_m"]) # type: ignore[arg-type] + sample.raw_heading_deg = float(parsed["raw_heading_deg"]) # type: ignore[arg-type] + sample.vehicle_heading_deg = float(parsed["vehicle_heading_deg"]) # type: ignore[arg-type] + sample.fix = int(parsed["fix"]) # type: ignore[arg-type] + sample.sat = int(parsed["sat"]) # type: ignore[arg-type] + sample.position_valid = bool(parsed["position_valid"]) + sample.heading_valid = bool(parsed["heading_valid"]) + sample.heading_solution = str(parsed["heading_solution"]) + sample.position_time = str(parsed["position_time"]) + sample.heading_time = str(parsed["heading_time"]) + sample.last_line = parsed["last_line"] # type: ignore[assignment] + sample.raw_text = json.dumps(parsed, ensure_ascii=False, separators=(",", ":")) + stats["parsed_ok"] += 1 + except Exception as exc: + sample.parse_error = f"{type(exc).__name__}: {exc}" + stats["parse_errors"] += 1 + samples.append(sample) + return samples, stats + + +def load_gps_post_samples( + root: Path, + dorec_index: dict[str, list[Path]], + tz: timezone, +) -> tuple[list[dict[str, object]], dict[str, int]]: + records = discover_records(root, "GPS-POST") + rows: list[dict[str, object]] = [] + stats = {"discovered": len(records), "parsed_ok": 0, "parse_errors": 0, "read_errors": 0} + for index, record in enumerate(records): + timestamp_iso, unix_ns = dotnet_ticks_to_values(record.dotnet_ticks, tz) + row: dict[str, object] = { + "index": index, + "object_name": record.object_name, + "dotnet_ticks": record.dotnet_ticks, + "timestamp_iso_local": timestamp_iso, + "unix_time_ns": unix_ns, + "log_time": record.log_time, + "record_id": record.log_record_id, + "source_log": record.source_log, + "source_dorec": record.source_dorec, + "source_offset": record.source_offset, + "payload_length": record.payload_length, + "parse_error": "", + } + try: + dorec_path = choose_dorec(dorec_index, record.source_dorec) + record_meta, payload = read_record(dorec_path, record) + row["record_id"] = record_meta["record_id"] + parsed = parse_gps_post_payload(payload) + row.update(parsed) + stats["parsed_ok"] += 1 + except Exception as exc: + row["parse_error"] = f"{type(exc).__name__}: {exc}" + stats["parse_errors"] += 1 + rows.append(row) + return rows, stats + + +def load_gps_post_z_text_logs(root: Path) -> tuple[list[dict[str, object]], dict[str, int]]: + rows: list[dict[str, object]] = [] + stats = {"files": 0, "lines": 0, "parsed_ok": 0, "parse_errors": 0} + text_root = root / "GPS-POST-Z" + if not text_root.is_dir(): + return rows, stats + for log_path in sorted(text_root.rglob("*.log")): + stats["files"] += 1 + relative = log_path.relative_to(root).as_posix() + with log_path.open("r", encoding="utf-8", errors="replace") as stream: + for line_no, line in enumerate(stream, start=1): + match = GPS_TEXT_RE.search(line) + if not match: + continue + stats["lines"] += 1 + body = match.group("body").strip() + row: dict[str, object] = { + "index": len(rows), + "source": "GPS-POST-Z-text-log", + "source_log": relative, + "line_no": line_no, + "log_time": match.group("log_time"), + "raw_text": body, + "parse_error": "", + } + try: + parsed = parse_gps_post_z_text(body) + row.update( + { + "lat": parsed["lat"], + "lon": parsed["lon"], + "alt_m": parsed["alt_m"], + "raw_heading_deg": parsed["raw_heading_deg"], + "vehicle_heading_deg": parsed["vehicle_heading_deg"], + "fix": parsed["fix"], + "sat": parsed["sat"], + "position_valid": parsed["position_valid"], + "heading_valid": parsed["heading_valid"], + "heading_solution": parsed["heading_solution"], + "position_time": parsed["position_time"], + "heading_time": parsed["heading_time"], + } + ) + stats["parsed_ok"] += 1 + except Exception as exc: + row["parse_error"] = f"{type(exc).__name__}: {exc}" + stats["parse_errors"] += 1 + rows.append(row) + return rows, stats + + +def match_rtk(unix_time_ns: int, samples: Sequence[RtkSample]) -> dict[str, object]: + if not samples: + return { + "matched": False, + "nearest_index": -1, + "prev_index": -1, + "next_index": -1, + "dt_ns": None, + "prev_dt_ns": None, + "next_dt_ns": None, + "sample": None, + } + ticks = [sample.unix_time_ns for sample in samples] + pos = bisect.bisect_left(ticks, unix_time_ns) + if pos < len(samples) and ticks[pos] == unix_time_ns: + nearest = prev_index = next_index = pos + else: + candidates: list[int] = [] + if pos < len(samples): + candidates.append(pos) + if pos > 0: + candidates.append(pos - 1) + nearest = min(candidates, key=lambda idx: (abs(ticks[idx] - unix_time_ns), idx)) + prev_index = pos - 1 if pos > 0 else -1 + next_index = pos if pos < len(samples) else -1 + sample = samples[nearest] + prev_dt = None if prev_index < 0 else unix_time_ns - samples[prev_index].unix_time_ns + next_dt = None if next_index < 0 else samples[next_index].unix_time_ns - unix_time_ns + return { + "matched": True, + "nearest_index": nearest, + "prev_index": prev_index, + "next_index": next_index, + "dt_ns": unix_time_ns - sample.unix_time_ns, + "prev_dt_ns": prev_dt, + "next_dt_ns": next_dt, + "sample": sample, + } + + +def rtk_frame_arrays(match: dict[str, object]) -> dict[str, tuple[str, Sequence[int], bytes]]: + sample: RtkSample | None = match["sample"] # type: ignore[assignment] + return { + "rtk_nearest_index": (" dict[str, object]: + rtk_dir = output / "rtk" + rtk_dir.mkdir(exist_ok=True) + written: dict[str, object] = {"directory": "rtk"} + + export_rtk_table_npz(rtk_dir / "gps_post_z.npz", gps_post_z, compress) + gps_dicts = [sample_to_public_dict(sample) for sample in gps_post_z] + write_jsonl(rtk_dir / "gps_post_z.jsonl", gps_dicts) + write_csv_rows( + rtk_dir / "gps_post_z.csv", + [ + "index", + "dotnet_ticks", + "timestamp_iso_local", + "unix_time_ns", + "log_time", + "record_id", + "counter", + "lat", + "lon", + "alt_m", + "raw_heading_deg", + "vehicle_heading_deg", + "fix", + "sat", + "position_valid", + "heading_valid", + "heading_solution", + "position_time", + "heading_time", + "last_line", + "source_log", + "source_dorec", + "source_offset", + "payload_length", + "parse_error", + "raw_text", + ], + gps_dicts, + ) + written["gps_post_z"] = { + "npz": "rtk/gps_post_z.npz", + "csv": "rtk/gps_post_z.csv", + "jsonl": "rtk/gps_post_z.jsonl", + "count": len(gps_post_z), + } + + export_rtk_table_npz(rtk_dir / "rtk_binary.npz", rtk_binary, compress) + rtk_dicts = [sample_to_public_dict(sample) for sample in rtk_binary] + write_jsonl(rtk_dir / "rtk_binary.jsonl", rtk_dicts) + write_csv_rows( + rtk_dir / "rtk_binary.csv", + [ + "index", + "dotnet_ticks", + "timestamp_iso_local", + "unix_time_ns", + "log_time", + "record_id", + "device_name", + "device_stamp_hex", + "counter", + "lat", + "lon", + "alt_m", + "raw_heading_deg", + "vehicle_heading_deg", + "fix", + "sat", + "position_valid", + "heading_valid", + "heading_solution", + "position_time", + "heading_time", + "last_line", + "source_log", + "source_dorec", + "source_offset", + "payload_length", + "parse_error", + ], + rtk_dicts, + ) + written["rtk_binary"] = { + "npz": "rtk/rtk_binary.npz", + "csv": "rtk/rtk_binary.csv", + "jsonl": "rtk/rtk_binary.jsonl", + "count": len(rtk_binary), + } + + write_jsonl(rtk_dir / "gps_post.jsonl", gps_post_rows) + write_csv_rows( + rtk_dir / "gps_post.csv", + [ + "index", + "dotnet_ticks", + "timestamp_iso_local", + "unix_time_ns", + "log_time", + "record_id", + "device_name", + "counter", + "unknown0", + "flags_hex", + "device_stamp_hex", + "source_log", + "source_dorec", + "source_offset", + "payload_length", + "payload_hex", + "parse_error", + ], + gps_post_rows, + ) + # Compact NPZ for GPS-POST counters / stamps only. + temp = (rtk_dir / "gps_post.npz").with_suffix(".npz.tmp") + with open_npz_writer(temp, compress) as archive: + n = len(gps_post_rows) + write_npy_entry( + archive, + "index", + " dict[str, object]: + abs_dts = [abs(value) for value in match_dts_ns] + abs_interior = [abs(value) for value in interior_dts_ns] + threshold_ns = rtk_max_dt_ms * 1_000_000.0 + over_threshold = sum(1 for value in abs_interior if value > threshold_ns) + checks: list[dict[str, object]] = [ + { + "name": "lidar_payload_length", + "ok": lidar_stats.get("frames_with_errors", 0) == 0, + "detail": "Each lidar payload must equal 8 + point_count * 20.", + }, + { + "name": "gps_post_z_parse", + "ok": gps_post_z_stats.get("parse_errors", 0) == 0 + and gps_post_z_stats.get("read_errors", 0) == 0, + "detail": "All GPS-POST-Z dobject payloads should parse.", + }, + { + "name": "lidar_rtk_coverage", + "ok": unmatched_frames == 0, + "detail": "Every exported lidar frame should have a nearest GPS-POST-Z sample.", + }, + { + "name": "rtk_time_alignment", + "ok": over_threshold == 0, + "detail": ( + f"Interior frames (with both prev/next RTK) should have nearest |dt| " + f"<= {rtk_max_dt_ms} ms." + ), + }, + ] + if rtk_sidecars: + checks.insert( + 2, + { + "name": "rtk_binary_parse", + "ok": rtk_binary_stats.get("parse_errors", 0) == 0, + "detail": "All rtk binary payloads should parse as UNICORE_N4_RTK_V1.", + }, + ) + report = { + "format_version": FORMAT_VERSION, + "generated_at": datetime.now().astimezone().isoformat(), + "lidar": lidar_stats, + "rtk_sidecars": rtk_sidecars, + "rtk_sources": { + "GPS-POST-Z_dobject": gps_post_z_stats, + "rtk_dobject": rtk_binary_stats if rtk_sidecars else {"skipped": True}, + "GPS-POST_dobject": gps_post_stats if rtk_sidecars else {"skipped": True}, + "GPS-POST-Z_text_log": text_stats if rtk_sidecars else {"skipped": True}, + }, + "matching": { + "primary_source": "GPS-POST-Z", + "matched_frames": matched_frames, + "unmatched_frames": unmatched_frames, + "edge_frames_outside_rtk_span": edge_frames, + "rtk_max_dt_ms_threshold": rtk_max_dt_ms, + "interior_frames_over_threshold": over_threshold, + "dt_ns": { + "count": len(abs_dts), + "min": min(match_dts_ns) if match_dts_ns else None, + "max": max(match_dts_ns) if match_dts_ns else None, + "abs_min": min(abs_dts) if abs_dts else None, + "abs_max": max(abs_dts) if abs_dts else None, + "abs_mean": (sum(abs_dts) / len(abs_dts)) if abs_dts else None, + }, + "interior_dt_ns": { + "count": len(abs_interior), + "abs_min": min(abs_interior) if abs_interior else None, + "abs_max": max(abs_interior) if abs_interior else None, + "abs_mean": (sum(abs_interior) / len(abs_interior)) if abs_interior else None, + }, + }, + "checks": checks, + } + report["ok"] = all(bool(check["ok"]) for check in checks) + return report + + +def write_package_readme( + path: Path, + *, + format_version: str, + frame_count: int, + include_rtk: bool, + timezone_text: str, +) -> None: + rtk_note = ( + "每帧 NPZ 已嵌入最近 RTK：`rtk_lat` / `rtk_lon` / `rtk_alt_m` / " + "`rtk_vehicle_heading_deg` / `rtk_fix` / `rtk_sat` 等。\n" + if include_rtk + else "本包未嵌入 RTK（导出时使用了 `--skip-rtk`）。\n" + ) + text = """# FrontLidar 数据集 + +格式版本：`@@FORMAT_VERSION@@` +帧数：`@@FRAME_COUNT@@` +时区：`@@TIMEZONE@@` + +## 目录 + +```text +. + README.md # 本说明 + frames/ # 逐帧 NPZ + frontlidar___frame.npz +``` + +直接打包本目录即可分发。 + +## 依赖 + +```powershell +pip install numpy +``` + +## 快速读取 + +```python +import json +from pathlib import Path +import numpy as np + +root = Path(__file__).resolve().parent # 或改成数据集路径 +frame_path = next(sorted((root / "frames").glob("*.npz"))) + +with np.load(frame_path, allow_pickle=False) as f: + points = f["points_raw"] # float32 (N, 5) + unix_ns = int(f["unix_time_ns"][0]) + meta = json.loads(f["metadata_json_utf8"].tobytes().decode("utf-8")) + + # 列: d_mm, azimuth_deg, altitude_deg, intensity, progression + d_mm, az, alt, intensity, prog = (points[:, i] for i in range(5)) + + # RTK（若导出时未 --skip-rtk） + if "rtk_lat" in f.files: + lat = float(f["rtk_lat"][0]) + lon = float(f["rtk_lon"][0]) + heading = float(f["rtk_vehicle_heading_deg"][0]) + dt_ms = int(f["rtk_dt_ns"][0]) / 1e6 + print(lat, lon, heading, dt_ms, meta.get("rtk", {}).get("heading_solution")) +``` + +@@RTK_NOTE@@ +## 点云列 + +| 列 | 字段 | 单位 | +|---:|---|---| +| 0 | d_mm | mm | +| 1 | azimuth_deg | ° | +| 2 | altitude_deg | ° | +| 3 | intensity | 设备定义 | +| 4 | progression | 0–1 | + +极坐标转传感器 XYZ（mm）： + +```python +az = np.deg2rad(points[:, 1]) +alt = np.deg2rad(points[:, 2]) +d = points[:, 0] +xyz = np.column_stack(( + d * np.cos(alt) * np.cos(az), + d * np.cos(alt) * np.sin(az), + d * np.sin(alt), +)).astype(np.float32) +``` + +## 时间戳 + +- `dotnet_ticks`：原始权威时间 +- `unix_time_ns`：按导出时区转换的 Unix 纳秒 +- 标量字段均为 shape `(1,)`，用 `[0]` 取出 + +## 批量遍历 + +```python +for path in sorted((root / "frames").glob("*.npz")): + with np.load(path, allow_pickle=False) as f: + pts = f["points_raw"] + # ... +``` + +更多字段说明见导出仓库中的 `FRONTLIDAR_NPZ_READ.md`（若一并提供）。 +""" + text = ( + text.replace("@@FORMAT_VERSION@@", format_version) + .replace("@@FRAME_COUNT@@", str(frame_count)) + .replace("@@TIMEZONE@@", timezone_text) + .replace("@@RTK_NOTE@@", rtk_note) + ) + path.write_text(text, encoding="utf-8") + + +def export_dataset(args: argparse.Namespace) -> int: + started = time.time() + root = resolve_dlog_root(args.dlog) + output = Path(args.out).expanduser().resolve() + output.mkdir(parents=True, exist_ok=True) + frames_dir = output / "frames" + frames_dir.mkdir(exist_ok=True) + tz = parse_timezone(args.timezone) + records = discover_records(root, args.object) + selected = records[:: args.stride] + if args.max_frames: + selected = selected[: args.max_frames] + if not selected: + raise RuntimeError(f"no DObject records named {args.object!r} found under {root}") + dorec_index = index_dorec_files(root) + rotation = rotation_matrix(args.yaw, args.pitch, args.roll) + translation = (args.x, args.y, args.z) + extension = "npz" if args.format == "npz" else "pkl" + include_rtk = not args.skip_rtk + rtk_sidecars = bool(args.rtk_sidecars) + + gps_post_z: list[RtkSample] = [] + rtk_binary: list[RtkSample] = [] + gps_post_rows: list[dict[str, object]] = [] + text_rows: list[dict[str, object]] = [] + gps_post_z_stats = {"discovered": 0, "parsed_ok": 0, "parse_errors": 0, "read_errors": 0} + rtk_binary_stats = {"discovered": 0, "parsed_ok": 0, "parse_errors": 0, "read_errors": 0} + gps_post_stats = {"discovered": 0, "parsed_ok": 0, "parse_errors": 0, "read_errors": 0} + text_stats = {"files": 0, "lines": 0, "parsed_ok": 0, "parse_errors": 0} + rtk_outputs: dict[str, object] = {} + + if include_rtk: + print("Loading GPS-POST-Z for per-frame RTK matching...", flush=True) + gps_post_z, gps_post_z_stats = load_gps_post_z_samples(root, dorec_index, tz, args.timezone) + if rtk_sidecars: + print("Loading RTK sidecars...", flush=True) + rtk_binary, rtk_binary_stats = load_rtk_binary_samples(root, dorec_index, tz) + gps_post_rows, gps_post_stats = load_gps_post_samples(root, dorec_index, tz) + text_rows, text_stats = load_gps_post_z_text_logs(root) + rtk_outputs = export_rtk_sidecars( + output, + bool(args.compress and args.format == "npz"), + gps_post_z, + rtk_binary, + gps_post_rows, + text_rows, + ) + print( + f"RTK loaded: GPS-POST-Z={len(gps_post_z)} rtk={len(rtk_binary)} " + f"GPS-POST={len(gps_post_rows)} text={len(text_rows)} (sidecars on)", + flush=True, + ) + else: + print( + f"RTK loaded: GPS-POST-Z={len(gps_post_z)} (sidecars off; use --rtk-sidecars to write rtk/)", + flush=True, + ) + + write_reports = bool(args.write_reports) + reports_dir = output / "reports" + if write_reports: + reports_dir.mkdir(exist_ok=True) + manifest_tmp = reports_dir / "manifest.partial.csv" + manifest_final = reports_dir / "manifest.csv" + match_tmp = reports_dir / "rtk_match.partial.csv" + match_final = reports_dir / "rtk_match.csv" + manifest_stream_cm: object = manifest_tmp.open("w", newline="", encoding="utf-8-sig") + match_stream = ( + match_tmp.open("w", newline="", encoding="utf-8-sig") if include_rtk else None + ) + else: + manifest_tmp = manifest_final = match_tmp = match_final = None + manifest_stream_cm = nullcontext(io.StringIO()) + match_stream = io.StringIO() if include_rtk else None + + exported = skipped = resumed = total_points = 0 + matched_frames = unmatched_frames = edge_frames = 0 + match_dts_ns: list[int] = [] + interior_dts_ns: list[int] = [] + match_fieldnames = [ + "sequence", + "lidar_unix_time_ns", + "lidar_dotnet_ticks", + "output_file", + "rtk_matched", + "rtk_nearest_index", + "rtk_prev_index", + "rtk_next_index", + "rtk_dt_ns", + "rtk_prev_dt_ns", + "rtk_next_dt_ns", + "rtk_dotnet_ticks", + "rtk_unix_time_ns", + "rtk_lat", + "rtk_lon", + "rtk_alt_m", + "rtk_raw_heading_deg", + "rtk_vehicle_heading_deg", + "rtk_fix", + "rtk_sat", + "rtk_position_valid", + "rtk_heading_valid", + "rtk_heading_solution", + "rtk_position_time", + "rtk_heading_time", + "rtk_last_line", + "rtk_source_dorec", + "rtk_source_offset", + ] + + match_writer = csv.DictWriter(match_stream, fieldnames=match_fieldnames) if match_stream else None + if match_writer: + match_writer.writeheader() + + with manifest_stream_cm as manifest_stream: + writer = csv.DictWriter(manifest_stream, fieldnames=manifest_fields(include_rtk)) + writer.writeheader() + for selected_index, record in enumerate(selected): + row: dict[str, object] = { + "sequence": record.sequence, + "status": "error", + "object_name": record.object_name, + "dotnet_ticks": record.dotnet_ticks, + "timestamp_iso_local": "", + "unix_time_ns": "", + "log_time": record.log_time, + "record_id": record.log_record_id, + "record_id_bytes_hex": "", + "frame_counter": "", + "point_count": "", + "payload_length": record.payload_length, + "source_log": record.source_log, + "source_dorec": record.source_dorec, + "source_offset": record.source_offset, + "output_file": "", + "error": "", + } + if include_rtk: + row.update( + { + "rtk_matched": False, + "rtk_nearest_index": -1, + "rtk_prev_index": -1, + "rtk_next_index": -1, + "rtk_dt_ns": "", + "rtk_prev_dt_ns": "", + "rtk_next_dt_ns": "", + "rtk_lat": "", + "rtk_lon": "", + "rtk_alt_m": "", + "rtk_vehicle_heading_deg": "", + "rtk_raw_heading_deg": "", + "rtk_fix": "", + "rtk_sat": "", + "rtk_position_valid": "", + "rtk_heading_valid": "", + "rtk_heading_solution": "", + } + ) + try: + dorec_path = choose_dorec(dorec_index, record.source_dorec) + record_meta, payload = read_record(dorec_path, record) + frame_counter, point_count, raw_points = parse_lidar_payload(payload) + timestamp_iso, unix_ns = dotnet_ticks_to_values(record.dotnet_ticks, tz) + filename = frame_filename(record, frame_counter, extension) + relative_output = (Path("frames") / filename).as_posix() + frame_path = output / relative_output + metadata: dict[str, object] = { + "format_version": FORMAT_VERSION, + "sequence": record.sequence, + "object_name": record.object_name, + "dotnet_ticks": record.dotnet_ticks, + "timestamp_iso_local": timestamp_iso, + "unix_time_ns": unix_ns, + "timezone": args.timezone, + "log_time": record.log_time, + "record_id": record_meta["record_id"], + "record_id_bytes_hex": record_meta["record_id_bytes_hex"], + "frame_counter": frame_counter, + "point_count": point_count, + "payload_length": record_meta["payload_length"], + "source_log": record.source_log, + "source_dorec": record.source_dorec, + "source_offset": record.source_offset, + "point_columns": ["d_mm", "azimuth_deg", "altitude_deg", "intensity", "progression"], + "extrinsic": { + "translation_mm": list(translation), + "yaw_pitch_roll_deg": [args.yaw, args.pitch, args.roll], + "rotation_row_major": list(rotation), + }, + } + rtk_arrays = None + match_info: dict[str, object] | None = None + if include_rtk: + match_info = match_rtk(unix_ns, gps_post_z) + sample: RtkSample | None = match_info["sample"] # type: ignore[assignment] + if match_info["matched"]: + matched_frames += 1 + dt_ns = int(match_info["dt_ns"]) + match_dts_ns.append(dt_ns) + prev_i = int(match_info["prev_index"]) + next_i = int(match_info["next_index"]) + if prev_i < 0 or next_i < 0: + edge_frames += 1 + else: + interior_dts_ns.append(dt_ns) + else: + unmatched_frames += 1 + rtk_meta = { + "matched": bool(match_info["matched"]), + "nearest_index": match_info["nearest_index"], + "prev_index": match_info["prev_index"], + "next_index": match_info["next_index"], + "dt_ns": match_info["dt_ns"], + "prev_dt_ns": match_info["prev_dt_ns"], + "next_dt_ns": match_info["next_dt_ns"], + "primary_source": "GPS-POST-Z", + } + if sample is not None: + rtk_meta.update( + { + "dotnet_ticks": sample.dotnet_ticks, + "unix_time_ns": sample.unix_time_ns, + "timestamp_iso_local": sample.timestamp_iso_local, + "lat": sample.lat, + "lon": sample.lon, + "alt_m": sample.alt_m, + "raw_heading_deg": sample.raw_heading_deg, + "vehicle_heading_deg": sample.vehicle_heading_deg, + "fix": sample.fix, + "sat": sample.sat, + "position_valid": sample.position_valid, + "heading_valid": sample.heading_valid, + "heading_solution": sample.heading_solution, + "position_time": sample.position_time, + "heading_time": sample.heading_time, + "last_line": sample.last_line, + "counter": sample.counter, + "record_id": sample.record_id, + "source_dorec": sample.source_dorec, + "source_offset": sample.source_offset, + } + ) + metadata["rtk"] = rtk_meta + rtk_arrays = rtk_frame_arrays(match_info) + row.update( + { + "rtk_matched": bool(match_info["matched"]), + "rtk_nearest_index": match_info["nearest_index"], + "rtk_prev_index": match_info["prev_index"], + "rtk_next_index": match_info["next_index"], + "rtk_dt_ns": "" if match_info["dt_ns"] is None else match_info["dt_ns"], + "rtk_prev_dt_ns": "" if match_info["prev_dt_ns"] is None else match_info["prev_dt_ns"], + "rtk_next_dt_ns": "" if match_info["next_dt_ns"] is None else match_info["next_dt_ns"], + "rtk_lat": "" if sample is None or sample.lat is None else sample.lat, + "rtk_lon": "" if sample is None or sample.lon is None else sample.lon, + "rtk_alt_m": "" if sample is None or sample.alt_m is None else sample.alt_m, + "rtk_vehicle_heading_deg": ( + "" if sample is None or sample.vehicle_heading_deg is None else sample.vehicle_heading_deg + ), + "rtk_raw_heading_deg": ( + "" if sample is None or sample.raw_heading_deg is None else sample.raw_heading_deg + ), + "rtk_fix": "" if sample is None or sample.fix is None else sample.fix, + "rtk_sat": "" if sample is None or sample.sat is None else sample.sat, + "rtk_position_valid": "" if sample is None else sample.position_valid, + "rtk_heading_valid": "" if sample is None else sample.heading_valid, + "rtk_heading_solution": "" if sample is None else sample.heading_solution, + } + ) + if match_writer is not None: + match_writer.writerow( + { + "sequence": record.sequence, + "lidar_unix_time_ns": unix_ns, + "lidar_dotnet_ticks": record.dotnet_ticks, + "output_file": relative_output, + "rtk_matched": bool(match_info["matched"]), + "rtk_nearest_index": match_info["nearest_index"], + "rtk_prev_index": match_info["prev_index"], + "rtk_next_index": match_info["next_index"], + "rtk_dt_ns": "" if match_info["dt_ns"] is None else match_info["dt_ns"], + "rtk_prev_dt_ns": "" if match_info["prev_dt_ns"] is None else match_info["prev_dt_ns"], + "rtk_next_dt_ns": "" if match_info["next_dt_ns"] is None else match_info["next_dt_ns"], + "rtk_dotnet_ticks": "" if sample is None else sample.dotnet_ticks, + "rtk_unix_time_ns": "" if sample is None else sample.unix_time_ns, + "rtk_lat": "" if sample is None else sample.lat, + "rtk_lon": "" if sample is None else sample.lon, + "rtk_alt_m": "" if sample is None else sample.alt_m, + "rtk_raw_heading_deg": "" if sample is None else sample.raw_heading_deg, + "rtk_vehicle_heading_deg": "" if sample is None else sample.vehicle_heading_deg, + "rtk_fix": "" if sample is None else sample.fix, + "rtk_sat": "" if sample is None else sample.sat, + "rtk_position_valid": "" if sample is None else sample.position_valid, + "rtk_heading_valid": "" if sample is None else sample.heading_valid, + "rtk_heading_solution": "" if sample is None else sample.heading_solution, + "rtk_position_time": "" if sample is None else sample.position_time, + "rtk_heading_time": "" if sample is None else sample.heading_time, + "rtk_last_line": "" if sample is None else sample.last_line, + "rtk_source_dorec": "" if sample is None else sample.source_dorec, + "rtk_source_offset": "" if sample is None else sample.source_offset, + } + ) + + row.update( + { + "timestamp_iso_local": timestamp_iso, + "unix_time_ns": unix_ns, + "record_id": record_meta["record_id"], + "record_id_bytes_hex": record_meta["record_id_bytes_hex"], + "frame_counter": frame_counter, + "point_count": point_count, + "output_file": relative_output, + } + ) + if args.resume and frame_path.exists(): + row["status"] = "resumed" + resumed += 1 + else: + xyz_sensor, xyz_cart = generate_xyz(raw_points, args.include_xyz, translation, rotation) + if args.format == "npz": + export_npz( + frame_path, + metadata, + raw_points, + point_count, + xyz_sensor, + xyz_cart, + args.compress, + rtk_arrays=rtk_arrays, + ) + else: + export_pickle(frame_path, metadata, raw_points, point_count, xyz_sensor, xyz_cart) + row["status"] = "exported" + exported += 1 + total_points += point_count + except Exception as exc: + skipped += 1 + row["error"] = f"{type(exc).__name__}: {exc}" + writer.writerow(row) + if (selected_index + 1) % 25 == 0 or selected_index + 1 == len(selected): + manifest_stream.flush() + if match_stream is not None: + match_stream.flush() + print( + f"[{selected_index + 1}/{len(selected)}] exported={exported} resumed={resumed} " + f"errors={skipped} points={total_points}", + flush=True, + ) + + if match_stream is not None: + match_stream.close() + if write_reports and match_tmp is not None and match_final is not None: + os.replace(match_tmp, match_final) + if write_reports and manifest_tmp is not None and manifest_final is not None: + os.replace(manifest_tmp, manifest_final) + + lidar_stats = { + "records_discovered": len(records), + "records_selected": len(selected), + "frames_exported": exported, + "frames_resumed": resumed, + "frames_with_errors": skipped, + "total_points_in_manifest": total_points, + } + validation = build_validation_report( + lidar_stats=lidar_stats, + gps_post_z_stats=gps_post_z_stats, + rtk_binary_stats=rtk_binary_stats, + gps_post_stats=gps_post_stats, + text_stats=text_stats, + match_dts_ns=match_dts_ns, + interior_dts_ns=interior_dts_ns, + edge_frames=edge_frames, + matched_frames=matched_frames, + unmatched_frames=unmatched_frames, + rtk_max_dt_ms=args.rtk_max_dt_ms, + rtk_sidecars=rtk_sidecars, + ) + + write_package_readme( + output / "README.md", + format_version=FORMAT_VERSION, + frame_count=exported + resumed, + include_rtk=include_rtk, + timezone_text=args.timezone, + ) + + metadata = { + "format_version": FORMAT_VERSION, + "generated_at": datetime.now().astimezone().isoformat(), + "source_dlog": str(root), + "object_name": args.object, + "output_format": args.format, + "npz_compressed": bool(args.compress and args.format == "npz"), + "include_xyz": args.include_xyz, + "include_rtk": include_rtk, + "rtk_sidecars": rtk_sidecars, + "write_reports": write_reports, + "timezone": args.timezone, + "timezone_offset_minutes": timezone_minutes(tz), + "rtk_max_dt_ms": args.rtk_max_dt_ms, + "rtk_objects": list(RTK_OBJECT_NAMES), + "rtk_outputs": rtk_outputs if rtk_sidecars else None, + "package_contents": ["README.md", "frames/"] + + (["rtk/"] if rtk_sidecars else []) + + (["reports/"] if write_reports else []), + **lidar_stats, + "point_columns": [ + {"name": "d_mm", "dtype": "float32", "unit": "mm"}, + {"name": "azimuth_deg", "dtype": "float32", "unit": "degree"}, + {"name": "altitude_deg", "dtype": "float32", "unit": "degree"}, + {"name": "intensity", "dtype": "float32", "unit": "device-specific"}, + {"name": "progression", "dtype": "float32", "unit": "scan fraction"}, + ], + "extrinsic": { + "translation_mm": list(translation), + "yaw_pitch_roll_deg": [args.yaw, args.pitch, args.roll], + "rotation_row_major": list(rotation), + }, + "validation_ok": validation["ok"], + "duration_seconds": round(time.time() - started, 3), + } + if write_reports: + with (reports_dir / "metadata.json").open("w", encoding="utf-8") as stream: + json.dump(metadata, stream, ensure_ascii=False, indent=2) + stream.write("\n") + with (reports_dir / "validation_report.json").open("w", encoding="utf-8") as stream: + json.dump(validation, stream, ensure_ascii=False, indent=2) + stream.write("\n") + + print(json.dumps(metadata, ensure_ascii=False, indent=2), flush=True) + print( + json.dumps( + {"validation_ok": validation["ok"], "checks": validation["checks"]}, + ensure_ascii=False, + indent=2, + ), + flush=True, + ) + if skipped != 0: + return 1 + if include_rtk and not validation["ok"]: + return 1 + return 0 + + +def main() -> int: + try: + return export_dataset(parse_args()) + except Exception as exc: + print(f"ERROR: {type(exc).__name__}: {exc}", file=sys.stderr) + return 2 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/tools/prepare_station_dataset.py b/tools/prepare_station_dataset.py new file mode 100644 index 0000000..3cdf419 --- /dev/null +++ b/tools/prepare_station_dataset.py @@ -0,0 +1,194 @@ +#!/usr/bin/env python3 +"""Prepare one static LiDAR frame and one yaw-only RTK body pose per station.""" + +from __future__ import annotations + +import argparse +import csv +import json +import math +import re +import shutil +import sys +from pathlib import Path +from typing import Any + +import numpy as np + +POSE_FIELDS = ["time", "x", "y", "z", "qx", "qy", "qz", "qw"] + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser( + description="Select one static LiDAR frame per exported station and rebuild yaw-only RTK body poses." + ) + parser.add_argument("--export-root", type=Path, required=True, help="Directory containing exported station directories.") + parser.add_argument("--output", type=Path, required=True, help="Output prepared directory.") + parser.add_argument("--pose-name", default="rear_gga_raw_rear_to_front", help="Suffix of body_poses_.csv.") + parser.add_argument("--heading-offset-deg", type=float, required=True, help="Added to raw_heading_deg before ENU yaw conversion.") + parser.add_argument("--antenna-lever", type=float, nargs=3, metavar=("X", "Y", "Z"), required=True, help="Antenna position in body coordinates [m].") + parser.add_argument("--expected-stations", type=int, default=0, help="Require exactly this many usable stations; 0 disables.") + parser.add_argument("--min-stations", type=int, default=30, help="Fail below this many usable stations.") + parser.add_argument("--groups", nargs="*", default=None, help="Optional explicit station directory names.") + parser.add_argument("--accepted-fixes", type=int, nargs="+", default=[4, 5], help="Accepted GGA fix values.") + parser.add_argument("--heading-std-limit-deg", type=float, default=float("inf"), help="Reject a station above this heading circular stddev.") + parser.add_argument("--overwrite", action="store_true", help="Allow replacing generated files in a non-empty output directory.") + return parser.parse_args() + + +def truth(value: Any) -> bool: + return str(value).strip().lower() in {"1", "true", "yes", "y"} + + +def natural_key(value: str) -> list[Any]: + return [int(part) if part.isdigit() else part.lower() for part in re.split(r"(\d+)", value)] + +def read_csv(path: Path) -> list[dict[str, str]]: + with path.open("r", encoding="utf-8-sig", newline="") as stream: + return list(csv.DictReader(stream)) + + +def require_columns(rows: list[dict[str, str]], path: Path, columns: list[str]) -> None: + if not rows: + raise ValueError(f"{path}: no rows") + missing = [column for column in columns if column not in rows[0]] + if missing: + raise ValueError(f"{path}: missing required columns: {', '.join(missing)}") + + +def circular_mean_deg(values: np.ndarray) -> float: + radians = np.deg2rad(values) + return float(np.rad2deg(math.atan2(float(np.mean(np.sin(radians))), float(np.mean(np.cos(radians))))) % 360.0) + + +def circular_std_deg(values: np.ndarray) -> float: + radians = np.deg2rad(values) + resultant = max(math.hypot(float(np.mean(np.cos(radians))), float(np.mean(np.sin(radians)))), 1e-12) + return float(np.rad2deg(math.sqrt(-2.0 * math.log(resultant)))) + + +def geodetic_to_ecef(lat_deg: float, lon_deg: float, height_m: float) -> np.ndarray: + a, e2 = 6378137.0, 6.69437999014e-3 + lat, lon = math.radians(lat_deg), math.radians(lon_deg) + sin_lat, cos_lat, sin_lon, cos_lon = math.sin(lat), math.cos(lat), math.sin(lon), math.cos(lon) + n = a / math.sqrt(1.0 - e2 * sin_lat * sin_lat) + return np.array([(n + height_m) * cos_lat * cos_lon, (n + height_m) * cos_lat * sin_lon, (n * (1.0 - e2) + height_m) * sin_lat], dtype=float) + + +def ecef_to_enu(ecef: np.ndarray, origin_ecef: np.ndarray, origin_lat_deg: float, origin_lon_deg: float) -> np.ndarray: + lat, lon = math.radians(origin_lat_deg), math.radians(origin_lon_deg) + sin_lat, cos_lat, sin_lon, cos_lon = math.sin(lat), math.cos(lat), math.sin(lon), math.cos(lon) + rotation = np.array([[-sin_lon, cos_lon, 0.0], [-sin_lat * cos_lon, -sin_lat * sin_lon, cos_lat], [cos_lat * cos_lon, cos_lat * sin_lon, sin_lat]], dtype=float) + return rotation @ (ecef - origin_ecef) + + +def yaw_rotation(yaw: float) -> np.ndarray: + c, s = math.cos(yaw), math.sin(yaw) + return np.array([[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]], dtype=float) + + +def select_frame(station: Path) -> tuple[dict[str, str], int, int]: + path = station / "reports" / "manifest.csv" + rows = read_csv(path) + require_columns(rows, path, ["status", "unix_time_ns", "output_file"]) + exported = [row for row in rows if row["status"] in {"exported", "resumed"}] + if not exported: + raise ValueError(f"{station.name}: no exported/resumed LiDAR frames") + valid = [row for row in exported if truth(row.get("rtk_position_valid")) and truth(row.get("rtk_heading_valid"))] + candidates = valid or exported + candidates.sort(key=lambda row: int(row["unix_time_ns"])) + return candidates[len(candidates) // 2], len(exported), len(valid) + + +def validation_checks(station: Path) -> dict[str, bool]: + path = station / "reports" / "validation_report.json" + if not path.exists(): + return {} + document = json.loads(path.read_text(encoding="utf-8")) + return {str(item.get("name")): bool(item.get("ok")) for item in document.get("checks", [])} + + +def process_station(station: Path, args: argparse.Namespace) -> tuple[dict[str, Any], dict[str, Any]]: + rtk_path = station / "rtk" / "gps_post_z.csv" + rows = read_csv(rtk_path) + columns = ["lat", "lon", "alt_m", "raw_heading_deg", "fix", "position_valid", "heading_valid", "unix_time_ns"] + require_columns(rows, rtk_path, columns) + fixes = set(args.accepted_fixes) + good = [row for row in rows if truth(row["position_valid"]) and truth(row["heading_valid"]) and int(row["fix"]) in fixes] + if not good: + raise ValueError(f"{station.name}: no RTK samples pass position/heading/fix filters") + lat = np.asarray([float(row["lat"]) for row in good]) + lon = np.asarray([float(row["lon"]) for row in good]) + alt = np.asarray([float(row["alt_m"]) for row in good]) + heading = np.asarray([float(row["raw_heading_deg"]) for row in good]) + times = np.asarray([int(row["unix_time_ns"]) for row in good], dtype=np.int64) + heading_std = circular_std_deg(heading) + if heading_std > args.heading_std_limit_deg: + raise ValueError(f"{station.name}: heading circular stddev {heading_std:.3f} deg exceeds {args.heading_std_limit_deg:.3f} deg") + frame, frame_count, valid_frame_count = select_frame(station) + source = station / frame["output_file"] + if not source.is_file(): + raise FileNotFoundError(f"{station.name}: selected frame is absent: {source}") + checks = validation_checks(station) + selected = {"station": station.name, "source_frame": source, "frame_time": int(frame["unix_time_ns"]) / 1e9, "lat_deg": float(np.mean(lat)), "lon_deg": float(np.mean(lon)), "alt_m": float(np.mean(alt)), "raw_heading_deg": circular_mean_deg(heading), "raw_heading_std_deg": heading_std} + summary = {"station": station.name, "selected_frame": frame["output_file"], "exported_frames": frame_count, "frames_with_valid_rtk": valid_frame_count, "valid_rtk_samples": len(good), "raw_heading_mean_deg": selected["raw_heading_deg"], "raw_heading_std_deg": heading_std, "alt_std_m": float(np.std(alt)), "rtk_span_sec": float((times.max() - times.min()) / 1e9) if len(times) > 1 else 0.0, "payload_ok": checks.get("lidar_payload_length", ""), "rtk_parse_ok": checks.get("gps_post_z_parse", ""), "time_check_3s_ok": checks.get("rtk_time_alignment", "")} + return selected, summary + + +def main() -> int: + args = parse_args() + if args.min_stations < 2 or args.expected_stations < 0: + raise ValueError("--min-stations must be >=2 and --expected-stations must be >=0") + if not args.export_root.is_dir(): + raise NotADirectoryError(args.export_root) + names = args.groups or [path.name for path in args.export_root.iterdir() if path.is_dir()] + stations = [args.export_root / name for name in sorted(names, key=natural_key)] + missing = [str(path) for path in stations if not path.is_dir()] + if missing: + raise FileNotFoundError("station directories do not exist: " + ", ".join(missing)) + selected, summaries, rejected = [], [], [] + for station in stations: + try: + item, summary = process_station(station, args) + selected.append(item); summaries.append(summary) + except (ValueError, FileNotFoundError) as error: + rejected.append({"station": station.name, "reason": str(error)}) + if args.expected_stations and len(selected) != args.expected_stations: + raise RuntimeError(f"expected {args.expected_stations} usable stations, got {len(selected)}; rejected={rejected}") + if len(selected) < args.min_stations: + raise RuntimeError(f"need at least {args.min_stations} usable stations, got {len(selected)}; rejected={rejected}") + output, frames_dir = args.output, args.output / "frames_all" + if output.exists() and any(output.iterdir()) and not args.overwrite: + raise FileExistsError(f"{output} is non-empty; pass --overwrite to replace generated files") + frames_dir.mkdir(parents=True, exist_ok=True) + origin = selected[0] + origin_ecef = geodetic_to_ecef(origin["lat_deg"], origin["lon_deg"], origin["alt_m"]) + lever = np.asarray(args.antenna_lever, dtype=float) + pose_rows = [] + for index, item in enumerate(selected, 1): + destination = frames_dir / f"station_{index:02d}.npz" + shutil.copy2(item["source_frame"], destination) + antenna_enu = ecef_to_enu(geodetic_to_ecef(item["lat_deg"], item["lon_deg"], item["alt_m"]), origin_ecef, origin["lat_deg"], origin["lon_deg"]) + corrected_heading = (item["raw_heading_deg"] + args.heading_offset_deg) % 360.0 + yaw = math.radians(90.0 - corrected_heading) + body_position = antenna_enu - yaw_rotation(yaw) @ lever + pose_rows.append(dict(zip(POSE_FIELDS, [item["frame_time"], *body_position, 0.0, 0.0, math.sin(yaw / 2.0), math.cos(yaw / 2.0)]))) + summaries[index - 1].update({"sequence": index, "prepared_frame": destination.name, "corrected_heading_deg": corrected_heading}) + pose_path = output / f"body_poses_{args.pose_name}.csv" + with pose_path.open("w", encoding="utf-8", newline="") as stream: + writer = csv.DictWriter(stream, fieldnames=POSE_FIELDS); writer.writeheader(); writer.writerows(pose_rows) + fields = sorted({key for row in summaries for key in row}) + with (output / "station_summary.csv").open("w", encoding="utf-8", newline="") as stream: + writer = csv.DictWriter(stream, fieldnames=fields); writer.writeheader(); writer.writerows(summaries) + manifest = {"source_export_root": str(args.export_root.resolve()), "station_count": len(selected), "pose_csv": pose_path.name, "frame_directory": "frames_all", "selection_policy": "middle frame among exported frames with valid RTK; falls back to middle exported frame", "rtk_filter": {"position_valid": True, "heading_valid": True, "accepted_fixes": args.accepted_fixes, "heading_std_limit_deg": args.heading_std_limit_deg}, "body_pose_configuration": {"raw_heading_offset_deg": args.heading_offset_deg, "antenna_lever_body_m": args.antenna_lever, "body_axes": "x forward, y left, z up; origin must match the supplied lever", "orientation_model": "yaw-only from raw_heading_deg; this tool does not reconstruct RTK pitch or roll"}, "enu_origin": {"lat_deg": origin["lat_deg"], "lon_deg": origin["lon_deg"], "alt_m": origin["alt_m"]}, "stations": [{"sequence": index + 1, "source_station": item["station"], "source_frame": str(item["source_frame"]), "prepared_frame": f"station_{index + 1:02d}.npz"} for index, item in enumerate(selected)]} + (output / "manifest.json").write_text(json.dumps(manifest, ensure_ascii=False, indent=2), encoding="utf-8") + print(json.dumps({"prepared": str(output.resolve()), "stations": len(selected), "rejected": rejected, "pose_csv": pose_path.name}, ensure_ascii=False, indent=2)) + return 0 + + +if __name__ == "__main__": + try: + raise SystemExit(main()) + except Exception as error: + print(f"ERROR: {error}", file=sys.stderr) + raise