From 13624b0be88df101300ae3c9ba6ac70e49174036 Mon Sep 17 00:00:00 2001 From: "lichun.qu" <16975270+zzqlc@user.noreply.gitee.com> Date: Mon, 3 Aug 2026 16:08:37 +0800 Subject: [PATCH] =?UTF-8?q?=E6=96=B0=E5=A2=9E=E5=8E=9F=E5=A7=8B=E6=95=B0?= =?UTF-8?q?=E6=8D=AE=E4=B8=80=E6=AD=A5=E5=AF=BC=E5=87=BA=E5=88=B0=20combin?= =?UTF-8?q?ed=EF=BC=9A=E5=AF=B9=E9=BD=90=20Lidar-IMU=20=E5=AF=BC=E5=87=BA?= =?UTF-8?q?=E5=85=A5=E5=8F=A3=EF=BC=8C=E9=80=82=E9=85=8D=20H32/G90/N300?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Cursor --- README.md | 54 ++- data/README.md | 12 +- run/README.md | 16 +- run/export_multisensor_stations.ps1 | 85 ++--- run/run_full_pipeline.ps1 | 5 +- tools/README.md | 23 +- tools/build_multisensor_npz.py | 183 +++++++--- tools/export_h32_rscap_station.py | 172 +++++++++ tools/export_raw_to_combined.py | 322 +++++++++++++++++ tools/rscap_v2/h32_msop.py | 371 ++++++++++++++++++++ tools/rscap_v2/n300_imu.py | 113 ++++++ tools/rscap_v2/pipeline_common.py | 35 ++ tools/rscap_v2/pipeline_common_corrected.py | 111 +++++- 雷达与RTK标定说明书.md | 228 ++++++++++++ 14 files changed, 1600 insertions(+), 130 deletions(-) create mode 100644 tools/export_h32_rscap_station.py create mode 100644 tools/export_raw_to_combined.py create mode 100644 tools/rscap_v2/h32_msop.py create mode 100644 tools/rscap_v2/n300_imu.py create mode 100644 雷达与RTK标定说明书.md diff --git a/README.md b/README.md index d8b8f9d..50dac36 100644 --- a/README.md +++ b/README.md @@ -29,10 +29,9 @@ T_body_lidar = T_body_rtk · T_RTK_lidar ## 2. 算法流程 ```text -逐站LiDAR dlog + RTK.rscap + IMU.rscap - → 分别解析并保留原始字段 - → 以LiDAR帧时间为索引关联RTK/IMU，生成combined NPZ - → 每站选择一帧静态点云，GGA转局部ENU，rawHeading构造yaw-only RTK pose +逐站 H32.rscap + 全程 RTK.rscap + IMU.rscap + → tools/export_raw_to_combined.py（一步导出标定中间包 combined/） + → 每站选择一帧静态点云，位置转局部ENU，rawHeading构造yaw-only RTK pose → Open3D GICP和small_gicp分别求 B_ij = T_Li_Lj → 留出点、Hessian、正反向、多初值和旋转共轭不变量筛选 → 两后端共同认可的边形成consensus B @@ -51,21 +50,21 @@ X = T_RTK_lidar ## 3. 原始数据目录 -大体积数据不提交Git。`DataRoot`下每个站点必须是一个独立dlog目录，至少包含： +大体积数据不提交Git。**新车默认布局**（H32 / G90 / N300 新插件，不再出 dlog）： ```text raw_dataset/ -├── stations/ -│ ├── 001/ -│ │ ├── dobject/ -│ │ └── dobject_recording/ -│ ├── 002/ +├── stations/ # 每站停稳后单独录一段雷达 +│ ├── 001/h32.rscap +│ ├── 002/h32.rscap │ └── ... └── captures/ - ├── rtk.rscap - └── imu.rscap + ├── rtk.rscap # 进场到收工连续录（G90：#PVTSLNA + #UNIHEADINGA） + └── imu.rscap # 连续录（N300；仅关联，不参与外参求解） ``` +一键导出默认：雷达用 **MSOP 设备时间**，RTK 用 **GNSS week/TOW** 做最近邻关联（`-TimeBasis device_gnss`）。旧 dlog 数据集可继续放在同结构的 `dobject/` + `dobject_recording/` 下，并用 `-TimeBasis host`。 + 每个站点应在车辆完全静止后记录点云；建议不少于30站，并包含充足的直行、左转、右转和大角度转向姿态变化。 ## 4. 环境安装 @@ -80,7 +79,30 @@ python -m pip install -r requirements.txt ## 5. 从原始数据一键复现 -在仓库根目录执行，路径由使用者通过参数传入，脚本内没有本机绝对路径： +导出与 Lidar-IMU 的 `export_rscap_to_v1` 同级：**一条命令**把原始 rscap 变成标定可直接使用的 `combined/`。 + +仅导出中间包： + +```powershell +python tools\export_raw_to_combined.py ` + --stations-root "$Raw\stations" ` + --rtk-rscap "$Raw\captures\rtk.rscap" ` + --imu-rscap "$Raw\captures\imu.rscap" ` + --out "$Out\exported" ` + --overwrite +``` + +产物： + +```text +$Out\exported\ +├── export/ # 内部：各站雷达帧（调试用） +├── parsed/ # 内部：RTK/IMU JSONL +├── combined/ # ★ 标定入口：关联后的多传感器 NPZ + manifest.csv +└── export_summary.json +``` + +完整求解（导出 + prepare + AX=XB）在仓库根目录执行： ```powershell $Repo = (Resolve-Path ".").Path @@ -101,9 +123,9 @@ powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_full_pipe ```text $Out/ ├── exported/ -│ ├── export/ # 各站LiDAR逐帧NPZ -│ ├── parsed/ # RTK/IMU JSONL -│ └── combined/ # 按LiDAR帧关联后的多传感器NPZ +│ ├── export/ # 内部各站LiDAR帧 +│ ├── parsed/ # 内部 RTK/IMU JSONL +│ └── combined/ # ★ 按LiDAR帧关联后的多传感器NPZ ├── prepared_rtk_direct/ │ ├── frames_all/ # 每站选中的静态帧 │ └── reference_poses_rtk_gga_raw_heading.csv diff --git a/data/README.md b/data/README.md index 7aadaba..c408825 100644 --- a/data/README.md +++ b/data/README.md @@ -1,5 +1,13 @@ # 数据说明 -原始LiDAR dlog、RTK/IMU rscap、逐帧NPZ和prepared点云体积较大，不进入Git。请从项目云盘取得数据，并按根README中的目录示例放置；实际路径通过命令参数传入。 +原始 H32/G90/N300 `.rscap`（以及旧版 LiDAR dlog）、逐帧 NPZ 和 prepared 点云体积较大，不进入 Git。请从项目云盘取得数据，并按根 README 中的目录示例放置；实际路径通过命令参数传入。 -公开数据包应同时提供：采集日期、车辆/传感器安装版本、站点数量、ANT1/ANT2接线、rawHeading方向、RTK参考点离地高度及其测量方法。 +推荐原始布局： + +```text +raw_dataset/ +├── stations/<站号>/h32.rscap +└── captures/rtk.rscap, imu.rscap +``` + +公开数据包应同时提供：采集日期、车辆/传感器安装版本、站点数量、ANT1/ANT2 接线、rawHeading 方向、RTK 参考点离地高度及其测量方法。 diff --git a/run/README.md b/run/README.md index ef3b4bd..b2549ba 100644 --- a/run/README.md +++ b/run/README.md @@ -4,12 +4,24 @@ | 脚本 | 用途 | |---|---| -| `run_full_pipeline.ps1` | 从逐站LiDAR dlog、RTK rscap、IMU rscap一直运行到最终`T_RTK_lidar` | -| `export_multisensor_stations.ps1` | 解析原始三传感器数据并按LiDAR帧生成combined NPZ | +| `run_full_pipeline.ps1` | 调用一步导出得到 `combined/`，再跑到最终 `T_RTK_lidar` | +| `export_multisensor_stations.ps1` | 薄封装：调用 `tools/export_raw_to_combined.py` | | `prepare_multisensor_dataset.ps1` | 每站选一帧，生成yaw-only RTK参考轨迹和`frames_all` | | `run_direct_rtk_lidar.ps1` | 从combined数据运行RTK直接标定和最终结果封装 | | `run_single_dataset.ps1` | 执行地面、两个GICP后端、精筛、共识和AX=XB求解 | | `run_joint_rtk_lidar.ps1` | 合并多个独立批次的批内共识运动对和地面平面，求解共享外参 | | `view_result.ps1` | 打开3D运动对对比并打印数值增量 | +原始→中间包请优先直接用 Python 一步导出（与 Lidar-IMU 用法对齐）： + +```powershell +python tools\export_raw_to_combined.py --stations-root ... --rtk-rscap ... --imu-rscap ... --out ... --overwrite +``` + +常用参数： + +- `-LidarCaptureName h32.rscap`：每站雷达文件名（也接受 `lidar.rscap`） +- `-TimeBasis device_gnss`（默认）：雷达设备时 ↔ GNSS week/TOW +- `-TimeBasis host`：旧 dlog + 主机接收时间关联 + 所有路径均为命令行参数。标定入口要求显式传入RTK/GGA参考点离地高度，避免静默使用与实车不符的默认值；默认生成目录`work/`和`outputs/`不会提交Git。 diff --git a/run/export_multisensor_stations.ps1 b/run/export_multisensor_stations.ps1 index e96be10..41e5433 100644 --- a/run/export_multisensor_stations.ps1 +++ b/run/export_multisensor_stations.ps1 @@ -4,86 +4,55 @@ param( [Parameter(Mandatory = $true)][string]$RtkCapture, [Parameter(Mandatory = $true)][string]$ImuCapture, [string]$LidarObject = "frontlidar", + [string]$LidarCaptureName = "h32.rscap", [string]$Timezone = "+08:00", [string[]]$StationNames = @(), [int]$Stride = 1, [double]$RtkMaxDtMs = 150.0, [double]$ImuBeforeMs = 100.0, [double]$ImuAfterMs = 100.0, + [ValidateSet("device_gnss", "host")][string]$TimeBasis = "device_gnss", [switch]$SkipLidarExport, [switch]$SkipSerialParsing ) $ErrorActionPreference = "Stop" $RepoRoot = Split-Path -Parent $PSScriptRoot -$Exporter = Join-Path $RepoRoot "tools\frontlidar_dlog_export.py" -$Builder = Join-Path $RepoRoot "tools\build_multisensor_npz.py" -$Parser = Join-Path $RepoRoot "tools\rscap_v2\parse_rtk_imu_v2.py" -$Auditor = Join-Path $RepoRoot "tools\rscap_v2\audit_capture_v2.py" -$ExportRoot = Join-Path $OutputRoot "export" -$ParsedRoot = Join-Path $OutputRoot "parsed" -$CombinedRoot = Join-Path $OutputRoot "combined" - -function Run-Python { - param([string]$Stage, [string[]]$Arguments) - Write-Host "[$Stage]" - & python @Arguments - if ($LASTEXITCODE -ne 0) { - throw "$Stage failed with Python exit code $LASTEXITCODE" - } -} +$Exporter = Join-Path $RepoRoot "tools\export_raw_to_combined.py" foreach ($Path in @($DataRoot, $RtkCapture, $ImuCapture)) { if (-not (Test-Path -LiteralPath $Path)) { throw "Input does not exist: $Path" } } if ($Stride -lt 1) { throw "Stride must be at least 1" } - -if ($StationNames.Count -gt 0) { - $Stations = @($StationNames | ForEach-Object { Get-Item -LiteralPath (Join-Path $DataRoot $_) }) -} else { - $Stations = @(Get-ChildItem -LiteralPath $DataRoot -Directory | Where-Object { - (Test-Path -LiteralPath (Join-Path $_.FullName "dobject")) -and - (Test-Path -LiteralPath (Join-Path $_.FullName "dobject_recording")) - } | Sort-Object Name) -} -if ($Stations.Count -eq 0) { throw "No station directory containing dobject and dobject_recording was found" } - -New-Item -ItemType Directory -Force -Path $OutputRoot | Out-Null -if (-not $SkipSerialParsing) { - New-Item -ItemType Directory -Force -Path $ParsedRoot | Out-Null - Run-Python "capture audit" @($Auditor, $RtkCapture, $ImuCapture, "--out", (Join-Path $OutputRoot "capture_audit.json")) - Run-Python "RTK/IMU parse" @($Parser, "--rtk", $RtkCapture, "--imu", $ImuCapture, "--out", $ParsedRoot) +if ($SkipLidarExport -or $SkipSerialParsing) { + throw "Partial skip flags are no longer supported; use tools/export_raw_to_combined.py internals or run_direct_rtk_lidar.ps1 on an existing combined/" } -foreach ($Station in $Stations) { - $StationOut = Join-Path $ExportRoot $Station.Name - if (-not $SkipLidarExport) { - Run-Python "LiDAR station $($Station.Name)" @( - $Exporter, "--dlog", $Station.FullName, "--out", $StationOut, - "--object", $LidarObject, "--format", "npz", "--timezone", $Timezone, - "--stride", "$Stride", "--compress", "--skip-rtk", "--write-reports", "--resume" - ) - } - if (-not (Test-Path -LiteralPath (Join-Path $StationOut "frames"))) { - throw "Exported frame directory is absent for station $($Station.Name): $StationOut" - } -} - -$BuildArgs = @($Builder) -foreach ($Station in $Stations) { - $Frames = Join-Path (Join-Path $ExportRoot $Station.Name) "frames" - $BuildArgs += @("--lidar", "$($Station.Name)=$Frames") -} -$BuildArgs += @( - "--rtk", (Join-Path $ParsedRoot "rtk.jsonl"), - "--imu", (Join-Path $ParsedRoot "imu.jsonl"), - "--out", $CombinedRoot, +$Args = @( + $Exporter, + "--stations-root", $DataRoot, + "--rtk-rscap", $RtkCapture, + "--imu-rscap", $ImuCapture, + "--out", $OutputRoot, + "--lidar-capture-name", $LidarCaptureName, + "--lidar-object", $LidarObject, + "--timezone", $Timezone, + "--stride", "$Stride", "--rtk-max-dt-ms", "$RtkMaxDtMs", "--imu-before-ms", "$ImuBeforeMs", "--imu-after-ms", "$ImuAfterMs", + "--time-basis", $TimeBasis, "--overwrite" ) -Run-Python "LiDAR/RTK/IMU association" $BuildArgs +foreach ($Name in $StationNames) { + $Args += @("--station", $Name) +} -Write-Host "Completed stations: $($Stations.Count)" -Write-Host "Combined NPZ: $CombinedRoot" +Write-Host "[raw → combined one-shot export]" +& python @Args +if ($LASTEXITCODE -ne 0) { + throw "export_raw_to_combined failed with Python exit code $LASTEXITCODE" +} + +Write-Host "Combined NPZ: $(Join-Path $OutputRoot 'combined')" +Write-Host "Summary: $(Join-Path $OutputRoot 'export_summary.json')" diff --git a/run/run_full_pipeline.ps1 b/run/run_full_pipeline.ps1 index 200ebe6..67e7cd2 100644 --- a/run/run_full_pipeline.ps1 +++ b/run/run_full_pipeline.ps1 @@ -4,8 +4,10 @@ [Parameter(Mandatory = $true)][string]$ImuCapture, [Parameter(Mandatory = $true)][string]$OutputRoot, [string]$LidarObject = "frontlidar", + [string]$LidarCaptureName = "h32.rscap", [Parameter(Mandatory = $true)][double]$RtkReferenceHeightAboveGroundM, [string]$Timezone = "+08:00", + [ValidateSet("device_gnss", "host")][string]$TimeBasis = "device_gnss", [int]$ExpectedStations = 34, [int]$MinPairs = 20, [int]$Bootstrap = 200 @@ -18,7 +20,8 @@ $CalibrationRoot = Join-Path $OutputRoot "calibration" & (Join-Path $PSScriptRoot "export_multisensor_stations.ps1") ` -DataRoot $DataRoot -RtkCapture $RtkCapture -ImuCapture $ImuCapture ` - -OutputRoot $ExportRoot -LidarObject $LidarObject -Timezone $Timezone + -OutputRoot $ExportRoot -LidarObject $LidarObject -LidarCaptureName $LidarCaptureName ` + -Timezone $Timezone -TimeBasis $TimeBasis if ($LASTEXITCODE -ne 0) { throw "Raw-data export failed" } & (Join-Path $PSScriptRoot "run_direct_rtk_lidar.ps1") ` diff --git a/tools/README.md b/tools/README.md index c670433..56968b8 100644 --- a/tools/README.md +++ b/tools/README.md @@ -2,10 +2,27 @@ | 文件 | 输入→输出 | |---|---| -| `frontlidar_dlog_export.py` | LiDAR dlog → 逐帧原始点云NPZ；时间来自DObject post tick | -| `rscap_v2/parse_rtk_imu_v2.py` | RTK/IMU rscap → JSONL，保存校验状态、主机时间、GNSS/IMU设备字段和原始报文 | +| **`export_raw_to_combined.py`** | **一步导出**：逐站 H32 + 全程 G90/N300 `.rscap` → `combined/`（标定直接入口，对标 Lidar-IMU `export_rscap_to_v1`） | +| `export_h32_rscap_station.py` | 内部零件：单站 H32 → 雷达帧 NPZ（一般不必单独跑） | +| `frontlidar_dlog_export.py` | **旧数据** LiDAR dlog → 逐帧 NPZ；由一步导出在遇到 dlog 站时自动调用 | +| `rscap_v2/parse_rtk_imu_v2.py` | 单独解析 RTK/IMU（调试用）；一步导出已内嵌同等逻辑 | +| `rscap_v2/h32_msop.py` | H32 MSOP 解码（XYZ / 极坐标 `points_raw`） | +| `rscap_v2/n300_imu.py` | N300 FDILink 采样解码 | | `rscap_v2/audit_capture_v2.py` | 检查rscap结构、时间范围和记录统计 | -| `build_multisensor_npz.py` | 按LiDAR帧最近邻关联GGA/heading，并附加IMU时间窗 → combined NPZ | +| `build_multisensor_npz.py` | 关联雷达帧与 RTK/IMU → combined；一步导出内部调用 | | `prepare_multisensor_station_dataset.py` | combined NPZ → 每站一帧`frames_all`和`reference_poses_*.csv` | +推荐用法： + +```powershell +python tools\export_raw_to_combined.py ` + --stations-root path\to\stations ` + --rtk-rscap path\to\rtk.rscap ` + --imu-rscap path\to\imu.rscap ` + --out path\to\exported ` + --overwrite +``` + 当前标定只使用LiDAR和RTK；IMU保持原始传感器坐标，不参与点云去畸变或外参求解。prepared阶段对站内有效RTK取平均、对heading取圆均值，并选择有效帧序列的中间LiDAR帧。 + +G90 `#PVTSLNA` 没有 NMEA `fix_quality` 字段时，解析会写入合成值 `4`，以便沿用 prepare 的固定解筛选（`{4,5}`）。 diff --git a/tools/build_multisensor_npz.py b/tools/build_multisensor_npz.py index 2da2043..80e0de8 100644 --- a/tools/build_multisensor_npz.py +++ b/tools/build_multisensor_npz.py @@ -1,9 +1,15 @@ #!/usr/bin/env python3 """Build one LiDAR-centric NPZ per frame with matched RTK and an IMU window. -Inputs are LiDAR frame NPZ files from frontlidar_dlog_export.py and parsed -RTK/IMU JSONL files from parse_rtk_imu_v2.py. Raw .rscap files remain the -traceability source; this script never modifies them. +Inputs are LiDAR frame NPZ files from ``export_h32_rscap_station.py`` (or legacy +``frontlidar_dlog_export.py``) and parsed RTK/IMU JSONL from +``parse_rtk_imu_v2.py``. Raw ``.rscap`` files remain the traceability source; +this script never modifies them. + +Position rows may be NMEA ``GGA`` or G90 ``PVTSLNA`` (both expose ``lat_deg`` / +``lon_deg`` / ``altitude_m``). Default time basis is LiDAR device time vs GNSS +week/TOW; ``--time-basis host`` keeps the legacy host-receive nearest-neighbour +association for old dlog datasets. """ from __future__ import annotations @@ -18,6 +24,7 @@ import numpy as np GPS_EPOCH_UNIX_NS = 315964800 * 1_000_000_000 +POSITION_TYPES = {"GGA", "PVTSLNA"} def parse_named_path(text: str) -> tuple[str, Path]: @@ -46,6 +53,12 @@ def parse_args() -> argparse.Namespace: parser.add_argument("--imu-before-ms", type=float, default=100.0) parser.add_argument("--imu-after-ms", type=float, default=100.0) parser.add_argument("--gps-utc-leap-seconds", type=int, default=18) + parser.add_argument( + "--time-basis", + choices=("device_gnss", "host"), + default="device_gnss", + help="device_gnss: LiDAR unix_time_ns ↔ GNSS week/TOW; host: legacy host-receive association.", + ) parser.add_argument("--overwrite", action="store_true") return parser.parse_args() @@ -83,7 +96,7 @@ def nearest_index(times: np.ndarray, target: int) -> int: def estimate_imu_times(rows: list[dict[str, Any]]) -> list[dict[str, Any]]: - """Recover 100 Hz timing inside each serial chunk from device timestamps. + """Recover timing inside each serial chunk from device timestamps. A capture chunk has one host receive timestamp but may contain several IMU frames. The last frame is anchored to the chunk receive time and earlier @@ -119,6 +132,17 @@ def gnss_utc_ns(row: dict[str, Any], leap_seconds: int) -> int | None: return GPS_EPOCH_UNIX_NS + int(round(seconds * 1_000_000_000)) +def association_time_ns(row: dict[str, Any], time_basis: str, leap_seconds: int) -> int | None: + if time_basis == "host": + host = row.get("host_receive_utc_ns") + return int(host) if host is not None else None + device = gnss_utc_ns(row, leap_seconds) + if device is not None: + return device + host = row.get("host_receive_utc_ns") + return int(host) if host is not None else None + + def numeric_array(rows: list[dict[str, Any]], key: str, dtype: Any, default: Any) -> np.ndarray: return np.asarray([row.get(key, default) if row.get(key) is not None else default for row in rows], dtype=dtype) @@ -168,34 +192,66 @@ def initialize_rtk_measurements(values: dict[str, np.ndarray]) -> None: values["rtk_heading_gnss_utc_ns"] = np.asarray([0], dtype=np.int64) values["rtk_heading_host_minus_gnss_ns"] = np.asarray([0], dtype=np.int64) -def main() -> int: - args = parse_args() - if args.out.exists() and any(args.out.iterdir()) and not args.overwrite: - raise FileExistsError(f"{args.out} is non-empty; pass --overwrite") - frames_out = args.out / "frames" + +def build_combined( + lidar_segments: list[tuple[str, Path]], + rtk_paths: list[Path], + imu_paths: list[Path], + out: Path, + *, + rtk_max_dt_ms: float = 150.0, + imu_before_ms: float = 100.0, + imu_after_ms: float = 100.0, + gps_utc_leap_seconds: int = 18, + time_basis: str = "device_gnss", + overwrite: bool = False, +) -> dict[str, Any]: + """Associate LiDAR frames with RTK/IMU and write ``out/`` combined package.""" + + if out.exists() and any(out.iterdir()) and not overwrite: + raise FileExistsError(f"{out} is non-empty; pass overwrite=True") + frames_out = out / "frames" frames_out.mkdir(parents=True, exist_ok=True) - rtk_rows = load_jsonl(args.rtk) - gga = sorted( - [row for row in rtk_rows if row.get("type") == "GGA" and row.get("checksum_valid") and row.get("lat_deg") is not None], - key=lambda row: int(row["host_receive_utc_ns"]), - ) - heading = sorted( - [row for row in rtk_rows if row.get("type") == "UNIHEADINGA" and row.get("checksum_valid") and row.get("heading_valid")], - key=lambda row: int(row["host_receive_utc_ns"]), - ) - imu = estimate_imu_times(load_jsonl(args.imu)) - gga_times = np.asarray([int(row["host_receive_utc_ns"]) for row in gga], dtype=np.int64) - heading_times = np.asarray([int(row["host_receive_utc_ns"]) for row in heading], dtype=np.int64) + rtk_rows = load_jsonl(rtk_paths) + positions = [] + for row in rtk_rows: + if row.get("type") not in POSITION_TYPES or not row.get("checksum_valid"): + continue + if row.get("lat_deg") is None or row.get("lon_deg") is None: + continue + assoc = association_time_ns(row, time_basis, gps_utc_leap_seconds) + if assoc is None: + continue + copied = dict(row) + copied["_assoc_time_ns"] = assoc + positions.append(copied) + positions.sort(key=lambda row: int(row["_assoc_time_ns"])) + + heading = [] + for row in rtk_rows: + if row.get("type") != "UNIHEADINGA" or not row.get("checksum_valid") or not row.get("heading_valid"): + continue + assoc = association_time_ns(row, time_basis, gps_utc_leap_seconds) + if assoc is None: + continue + copied = dict(row) + copied["_assoc_time_ns"] = assoc + heading.append(copied) + heading.sort(key=lambda row: int(row["_assoc_time_ns"])) + + imu = estimate_imu_times(load_jsonl(imu_paths)) + position_times = np.asarray([int(row["_assoc_time_ns"]) for row in positions], dtype=np.int64) + heading_times = np.asarray([int(row["_assoc_time_ns"]) for row in heading], dtype=np.int64) imu_times = np.asarray([int(row["estimated_time_ns"]) for row in imu], dtype=np.int64) manifest: list[dict[str, Any]] = [] global_index = 0 - max_rtk_ns = int(args.rtk_max_dt_ms * 1_000_000) - before_ns = int(args.imu_before_ms * 1_000_000) - after_ns = int(args.imu_after_ms * 1_000_000) + max_rtk_ns = int(rtk_max_dt_ms * 1_000_000) + before_ns = int(imu_before_ms * 1_000_000) + after_ns = int(imu_after_ms * 1_000_000) - for segment_name, frame_dir in args.lidar: + for segment_name, frame_dir in lidar_segments: frame_paths = sorted(frame_dir.glob("*.npz")) if not frame_paths: raise FileNotFoundError(f"no NPZ frames under {frame_dir}") @@ -204,28 +260,31 @@ def main() -> int: values = {key: np.asarray(frame[key]) for key in frame.files} lidar_time_ns = int(scalar(values["unix_time_ns"])) - gga_index = nearest_index(gga_times, lidar_time_ns) + position_index = nearest_index(position_times, lidar_time_ns) heading_index = nearest_index(heading_times, lidar_time_ns) - gga_row = gga[gga_index] if gga_index >= 0 else None + position_row = positions[position_index] if position_index >= 0 else None heading_row = heading[heading_index] if heading_index >= 0 else None - gga_dt = int(gga_times[gga_index]) - lidar_time_ns if gga_index >= 0 else None + position_dt = int(position_times[position_index]) - lidar_time_ns if position_index >= 0 else None heading_dt = int(heading_times[heading_index]) - lidar_time_ns if heading_index >= 0 else None - gga_ok = gga_row is not None and abs(gga_dt or 0) <= max_rtk_ns + position_ok = position_row is not None and abs(position_dt or 0) <= max_rtk_ns heading_ok = heading_row is not None and abs(heading_dt or 0) <= max_rtk_ns - add_rtk(values, "rtk_gga", gga_row if gga_ok else None, gga_dt) + add_rtk(values, "rtk_gga", position_row if position_ok else None, position_dt) add_rtk(values, "rtk_heading", heading_row if heading_ok else None, heading_dt) initialize_rtk_measurements(values) - if gga_ok and gga_row: + if position_ok and position_row: for key, dtype, default in ( ("lat_deg", np.float64, np.nan), ("lon_deg", np.float64, np.nan), ("altitude_m", np.float64, np.nan), ("hdop", np.float64, np.nan), ("fix_quality", np.int32, -1), ("gga_satellites", np.int32, -1), ("differential_age_s", np.float64, np.nan), ): - values[f"rtk_{key}"] = np.asarray([gga_row.get(key, default)], dtype=dtype) - values["rtk_gga_satellites"] = np.asarray([gga_row.get("satellites", -1)], dtype=np.int32) - values["rtk_fixed"] = np.asarray([int(gga_row.get("fix_quality", -1)) in {4, 5}], dtype=np.uint8) + values[f"rtk_{key}"] = np.asarray([position_row.get(key, default)], dtype=dtype) + values["rtk_gga_satellites"] = np.asarray([position_row.get("satellites", -1)], dtype=np.int32) + if position_row.get("gnss_week") is not None: + values["rtk_gnss_week"] = np.asarray([position_row.get("gnss_week", -1)], dtype=np.int32) + values["rtk_gnss_tow_ms"] = np.asarray([position_row.get("gnss_tow_ms", -1)], dtype=np.int64) + values["rtk_fixed"] = np.asarray([int(position_row.get("fix_quality", -1)) in {4, 5}], dtype=np.uint8) if heading_ok and heading_row: for key, dtype, default in ( ("gnss_week", np.int32, -1), ("gnss_tow_ms", np.int64, -1), @@ -237,7 +296,7 @@ def main() -> int: values[f"rtk_{key}"] = np.asarray([heading_row.get(key, default)], dtype=dtype) values["rtk_heading_satellites"] = np.asarray([heading_row.get("satellites", -1)], dtype=np.int32) values["rtk_heading_solution_utf8"] = utf8_array(heading_row.get("heading_solution", "")) - device_ns = gnss_utc_ns(heading_row, args.gps_utc_leap_seconds) + device_ns = gnss_utc_ns(heading_row, gps_utc_leap_seconds) values["rtk_heading_gnss_utc_ns"] = np.asarray([device_ns or 0], dtype=np.int64) values["rtk_heading_host_minus_gnss_ns"] = np.asarray( [int(heading_row["host_receive_utc_ns"]) - device_ns if device_ns is not None else 0], dtype=np.int64 @@ -263,7 +322,9 @@ def main() -> int: raw_matrix, raw_lengths = raw_frame_matrix(window) values["imu_raw_frame_bytes"] = raw_matrix values["imu_raw_frame_length"] = raw_lengths - values["imu_source_files_json_utf8"] = utf8_array(json.dumps([str(path.resolve()) for path in args.imu], ensure_ascii=False)) + values["imu_source_files_json_utf8"] = utf8_array( + json.dumps([str(path.resolve()) for path in imu_paths], ensure_ascii=False) + ) values["source_lidar_file_utf8"] = utf8_array(source.resolve()) values["segment_name_utf8"] = utf8_array(segment_name) @@ -273,37 +334,65 @@ def main() -> int: "global_index": global_index, "segment": segment_name, "segment_index": segment_index, - "output": str(output.relative_to(args.out)), + "output": str(output.relative_to(out)), "source_lidar": str(source.resolve()), "lidar_time_ns": lidar_time_ns, - "rtk_gga_dt_ns": gga_dt, + "rtk_gga_dt_ns": position_dt, "rtk_heading_dt_ns": heading_dt, - "rtk_valid": gga_ok, + "rtk_valid": position_ok, "heading_valid": heading_ok, - "rtk_fix_quality": gga_row.get("fix_quality") if gga_ok and gga_row else None, - "rtk_fixed": bool(gga_ok and gga_row and int(gga_row.get("fix_quality", -1)) in {4, 5}), + "rtk_fix_quality": position_row.get("fix_quality") if position_ok and position_row else None, + "rtk_fixed": bool(position_ok and position_row and int(position_row.get("fix_quality", -1)) in {4, 5}), "imu_window_count": len(window), }) global_index += 1 fields = sorted({key for row in manifest for key in row}) - with (args.out / "manifest.csv").open("w", encoding="utf-8", newline="") as stream: + with (out / "manifest.csv").open("w", encoding="utf-8", newline="") as stream: writer = csv.DictWriter(stream, fieldnames=fields) writer.writeheader() writer.writerows(manifest) + if time_basis == "device_gnss": + time_basis_text = ( + "LiDAR MSOP/device unix_time_ns ↔ RTK GNSS week/TOW (fallback host receive); " + "IMU still windowed on host-anchored device deltas" + ) + else: + time_basis_text = ( + "LiDAR and serial host UTC; RTK GNSS time and IMU device time are retained for clock-model refinement" + ) summary = { "frames": len(manifest), - "segments": {name: sum(row["segment"] == name for row in manifest) for name, _ in args.lidar}, + "segments": {name: sum(row["segment"] == name for row in manifest) for name, _ in lidar_segments}, "rtk_valid": sum(bool(row["rtk_valid"]) for row in manifest), "heading_valid": sum(bool(row["heading_valid"]) for row in manifest), "rtk_fixed": sum(bool(row["rtk_fixed"]) for row in manifest), "imu_window_nonempty": sum(int(row["imu_window_count"]) > 0 for row in manifest), - "rtk_max_dt_ms": args.rtk_max_dt_ms, - "imu_window_ms": [-args.imu_before_ms, args.imu_after_ms], - "time_basis": "LiDAR and serial host UTC; RTK GNSS time and IMU device time are retained for clock-model refinement", + "rtk_max_dt_ms": rtk_max_dt_ms, + "imu_window_ms": [-imu_before_ms, imu_after_ms], + "time_basis": time_basis_text, + "time_basis_mode": time_basis, + "position_message_types": sorted(POSITION_TYPES), "imu_orientation_warning": "IMU values are in the raw IMU sensor frame; no LiDAR/body extrinsic is applied", } - (args.out / "dataset_summary.json").write_text(json.dumps(summary, ensure_ascii=False, indent=2), encoding="utf-8") + (out / "dataset_summary.json").write_text(json.dumps(summary, ensure_ascii=False, indent=2), encoding="utf-8") + return summary + + +def main() -> int: + args = parse_args() + summary = build_combined( + args.lidar, + args.rtk, + args.imu, + args.out, + rtk_max_dt_ms=args.rtk_max_dt_ms, + imu_before_ms=args.imu_before_ms, + imu_after_ms=args.imu_after_ms, + gps_utc_leap_seconds=args.gps_utc_leap_seconds, + time_basis=args.time_basis, + overwrite=args.overwrite, + ) print(json.dumps(summary, ensure_ascii=False, indent=2)) return 0 diff --git a/tools/export_h32_rscap_station.py b/tools/export_h32_rscap_station.py new file mode 100644 index 0000000..52cb649 --- /dev/null +++ b/tools/export_h32_rscap_station.py @@ -0,0 +1,172 @@ +#!/usr/bin/env python3 +"""Export one static-station H32 V2 .rscap into LiDAR frame NPZs. + +This is an **internal** helper used by ``export_raw_to_combined.py``. +For RTK–LiDAR calibration, prefer the one-shot exporter that writes ``combined/``. + +Output frame contract (consumed by ``build_multisensor_npz.py``): + +- ``points_raw``: (N, 5) polar ``d_mm, azimuth_deg, altitude_deg, intensity, progression`` +- ``unix_time_ns``: H32 MSOP device timestamp (seconds+us → ns) +- ``frame_counter``, ``point_count``, optional host receive stamp + +Raw ``.rscap`` files are never modified. +""" + +from __future__ import annotations + +import argparse +import csv +import json +import sys +from pathlib import Path +from typing import Any + +import numpy as np + +ROOT = Path(__file__).resolve().parent +sys.path.insert(0, str(ROOT / "rscap_v2")) + +from capture_format_v2 import file_summary, read_capture # noqa: E402 +from h32_msop import iter_h32_frames_polar # noqa: E402 + + +def resolve_lidar_rscap(station_dir: Path, capture_name: str = "h32.rscap") -> Path: + candidates = [ + station_dir / capture_name, + station_dir / "h32.rscap", + station_dir / "lidar.rscap", + ] + for path in candidates: + if path.is_file(): + return path + raise FileNotFoundError( + f"no LiDAR .rscap under {station_dir}; tried {[str(p.name) for p in candidates]}" + ) + + +def export_station_h32( + station: Path, + out: Path, + *, + capture_name: str = "h32.rscap", + stride: int = 1, + min_frame_points: int = 100, + min_range_m: float = 0.3, + max_range_m: float = 120.0, + compress: bool = True, + write_reports: bool = False, + resume: bool = False, +) -> dict[str, Any]: + """Decode one station H32 capture into ``out/frames/*.npz``. Returns metadata.""" + + rscap = station if station.is_file() and station.suffix.lower() == ".rscap" else resolve_lidar_rscap(station, capture_name) + frames_dir = out / "frames" + frames_dir.mkdir(parents=True, exist_ok=True) + + capture = read_capture(rscap) + frames = iter_h32_frames_polar( + capture, + min_frame_points=min_frame_points, + frame_stride=max(1, stride), + min_range_m=min_range_m, + max_range_m=max_range_m, + ) + if not frames: + raise RuntimeError(f"no H32 frames decoded from {rscap}") + + saver = np.savez_compressed if compress else np.savez + manifest_rows: list[dict[str, Any]] = [] + written = 0 + for index, frame in enumerate(frames): + unix_time_ns = int(round(frame.t_start_s * 1_000_000_000)) + name = f"h32_{index:06d}_{unix_time_ns}_frame{index}.npz" + destination = frames_dir / name + if resume and destination.exists(): + continue + points = np.asarray(frame.points_raw, dtype=np.float32) + payload = { + "points_raw": points, + "frame_counter": np.asarray([index], dtype=np.int32), + "point_count": np.asarray([points.shape[0]], dtype=np.int32), + "unix_time_ns": np.asarray([unix_time_ns], dtype=np.int64), + "device_time_s": np.asarray([frame.t_start_s], dtype=np.float64), + "device_time_end_s": np.asarray([frame.t_end_s], dtype=np.float64), + "host_receive_utc_ns": np.asarray([frame.host_receive_utc_ns], dtype=np.int64), + "source_file_utf8": np.frombuffer(str(rscap.resolve()).encode("utf-8"), dtype=np.uint8), + } + saver(destination, **payload) + written += 1 + manifest_rows.append( + { + "index": index, + "output": name, + "unix_time_ns": unix_time_ns, + "point_count": int(points.shape[0]), + "host_receive_utc_ns": int(frame.host_receive_utc_ns), + } + ) + + metadata: dict[str, Any] = { + "source_rscap": str(rscap.resolve()), + "capture": file_summary(capture), + "frames_decoded": len(frames), + "frames_written": written, + "frames_dir": str(frames_dir.resolve()), + "time_basis": "H32 MSOP device timestamp (packet seconds+microseconds)", + "points_raw_columns": ["d_mm", "azimuth_deg", "altitude_deg", "intensity", "progression"], + } + (out / "metadata.json").write_text(json.dumps(metadata, ensure_ascii=False, indent=2), encoding="utf-8") + (out / "README.md").write_text( + "# H32 station export (internal)\n\n" + f"- source: `{rscap}`\n" + f"- frames: `{frames_dir}`\n" + "- Prefer ``tools/export_raw_to_combined.py`` for the full RTK–LiDAR package.\n", + encoding="utf-8", + ) + if write_reports: + reports = out / "reports" + reports.mkdir(parents=True, exist_ok=True) + with (reports / "manifest.csv").open("w", encoding="utf-8", newline="") as stream: + writer = csv.DictWriter(stream, fieldnames=list(manifest_rows[0].keys()) if manifest_rows else ["index"]) + writer.writeheader() + writer.writerows(manifest_rows) + (reports / "export_summary.json").write_text(json.dumps(metadata, ensure_ascii=False, indent=2), encoding="utf-8") + return metadata + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--station", type=Path, required=True, help="Station directory or .rscap file") + parser.add_argument("--out", type=Path, required=True) + parser.add_argument("--capture-name", default="h32.rscap") + parser.add_argument("--stride", type=int, default=1) + parser.add_argument("--min-frame-points", type=int, default=100) + 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("--compress", action="store_true", default=True) + parser.add_argument("--write-reports", action="store_true") + parser.add_argument("--resume", action="store_true", help="Skip frames that already exist") + return parser.parse_args() + + +def main() -> int: + args = parse_args() + metadata = export_station_h32( + args.station, + args.out, + capture_name=args.capture_name, + stride=args.stride, + min_frame_points=args.min_frame_points, + min_range_m=args.min_range_m, + max_range_m=args.max_range_m, + compress=args.compress, + write_reports=args.write_reports, + resume=args.resume, + ) + print(json.dumps(metadata, ensure_ascii=False, indent=2)) + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/tools/export_raw_to_combined.py b/tools/export_raw_to_combined.py new file mode 100644 index 0000000..5a5e8ff --- /dev/null +++ b/tools/export_raw_to_combined.py @@ -0,0 +1,322 @@ +#!/usr/bin/env python3 +"""One-shot export: raw H32/G90/N300 captures → RTK–LiDAR ``combined/`` package. + +Analogous to Lidar-IMU ``tools/export_rscap_to_v1.py``: raw ``.rscap`` in, +calibration-ready intermediate out. Downstream prepare/solve consume ``combined/`` +only (``manifest.csv`` + associated frame NPZs). + +Expected raw layout: + + stations/ + 001/h32.rscap + 002/h32.rscap + ... + captures/ (paths passed explicitly) + rtk.rscap # G90: #PVTSLNA + #UNIHEADINGA + imu.rscap # N300 (associated only; not used in AX=XB) + +Output under ``--out``: + + export//frames/*.npz # internal LiDAR frames + parsed/rtk.jsonl, imu.jsonl + combined/frames/*.npz + manifest.csv + dataset_summary.json + export_summary.json + +Legacy dlog stations (``dobject`` + ``dobject_recording``) are still accepted; +use ``--time-basis host`` for those datasets. + +Raw ``.rscap`` / dlog files are never modified. +""" + +from __future__ import annotations + +import argparse +import json +import shutil +import subprocess +import sys +from pathlib import Path +from typing import Any + +ROOT = Path(__file__).resolve().parent +REPO = ROOT.parent +sys.path.insert(0, str(ROOT)) +sys.path.insert(0, str(ROOT / "rscap_v2")) + +from build_multisensor_npz import build_combined # noqa: E402 +from capture_format_v2 import file_summary, read_capture # noqa: E402 +from export_h32_rscap_station import export_station_h32, resolve_lidar_rscap # noqa: E402 +from pipeline_common_corrected import ( # noqa: E402 + parse_imu_capture, + parse_rtk_capture, + write_json, + write_jsonl, +) + + +def is_h32_station(station: Path, capture_name: str) -> bool: + try: + resolve_lidar_rscap(station, capture_name) + return True + except FileNotFoundError: + return False + + +def is_dlog_station(station: Path) -> bool: + return (station / "dobject").is_dir() and (station / "dobject_recording").is_dir() + + +def discover_stations(stations_root: Path, names: list[str], capture_name: str) -> list[Path]: + if names: + stations = [stations_root / name for name in names] + missing = [str(path) for path in stations if not path.is_dir()] + if missing: + raise FileNotFoundError(f"station directories missing: {missing}") + return stations + stations = sorted( + [ + path + for path in stations_root.iterdir() + if path.is_dir() and (is_h32_station(path, capture_name) or is_dlog_station(path)) + ], + key=lambda path: path.name, + ) + if not stations: + raise FileNotFoundError( + f"no station with {capture_name}/lidar.rscap or dobject+dobject_recording under {stations_root}" + ) + return stations + + +def export_legacy_dlog_station( + station: Path, + out: Path, + *, + lidar_object: str, + timezone: str, + stride: int, +) -> None: + exporter = ROOT / "frontlidar_dlog_export.py" + command = [ + sys.executable, + str(exporter), + "--dlog", + str(station), + "--out", + str(out), + "--object", + lidar_object, + "--format", + "npz", + "--timezone", + timezone, + "--stride", + str(stride), + "--compress", + "--skip-rtk", + "--write-reports", + "--resume", + ] + completed = subprocess.run(command, check=False) + if completed.returncode != 0: + raise RuntimeError(f"legacy dlog export failed for {station} (exit {completed.returncode})") + + +def parse_serial(rtk_rscap: Path, imu_rscap: Path, parsed_root: Path) -> dict[str, Any]: + parsed_root.mkdir(parents=True, exist_ok=True) + rtk_capture = read_capture(rtk_rscap) + imu_capture = read_capture(imu_rscap) + rtk_rows = parse_rtk_capture(rtk_capture) + imu_rows = parse_imu_capture(imu_capture) + write_jsonl(parsed_root / "rtk.jsonl", rtk_rows) + write_jsonl(parsed_root / "imu.jsonl", imu_rows) + summary = { + "rtk_capture": file_summary(rtk_capture), + "imu_capture": file_summary(imu_capture), + "rtk_records": len(rtk_rows), + "rtk_checksum_valid": sum(bool(row.get("checksum_valid")) for row in rtk_rows), + "rtk_pvtslna": sum(row.get("type") == "PVTSLNA" and row.get("checksum_valid") for row in rtk_rows), + "rtk_gga": sum(row.get("type") == "GGA" and row.get("checksum_valid") for row in rtk_rows), + "rtk_heading_valid": sum(row.get("type") == "UNIHEADINGA" and row.get("heading_valid") for row in rtk_rows), + "imu_frames": len(imu_rows), + "imu_crc_valid": sum(bool(row.get("crc_valid")) for row in imu_rows), + "imu_types": sorted({str(row.get("type")) for row in imu_rows}), + } + write_json(parsed_root / "parse_summary.json", summary) + return summary + + +def export_raw_to_combined( + *, + stations_root: Path, + rtk_rscap: Path, + imu_rscap: Path, + out: Path, + station_names: list[str] | None = None, + lidar_capture_name: str = "h32.rscap", + lidar_object: str = "frontlidar", + timezone: str = "+08:00", + stride: int = 1, + rtk_max_dt_ms: float = 150.0, + imu_before_ms: float = 100.0, + imu_after_ms: float = 100.0, + time_basis: str = "device_gnss", + overwrite: bool = False, +) -> dict[str, Any]: + """Full raw → combined export. Returns ``export_summary`` dict.""" + + if not stations_root.is_dir(): + raise FileNotFoundError(f"stations root does not exist: {stations_root}") + if not rtk_rscap.is_file(): + raise FileNotFoundError(f"RTK capture missing: {rtk_rscap}") + if not imu_rscap.is_file(): + raise FileNotFoundError(f"IMU capture missing: {imu_rscap}") + if out.exists() and any(out.iterdir()) and not overwrite: + raise FileExistsError(f"{out} is non-empty; pass --overwrite") + if overwrite and out.exists(): + # Keep out root but clear known children so rebuild is deterministic. + for child in ("export", "parsed", "combined", "export_summary.json", "capture_audit.json"): + target = out / child + if target.is_dir(): + shutil.rmtree(target) + elif target.is_file(): + target.unlink() + + out.mkdir(parents=True, exist_ok=True) + export_root = out / "export" + parsed_root = out / "parsed" + combined_root = out / "combined" + + stations = discover_stations(stations_root, station_names or [], lidar_capture_name) + parse_summary = parse_serial(rtk_rscap, imu_rscap, parsed_root) + + station_meta: list[dict[str, Any]] = [] + lidar_segments: list[tuple[str, Path]] = [] + saw_dlog = False + for station in stations: + station_out = export_root / station.name + if is_h32_station(station, lidar_capture_name): + meta = export_station_h32( + station, + station_out, + capture_name=lidar_capture_name, + stride=stride, + write_reports=True, + resume=False, + ) + kind = "h32_rscap" + elif is_dlog_station(station): + saw_dlog = True + export_legacy_dlog_station( + station, + station_out, + lidar_object=lidar_object, + timezone=timezone, + stride=stride, + ) + meta = {"source": str(station.resolve()), "kind": "legacy_dlog"} + kind = "legacy_dlog" + else: + raise RuntimeError(f"station {station.name} has neither H32 .rscap nor dlog layout") + frames_dir = station_out / "frames" + if not frames_dir.is_dir() or not any(frames_dir.glob("*.npz")): + raise RuntimeError(f"no exported frames for station {station.name}: {frames_dir}") + lidar_segments.append((station.name, frames_dir)) + station_meta.append({"station": station.name, "kind": kind, "frames_dir": str(frames_dir), **meta}) + + if saw_dlog and time_basis == "device_gnss": + print( + "[warn] legacy dlog stations use host/DObject time; prefer --time-basis host", + file=sys.stderr, + ) + + combined_summary = build_combined( + lidar_segments, + [parsed_root / "rtk.jsonl"], + [parsed_root / "imu.jsonl"], + combined_root, + rtk_max_dt_ms=rtk_max_dt_ms, + imu_before_ms=imu_before_ms, + imu_after_ms=imu_after_ms, + time_basis=time_basis, + overwrite=True, + ) + + summary = { + "role": "RTK-LiDAR one-shot raw export (like Lidar-IMU export_rscap_to_v1)", + "stations_root": str(stations_root.resolve()), + "rtk_rscap": str(rtk_rscap.resolve()), + "imu_rscap": str(imu_rscap.resolve()), + "out": str(out.resolve()), + "station_count": len(stations), + "stations": station_meta, + "parsed": parse_summary, + "combined": combined_summary, + "outputs": { + "combined": str(combined_root.resolve()), + "manifest": str((combined_root / "manifest.csv").resolve()), + "parsed": str(parsed_root.resolve()), + "export": str(export_root.resolve()), + }, + "timestamp_policy": { + "default_time_basis": time_basis, + "lidar_h32": "MSOP device timestamp → unix_time_ns", + "rtk": "GNSS week/TOW when time_basis=device_gnss; else host_receive_utc_ns", + "imu": "associated only; host-anchored device deltas in combined window", + "host_utc": "kept for audit; not the default calibration timeline for new captures", + }, + "next_step": "run/run_direct_rtk_lidar.ps1 -CombinedRoot /combined ...", + } + (out / "export_summary.json").write_text( + json.dumps(summary, ensure_ascii=False, indent=2) + "\n", + encoding="utf-8", + ) + return summary + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) + parser.add_argument("--stations-root", type=Path, required=True, help="Directory of per-station folders") + parser.add_argument("--rtk-rscap", type=Path, required=True, help="Continuous G90/RTK V2 .rscap") + parser.add_argument("--imu-rscap", type=Path, required=True, help="Continuous N300/IMU V2 .rscap") + parser.add_argument("--out", type=Path, required=True, help="Output package root (contains combined/)") + parser.add_argument("--station", action="append", default=[], help="Optional station name filter; repeatable") + parser.add_argument("--lidar-capture-name", default="h32.rscap") + parser.add_argument("--lidar-object", default="frontlidar", help="Legacy dlog DObject name") + parser.add_argument("--timezone", default="+08:00", help="Legacy dlog tick timezone") + parser.add_argument("--stride", type=int, default=1) + parser.add_argument("--rtk-max-dt-ms", type=float, default=150.0) + parser.add_argument("--imu-before-ms", type=float, default=100.0) + parser.add_argument("--imu-after-ms", type=float, default=100.0) + parser.add_argument("--time-basis", choices=("device_gnss", "host"), default="device_gnss") + parser.add_argument("--overwrite", action="store_true") + return parser.parse_args() + + +def main() -> int: + args = parse_args() + if args.stride < 1: + raise SystemExit("stride must be >= 1") + summary = export_raw_to_combined( + stations_root=args.stations_root, + rtk_rscap=args.rtk_rscap, + imu_rscap=args.imu_rscap, + out=args.out, + station_names=args.station, + lidar_capture_name=args.lidar_capture_name, + lidar_object=args.lidar_object, + timezone=args.timezone, + stride=args.stride, + rtk_max_dt_ms=args.rtk_max_dt_ms, + imu_before_ms=args.imu_before_ms, + imu_after_ms=args.imu_after_ms, + time_basis=args.time_basis, + overwrite=args.overwrite, + ) + print(json.dumps(summary, ensure_ascii=False, indent=2)) + print(f"\nCombined package ready: {summary['outputs']['combined']}") + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/tools/rscap_v2/h32_msop.py b/tools/rscap_v2/h32_msop.py new file mode 100644 index 0000000..ea50c01 --- /dev/null +++ b/tools/rscap_v2/h32_msop.py @@ -0,0 +1,371 @@ +"""Decode RoboSense H32 MSOP V2 .rscap into Cartesian frames (metres). + +Angle / distance conventions follow ``RSLidarH32_3D_RawCaptureNet48``: +azimuth = normalize(-(block_az + horizontal[ch])), altitude = vertical[ch], +distance_mm = raw * distance_unit_mm, then: + + x = d_m * cos(alt) * cos(az) + y = d_m * cos(alt) * sin(az) + z = d_m * sin(alt) + +MSOP-only captures do not include DIFOP; vertical angles default to a uniform +-16°…+16° fan, horizontal channel offsets default to 0. +""" + +from __future__ import annotations + +from dataclasses import dataclass + +import numpy as np + +from capture_format_v2 import CaptureFile + +PACKET_LENGTH = 1248 +DATA_START = 42 +BLOCKS = 12 +BLOCK_LENGTH = 100 +CHANNELS = 32 +MIN_FRAME_POINTS_DEFAULT = 100 +DOTNET_UNIX_EPOCH_TICKS = 621355968000000000 + + +def ticks_to_unix_ns(ticks: int) -> int: + return (ticks - DOTNET_UNIX_EPOCH_TICKS) * 100 + + +def default_vertical_deg() -> np.ndarray: + return -16.0 + np.arange(CHANNELS, dtype=np.float64) * (32.0 / (CHANNELS - 1)) + + +def default_horizontal_deg() -> np.ndarray: + return np.zeros(CHANNELS, dtype=np.float64) + + +def read_u16_be(packet: bytes, index: int) -> int: + return (packet[index] << 8) | packet[index + 1] + + +def device_timestamp_ms(packet: bytes) -> int: + seconds = int.from_bytes(packet[20:26], "big") + microseconds = int.from_bytes(packet[26:30], "big") + return seconds * 1000 + microseconds // 1000 + + +def distance_unit_mm(packet: bytes, *, auto: bool = True, fallback: float = 2.5) -> float: + if not auto: + return float(fallback) + return 2.5 if packet[17] == 1 else 0.5 + + +def normalize_azimuth_deg(angle: float) -> float: + while angle > 180.0: + angle -= 360.0 + while angle < -180.0: + angle += 360.0 + return angle + + +@dataclass +class LidarFrameExport: + t_start_s: float + t_end_s: float + points_xyz: np.ndarray # (N, 3) metres + + +@dataclass +class LidarFramePolarExport: + """One H32 frame in the calibration ``points_raw`` polar contract. + + Columns: ``d_mm, azimuth_deg, altitude_deg, intensity, progression``. + Azimuth already includes the H32 channel horizontal offset and sign flip so + ``rigorous_calibration.load_npz_xyz`` reproduces the same Cartesian points. + """ + + t_start_s: float + t_end_s: float + points_raw: np.ndarray # (N, 5) float32 + host_receive_utc_ns: int + + +def decode_packet_points( + packet: bytes, + vertical_deg: np.ndarray, + horizontal_deg: np.ndarray, + *, + min_range_m: float = 0.3, + max_range_m: float = 120.0, +) -> tuple[list[float], np.ndarray]: + """Decode one MSOP packet into block azimuths and concatenated XYZ points.""" + + if len(packet) != PACKET_LENGTH: + return [], np.zeros((0, 3), dtype=np.float64) + unit = distance_unit_mm(packet) + az_list: list[float] = [] + chunks: list[np.ndarray] = [] + idx = DATA_START + for _block in range(BLOCKS): + if idx + BLOCK_LENGTH > PACKET_LENGTH or packet[idx] != 255 or packet[idx + 1] != 238: + break + az = read_u16_be(packet, idx + 2) * 0.01 + az_list.append(az) + pts = _block_points( + packet, + idx, + az, + unit, + vertical_deg, + horizontal_deg, + min_range_m=min_range_m, + max_range_m=max_range_m, + ) + if pts.shape[0]: + chunks.append(pts) + idx += BLOCK_LENGTH + if not chunks: + return az_list, np.zeros((0, 3), dtype=np.float64) + return az_list, np.vstack(chunks) + + +def _block_points( + packet: bytes, + block_offset: int, + az_deg: float, + unit_mm: float, + vertical_deg: np.ndarray, + horizontal_deg: np.ndarray, + *, + min_range_m: float, + max_range_m: float, +) -> np.ndarray: + xs: list[float] = [] + ys: list[float] = [] + zs: list[float] = [] + idx = block_offset + 4 # after FF EE + azimuth + for ch in range(CHANNELS): + raw = read_u16_be(packet, idx) + idx += 3 + if raw == 0: + continue + d_m = (raw * unit_mm) * 0.001 + if d_m < min_range_m or d_m > max_range_m: + continue + az_ch = np.deg2rad(normalize_azimuth_deg(-(az_deg + float(horizontal_deg[ch])))) + alt = np.deg2rad(float(vertical_deg[ch])) + cos_alt = np.cos(alt) + xs.append(d_m * cos_alt * np.cos(az_ch)) + ys.append(d_m * cos_alt * np.sin(az_ch)) + zs.append(d_m * np.sin(alt)) + if not xs: + return np.zeros((0, 3), dtype=np.float64) + return np.column_stack([xs, ys, zs]).astype(np.float64, copy=False) + + +def _block_points_raw( + packet: bytes, + block_offset: int, + az_deg: float, + unit_mm: float, + vertical_deg: np.ndarray, + horizontal_deg: np.ndarray, + *, + min_range_m: float, + max_range_m: float, +) -> np.ndarray: + """Return polar ``points_raw`` rows compatible with ``load_npz_xyz``.""" + + rows: list[list[float]] = [] + idx = block_offset + 4 + for ch in range(CHANNELS): + raw = read_u16_be(packet, idx) + intensity = float(packet[idx + 2]) + idx += 3 + if raw == 0: + continue + d_mm = float(raw) * unit_mm + d_m = d_mm * 0.001 + if d_m < min_range_m or d_m > max_range_m: + continue + az_ch = normalize_azimuth_deg(-(az_deg + float(horizontal_deg[ch]))) + rows.append([d_mm, az_ch, float(vertical_deg[ch]), intensity, float(ch)]) + if not rows: + return np.zeros((0, 5), dtype=np.float32) + return np.asarray(rows, dtype=np.float32) + + +def iter_h32_frames_polar( + capture: CaptureFile, + *, + min_frame_points: int = MIN_FRAME_POINTS_DEFAULT, + frame_stride: int = 1, + min_range_m: float = 0.3, + max_range_m: float = 120.0, + max_points_per_frame: int | None = None, + vertical_deg: np.ndarray | None = None, + horizontal_deg: np.ndarray | None = None, +) -> list[LidarFramePolarExport]: + """Assemble MSOP packets into polar frames for the RTK–LiDAR combined contract.""" + + vertical = default_vertical_deg() if vertical_deg is None else np.asarray(vertical_deg, dtype=np.float64) + horizontal = default_horizontal_deg() if horizontal_deg is None else np.asarray(horizontal_deg, dtype=np.float64) + if vertical.shape != (CHANNELS,) or horizontal.shape != (CHANNELS,): + raise ValueError(f"vertical/horizontal must have shape ({CHANNELS},)") + + frames: list[LidarFramePolarExport] = [] + point_chunks: list[np.ndarray] = [] + t_start: float | None = None + t_end: float | None = None + host_ns = 0 + prev_az: float | None = None + kept = 0 + stride = max(1, int(frame_stride)) + + def emit() -> None: + nonlocal point_chunks, t_start, t_end, host_ns, kept + if not point_chunks or t_start is None or t_end is None: + point_chunks = [] + t_start = t_end = None + return + points = np.vstack(point_chunks) + point_chunks = [] + start_s, end_s = t_start, t_end + frame_host = host_ns + t_start = t_end = None + if points.shape[0] < min_frame_points: + return + if kept % stride != 0: + kept += 1 + return + kept += 1 + if max_points_per_frame is not None and points.shape[0] > max_points_per_frame: + select = np.linspace(0, points.shape[0] - 1, max_points_per_frame, dtype=int) + points = points[select] + if end_s <= start_s: + end_s = start_s + 0.1 + frames.append( + LidarFramePolarExport( + t_start_s=start_s, + t_end_s=end_s, + points_raw=points.astype(np.float32, copy=False), + host_receive_utc_ns=int(frame_host), + ) + ) + + for chunk in capture.chunks: + packet = chunk.raw + if len(packet) != PACKET_LENGTH: + continue + packet_t = device_timestamp_ms(packet) * 1e-3 + unit = distance_unit_mm(packet) + chunk_host = ticks_to_unix_ns(chunk.receive_utc_ticks) + idx = DATA_START + for _block in range(BLOCKS): + if idx + BLOCK_LENGTH > PACKET_LENGTH or packet[idx] != 255 or packet[idx + 1] != 238: + break + az = read_u16_be(packet, idx + 2) * 0.01 + if prev_az is not None and prev_az > 270.0 and az < 90.0: + emit() + prev_az = az + pts = _block_points_raw( + packet, + idx, + az, + unit, + vertical, + horizontal, + min_range_m=min_range_m, + max_range_m=max_range_m, + ) + if pts.shape[0]: + if t_start is None: + t_start = packet_t + t_end = packet_t + host_ns = chunk_host + point_chunks.append(pts) + idx += BLOCK_LENGTH + + emit() + return frames + + +def iter_h32_frames( + capture: CaptureFile, + *, + min_frame_points: int = MIN_FRAME_POINTS_DEFAULT, + frame_stride: int = 1, + min_range_m: float = 0.3, + max_range_m: float = 120.0, + max_points_per_frame: int | None = None, + vertical_deg: np.ndarray | None = None, + horizontal_deg: np.ndarray | None = None, +) -> list[LidarFrameExport]: + """Assemble MSOP packets into frames using the 270°→90° azimuth wrap.""" + + vertical = default_vertical_deg() if vertical_deg is None else np.asarray(vertical_deg, dtype=np.float64) + horizontal = default_horizontal_deg() if horizontal_deg is None else np.asarray(horizontal_deg, dtype=np.float64) + if vertical.shape != (CHANNELS,) or horizontal.shape != (CHANNELS,): + raise ValueError(f"vertical/horizontal must have shape ({CHANNELS},)") + + frames: list[LidarFrameExport] = [] + point_chunks: list[np.ndarray] = [] + t_start: float | None = None + t_end: float | None = None + prev_az: float | None = None + kept = 0 + stride = max(1, int(frame_stride)) + + def emit() -> None: + nonlocal point_chunks, t_start, t_end, kept + if not point_chunks or t_start is None or t_end is None: + point_chunks = [] + t_start = t_end = None + return + points = np.vstack(point_chunks) + point_chunks = [] + start_s, end_s = t_start, t_end + t_start = t_end = None + if points.shape[0] < min_frame_points: + return + if kept % stride != 0: + kept += 1 + return + kept += 1 + if max_points_per_frame is not None and points.shape[0] > max_points_per_frame: + select = np.linspace(0, points.shape[0] - 1, max_points_per_frame, dtype=int) + points = points[select] + if end_s <= start_s: + end_s = start_s + 0.1 + frames.append(LidarFrameExport(t_start_s=start_s, t_end_s=end_s, points_xyz=points)) + + for chunk in capture.chunks: + packet = chunk.raw + if len(packet) != PACKET_LENGTH: + continue + packet_t = device_timestamp_ms(packet) * 1e-3 + unit = distance_unit_mm(packet) + idx = DATA_START + for _block in range(BLOCKS): + if idx + BLOCK_LENGTH > PACKET_LENGTH or packet[idx] != 255 or packet[idx + 1] != 238: + break + az = read_u16_be(packet, idx + 2) * 0.01 + if prev_az is not None and prev_az > 270.0 and az < 90.0: + emit() + prev_az = az + pts = _block_points( + packet, + idx, + az, + unit, + vertical, + horizontal, + min_range_m=min_range_m, + max_range_m=max_range_m, + ) + if pts.shape[0]: + if t_start is None: + t_start = packet_t + t_end = packet_t + point_chunks.append(pts) + idx += BLOCK_LENGTH + + emit() + return frames diff --git a/tools/rscap_v2/n300_imu.py b/tools/rscap_v2/n300_imu.py new file mode 100644 index 0000000..fc89dda --- /dev/null +++ b/tools/rscap_v2/n300_imu.py @@ -0,0 +1,113 @@ +"""Decode Wheeltec N300 FDILink IMU frames from a V2 .rscap capture.""" + +from __future__ import annotations + +import struct +from dataclasses import dataclass + +import numpy as np + +from capture_format_v2 import CaptureFile, RawChunk, iter_contiguous_segments + + +@dataclass(frozen=True) +class ImuSample: + t_s: float + gyro_rad_s: tuple[float, float, float] + accel_m_s2: tuple[float, float, float] + host_receive_utc_ticks: int + device_timestamp_us: int + + +def crc8_fdilink(data: bytes) -> int: + crc = 0 + for value in data: + crc ^= value + for _ in range(8): + crc = ((crc << 1) ^ 0x07) & 0xFF if crc & 0x80 else (crc << 1) & 0xFF + return crc + + +def crc16_fdilink(data: bytes) -> int: + crc = 0 + for value in data: + crc ^= value << 8 + for _ in range(8): + crc = ((crc << 1) ^ 0x1021) & 0xFFFF if crc & 0x8000 else (crc << 1) & 0xFFFF + return crc + + +def _host_ticks_for_span(chunks: list[RawChunk], start: int, end: int) -> int: + stream_offset = 0 + last = chunks[0] + for chunk in chunks: + next_offset = stream_offset + len(chunk.raw) + if start < next_offset and end > stream_offset: + last = chunk + stream_offset = next_offset + return last.receive_utc_ticks + + +def iter_n300_imu_samples(capture: CaptureFile) -> list[ImuSample]: + """Return CRC-valid MSG_IMU (0x40) samples sorted by device timestamp.""" + + samples: list[ImuSample] = [] + expected_lengths = {0x40: 56, 0x41: 48} + for _segment_id, chunks in iter_contiguous_segments(capture.chunks): + stream = b"".join(chunk.raw for chunk in chunks) + cursor = 0 + while cursor < len(stream): + start = stream.find(b"\xFC", cursor) + if start < 0: + break + if start + 8 > len(stream): + break + payload_length = stream[start + 2] + end = start + payload_length + 8 + if end > len(stream): + if stream.find(b"\xFC", start + 1) < 0: + break + cursor = start + 1 + continue + frame = stream[start:end] + if frame[-1] != 0xFD: + cursor = start + 1 + continue + packet_id = frame[1] + payload = frame[7:-1] + header_ok = crc8_fdilink(frame[:4]) == frame[4] + payload_ok = crc16_fdilink(payload) == int.from_bytes(frame[5:7], "big") + expected = expected_lengths.get(packet_id) + length_ok = expected is None or len(payload) == expected + if not (header_ok and payload_ok and length_ok): + cursor = start + 1 + continue + if packet_id == 0x40: + gyro = struct.unpack_from("<3f", payload, 0) + accel = struct.unpack_from("<3f", payload, 12) + device_us = struct.unpack_from(" tuple[np.ndarray, np.ndarray, np.ndarray]: + if not samples: + return ( + np.zeros(0, dtype=np.float64), + np.zeros((0, 3), dtype=np.float64), + np.zeros((0, 3), dtype=np.float64), + ) + t = np.asarray([sample.t_s for sample in samples], dtype=np.float64) + gyro = np.asarray([sample.gyro_rad_s for sample in samples], dtype=np.float64) + accel = np.asarray([sample.accel_m_s2 for sample in samples], dtype=np.float64) + return t, gyro, accel diff --git a/tools/rscap_v2/pipeline_common.py b/tools/rscap_v2/pipeline_common.py index 3f733ce..7bdf230 100644 --- a/tools/rscap_v2/pipeline_common.py +++ b/tools/rscap_v2/pipeline_common.py @@ -131,6 +131,39 @@ def parse_heading(line: str) -> dict: } +def parse_pvtslna(line: str) -> dict: + """Parse Unicore/G90 ``#PVTSLNA`` into GGA-compatible position fields. + + ``fix_quality`` is synthesized as 4 when checksum-valid coordinates exist so + the existing prepare gate (accepted fixes {4,5}) keeps working. Position + stddevs are retained for audits. + """ + star = line.rfind("*") + fields = line[1:star if star >= 0 else None].split(",") + if len(fields) < 16: + raise ValueError("PVTSLNA has too few fields") + tow = safe_float(fields[5]) + return { + "type": "PVTSLNA", + "gnss_week": safe_int(fields[4]), + "gnss_tow_ms": int(tow) if tow is not None else None, + "altitude_m": safe_float(fields[10]), + "lat_deg": safe_float(fields[11]), + "lon_deg": safe_float(fields[12]), + "height_std_m": safe_float(fields[13]), + "latitude_std_m": safe_float(fields[14]), + "longitude_std_m": safe_float(fields[15]), + # Downstream prepare still filters on NMEA-style fix quality. + "fix_quality": 4, + "satellites": -1, + "hdop": None, + "differential_age_s": None, + "position_time_utc": "", + "geoid_separation_m": None, + "station_id": "", + } + + def chunk_source(chunks: list[RawChunk], offset: int, end: int) -> dict: first = chunks[0] last = chunks[-1] @@ -184,6 +217,8 @@ def parse_rtk_capture(capture: CaptureFile) -> list[dict]: try: if line.startswith("$GNGGA") or line.startswith("$GPGGA"): row.update(parse_gga(line)) + elif line.startswith("#PVTSLNA"): + row.update(parse_pvtslna(line)) elif line.startswith("#UNIHEADINGA"): row.update(parse_heading(line)) except ValueError as ex: diff --git a/tools/rscap_v2/pipeline_common_corrected.py b/tools/rscap_v2/pipeline_common_corrected.py index 35b37ef..ae74c68 100644 --- a/tools/rscap_v2/pipeline_common_corrected.py +++ b/tools/rscap_v2/pipeline_common_corrected.py @@ -1,6 +1,7 @@ from __future__ import annotations import bisect +import struct from pipeline_common import * from capture_format_v2 import CaptureFile, RawChunk, iter_contiguous_segments @@ -39,6 +40,7 @@ def source_for_span(chunks: list[RawChunk], start: int, end: int, segment_id: in "host_receive_monotonic_ticks": end_chunk.receive_monotonic_ticks, } + def parse_rtk_capture(capture: CaptureFile) -> list[dict]: rows = [] for segment_id, chunks in iter_contiguous_segments(capture.chunks): @@ -60,6 +62,8 @@ def parse_rtk_capture(capture: CaptureFile) -> list[dict]: try: if line.startswith("$GNGGA") or line.startswith("$GPGGA"): row.update(parse_gga(line)) + elif line.startswith("#PVTSLNA"): + row.update(parse_pvtslna(line)) elif line.startswith("#UNIHEADINGA"): row.update(parse_heading(line)) except ValueError as ex: @@ -68,7 +72,103 @@ def parse_rtk_capture(capture: CaptureFile) -> list[dict]: return rows -def parse_imu_capture(capture: CaptureFile) -> list[dict]: +def crc8_fdilink(data: bytes) -> int: + crc = 0 + for value in data: + crc ^= value + for _ in range(8): + crc = ((crc << 1) ^ 0x07) & 0xFF if crc & 0x80 else (crc << 1) & 0xFF + return crc + + +def crc16_fdilink(data: bytes) -> int: + crc = 0 + for value in data: + crc ^= value << 8 + for _ in range(8): + crc = ((crc << 1) ^ 0x1021) & 0xFFFF if crc & 0x8000 else (crc << 1) & 0xFFFF + return crc + + +def parse_n300_imu_capture(capture: CaptureFile) -> list[dict]: + """Parse Wheeltec N300 FDILink IMU frames; normalize to HI13-like keys.""" + + rows = [] + expected_lengths = {0x40: 56, 0x41: 48} + for segment_id, chunks in iter_contiguous_segments(capture.chunks): + stream = b"".join(chunk.raw for chunk in chunks) + cursor = 0 + while cursor < len(stream): + start = stream.find(b"\xFC", cursor) + if start < 0: + break + if start + 8 > len(stream): + break + payload_length = stream[start + 2] + end = start + payload_length + 8 + if end > len(stream): + if stream.find(b"\xFC", start + 1) < 0: + break + cursor = start + 1 + continue + frame = stream[start:end] + if frame[-1] != 0xFD: + cursor = start + 1 + continue + packet_id = frame[1] + payload = frame[7:-1] + header_ok = crc8_fdilink(frame[:4]) == frame[4] + payload_ok = crc16_fdilink(payload) == int.from_bytes(frame[5:7], "big") + expected = expected_lengths.get(packet_id) + length_ok = expected is None or len(payload) == expected + row = { + "type": "N300", + "tag": int(packet_id), + "frame_length": len(frame), + "crc_valid": bool(header_ok and payload_ok and length_ok), + "raw_frame_hex": frame.hex(), + } + row.update(source_for_span(chunks, start, end, segment_id)) + if row["crc_valid"] and packet_id == 0x40: + try: + gyro = struct.unpack_from("<3f", payload, 0) + accel = struct.unpack_from("<3f", payload, 12) + device_us = struct.unpack_from(" list[dict]: rows = [] for segment_id, chunks in iter_contiguous_segments(capture.chunks): stream = b"".join(chunk.raw for chunk in chunks) @@ -104,3 +204,12 @@ def parse_imu_capture(capture: CaptureFile) -> list[dict]: rows.append(row) cursor = end return rows + + +def parse_imu_capture(capture: CaptureFile) -> list[dict]: + """Prefer N300 FDILink when present; fall back to legacy HI13.""" + + n300 = parse_n300_imu_capture(capture) + if any(row.get("crc_valid") and row.get("type") == "N300" for row in n300): + return n300 + return parse_hi13_imu_capture(capture) diff --git a/雷达与RTK标定说明书.md b/雷达与RTK标定说明书.md new file mode 100644 index 0000000..98bbab4 --- /dev/null +++ b/雷达与RTK标定说明书.md @@ -0,0 +1,228 @@ +# 雷达与 RTK 标定说明书 + +本文说明如何用本仓库完成 **双天线 RTK ↔ 3D 激光雷达** 外参标定，得到可直接使用的 `T_RTK_lidar`。 + +--- + +## 1. 标定目标 + +求解外参 `T_RTK_lidar`，把雷达点变换到 RTK 导航系： + +```text +p_RTK = T_RTK_lidar · p_lidar +``` + +| 项目 | 说明 | +|---|---| +| 输出文件 | `final_T_RTK_lidar.json` | +| 坐标系 | RTK 导航系（GGA 原点 + 双天线航向），**不是**车体后轮轴系 | +| 不用到的量 | 车体航向偏置、天线 XY 杆臂、IMU 姿态 | +| 必须提供 | RTK 参考点（通常 ANT1）离地高度 | + +若下游需要车体外参，需另有已确认的 `T_body_rtk`： + +```text +T_body_lidar = T_body_rtk · T_RTK_lidar +``` + +--- + +## 2. 环境准备 + +- 系统：Windows + PowerShell +- Python：3.11 +- 安装依赖： + +```powershell +python -m pip install -r requirements.txt +``` + +依赖：NumPy、SciPy、Open3D、small_gicp。完整流程需要 **Open3D 与 small_gicp 两个配准后端**；若 Windows 无 small_gicp wheel，可改用 WSL2。 + +--- + +## 3. 数据采集 + +### 3.1 目录结构 + +**新车（默认）**：每站一段 H32 雷达 `.rscap`，RTK/IMU 全程各一条： + +```text +raw_dataset/ +├── stations/ +│ ├── 001/h32.rscap +│ ├── 002/h32.rscap +│ └── ... +└── captures/ + ├── rtk.rscap # G90：#PVTSLNA 位置 + #UNIHEADINGA 航向 + └── imu.rscap # N300；仅关联保存，不参与外参求解 +``` + +旧车 dlog 布局（`dobject/` + `dobject_recording/`）仍可被导出脚本识别；关联时间请用 `-TimeBasis host`。 + +### 3.2 采集要求 + +| 要求 | 建议 | +|---|---| +| 站点数 | ≥ 30 站 | +| 车辆状态 | **完全静止**后再记点云 | +| 姿态覆盖 | 直行、左转、右转、大角度转向都要有 | +| RTK 质量 | 固定解（质量 4/5），航向有效 | +| 站内航向稳定 | 圆标准差 ≤ 0.5° | +| 必测量 | **ANT1（GGA 参考点）离地高度**，含天线相位中心修正 | + +### 3.3 现场确认（标定前必做） + +1. **哪根天线是 GGA 原点**（通常 ANT1） +2. **`rawHeading` 方向**：ANT1→ANT2 还是相反（搞反会导致 yaw 差约 180°） +3. **离地高度测法**：例如安装底面高度 + 天线 PCO，写入求解参数，不要事后只改 JSON 里的 z + +--- + +## 4. 一键标定 + +### 4.1 仅导出标定中间包（推荐先跑通） + +与 Lidar-IMU 的 `export_rscap_to_v1` 同级：原始数据 → `combined/`。 + +```powershell +python tools\export_raw_to_combined.py ` + --stations-root "E:\calibration_data\stations" ` + --rtk-rscap "E:\calibration_data\captures\rtk.rscap" ` + --imu-rscap "E:\calibration_data\captures\imu.rscap" ` + --out "E:\calibration_output\exported" ` + --overwrite +``` + +### 4.2 导出 + 求解到最终外参 + +在仓库根目录执行（路径按本机修改）： + +```powershell +$Repo = (Resolve-Path ".").Path +$Raw = "E:\calibration_data\data4" +$Out = "E:\calibration_output\rtk_lidar" + +powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_full_pipeline.ps1" ` + -DataRoot "$Raw\stations" ` + -RtkCapture "$Raw\captures\rtk.rscap" ` + -ImuCapture "$Raw\captures\imu.rscap" ` + -OutputRoot $Out ` + -RtkReferenceHeightAboveGroundM 0.758 ` + -ExpectedStations 34 +``` + +| 关键参数 | 含义 | +|---|---| +| `-RtkReferenceHeightAboveGroundM` | RTK 参考点离地高度（米），**必填** | +| `-ExpectedStations` | 期望站点数 | +| `-MinPairs` | 最少共识运动对，默认 20 | +| `-Bootstrap` | bootstrap 次数，默认 200 | + +### 已有 combined 数据时 + +可跳过原始导出，直接标定： + +```powershell +powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_direct_rtk_lidar.ps1" ` + -CombinedRoot "...\exported\combined" ` + -WorkRoot "...\prepared_rtk_direct" ` + -OutputRoot "...\calibration" ` + -RtkReferenceHeightAboveGroundM 0.758 ` + -ExpectedStations 34 +``` + +--- + +## 5. 输出说明 + +```text +$Out/ +├── exported/ # 解析与关联中间结果 +├── prepared_rtk_direct/ # 每站一帧 + RTK 位姿表 +└── calibration/ + ├── open3d_gicp/ # 后端 1 + ├── small_gicp/ # 后端 2 + ├── consensus/ # 双后端共识运动对 + ├── summary.json # 质量汇总 + └── final_T_RTK_lidar.json ← 最终交付物 +``` + +`final_T_RTK_lidar.json` 主要字段： + +- `translation_m`：平移 (x, y, z)，单位米 +- `rotation_rpy_deg_xyz`：滚转 / 俯仰 / 偏航，单位度 +- `matrix_4x4`：4×4 齐次变换矩阵 + +质量指标看 `summary.json`：共识对数、AX 残差 RMS/中位数/P95、bootstrap 标准差、双后端差异。 + +> 内部一致性好 ≠ 已达到 ±3 cm 绝对真值；正式部署前建议再做独立轨迹验证。 + +--- + +## 6. 结果检查（可视化） + +```powershell +powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\view_result.ps1" ` + -Frames "$Out\prepared_rtk_direct\frames_all" ` + -Pairs "$Out\calibration\consensus\B_consensus.npz" ` + -Extrinsic "$Out\calibration\final_T_RTK_lidar.json" ` + -PairIndex 0 +``` + +| 按键 | 含义 | +|---|---| +| `1` | 原始点云 | +| `2` | 仅用 RTK 运动作初值 | +| `3` | GICP 测得的 B | +| `4` | 外参预测 `X⁻¹ A X`（应与 3 重合） | +| `Q` / `Esc` | 退出 | + +蓝 = 目标站 i，橙 = 源站 j。重点看模式 **3 与 4**：墙面、立柱、路缘、地面应基本重合。**多看几对**，不要只挑视觉最好的一对。 + +--- + +## 7. 多批次联合（可选） + +传感器安装未变、坐标定义一致时，可合并多批共识运动对再求共享外参： + +```powershell +powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_joint_rtk_lidar.ps1" ` + -BatchNames @("data4","data5") ` + -Pairs @("...\data4\consensus\B_consensus.npz","...\data5\consensus\B_consensus.npz") ` + -GroundPlanes @("...\data4\common\ground_planes.csv","...\data5\common\ground_planes.csv") ` + -OutputRoot "...\data4_data5_joint" ` + -RtkReferenceHeightAboveGroundM 0.758 ` + -Bootstrap 200 +``` + +任一批与首批相差超过 **0.25 m** 或 **5°** 会中止，需先检查航向定义与安装是否一致。 + +--- + +## 8. 注意事项 + +1. **z 不能只靠水平运动估出来**，必须靠实测天线高度约束；改高度后要 **重新跑求解**，禁止只改 JSON 的 z。 +2. **不要改站点目录名 / `station_*.npz` 顺序**，运动对索引依赖该顺序。 +3. 标定用 **原始雷达点**（`points_raw`），不要用已变换到车体的点。 +4. 当前时间对齐以主机接收时间为主，尚未估计设备时钟偏差。 +5. IMU 只解析关联，**不求解 IMU 外参**，静止站也不做运动去畸变。 +6. 仓库内 `results/reference_data4` 为历史参考（旧高度），**不要**当作当前部署外参直接下发。 + +--- + +## 9. 流程一览 + +```text +静止多站采集（LiDAR dlog + RTK/IMU rscap） + ↓ +解析关联 → 每站选一帧 + yaw-only RTK 位姿 + ↓ +双后端 GICP 求站间运动 B → 精筛 → 共识 + ↓ +AX=XB + 地面高度约束 → T_RTK_lidar + ↓ +可视化 / summary 检查 → 交付 final_T_RTK_lidar.json +``` + +更细的算法说明与指标对比见根目录 [`README.md`](README.md)；脚本入口见 [`run/README.md`](run/README.md)。