新增原始数据一步导出到 combined：对齐 Lidar-IMU 导出入口，适配 H32/G90/N300
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
lichun.qu
Cursor
@@ -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
|
||||
|
||||
+10
-2
@@ -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 参考点离地高度及其测量方法。
|
||||
|
||||
+14
-2
@@ -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。
|
||||
|
||||
@@ -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')"
|
||||
|
||||
@@ -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") `
|
||||
|
||||
+20
-3
@@ -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}`）。
|
||||
|
||||
+136
-47
@@ -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
|
||||
|
||||
|
||||
@@ -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())
|
||||
@@ -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/<station>/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 <out>/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())
|
||||
@@ -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
|
||||
@@ -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("<q", payload, 48)[0]
|
||||
samples.append(
|
||||
ImuSample(
|
||||
t_s=float(device_us) * 1e-6,
|
||||
gyro_rad_s=gyro,
|
||||
accel_m_s2=accel,
|
||||
host_receive_utc_ticks=_host_ticks_for_span(chunks, start, end),
|
||||
device_timestamp_us=int(device_us),
|
||||
)
|
||||
)
|
||||
cursor = end
|
||||
samples.sort(key=lambda sample: (sample.t_s, sample.device_timestamp_us))
|
||||
return samples
|
||||
|
||||
|
||||
def samples_to_arrays(samples: list[ImuSample]) -> 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
|
||||
@@ -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:
|
||||
|
||||
@@ -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("<q", payload, 48)[0]
|
||||
row.update(
|
||||
{
|
||||
"device_timestamp_us": int(device_us),
|
||||
# build_multisensor_npz.estimate_imu_times uses ms.
|
||||
"device_timestamp_ms": int(device_us) // 1000,
|
||||
"gyro_x_radps": gyro[0],
|
||||
"gyro_y_radps": gyro[1],
|
||||
"gyro_z_radps": gyro[2],
|
||||
"accel_x_mps2": accel[0],
|
||||
"accel_y_mps2": accel[1],
|
||||
"accel_z_mps2": accel[2],
|
||||
"pps_sync_stamp_ms": -1,
|
||||
}
|
||||
)
|
||||
except (IndexError, struct.error, ValueError) as ex:
|
||||
row["parse_error"] = str(ex)
|
||||
row["crc_valid"] = False
|
||||
elif row["crc_valid"] and packet_id == 0x41:
|
||||
try:
|
||||
device_us = struct.unpack_from("<q", payload, 40)[0]
|
||||
row.update(
|
||||
{
|
||||
"device_timestamp_us": int(device_us),
|
||||
"device_timestamp_ms": int(device_us) // 1000,
|
||||
"pps_sync_stamp_ms": -1,
|
||||
}
|
||||
)
|
||||
except (IndexError, struct.error, ValueError) as ex:
|
||||
row["parse_error"] = str(ex)
|
||||
rows.append(row)
|
||||
cursor = end if row["crc_valid"] else start + 1
|
||||
return rows
|
||||
|
||||
|
||||
def parse_hi13_imu_capture(capture: CaptureFile) -> 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)
|
||||
|
||||
+228
@@ -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)。
|
||||
Reference in New Issue
Block a user