更新README并添加标定工具
This commit is contained in:
lichun.qu
@@ -99,6 +99,19 @@ X=T_{body\leftarrow lidar}
|
||||
这里的 Z Bootstrap 只描述当前地面模型条件下的内部稳定性，不代表机械高度的绝对精度。
|
||||
|
||||
重要限制：旋转 RMS 仍约 1°，所以这不是厘米级绝对精度认证。远距离点云仍可能看到角度错层；Bootstrap 也不包含 RTK 参考点、航向偏置或杆臂定义错误等系统误差。
|
||||
### `code/` 文件职责
|
||||
|
||||
`code/` 中的 Python 脚本按“生成 B → 独立筛选 B → 求解 X → 汇总/诊断”的顺序组织。通常应通过 `run/` 下的 PowerShell 脚本调度，而不是手工跳过中间筛选步骤。
|
||||
|
||||
| 文件 | 职责 | 主要输入 → 主要输出 |
|
||||
|---|---|---|
|
||||
| [`rigorous_calibration.py`](code/rigorous_calibration.py) | **主求解器**，实现坐标变换、Open3D GICP/small_gicp 配准、质量计算和手眼优化。`ground` 从 `points_raw` 做 RANSAC 地面平面；`pairs` 将 RTK 车体轨迹构造成 A、将点云配准构造成 B，并输出海森矩阵、留出点、正反向和多初值质量报告；`calibrate` 用 Huber 鲁棒最小二乘、地面约束、多初值和 Bootstrap 求 `T_body_lidar`；`validate` 只计算给定 X 在独立 B 集上的 AX 残差。 | prepared 的 `frames_all`、车体位姿 CSV、B/地面文件 → `.npz` B 集、质量 JSON/CSV、外参 JSON、辅助复核 JSON。 |
|
||||
| [`refine_pairs.py`](code/refine_pairs.py) | **第二级 B 精筛器**。按留出点重叠率/RMSE、旋转共轭不变量、正反向一致性剔除差的 B；不读取 X，也不用 AX 残差选 B。 | 初始 B `.npz` + 配准质量 JSON → `*_refined.npz` + `*.refinement.json` 审计记录。 |
|
||||
| [`cross_backend_filter.py`](code/cross_backend_filter.py) | **跨后端一致性门控**。只保留 Open3D 与 small_gicp 对同一运动对的 B 差异不超过默认 `0.05 m / 0.50°` 的边；最终 B 本体采用 Open3D，small_gicp 仅作为独立门控。 | 两套 refined B `.npz` → `consensus/B_batch*_consensus.npz` + `*.consensus.json`。 |
|
||||
| [`finalize_consensus.py`](code/finalize_consensus.py) | **发布共识解**。读取共识 B 求得的外参、第一批辅助复核和两单后端外参，写出带选择依据的 `final_extrinsic_recommended.json` 与 `final_summary.json`。 | 共识外参/复核 JSON、两后端外参 JSON → 推荐外参和最终摘要 JSON。 |
|
||||
| [`summarize_results.py`](code/summarize_results.py) | **单后端阶段汇总器**。比较 Open3D 与 small_gicp 的外参差异、第二批拟合残差和第一批辅助复核，生成 `comparison_summary.json`；完整流程随后由 `finalize_consensus.py` 以共识解覆盖推荐结果。 | 两单后端外参、质量、辅助复核 JSON → 对比摘要 JSON 与阶段性推荐 JSON。 |
|
||||
| [`scan_extrinsic_sensitivity.py`](code/scan_extrinsic_sensitivity.py) | **局部灵敏度诊断器**。对 X 在车体系左乘小的 roll/pitch/yaw 修正，比较某一对与全部 B 对的残差、改善/变差数量、地面指标和 Z 可观性。只生成扫描 JSON/CSV，**绝不改写 X**。 | B `.npz`、外参 JSON、可选地面 CSV → `diagnostics/*_scan.json/.csv`。 |
|
||||
| [`visualize_pair_3d.py`](code/visualize_pair_3d.py) | **交互式 3D 点云核查器**。蓝色为目标站 i、橙色为源站 j；按键 1/2/3/4 分别查看原始、RTK 初值 A、GICP B、外参预测 `X^-1 A X`，并打印 `B^-1(X^-1AX)` 的 cm/deg 增量。可选按键 5 显示临时 RPY 修正，修正不写入文件。 | 站点点云、B `.npz`、外参 JSON → Open3D 交互窗口与终端增量。 |
|
||||
|
||||
## 坐标与公式
|
||||
|
||||
@@ -149,6 +162,9 @@ run/
|
||||
run_sensitivity_scan.ps1
|
||||
view_open3d_result.ps1
|
||||
view_small_gicp_result.ps1
|
||||
tools/
|
||||
frontlidar_dlog_export.py # 从单个原始 dlog 导出 LiDAR NPZ 与 RTK sidecar
|
||||
prepare_station_dataset.py # 从逐站导出结果构建标定所需 prepared 数据集
|
||||
results/
|
||||
common/
|
||||
open3d_gicp/
|
||||
@@ -160,7 +176,162 @@ results/
|
||||
final_summary.json
|
||||
```
|
||||
|
||||
## 安装与完整重跑
|
||||
## 从原始数据到最终外参
|
||||
|
||||
下面是从云盘中的**原始 dlog**到最终 LiDAR—车体外参的完整流程。第 2、3 步由本仓库的 tools/ 脚本完成；第 4 步及之后由 run/ 调度。不要把云盘中的原始数据直接传给 run_all.ps1。
|
||||
|
||||
```text
|
||||
原始静止站点 dlog（两批）
|
||||
→ 逐站导出 LiDAR 原始点云 + RTK 旁路表（NPZ）
|
||||
→ 逐站质量筛选、选取静止帧、重建 RTK 车体位姿（prepared）
|
||||
→ Open3D / small_gicp 分别求 B
|
||||
→ X 无关的 B 精筛与跨后端共识
|
||||
→ AX=XB + 地面平面约束求 X
|
||||
→ 第一批辅助复核、3D 点云核查、确认部署 JSON
|
||||
```
|
||||
|
||||
### 1. 整理原始数据与标定元数据
|
||||
|
||||
每一个静止站点目录必须完整保留 LiDAR DObject 记录、对应 `.dorec` 文件和 RTK 日志。dlog 导出器要求站点目录至少具有：
|
||||
|
||||
```text
|
||||
<station_dlog>/
|
||||
dobject/ # DObject 索引日志
|
||||
dobject_recording/ # 与索引对应的 .dorec 二进制记录
|
||||
GPS-POST-Z/ # RTK 文本记录；建议同时保留完整原始 N4 串口流
|
||||
```
|
||||
|
||||
两批数据要分开存放：第二批高频 RTK 用于主要求解，第一批仅用于辅助复核。与数据一同归档的标定元数据必须包括：LiDAR DObject 名称、时区、RTK 天线参考点、双天线基线方向、heading offset、天线到后轮轴中心的杆臂、车体系定义，以及 LiDAR/RTK/IMU 的时间基准。
|
||||
|
||||
对于后续新录数据，RTK 原始记录应保留 `gnss_week`、`gnss_tow_ms`、GGA UTC、`pitch_deg`、基线长度、解算状态/类型、标准差、HDOP 和完整原文。PC 接收时间只能作为延迟诊断，不能替代 GNSS 测量时间。
|
||||
|
||||
### 2. 将每个原始站点 dlog 导出为 NPZ
|
||||
|
||||
本仓库的 [tools/frontlidar_dlog_export.py](tools/frontlidar_dlog_export.py) 从每个静止站点的原始 dlog 导出 LiDAR 原始点云与 RTK sidecar。对每个站点分别执行；必须开启 RTK sidecar 与审计报告：
|
||||
|
||||
```powershell
|
||||
$Repo = "D:\Outdoor Ackerman Cart Sensor Adaptation\LiDAR_RTK_Calibration_Rigorous_20260721"
|
||||
$Exporter = "$Repo\tools\frontlidar_dlog_export.py"
|
||||
$RawStation = "<某一个原始静止站点 dlog 目录>"
|
||||
$StationOut = "<工作目录>\export\001"
|
||||
|
||||
python $Exporter `
|
||||
--dlog "$RawStation" `
|
||||
--out "$StationOut" `
|
||||
--object frontlidar `
|
||||
--format npz `
|
||||
--timezone +08:00 `
|
||||
--rtk-sidecars `
|
||||
--write-reports
|
||||
```
|
||||
|
||||
导出后的站点目录应包含：
|
||||
|
||||
```text
|
||||
<StationOut>/
|
||||
frames/ # 多帧 LiDAR NPZ；每帧保留 points_raw
|
||||
rtk/ # gps_post_z.csv/jsonl/npz 等 RTK 旁路表
|
||||
reports/manifest.csv # 每帧导出状态、时间及 RTK 匹配信息
|
||||
reports/validation_report.json
|
||||
reports/metadata.json
|
||||
```
|
||||
|
||||
先检查每个站点的 `validation_report.json`、`manifest.csv` 和 `rtk/gps_post_z.csv`：LiDAR payload 必须可读；候选帧必须存在；RTK 位置/航向有效；并记录 LiDAR—RTK 时间差。若新插件已开始输出 GNSS 周/周内时间与 pitch，导出器也必须同步扩展并写出这些字段；旧导出器只保留旧插件字段时，不能伪称已完成严格时间同步。
|
||||
|
||||
### 3. 从导出 NPZ 构建 prepared 静止站点数据
|
||||
|
||||
对每个批次，将所有站点导出目录汇总后执行以下预处理逻辑，生成供本仓库使用的 `prepared` 目录：
|
||||
|
||||
1. 每个站点只选择一帧**车辆已静止**且 RTK 质量合格的 LiDAR 帧；通常取有效候选帧中的中间帧，避免启动/停车边缘。
|
||||
2. 仅保留 `position_valid=true`、`heading_valid=true`、Fix 为 4 或 5，且航向标准差满足项目门限的 RTK 样本；记录每站的样本数、时间跨度、航向圆标准差与被剔除原因。
|
||||
3. 使用 GNSS 测量时间把位置、heading 和 pitch 配成同一时刻；将经纬高转换到同一 ENU 世界系；根据天线杆臂和双天线方向求后轮轴中心车体位姿 `T_WB`。双天线可提供 heading/pitch，但不能提供 roll；roll 在后续 LiDAR—IMU 流程中由 IMU 补充。
|
||||
4. 将选出的 LiDAR 帧原样复制到 `frames_all/`。标定程序从每帧的 `points_raw` 转为 LiDAR 传感器系 XYZ，不得使用已经变换到车体系的点。
|
||||
5. 输出时间戳车体位姿 CSV。当前主求解器要求列为：
|
||||
|
||||
```text
|
||||
time,x,y,z,qx,qy,qz,qw
|
||||
```
|
||||
|
||||
最终目录必须类似：
|
||||
|
||||
```text
|
||||
<BatchPrepared>/
|
||||
frames_all/
|
||||
station_01.npz
|
||||
station_02.npz
|
||||
...
|
||||
body_poses_rear_gga_raw_rear_to_front.csv
|
||||
station_summary.csv # 推荐保留：站点质量和选择原因
|
||||
manifest.json # 推荐保留：参考点、杆臂、heading/pitch 定义与筛选配置
|
||||
```
|
||||
|
||||
本仓库的 [tools/prepare_station_dataset.py](tools/prepare_station_dataset.py) 完成上述固定站点筛选、点云复制和车体位姿重建。它要求每个站点均已由上一步使用 --rtk-sidecars --write-reports 导出。下面的参数仅用于复现本仓库历史结果：lat/lon/h 被假定为后天线相位中心，raw_heading_deg 被假定为后天线指向前天线；车体系为后轮轴中心、X 前 Y 左 Z 上。
|
||||
|
||||
~~~powershell
|
||||
$Repo = "D:\Outdoor Ackerman Cart Sensor Adaptation\LiDAR_RTK_Calibration_Rigorous_20260721"
|
||||
$Prepare = "$Repo\tools\prepare_station_dataset.py"
|
||||
|
||||
# 第二批：主要求解数据。ExportBatch2 下为 38 个逐站导出目录。
|
||||
python $Prepare --export-root "<ExportBatch2>" --output "<PreparedBatch2>" --expected-stations 38 --heading-std-limit-deg 0.5 --heading-offset-deg 21.226 --antenna-lever -0.320 -0.365 0.620 --pose-name rear_gga_raw_rear_to_front
|
||||
|
||||
# 第一批：辅助复核数据。若航向质量较低，不应设置过严的 heading 标准差阈值。
|
||||
python $Prepare --export-root "<ExportBatch1>" --output "<PreparedBatch1>" --expected-stations 38 --heading-offset-deg 21.226 --antenna-lever -0.320 -0.365 0.620 --pose-name rear_gga_raw_rear_to_front
|
||||
~~~
|
||||
|
||||
命令完成后检查 <PreparedBatch*>/station_summary.csv：每行均应有有效 RTK 样本，且没有超出设定的航向离散度。manifest.json 记录实际采用的杆臂、航向偏移、ENU 原点和输入站点；它应与原始数据一同归档。
|
||||
|
||||
不要把上述 21.226° 和 [-0.320,-0.365,0.620] m 当成通用常数：它们是本车、后天线、后天线到前天线航向定义、以及后轮轴中心车体原点的历史配置。换车、换参考天线、改变车体原点或改变 rawHeading 定义后，必须先复核并替换这些参数，再重建 A。
|
||||
|
||||
当前导出器可复现本仓库历史日志，但历史 RTK 插件只记录工控机接收时间，并未保存 GNSS 周/周内时间、双天线 pitch、质量标准差及完整原始串口流。因此它不能把旧数据宣称为严格 GNSS 时间同步。插件升级后，应先扩展导出器以保存新字段，再以 GNSS 测量时间重建位置/航向（及可用的 pitch）轨迹。
|
||||
### 4. 安装标定依赖并运行第一阶段
|
||||
|
||||
```powershell
|
||||
$Repo = "D:\Outdoor Ackerman Cart Sensor Adaptation\LiDAR_RTK_Calibration_Rigorous_20260721"
|
||||
python -m pip install -r "$Repo\requirements.txt"
|
||||
python -c "import numpy, scipy, open3d, small_gicp; print('dependencies OK')"
|
||||
|
||||
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_all.ps1" `
|
||||
-Batch1Prepared "<第一批 prepared 目录>" `
|
||||
-Batch2Prepared "<第二批 prepared 目录>"
|
||||
```
|
||||
|
||||
`run_all.ps1` 完成：第二批地面拟合；Open3D 与 small_gicp 两套 B 生成和精筛；两套单后端 X 求解；第一批独立 B 与辅助复核；以及单后端对比摘要。第一阶段至少应生成：
|
||||
|
||||
```text
|
||||
results/common/ground_planes_batch2.csv
|
||||
results/open3d_gicp/B_batch2_refined.npz
|
||||
results/small_gicp/B_batch2_refined.npz
|
||||
results/open3d_gicp/extrinsic_batch2_refined.json
|
||||
results/small_gicp/extrinsic_batch2_refined.json
|
||||
results/comparison_summary.json
|
||||
```
|
||||
|
||||
### 5. 构造跨后端共识并生成算法结果
|
||||
|
||||
第一阶段成功后执行：
|
||||
|
||||
```powershell
|
||||
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_consensus_finish.ps1"
|
||||
```
|
||||
|
||||
该步骤以 `0.05 m / 0.50°` 门限对同一运动对的 Open3D 与 small_gicp B 做一致性门控，用共识 B 重求第二批外参，并将第一批只用于辅助复核。主要输出为：
|
||||
|
||||
```text
|
||||
results/consensus/B_batch2_consensus.npz
|
||||
results/consensus/extrinsic_batch2_consensus.json
|
||||
results/consensus/batch1_auxiliary_check.json
|
||||
results/final_extrinsic_recommended.json
|
||||
results/final_summary.json
|
||||
```
|
||||
|
||||
### 6. 验证、可视化与部署确认
|
||||
|
||||
先检查 `results/final_summary.json`：当前历史数据的参考值是第二批共识 39 对、AX RMS 约 `0.07985 m / 0.96118°`。若明显偏离，应从第 2、3 步检查站点选择、RTK 时间/参考点/杆臂/heading 定义，而不是直接手调 X。
|
||||
|
||||
再使用 3D 可视化检查：模式 3 为点云配准 B，模式 4 为外参预测 `X^-1AX`；终端打印 `B^-1(X^-1AX)` 的平移与旋转增量。只有 B 质量、全体运动对统计、地面约束和可视化均合理时，才可确认外参。
|
||||
|
||||
`final_extrinsic_recommended.json` 是完整算法审计结果。`run_consensus_finish.ps1` 不会自动覆盖 `results/final_extrinsic_deployment.json`；部署 JSON 必须在人工确认坐标定义和高度约束后明确写入。
|
||||
## prepared 阶段的快速重跑
|
||||
|
||||
```powershell
|
||||
$Repo = "D:\Outdoor Ackerman Cart Sensor Adaptation\LiDAR_RTK_Calibration_Rigorous_20260721"
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,194 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Prepare one static LiDAR frame and one yaw-only RTK body pose per station."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import csv
|
||||
import json
|
||||
import math
|
||||
import re
|
||||
import shutil
|
||||
import sys
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
POSE_FIELDS = ["time", "x", "y", "z", "qx", "qy", "qz", "qw"]
|
||||
|
||||
|
||||
def parse_args() -> argparse.Namespace:
|
||||
parser = argparse.ArgumentParser(
|
||||
description="Select one static LiDAR frame per exported station and rebuild yaw-only RTK body poses."
|
||||
)
|
||||
parser.add_argument("--export-root", type=Path, required=True, help="Directory containing exported station directories.")
|
||||
parser.add_argument("--output", type=Path, required=True, help="Output prepared directory.")
|
||||
parser.add_argument("--pose-name", default="rear_gga_raw_rear_to_front", help="Suffix of body_poses_<name>.csv.")
|
||||
parser.add_argument("--heading-offset-deg", type=float, required=True, help="Added to raw_heading_deg before ENU yaw conversion.")
|
||||
parser.add_argument("--antenna-lever", type=float, nargs=3, metavar=("X", "Y", "Z"), required=True, help="Antenna position in body coordinates [m].")
|
||||
parser.add_argument("--expected-stations", type=int, default=0, help="Require exactly this many usable stations; 0 disables.")
|
||||
parser.add_argument("--min-stations", type=int, default=30, help="Fail below this many usable stations.")
|
||||
parser.add_argument("--groups", nargs="*", default=None, help="Optional explicit station directory names.")
|
||||
parser.add_argument("--accepted-fixes", type=int, nargs="+", default=[4, 5], help="Accepted GGA fix values.")
|
||||
parser.add_argument("--heading-std-limit-deg", type=float, default=float("inf"), help="Reject a station above this heading circular stddev.")
|
||||
parser.add_argument("--overwrite", action="store_true", help="Allow replacing generated files in a non-empty output directory.")
|
||||
return parser.parse_args()
|
||||
|
||||
|
||||
def truth(value: Any) -> bool:
|
||||
return str(value).strip().lower() in {"1", "true", "yes", "y"}
|
||||
|
||||
|
||||
def natural_key(value: str) -> list[Any]:
|
||||
return [int(part) if part.isdigit() else part.lower() for part in re.split(r"(\d+)", value)]
|
||||
|
||||
def read_csv(path: Path) -> list[dict[str, str]]:
|
||||
with path.open("r", encoding="utf-8-sig", newline="") as stream:
|
||||
return list(csv.DictReader(stream))
|
||||
|
||||
|
||||
def require_columns(rows: list[dict[str, str]], path: Path, columns: list[str]) -> None:
|
||||
if not rows:
|
||||
raise ValueError(f"{path}: no rows")
|
||||
missing = [column for column in columns if column not in rows[0]]
|
||||
if missing:
|
||||
raise ValueError(f"{path}: missing required columns: {', '.join(missing)}")
|
||||
|
||||
|
||||
def circular_mean_deg(values: np.ndarray) -> float:
|
||||
radians = np.deg2rad(values)
|
||||
return float(np.rad2deg(math.atan2(float(np.mean(np.sin(radians))), float(np.mean(np.cos(radians))))) % 360.0)
|
||||
|
||||
|
||||
def circular_std_deg(values: np.ndarray) -> float:
|
||||
radians = np.deg2rad(values)
|
||||
resultant = max(math.hypot(float(np.mean(np.cos(radians))), float(np.mean(np.sin(radians)))), 1e-12)
|
||||
return float(np.rad2deg(math.sqrt(-2.0 * math.log(resultant))))
|
||||
|
||||
|
||||
def geodetic_to_ecef(lat_deg: float, lon_deg: float, height_m: float) -> np.ndarray:
|
||||
a, e2 = 6378137.0, 6.69437999014e-3
|
||||
lat, lon = math.radians(lat_deg), math.radians(lon_deg)
|
||||
sin_lat, cos_lat, sin_lon, cos_lon = math.sin(lat), math.cos(lat), math.sin(lon), math.cos(lon)
|
||||
n = a / math.sqrt(1.0 - e2 * sin_lat * sin_lat)
|
||||
return np.array([(n + height_m) * cos_lat * cos_lon, (n + height_m) * cos_lat * sin_lon, (n * (1.0 - e2) + height_m) * sin_lat], dtype=float)
|
||||
|
||||
|
||||
def ecef_to_enu(ecef: np.ndarray, origin_ecef: np.ndarray, origin_lat_deg: float, origin_lon_deg: float) -> np.ndarray:
|
||||
lat, lon = math.radians(origin_lat_deg), math.radians(origin_lon_deg)
|
||||
sin_lat, cos_lat, sin_lon, cos_lon = math.sin(lat), math.cos(lat), math.sin(lon), math.cos(lon)
|
||||
rotation = np.array([[-sin_lon, cos_lon, 0.0], [-sin_lat * cos_lon, -sin_lat * sin_lon, cos_lat], [cos_lat * cos_lon, cos_lat * sin_lon, sin_lat]], dtype=float)
|
||||
return rotation @ (ecef - origin_ecef)
|
||||
|
||||
|
||||
def yaw_rotation(yaw: float) -> np.ndarray:
|
||||
c, s = math.cos(yaw), math.sin(yaw)
|
||||
return np.array([[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]], dtype=float)
|
||||
|
||||
|
||||
def select_frame(station: Path) -> tuple[dict[str, str], int, int]:
|
||||
path = station / "reports" / "manifest.csv"
|
||||
rows = read_csv(path)
|
||||
require_columns(rows, path, ["status", "unix_time_ns", "output_file"])
|
||||
exported = [row for row in rows if row["status"] in {"exported", "resumed"}]
|
||||
if not exported:
|
||||
raise ValueError(f"{station.name}: no exported/resumed LiDAR frames")
|
||||
valid = [row for row in exported if truth(row.get("rtk_position_valid")) and truth(row.get("rtk_heading_valid"))]
|
||||
candidates = valid or exported
|
||||
candidates.sort(key=lambda row: int(row["unix_time_ns"]))
|
||||
return candidates[len(candidates) // 2], len(exported), len(valid)
|
||||
|
||||
|
||||
def validation_checks(station: Path) -> dict[str, bool]:
|
||||
path = station / "reports" / "validation_report.json"
|
||||
if not path.exists():
|
||||
return {}
|
||||
document = json.loads(path.read_text(encoding="utf-8"))
|
||||
return {str(item.get("name")): bool(item.get("ok")) for item in document.get("checks", [])}
|
||||
|
||||
|
||||
def process_station(station: Path, args: argparse.Namespace) -> tuple[dict[str, Any], dict[str, Any]]:
|
||||
rtk_path = station / "rtk" / "gps_post_z.csv"
|
||||
rows = read_csv(rtk_path)
|
||||
columns = ["lat", "lon", "alt_m", "raw_heading_deg", "fix", "position_valid", "heading_valid", "unix_time_ns"]
|
||||
require_columns(rows, rtk_path, columns)
|
||||
fixes = set(args.accepted_fixes)
|
||||
good = [row for row in rows if truth(row["position_valid"]) and truth(row["heading_valid"]) and int(row["fix"]) in fixes]
|
||||
if not good:
|
||||
raise ValueError(f"{station.name}: no RTK samples pass position/heading/fix filters")
|
||||
lat = np.asarray([float(row["lat"]) for row in good])
|
||||
lon = np.asarray([float(row["lon"]) for row in good])
|
||||
alt = np.asarray([float(row["alt_m"]) for row in good])
|
||||
heading = np.asarray([float(row["raw_heading_deg"]) for row in good])
|
||||
times = np.asarray([int(row["unix_time_ns"]) for row in good], dtype=np.int64)
|
||||
heading_std = circular_std_deg(heading)
|
||||
if heading_std > args.heading_std_limit_deg:
|
||||
raise ValueError(f"{station.name}: heading circular stddev {heading_std:.3f} deg exceeds {args.heading_std_limit_deg:.3f} deg")
|
||||
frame, frame_count, valid_frame_count = select_frame(station)
|
||||
source = station / frame["output_file"]
|
||||
if not source.is_file():
|
||||
raise FileNotFoundError(f"{station.name}: selected frame is absent: {source}")
|
||||
checks = validation_checks(station)
|
||||
selected = {"station": station.name, "source_frame": source, "frame_time": int(frame["unix_time_ns"]) / 1e9, "lat_deg": float(np.mean(lat)), "lon_deg": float(np.mean(lon)), "alt_m": float(np.mean(alt)), "raw_heading_deg": circular_mean_deg(heading), "raw_heading_std_deg": heading_std}
|
||||
summary = {"station": station.name, "selected_frame": frame["output_file"], "exported_frames": frame_count, "frames_with_valid_rtk": valid_frame_count, "valid_rtk_samples": len(good), "raw_heading_mean_deg": selected["raw_heading_deg"], "raw_heading_std_deg": heading_std, "alt_std_m": float(np.std(alt)), "rtk_span_sec": float((times.max() - times.min()) / 1e9) if len(times) > 1 else 0.0, "payload_ok": checks.get("lidar_payload_length", ""), "rtk_parse_ok": checks.get("gps_post_z_parse", ""), "time_check_3s_ok": checks.get("rtk_time_alignment", "")}
|
||||
return selected, summary
|
||||
|
||||
|
||||
def main() -> int:
|
||||
args = parse_args()
|
||||
if args.min_stations < 2 or args.expected_stations < 0:
|
||||
raise ValueError("--min-stations must be >=2 and --expected-stations must be >=0")
|
||||
if not args.export_root.is_dir():
|
||||
raise NotADirectoryError(args.export_root)
|
||||
names = args.groups or [path.name for path in args.export_root.iterdir() if path.is_dir()]
|
||||
stations = [args.export_root / name for name in sorted(names, key=natural_key)]
|
||||
missing = [str(path) for path in stations if not path.is_dir()]
|
||||
if missing:
|
||||
raise FileNotFoundError("station directories do not exist: " + ", ".join(missing))
|
||||
selected, summaries, rejected = [], [], []
|
||||
for station in stations:
|
||||
try:
|
||||
item, summary = process_station(station, args)
|
||||
selected.append(item); summaries.append(summary)
|
||||
except (ValueError, FileNotFoundError) as error:
|
||||
rejected.append({"station": station.name, "reason": str(error)})
|
||||
if args.expected_stations and len(selected) != args.expected_stations:
|
||||
raise RuntimeError(f"expected {args.expected_stations} usable stations, got {len(selected)}; rejected={rejected}")
|
||||
if len(selected) < args.min_stations:
|
||||
raise RuntimeError(f"need at least {args.min_stations} usable stations, got {len(selected)}; rejected={rejected}")
|
||||
output, frames_dir = args.output, args.output / "frames_all"
|
||||
if output.exists() and any(output.iterdir()) and not args.overwrite:
|
||||
raise FileExistsError(f"{output} is non-empty; pass --overwrite to replace generated files")
|
||||
frames_dir.mkdir(parents=True, exist_ok=True)
|
||||
origin = selected[0]
|
||||
origin_ecef = geodetic_to_ecef(origin["lat_deg"], origin["lon_deg"], origin["alt_m"])
|
||||
lever = np.asarray(args.antenna_lever, dtype=float)
|
||||
pose_rows = []
|
||||
for index, item in enumerate(selected, 1):
|
||||
destination = frames_dir / f"station_{index:02d}.npz"
|
||||
shutil.copy2(item["source_frame"], destination)
|
||||
antenna_enu = ecef_to_enu(geodetic_to_ecef(item["lat_deg"], item["lon_deg"], item["alt_m"]), origin_ecef, origin["lat_deg"], origin["lon_deg"])
|
||||
corrected_heading = (item["raw_heading_deg"] + args.heading_offset_deg) % 360.0
|
||||
yaw = math.radians(90.0 - corrected_heading)
|
||||
body_position = antenna_enu - yaw_rotation(yaw) @ lever
|
||||
pose_rows.append(dict(zip(POSE_FIELDS, [item["frame_time"], *body_position, 0.0, 0.0, math.sin(yaw / 2.0), math.cos(yaw / 2.0)])))
|
||||
summaries[index - 1].update({"sequence": index, "prepared_frame": destination.name, "corrected_heading_deg": corrected_heading})
|
||||
pose_path = output / f"body_poses_{args.pose_name}.csv"
|
||||
with pose_path.open("w", encoding="utf-8", newline="") as stream:
|
||||
writer = csv.DictWriter(stream, fieldnames=POSE_FIELDS); writer.writeheader(); writer.writerows(pose_rows)
|
||||
fields = sorted({key for row in summaries for key in row})
|
||||
with (output / "station_summary.csv").open("w", encoding="utf-8", newline="") as stream:
|
||||
writer = csv.DictWriter(stream, fieldnames=fields); writer.writeheader(); writer.writerows(summaries)
|
||||
manifest = {"source_export_root": str(args.export_root.resolve()), "station_count": len(selected), "pose_csv": pose_path.name, "frame_directory": "frames_all", "selection_policy": "middle frame among exported frames with valid RTK; falls back to middle exported frame", "rtk_filter": {"position_valid": True, "heading_valid": True, "accepted_fixes": args.accepted_fixes, "heading_std_limit_deg": args.heading_std_limit_deg}, "body_pose_configuration": {"raw_heading_offset_deg": args.heading_offset_deg, "antenna_lever_body_m": args.antenna_lever, "body_axes": "x forward, y left, z up; origin must match the supplied lever", "orientation_model": "yaw-only from raw_heading_deg; this tool does not reconstruct RTK pitch or roll"}, "enu_origin": {"lat_deg": origin["lat_deg"], "lon_deg": origin["lon_deg"], "alt_m": origin["alt_m"]}, "stations": [{"sequence": index + 1, "source_station": item["station"], "source_frame": str(item["source_frame"]), "prepared_frame": f"station_{index + 1:02d}.npz"} for index, item in enumerate(selected)]}
|
||||
(output / "manifest.json").write_text(json.dumps(manifest, ensure_ascii=False, indent=2), encoding="utf-8")
|
||||
print(json.dumps({"prepared": str(output.resolve()), "stations": len(selected), "rejected": rejected, "pose_csv": pose_path.name}, ensure_ascii=False, indent=2))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
try:
|
||||
raise SystemExit(main())
|
||||
except Exception as error:
|
||||
print(f"ERROR: {error}", file=sys.stderr)
|
||||
raise
|
||||
Reference in New Issue
Block a user