lichun.qu/calibration
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

改为车头向前整链：主从装反机械初值、双天线 pitch/roll 姿态与默认 HeadingOffsetDeg=-90

Browse Source 
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
lichun.qu
2026-08-11 18:07:54 +08:00
co-authored by Cursor
parent 6242fd1081
commit 5f59bcd795
13 changed files with 698 additions and 199 deletions
Download Patch File Download Diff File Expand all files Collapse all files
README.md
+35 -38
View File
@@ -1,16 +1,17 @@
# 双天线RTK—3D LiDAR直接手眼标定
本仓库从静态站点原始数据复现 `T_RTK_lidar`：把原始雷达点变换到 **RTK 基线导航系**。
它**不是** `base_link` 车体外参；求解阶段不使用车体航向偏置，也不使用 RTK 到后轮轴的 XY 杆臂。
本仓库从静态站点原始数据复现 `T_RTK_lidar`：把原始雷达点变换到 **车头向前的 RTK 车体系**（主天线原点）。
求解不使用 RTK 到后轮轴的 XY 杆臂；与雷达–IMU 外参对照时旋转系一致，平移仍差天线原点。
当前交付标定（2026-08 室外车，27 站）约定如下：
| 项 | 值 |
|---|---|
| RTK 坐标系 | **基线系**（`HeadingOffsetDeg = 0`） |
| RTK 坐标系 | **车头向前**（`HeadingOffsetDeg = -90`；主从装反、基线朝右） |
| 天线相位中心离地高 | **1.9165 m**（1916.5 mm） |
| 机械平移初值 | `(0.414179474, 0.210859360, 0.004000001) m` |
| 机械旋转初值 | yaw ≈ **90°**（雷达 X 朝车头、双天线基线左右装） |
| 机械初值（车头系） | \(t=(+0.21086,-0.41418,+0.07850)\) m，yaw=**0°**（CAD 纵向已按车头正向取 +X） |
| 物理基线 | `baseline_points=vehicle_right`（主天线车左，从天线车右，后轴中心左右对称） |
| 姿态 | 双天线 pitch/roll：`R = Rz(yaw_raw) Ry(-pitch) Rx(roll) Rz(+90°)` |
| 地面点 ROI | LiDAR 系 **`z ∈ [-2.5, -1.5]`**（约 2 m 车顶安装） |
| pair 配准 | **禁止**使用外参 seed；B 与 X 独立 |
@@ -19,7 +20,7 @@
---
## 1. 输出坐标约定（基线系）
## 1. 输出坐标约定（车头向前）
统一约定 `T_A_B` 把 B 系点变换到 A 系：
@@ -27,22 +28,16 @@
p_RTK = T_RTK_lidar · p_lidar
```
本仓库默认 RTK 导航系（**基线系 / baseline_raw_heading**）：
本仓库默认 RTK 导航系（**车头向前 / vehicle_forward_heading_offset**）：
- 原点：GGA 位置参考点（通常为 ANT1 相位中心，须结合接收机配置确认）；
- X 轴：`rawHeading` 双天线基线在水平面的投影；
- 原点：GGA 位置参考点（主天线 / ANT1 相位中心）；
- X 轴：车头向前（`rawHeading + HeadingOffsetDeg`，本车 `HeadingOffsetDeg = -90`）；
- Y 轴：左；
- Z 轴：上；
- ENU 航向：`yaw = 90° - rawHeading`（`heading_offset = 0`）；
- roll、pitch：轨迹中固定为 0。
- 姿态：先在基线系应用双天线 pitch/roll，再乘固定 `Rz(-heading_offset)`；不是 IMU 融合姿态。
> 不要把基线系结果当成“车头向前系”。若下游需要车头向前，应另乘确认过的固定航向偏置，或显式使用 `-HeadingOffsetDeg 90` **整链重跑**，不要事后只改 JSON 里的 yaw。
若下游需要 `T_body_lidar`，须另有已确认的 `T_body_rtk`：
```text
T_body_lidar = T_body_rtk · T_RTK_lidar
```
> 改 `HeadingOffsetDeg` 或姿态模型后必须从 **prepare** 起重跑；禁止事后只改 JSON 里的 yaw。
> 旧基线系结果（`HeadingOffsetDeg = 0`）与车头系外参不可混用。
机械初值文件：[`run/rtk_lidar_mechanical_initial.json`](run/rtk_lidar_mechanical_initial.json)
**仅用于 AX=XB 求解初值，禁止用于 LiDAR pair 配准。**
@@ -54,7 +49,7 @@ T_body_lidar = T_body_rtk · T_RTK_lidar
```text
原始雷达 + RTK（+ 可选 IMU）
→ combined/（按站关联的多传感器 NPZ）
→ 每站选一帧静态点云 + yaw-only RTK pose（基线系）
→ 每站选一帧静态点云 + RTK pose（车头向前，含双天线 pitch/roll）
→ Open3D GICP 与 small_gicp 分别求 B_ij = T_Li_Lj（无外参 seed）
→ 留出点、正反向、旋转共轭不变量等精筛
→ 双后端共识边 → consensus B
@@ -137,10 +132,10 @@ $Data = "D:\data\rtk_lidar_run" # 含 combined/
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_direct_rtk_lidar.ps1" `
-CombinedRoot "$Data\combined" `
-WorkRoot "$Data\prepared_baseline_h19165" `
-OutputRoot "$Data\outputs_baseline_h19165" `
-WorkRoot "$Data\prepared_vehicle_h19165" `
-OutputRoot "$Data\outputs_vehicle_h19165" `
-RtkReferenceHeightAboveGroundM 1.9165 `
-HeadingOffsetDeg 0 `
-HeadingOffsetDeg -90 `
-ExpectedStations 27 `
-MinStations 20 `
-GroundZMin -2.5 `
@@ -153,7 +148,7 @@ powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_direct_rt
| 参数 | 本次取值 | 说明 |
|---|---|---|
| `-RtkReferenceHeightAboveGroundM` | **1.9165** | GGA/ANT1 相位中心离地高（m），必填 |
| `-HeadingOffsetDeg` | **0** | 基线系；非 0 时才变成车头向前系 |
| `-HeadingOffsetDeg` | **-90** | 车头向前（主从装反、基线朝右）；`0` 才是基线系 |
| `-GroundZMin/Max` | **-2.5 / -1.5** | 约 2 m 车顶雷达；旧默认 `[-1.4,-0.4]` 会拟合到墙 |
| `-ExpectedStations` | **27** | 本批站数 |
| `-MinStations` | **20** | 远程旧脚本曾写死 30，会跑不了本批 |
@@ -230,32 +225,34 @@ powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\view_result.p
---
## 7. 当前标定结果（基线系，h = 1.9165 m）
## 7. 当前标定结果（车头向前，h = 1.9165 m）
结果目录：`D:\data\rtk_lidar_run\outputs_baseline_h19165\`
交付文件：`final_T_RTK_lidar.json` / `summary.json`
> **状态：可作车头系候选交付**（`recommended_for_deployment: true`）。
> 约定：`HeadingOffsetDeg=-90`，双天线 pitch/roll，机械初值 \(t=(+0.21086,-0.41418,+0.07850)\)，yaw=0。
> 旧 `outputs_baseline_h19165` 仍不可用。
结果目录：`D:\data\rtk_lidar_run\outputs_vehicle_h19165\`
文件：`final_T_RTK_lidar.json` / `summary.json`
```text
translation_m = [0.412305582, 0.217309210, 0.104057606]
RPY_deg_xyz = [0.465513, 0.743343, 89.460018]
translation_m = [0.217822250, -0.411347802, 0.106542337]
RPY_deg_xyz = [0.066239, 0.809662, -0.551322]
T_RTK_lidar ≈
0.009424 -0.999922 0.008247 0.412306
0.999871 0.009529 0.012896 0.217309
-0.012973 0.008124 0.999883 0.104058
0.999854 0.009639 0.014119 0.217822
-0.009621 0.999953 -0.001292 -0.411348
-0.014131 0.001156 0.999899 0.106542
0 0 0 1
```
| 指标 | 值 |
|---|---:|
| 有效站点 / 共识对 | 27 / 20 |
| 平移残差 RMS / 中位 / P95 / max | 0.070 / 0.042 / 0.121 / **0.186** m |
| 旋转残差 RMS / 中位 / max | 0.978 / 0.585 / **2.73** ° |
| 双后端差 | 3.2 mm / 0.17° |
| Jacobian 条件数 | 6.88 |
| bootstrap σ(x,y,z) | 4.7 / 6.3 / 0.8 mm |
| 相对机械初值 | 旋转差 ≈ 1.03°（无近 180° 冲突） |
| `frame_mode` | `baseline_raw_heading` |
| 平移残差 RMS | ≈ 0.071 m |
| 旋转残差 RMS | ≈ 0.982 ° |
| 双后端差 | ≈ 3.1 mm / 0.12° |
| `frame_mode` | `vehicle_forward_heading_offset` |
| 相对机械初值 | XY 近机械杆臂；yaw≈0；无近 180° 冲突 |
与机械平移初值 XY 相差约数毫米；z 由天线高度约束，CAD 的 4 mm 不能代替实测 1.9165 m。
code/finalize_direct_rtk_lidar.py
+176 -15
View File
@@ -15,7 +15,17 @@ def load(path: Path) -> dict:
def write(path: Path, document: dict) -> None:
path.parent.mkdir(parents=True, exist_ok=True)
path.write_text(json.dumps(document, ensure_ascii=False, indent=2), encoding="utf-8")
def default(obj):
if isinstance(obj, (np.bool_, np.integer)):
return obj.item()
if isinstance(obj, np.floating):
return float(obj)
if isinstance(obj, np.ndarray):
return obj.tolist()
raise TypeError(f"Object of type {type(obj).__name__} is not JSON serializable")
path.write_text(json.dumps(document, ensure_ascii=False, indent=2, default=default), encoding="utf-8")
def inverse(t: np.ndarray) -> np.ndarray:
@@ -34,12 +44,127 @@ def delta(a: np.ndarray, b: np.ndarray) -> dict:
}
def wrap180(deg: float) -> float:
return (deg + 180.0) % 360.0 - 180.0
def yaw_deg_of(transform: np.ndarray) -> float:
return float(Rotation.from_matrix(transform[:3, :3]).as_euler("xyz", degrees=True)[2])
def mechanical_self_consistency(document: dict) -> dict:
"""Reject mechanical JSON that mixes incompatible baseline / body definitions."""
translation = np.asarray(document["translation_m"], float)
yaw = float(document["rotation_rpy_deg_xyz"][2])
side = str(document.get("baseline_points", "")).strip().lower()
frame_mode = str(document.get("frame_mode", "")).strip().lower()
heading_offset = float(document.get("heading_offset_deg", 0.0) or 0.0)
vehicle_forward = (
frame_mode == "vehicle_forward_heading_offset"
or abs(heading_offset) > 1e-6
)
issues: list[str] = []
if vehicle_forward:
if abs(wrap180(yaw)) > 15.0:
issues.append(
f"vehicle-forward mechanical initial requires yaw≈0°, got {yaw:g}°"
)
lever = document.get("vehicle_flu_lever_master_to_lidar_m")
if lever is not None:
if float(np.linalg.norm(translation - np.asarray(lever, float))) > 0.05:
issues.append(
"vehicle-forward translation_m must match vehicle_flu_lever_master_to_lidar_m"
)
if abs(heading_offset + 90.0) > 1e-6 and abs(heading_offset - 90.0) > 1e-6:
issues.append(
f"vehicle-forward heading_offset_deg should be ±90 for left/right baseline, got {heading_offset:g}"
)
elif side in {"vehicle_left", "left"}:
if abs(wrap180(yaw - (-90.0))) > 15.0:
issues.append(
f"baseline_points=vehicle_left requires yaw≈-90°, got {yaw:g}°"
)
if translation[0] <= 0.0 or translation[1] <= 0.0:
issues.append(
"baseline_points=vehicle_left expects +X/+Y lever in RTK baseline frame "
f"(got t_xy=({translation[0]:g}, {translation[1]:g}))"
)
elif side in {"vehicle_right", "right"}:
if abs(wrap180(yaw - 90.0)) > 15.0:
issues.append(
f"baseline_points=vehicle_right requires yaw≈+90°, got {yaw:g}°"
)
# Swapped but centerline-symmetric master (vehicle left): +X / -Y in baseline frame.
if translation[0] <= 0.0 or translation[1] >= 0.0:
issues.append(
"baseline_points=vehicle_right (master on vehicle left, baseline to the right) "
"expects +X/-Y lever in RTK baseline frame "
f"(got t_xy=({translation[0]:g}, {translation[1]:g}))"
)
else:
left_xy = translation[0] > 0.05 and translation[1] > 0.05
right_xy = translation[0] < -0.05 and translation[1] < -0.05
swapped_right_xy = translation[0] > 0.05 and translation[1] < -0.05
if left_xy and abs(wrap180(yaw - 90.0)) <= 15.0:
issues.append(
"mixed baseline definition: +X/+Y translation (left-baseline) combined with yaw≈+90° (right-baseline)"
)
if right_xy and abs(wrap180(yaw - (-90.0))) <= 15.0:
issues.append(
"mixed baseline definition: -X/-Y translation combined with yaw≈-90°"
)
if swapped_right_xy and abs(wrap180(yaw - (-90.0))) <= 15.0:
issues.append(
"mixed baseline definition: +X/-Y translation (swapped-master right-baseline) "
"combined with yaw≈-90° (left-baseline)"
)
return {
"baseline_points": side or None,
"frame_mode": frame_mode or None,
"heading_offset_deg": heading_offset,
"consistent": not issues,
"issues": issues,
}
def solution_matches_declared_side(solution: np.ndarray, document: dict) -> dict:
"""Check whether the solved extrinsic agrees with the mechanical baseline side."""
side = str(document.get("baseline_points", "")).strip().lower()
yaw = yaw_deg_of(solution)
t = solution[:3, 3]
expected_yaw = float(document["rotation_rpy_deg_xyz"][2])
yaw_err = abs(wrap180(yaw - expected_yaw))
xy_err = float(np.linalg.norm(t[:2] - np.asarray(document["translation_m"][:2], float)))
z_err = float(abs(t[2] - float(document["translation_m"][2])))
opposite_yaw = abs(wrap180(yaw - expected_yaw) - 180.0) <= 15.0 or abs(
wrap180(yaw - expected_yaw) + 180.0
) <= 15.0
# Same XY sign as mechanical but yaw flipped ~180° (classic mixed inheritance).
same_xy_sign = (t[0] * float(document["translation_m"][0]) > 0.0) and (
t[1] * float(document["translation_m"][1]) > 0.0
)
mixed_inheritance = same_xy_sign and opposite_yaw
return {
"baseline_points": side or None,
"solution_yaw_deg": yaw,
"expected_yaw_deg": expected_yaw,
"yaw_error_deg": yaw_err,
"xy_error_m": xy_err,
"z_error_m": z_err,
"mixed_translation_rotation_inheritance": bool(mixed_inheritance),
"near_expected_pose": bool(yaw_err <= 15.0 and xy_err <= 0.25),
}
def coordinate_contract_audit(raw: dict) -> dict:
"""Compare the data-driven solution with the declared mechanical initial.
"""Audit mechanical self-consistency and solution agreement.
A near-180-degree disagreement is not auto-corrected: it normally means
that one physical forward-axis statement is reversed. Silently rotating
the point cloud would preserve residuals while changing the frame contract.
that one physical forward-axis / baseline-direction statement is reversed.
"""
path_text = raw.get("solver_initial_extrinsic")
if not path_text:
@@ -59,14 +184,29 @@ def coordinate_contract_audit(raw: dict) -> dict:
solution = np.asarray(raw["matrix_4x4"], float)
comparison = delta(initial, solution)
near_180 = abs(comparison["rotation_deg"] - 180.0) <= 15.0
mech_check = mechanical_self_consistency(initial_document)
match = solution_matches_declared_side(solution, initial_document)
if not mech_check["consistent"]:
status = "mechanical_initial_inconsistent"
elif match["mixed_translation_rotation_inheritance"] or near_180:
status = "near_180_degree_axis_conflict"
elif not match["near_expected_pose"]:
status = "solution_disagrees_with_mechanical_baseline_side"
else:
status = "no_near_180_degree_axis_conflict"
requires = status != "no_near_180_degree_axis_conflict"
return {
"status": "near_180_degree_axis_conflict" if near_180 else "no_near_180_degree_axis_conflict",
"requires_physical_axis_confirmation": near_180,
"status": status,
"requires_physical_axis_confirmation": requires,
"mechanical_initial_path": str(path.resolve()),
"mechanical_self_consistency": mech_check,
"solution_vs_declared_baseline_side": match,
"solution_relative_to_mechanical_initial": comparison,
"note": (
"No automatic 180-degree point-cloud flip was applied. Confirm the Helios "
"aviation-connector side and the G90 vehicle-forward definition before deployment."
"No automatic 180-degree correction was applied. Confirm static GNHPR "
"left/right vs vehicle heading and Helios +X vs vehicle forward before deployment."
),
}
@@ -87,12 +227,18 @@ def corrected(raw: dict, backend: str, reference_height: float, heading_offset_d
"RTK": {
"origin": "GGA positioning reference point; confirm ANT1/reference antenna in receiver configuration",
"x_axis": x_axis,
"y_axis": "left",
"y_axis": "left of the RTK X/baseline axis (not necessarily vehicle-left)",
"z_axis": "up",
"yaw_enu_deg": f"90 - (rawHeadingDeg + {heading_offset_deg:g})",
"frame_mode": "baseline_raw_heading" if baseline_frame else "vehicle_forward_heading_offset",
},
"LiDAR": "raw LiDAR sensor frame",
"LiDAR": {
"description": "raw Helios sensor frame from points_raw polar decode",
"x_axis": "+X at azimuth 0° (forward when aviation connector faces vehicle rear)",
"y_axis": "+Y at azimuth +90° (left when +X is vehicle-forward)",
"z_axis": "up",
"origin_note": "optical/center per Helios manual; mounting height includes 63.5 mm base offset when deriving mechanical ΔZ",
},
},
"backend": backend,
"measured_lidar_extrinsic_used_as_initial": bool(raw.get("measured_extrinsic_used_as_initial")),
@@ -154,18 +300,33 @@ def main() -> None:
needs_axis_confirmation = bool(
final["coordinate_contract_audit"]["requires_physical_axis_confirmation"]
)
status = final["coordinate_contract_audit"]["status"]
reason_map = {
"mechanical_initial_inconsistent": (
"Mechanical initial mixes incompatible baseline-left/right translation and yaw; "
"fix run/rtk_lidar_mechanical_initial.json before trusting deployment"
),
"near_180_degree_axis_conflict": (
"Physical axis confirmation is required because the data-driven solution differs "
"from the declared mechanical initial by approximately 180 degrees "
"(or inherits mixed translation/rotation signs)"
),
"solution_disagrees_with_mechanical_baseline_side": (
"Solution yaw/XY disagree with the declared mechanical baseline side; "
"confirm static GNHPR direction before deployment"
),
}
final["selection"] = {
"recommended": not needs_axis_confirmation,
"reason": (
"Physical axis confirmation is required because the data-driven solution differs "
"from the declared mechanical initial by approximately 180 degrees"
if needs_axis_confirmation else
"Uses only motion pairs accepted independently by both Open3D GICP and small_gicp"
reason_map.get(
status,
"Uses only motion pairs accepted independently by both Open3D GICP and small_gicp",
)
),
"open3d_vs_small_gicp": delta(open_t, small_t),
}
write(args.result_root / "final_T_RTK_lidar.json", final)
summary = {
"final": {
run/README.md
+10 -10
View File
@@ -6,18 +6,18 @@
|---|---|
| `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 位姿 |
| `run_direct_rtk_lidar.ps1` | 从 `combined/` 标定并封装最终结果（**默认基线系**） |
| `prepare_multisensor_dataset.ps1` | 每站一帧 + RTK 位姿（默认含双天线 pitch/roll） |
| `run_direct_rtk_lidar.ps1` | 从 `combined/` 标定并封装最终结果（**默认车头向前 -90**） |
| `run_single_dataset.ps1` | 地面、双 GICP、精筛、共识、AX=XB |
| `run_joint_rtk_lidar.ps1` | 多批共识对联合求解 |
| `view_result.ps1` | 3D 运动对对比 |
| `rtk_lidar_mechanical_initial.json` | 仅 AX=XB 初值；**禁止**用于 pair |
## 默认参数（匹配当前约 2 m 车顶雷达 / 基线系）
## 默认参数（匹配当前约 2 m 车顶雷达 / 车头向前）
| 参数 | 默认 |
|---|---|
| `HeadingOffsetDeg` | `0`（基线系） |
| `HeadingOffsetDeg` | `-90`（车头向前；主从装反、基线朝右） |
| `GroundZMin/Max` | `-2.5` / `-1.5` |
| `ExpectedStations` | `27` |
| `MinStations` | `20` |
@@ -53,10 +53,10 @@ python tools\export_g90_h32_windows_to_combined.py `
```powershell
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_direct_rtk_lidar.ps1" `
-CombinedRoot "D:\data\rtk_lidar_run\combined" `
-WorkRoot "D:\data\rtk_lidar_run\prepared_baseline_h19165" `
-OutputRoot "D:\data\rtk_lidar_run\outputs_baseline_h19165" `
-WorkRoot "D:\data\rtk_lidar_run\prepared_vehicle_h19165" `
-OutputRoot "D:\data\rtk_lidar_run\outputs_vehicle_h19165" `
-RtkReferenceHeightAboveGroundM 1.9165 `
-HeadingOffsetDeg 0 `
-HeadingOffsetDeg -90 `
-ExpectedStations 27 `
-GroundZMin -2.5 -GroundZMax -1.5
```
@@ -65,8 +65,8 @@ powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_direct_rt
```powershell
powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\view_result.ps1" `
-Frames "D:\data\rtk_lidar_run\prepared_baseline_h19165\frames_all" `
-Pairs "D:\data\rtk_lidar_run\outputs_baseline_h19165\consensus\B_consensus.npz" `
-Extrinsic "D:\data\rtk_lidar_run\outputs_baseline_h19165\final_T_RTK_lidar.json" `
-Frames "D:\data\rtk_lidar_run\prepared_vehicle_h19165\frames_all" `
-Pairs "D:\data\rtk_lidar_run\outputs_vehicle_h19165\consensus\B_consensus.npz" `
-Extrinsic "D:\data\rtk_lidar_run\outputs_vehicle_h19165\final_T_RTK_lidar.json" `
-PairIndex 0
```
run/rtk_lidar_mechanical_initial.json
+28 -10
View File
@@ -1,23 +1,41 @@
{
"schema_version": 1,
"convention": "T_RTK_lidar maps raw LiDAR points into the RTK baseline frame (X = rawHeading baseline, Y left, Z up; heading_offset_deg = 0)",
"translation_m": [
0.414179474,
"schema_version": 3,
"convention": "T_RTK_lidar maps raw LiDAR points into the vehicle-forward RTK body frame (X forward, Y left, Z up) after HeadingOffsetDeg=-90",
"frame_mode": "vehicle_forward_heading_offset",
"heading_offset_deg": -90.0,
"baseline_points": "vehicle_right",
"baseline_points_note": "Field-confirmed: master/slave assignment reversed vs G90 diagram, antennas left-right symmetric about rear-axle centerline. Master/GGA on vehicle left, slave on right; rawHeading points vehicle right.",
"vehicle_flu_lever_master_to_lidar_m": [
0.210859360,
0.004000001
-0.414179474,
0.078500001
],
"vehicle_flu_note": "Vehicle FLU: LiDAR origin relative to master/GGA = ahead, right, above. CAD drawing X was opposite vehicle-forward; longitudinal sign is +X in true FLU (solver also converges to +X).",
"antenna_symmetry_note": "Master/slave are mirrors about the rear-axle centerline; swap flips baseline 180° and the vehicle-Y sign of the master→LiDAR lever",
"translation_m": [
0.210859360,
-0.414179474,
0.078500001
],
"rotation_rpy_deg_xyz": [
0.0,
0.0,
90.0
0.0
],
"matrix_4x4": [
[0.0, -1.0, 0.0, 0.414179474],
[1.0, 0.0, 0.0, 0.210859360],
[0.0, 0.0, 1.0, 0.004000001],
[0.0, 1.0, 0.0, -0.414179474],
[0.0, 0.0, 1.0, 0.078500001],
[0.0, 0.0, 0.0, 1.0]
],
"use": "Final AX=XB solver initialization only; never use for LiDAR pair registration",
"yaw_note": "≈90 deg yaw is expected when LiDAR X is vehicle-forward and the dual-antenna baseline is left-right",
"z_note": "CAD/mechanical z only; final z is constrained by measured GGA/ANT1 phase-center height above ground"
"yaw_note": "In vehicle-forward delivery, LiDAR +X ≈ vehicle forward ⇒ mechanical yaw ≈ 0",
"z_note": "78.500001 mm = H_L - H_R with H_L=1994.999879 mm, H_R=1916.499878 mm",
"attitude_composition": "R_W_body = Rz(yaw_raw) Ry(-pitch) Rx(roll) Rz(-heading_offset); pitch/roll stay in baseline frame",
"baseline_frame_equivalent": {
"heading_offset_deg": 0.0,
"translation_m": [0.414179474, 0.210859360, 0.078500001],
"rotation_rpy_deg_xyz": [0.0, 0.0, 90.0],
"note": "Same physical install expressed in rawHeading baseline frame"
}
}
run/run_direct_rtk_lidar.ps1
+3 -2
View File
@@ -14,8 +14,9 @@ param(
[int]$SmallGicpMaxGap = 26,
[int]$Open3DMaxGap = 26,
[double]$MaxReferenceTranslationM = 8.0,
# Baseline frame: rawHeading as RTK X. Use 90 only when deliberately targeting vehicle-forward.
[double]$HeadingOffsetDeg = 0.0,
# Baseline frame: rawHeading as RTK X. Default vehicle-forward for this car: -90
# (master/slave swapped, baseline points vehicle-right).
[double]$HeadingOffsetDeg = -90.0,
[string]$SolverInitialExtrinsic = "",
[double]$RefineMinInlierRatio = 0.63,
[double]$RefineMaxInlierRmseM = 0.14
run/run_full_pipeline.ps1
+1 -1
View File
@@ -15,7 +15,7 @@
# Roof-mounted H32 (~2 m). Do not reuse [-1.4, -0.4] on this vehicle.
[double]$GroundZMin = -2.5,
[double]$GroundZMax = -1.5,
[double]$HeadingOffsetDeg = 0.0
[double]$HeadingOffsetDeg = -90.0
)
$ErrorActionPreference = "Stop"
tests/test_rtk_lidar_coordinate_contract.py
+90 -5
View File
@@ -12,8 +12,12 @@ ROOT = Path(__file__).resolve().parents[1]
sys.path.insert(0, str(ROOT / "tools"))
sys.path.insert(0, str(ROOT / "code"))
from finalize_direct_rtk_lidar import coordinate_contract_audit # noqa: E402
from finalize_direct_rtk_lidar import ( # noqa: E402
coordinate_contract_audit,
mechanical_self_consistency,
)
from prepare_multisensor_station_dataset import heading_to_enu_yaw # noqa: E402
from rtk_attitude import attitude_rotation, rtk_body_rotation # noqa: E402
from rigorous_calibration import ( # noqa: E402
build_parser,
load_extrinsic_matrix,
@@ -34,6 +38,32 @@ def test_east_vehicle_heading_maps_to_zero_enu_yaw() -> None:
assert math.degrees(yaw) == 0.0
def test_attitude_rotation_applies_baseline_pitch_elevation() -> None:
_, yaw = heading_to_enu_yaw(0.0, 0.0) # heading north → body X = +North
rotation = attitude_rotation(yaw, pitch_deg=10.0, roll_deg=0.0)
body_x = rotation @ np.array([1.0, 0.0, 0.0])
np.testing.assert_allclose(
body_x,
[0.0, math.cos(math.radians(10.0)), math.sin(math.radians(10.0))],
atol=1e-12,
)
def test_vehicle_forward_offset_keeps_pitch_about_baseline() -> None:
# Baseline points east (vehicle right if nose north); pitch elevates baseline X.
# Vehicle-forward offset -90 must not simply Ry after vehicle yaw.
raw_heading = 90.0
pitch = 10.0
r_correct = rtk_body_rotation(raw_heading, -90.0, pitch_deg=pitch, roll_deg=0.0)
_, yaw_raw = heading_to_enu_yaw(raw_heading, 0.0)
r_baseline = attitude_rotation(yaw_raw, pitch_deg=pitch, roll_deg=0.0)
rz90 = np.array([[0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0]])
np.testing.assert_allclose(r_correct, r_baseline @ rz90, atol=1e-12)
# Level vehicle-forward X should point north.
r_level = rtk_body_rotation(raw_heading, -90.0, pitch_deg=0.0, roll_deg=0.0)
np.testing.assert_allclose(r_level @ np.array([1.0, 0.0, 0.0]), [0.0, 1.0, 0.0], atol=1e-12)
def test_pair_registration_has_no_extrinsic_argument() -> None:
parser = build_parser()
pair_options = {
@@ -45,18 +75,50 @@ def test_pair_registration_has_no_extrinsic_argument() -> None:
assert "--global-voxel" in pair_options
def test_mechanical_initial_round_trip() -> None:
def test_mechanical_initial_is_vehicle_forward_swapped_master() -> None:
path = ROOT / "run" / "rtk_lidar_mechanical_initial.json"
document = __import__("json").loads(path.read_text(encoding="utf-8-sig"))
transform = load_extrinsic_matrix(path)
np.testing.assert_allclose(transform[:3, 3], [0.414179474, 0.210859360, 0.004000001])
np.testing.assert_allclose(transform[:3, :3], [[0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0]])
np.testing.assert_allclose(transform[:3, 3], [0.210859360, -0.414179474, 0.078500001])
np.testing.assert_allclose(transform[:3, :3], np.eye(3))
np.testing.assert_allclose(params_transform(transform_params(transform)), transform, atol=1e-12)
assert document["baseline_points"] == "vehicle_right"
assert document["frame_mode"] == "vehicle_forward_heading_offset"
assert document["heading_offset_deg"] == -90.0
assert document["rotation_rpy_deg_xyz"][2] == 0.0
check = mechanical_self_consistency(document)
assert check["consistent"] is True
def test_mixed_left_xy_plus_right_yaw_mechanical_is_rejected() -> None:
mixed = {
"baseline_points": "vehicle_left",
"translation_m": [0.414179474, 0.210859360, 0.078500001],
"rotation_rpy_deg_xyz": [0.0, 0.0, 90.0],
"matrix_4x4": [
[0.0, -1.0, 0.0, 0.414179474],
[1.0, 0.0, 0.0, 0.210859360],
[0.0, 0.0, 1.0, 0.078500001],
[0.0, 0.0, 0.0, 1.0],
],
}
check = mechanical_self_consistency(mixed)
assert check["consistent"] is False
def test_deprecated_minus_xy_right_baseline_is_rejected_for_swapped_master() -> None:
deprecated = {
"baseline_points": "vehicle_right",
"translation_m": [-0.414179474, -0.210859360, 0.078500001],
"rotation_rpy_deg_xyz": [0.0, 0.0, 90.0],
}
check = mechanical_self_consistency(deprecated)
assert check["consistent"] is False
def test_near_180_degree_solution_is_flagged_for_physical_axis_check() -> None:
initial_path = ROOT / "run" / "rtk_lidar_mechanical_initial.json"
initial = load_extrinsic_matrix(initial_path)
# Flip the declared mechanical forward axis by ~180 deg about Z.
solution = np.eye(4)
solution[:3, :3] = initial[:3, :3] @ np.diag([-1.0, -1.0, 1.0])
solution[:3, 3] = initial[:3, 3]
@@ -66,3 +128,26 @@ def test_near_180_degree_solution_is_flagged_for_physical_axis_check() -> None:
})
assert audit["status"] == "near_180_degree_axis_conflict"
assert audit["requires_physical_axis_confirmation"] is True
def test_previous_mixed_result_branch_is_not_recommended() -> None:
"""Old baseline-frame mixed solution disagrees with vehicle-forward mechanical initial."""
initial_path = ROOT / "run" / "rtk_lidar_mechanical_initial.json"
solution = np.array(
[
[0.00942353438668686, -0.9999215926659111, 0.00824654595155475, 0.4123055815579212],
[0.9998714355322929, 0.009529416150714898, 0.012895837871912157, 0.2173092104098051],
[-0.012973411511822136, 0.008123961367132958, 0.9998828390593821, 0.10405760639434848],
[0.0, 0.0, 0.0, 1.0],
],
float,
)
audit = coordinate_contract_audit({
"solver_initial_extrinsic": str(initial_path),
"matrix_4x4": solution.tolist(),
})
assert audit["requires_physical_axis_confirmation"] is True
assert audit["status"] in {
"near_180_degree_axis_conflict",
"solution_disagrees_with_mechanical_baseline_side",
}
tools/build_multisensor_npz.py
+8 -3
View File
@@ -183,7 +183,8 @@ def initialize_rtk_measurements(values: dict[str, np.ndarray]) -> None:
("differential_age_s", np.float64, np.nan),
("gnss_week", np.int32, -1), ("gnss_tow_ms", np.int64, -1),
("baseline_length_m", np.float64, np.nan), ("raw_heading_deg", np.float64, np.nan),
("pitch_deg", np.float64, np.nan), ("heading_stddev_deg", np.float64, np.nan),
("pitch_deg", np.float64, np.nan), ("roll_deg", np.float64, np.nan),
("heading_stddev_deg", np.float64, np.nan),
("pitch_stddev_deg", np.float64, np.nan), ("heading_satellites", np.int32, -1),
("solution_satellites", np.int32, -1),
):
@@ -297,11 +298,15 @@ def build_combined(
for key, dtype, default in (
("gnss_week", np.int32, -1), ("gnss_tow_ms", np.int64, -1),
("baseline_length_m", np.float64, np.nan), ("raw_heading_deg", np.float64, np.nan),
("pitch_deg", np.float64, np.nan), ("heading_stddev_deg", np.float64, np.nan),
("pitch_deg", np.float64, np.nan), ("roll_deg", np.float64, np.nan),
("heading_stddev_deg", np.float64, np.nan),
("pitch_stddev_deg", np.float64, np.nan),
("solution_satellites", np.int32, -1),
):
values[f"rtk_{key}"] = np.asarray([heading_row.get(key, default)], dtype=dtype)
value = heading_row.get(key, default)
if key == "roll_deg" and value is None:
value = 0.0
values[f"rtk_{key}"] = np.asarray([value], 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, gps_utc_leap_seconds)
tools/build_slam_delivery.py
+119 -63
View File
@@ -13,6 +13,8 @@ from typing import Any
import numpy as np
from rtk_attitude import heading_to_enu_yaw, rotation_to_quat_xyzw, rtk_body_rotation
def args() -> argparse.Namespace:
p = argparse.ArgumentParser(description=__doc__)
@@ -23,9 +25,48 @@ def args() -> argparse.Namespace:
p.add_argument("--out", type=Path, required=True)
p.add_argument("--max-bracket-ms", type=float, default=150.0)
p.add_argument("--heading-std-limit-deg", type=float, default=0.5)
p.add_argument(
"--heading-offset-deg",
type=float,
default=None,
help="Added to rawHeading before ENU yaw. Default: body_heading_offset_deg from extrinsic JSON, else 0.",
)
p.add_argument(
"--orientation-model",
choices=("heading_pitch_roll", "yaw_only"),
default="heading_pitch_roll",
help="heading_pitch_roll uses GNHPR/UNIHEADINGA pitch+roll in T_W_RTK; yaw_only forces pitch=roll=0",
)
return p.parse_args()
POSITION_TYPES = {"GGA", "PVTSLNA"}
HEADING_TYPES = {"UNIHEADINGA", "GNHPR"}
def heading_row_valid(row: dict[str, Any]) -> bool:
if row.get("type") == "UNIHEADINGA":
return bool(row.get("checksum_valid") and row.get("heading_valid") and row.get("raw_heading_deg") is not None)
if row.get("type") == "GNHPR":
return bool(row.get("checksum_valid") and row.get("heading_valid") and row.get("raw_heading_deg") is not None)
return False
def heading_quality_ok(row: dict[str, Any], std_limit_deg: float) -> list[str]:
reasons: list[str] = []
if row.get("type") == "UNIHEADINGA":
if str(row.get("heading_solution", "")) != "NARROW_INT":
reasons.append("HEADING_NOT_NARROW_INT")
std = float(row.get("heading_stddev_deg") or math.inf)
if std > std_limit_deg:
reasons.append("HEADING_STD_EXCEEDED")
elif row.get("type") == "GNHPR":
quality = int(row.get("heading_quality", -1) or -1)
if quality not in {4, 5} and not row.get("heading_valid"):
reasons.append("HEADING_QUALITY_NOT_FIXED")
return reasons
def read_jsonl(path: Path) -> list[dict[str, Any]]:
with path.open(encoding="utf-8") as f:
return [json.loads(line) for line in f if line.strip()]
@@ -50,39 +91,6 @@ def ecef_to_enu(ecef: np.ndarray, origin: np.ndarray, lat_deg: float, lon_deg: f
return r @ (ecef - origin)
def yaw_matrix(yaw_rad: float) -> np.ndarray:
c, s = math.cos(yaw_rad), math.sin(yaw_rad)
return np.array([[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]], dtype=float)
def matrix_to_quat_xyzw(r: np.ndarray) -> np.ndarray:
# Stable branch-based conversion; output convention is x,y,z,w.
tr = float(np.trace(r))
if tr > 0.0:
s = math.sqrt(tr + 1.0) * 2.0
q = np.array([(r[2, 1] - r[1, 2]) / s,
(r[0, 2] - r[2, 0]) / s,
(r[1, 0] - r[0, 1]) / s, 0.25 * s])
else:
i = int(np.argmax(np.diag(r)))
if i == 0:
s = math.sqrt(1.0 + r[0, 0] - r[1, 1] - r[2, 2]) * 2.0
q = np.array([0.25 * s, (r[0, 1] + r[1, 0]) / s,
(r[0, 2] + r[2, 0]) / s, (r[2, 1] - r[1, 2]) / s])
elif i == 1:
s = math.sqrt(1.0 + r[1, 1] - r[0, 0] - r[2, 2]) * 2.0
q = np.array([(r[0, 1] + r[1, 0]) / s, 0.25 * s,
(r[1, 2] + r[2, 1]) / s, (r[0, 2] - r[2, 0]) / s])
else:
s = math.sqrt(1.0 + r[2, 2] - r[0, 0] - r[1, 1]) * 2.0
q = np.array([(r[0, 2] + r[2, 0]) / s,
(r[1, 2] + r[2, 1]) / s, 0.25 * s,
(r[1, 0] - r[0, 1]) / s])
if q[3] < 0.0:
q = -q
return q / np.linalg.norm(q)
def bracket(rows: list[dict[str, Any]], times: np.ndarray, t: int,
max_ns: int) -> tuple[dict[str, Any], dict[str, Any], float] | None:
right = int(np.searchsorted(times, t, side="left"))
@@ -100,6 +108,10 @@ def circular_lerp_deg(a: float, b: float, u: float) -> float:
return (a + u * delta) % 360.0
def linear_lerp(a: float, b: float, u: float) -> float:
return (1.0 - u) * a + u * b
def iso_utc(ns: int) -> str:
return dt.datetime.fromtimestamp(ns / 1e9, dt.timezone.utc).isoformat(timespec="microseconds")
@@ -128,22 +140,38 @@ def main() -> int:
lidar = [row for row in csv.DictReader(f) if not row.get("error")]
rtk = read_jsonl(a.rtk_jsonl)
imu = [row for row in read_jsonl(a.imu_jsonl) if row.get("crc_valid")]
gga = sorted([r for r in rtk if r.get("type") == "GGA" and r.get("checksum_valid")
and r.get("lat_deg") is not None], key=lambda r: int(r["host_receive_utc_ns"]))
heading = sorted([r for r in rtk if r.get("type") == "UNIHEADINGA" and r.get("checksum_valid")
and r.get("heading_valid") and r.get("raw_heading_deg") is not None],
key=lambda r: int(r["host_receive_utc_ns"]))
if not lidar or len(gga) < 2 or len(heading) < 2:
raise RuntimeError("insufficient LiDAR/GGA/heading data")
positions = sorted(
[
r for r in rtk
if r.get("type") in POSITION_TYPES
and r.get("checksum_valid")
and r.get("lat_deg") is not None
],
key=lambda r: int(r["host_receive_utc_ns"]),
)
heading = sorted(
[r for r in rtk if r.get("type") in HEADING_TYPES and heading_row_valid(r)],
key=lambda r: int(r["host_receive_utc_ns"]),
)
if not lidar or len(positions) < 2 or len(heading) < 2:
raise RuntimeError("insufficient LiDAR/GGA|PVTSLNA/heading(GNHPR|UNIHEADINGA) data")
ext = json.loads(a.extrinsic.read_text(encoding="utf-8"))
t_r_l = np.asarray(ext["matrix_4x4"], dtype=float)
if t_r_l.shape != (4, 4):
raise ValueError("extrinsic matrix_4x4 must be 4x4")
heading_offset_deg = (
float(a.heading_offset_deg)
if a.heading_offset_deg is not None
else float(ext.get("body_heading_offset_deg", 0.0) or 0.0)
)
gga_times = np.asarray([int(r["host_receive_utc_ns"]) for r in gga], dtype=np.int64)
position_times = np.asarray([int(r["host_receive_utc_ns"]) for r in positions], dtype=np.int64)
heading_times = np.asarray([int(r["host_receive_utc_ns"]) for r in heading], dtype=np.int64)
origin_row = next(r for r in gga if int(r.get("fix_quality", -1)) == 4)
origin_row = next(
(r for r in positions if int(r.get("fix_quality", -1)) in {4, 5}),
positions[0],
)
origin_lat, origin_lon, origin_alt = (float(origin_row[k]) for k in ("lat_deg", "lon_deg", "altitude_m"))
origin_ecef = geodetic_to_ecef(origin_lat, origin_lon, origin_alt)
max_ns = int(a.max_bracket_ms * 1_000_000)
@@ -151,7 +179,8 @@ def main() -> int:
for index, frame in enumerate(lidar):
t = int(frame["unix_time_ns"])
gb, hb = bracket(gga, gga_times, t, max_ns), bracket(heading, heading_times, t, max_ns)
gb = bracket(positions, position_times, t, max_ns)
hb = bracket(heading, heading_times, t, max_ns)
reasons: list[str] = []
available = gb is not None and hb is not None
row: dict[str, Any] = {
@@ -160,7 +189,7 @@ def main() -> int:
"pose_available": int(available), "gt_valid": 0, "invalid_reason": "",
}
if not available:
if gb is None: reasons.append("GGA_NOT_BRACKETED")
if gb is None: reasons.append("POSITION_NOT_BRACKETED")
if hb is None: reasons.append("HEADING_NOT_BRACKETED")
row.update({k: "" for k in ("x_m", "y_m", "z_m", "qx", "qy", "qz", "qw",
"rtk_x_m", "rtk_y_m", "rtk_z_m", "raw_heading_deg")})
@@ -174,31 +203,45 @@ def main() -> int:
p1 = geodetic_to_ecef(float(g1["lat_deg"]), float(g1["lon_deg"]), float(g1["altitude_m"]))
p_rtk = ecef_to_enu((1.0 - gu) * p0 + gu * p1, origin_ecef, origin_lat, origin_lon)
raw_heading = circular_lerp_deg(float(h0["raw_heading_deg"]), float(h1["raw_heading_deg"]), hu)
yaw = math.radians(90.0 - raw_heading)
corrected_heading, yaw = heading_to_enu_yaw(raw_heading, heading_offset_deg)
if a.orientation_model == "heading_pitch_roll":
pitch = linear_lerp(float(h0.get("pitch_deg") or 0.0), float(h1.get("pitch_deg") or 0.0), hu)
roll = linear_lerp(float(h0.get("roll_deg") or 0.0), float(h1.get("roll_deg") or 0.0), hu)
else:
pitch = 0.0
roll = 0.0
t_w_r = np.eye(4)
t_w_r[:3, :3] = yaw_matrix(yaw)
t_w_r[:3, :3] = rtk_body_rotation(
raw_heading, heading_offset_deg, pitch_deg=pitch, roll_deg=roll
)
t_w_r[:3, 3] = p_rtk
t_w_l = t_w_r @ t_r_l
q = matrix_to_quat_xyzw(t_w_l[:3, :3])
q = rotation_to_quat_xyzw(t_w_l[:3, :3])
fix0, fix1 = int(g0.get("fix_quality", -1)), int(g1.get("fix_quality", -1))
sol0, sol1 = str(h0.get("heading_solution", "")), str(h1.get("heading_solution", ""))
std0 = float(h0.get("heading_stddev_deg") or math.inf)
std1 = float(h1.get("heading_stddev_deg") or math.inf)
if fix0 != 4 or fix1 != 4: reasons.append("RTK_POSITION_NOT_FIXED")
if sol0 != "NARROW_INT" or sol1 != "NARROW_INT": reasons.append("HEADING_NOT_NARROW_INT")
if max(std0, std1) > a.heading_std_limit_deg: reasons.append("HEADING_STD_EXCEEDED")
if fix0 not in {4, 5} or fix1 not in {4, 5}:
reasons.append("RTK_POSITION_NOT_FIXED")
reasons.extend(heading_quality_ok(h0, a.heading_std_limit_deg))
reasons.extend(heading_quality_ok(h1, a.heading_std_limit_deg))
# Deduplicate while preserving order
reasons = list(dict.fromkeys(reasons))
row.update({
"gt_valid": int(not reasons), "invalid_reason": ";".join(reasons),
"x_m": t_w_l[0, 3], "y_m": t_w_l[1, 3], "z_m": t_w_l[2, 3],
"qx": q[0], "qy": q[1], "qz": q[2], "qw": q[3],
"rtk_x_m": p_rtk[0], "rtk_y_m": p_rtk[1], "rtk_z_m": p_rtk[2],
"raw_heading_deg": raw_heading, "yaw_enu_deg": math.degrees(yaw),
"gga_fix_before": fix0, "gga_fix_after": fix1,
"heading_solution_before": sol0, "heading_solution_after": sol1,
"heading_std_max_deg": max(std0, std1),
"gga_before_dt_ms": (t - int(g0["host_receive_utc_ns"])) / 1e6,
"gga_after_dt_ms": (int(g1["host_receive_utc_ns"]) - t) / 1e6,
"raw_heading_deg": raw_heading,
"corrected_heading_deg": corrected_heading,
"heading_offset_deg": heading_offset_deg,
"yaw_enu_deg": math.degrees(yaw),
"pitch_deg": pitch,
"roll_deg": roll,
"position_fix_before": fix0, "position_fix_after": fix1,
"heading_type_before": h0.get("type"), "heading_type_after": h1.get("type"),
"heading_solution_before": h0.get("heading_solution"),
"heading_solution_after": h1.get("heading_solution"),
"position_before_dt_ms": (t - int(g0["host_receive_utc_ns"])) / 1e6,
"position_after_dt_ms": (int(g1["host_receive_utc_ns"]) - t) / 1e6,
"heading_before_dt_ms": (t - int(h0["host_receive_utc_ns"])) / 1e6,
"heading_after_dt_ms": (int(h1["host_receive_utc_ns"]) - t) / 1e6,
})
@@ -213,9 +256,19 @@ def main() -> int:
summary = {
"coordinate_convention": "T_W_L maps raw LiDAR points to local ENU; T_W_L = T_W_RTK @ T_RTK_lidar",
"world_frame": "local ENU, origin is the first RTK FIX GGA sample",
"rtk_frame": "x is rawHeading baseline direction projected horizontally, y left, z up",
"orientation_model": "RTK pose is yaw-only; IMU orientation is not fused",
"world_frame": "local ENU, origin is the first RTK FIX position sample",
"rtk_frame": (
"delivered body X follows rawHeading after heading_offset_deg; "
"pitch/roll applied in baseline frame before the fixed offset"
),
"heading_offset_deg": heading_offset_deg,
"heading_sources_accepted": sorted(HEADING_TYPES),
"position_sources_accepted": sorted(POSITION_TYPES),
"orientation_model": a.orientation_model,
"orientation_composition": (
"R_W_body = Rz(yaw_raw) Ry(-pitch) Rx(roll) Rz(-heading_offset)"
),
"orientation_note": "Uses dual-antenna GNHPR/UNIHEADINGA pitch/roll; IMU orientation is not fused",
"time_basis": "LiDAR and serial host UTC; no jointly estimated clock offset/drift",
"lidar_frames": len(pose_rows),
"pose_available_frames": sum(int(r["pose_available"]) for r in pose_rows),
@@ -223,7 +276,10 @@ def main() -> int:
"gt_invalid_frames": sum(not int(r["gt_valid"]) for r in pose_rows),
"imu_frames": len(imu),
"enu_origin": {"lat_deg": origin_lat, "lon_deg": origin_lon, "altitude_m": origin_alt},
"quality_rule": "GGA endpoints fix_quality=4, heading endpoints NARROW_INT, heading std <= limit, both streams bracket LiDAR time",
"quality_rule": (
"position endpoints fix_quality in {4,5}; UNIHEADINGA endpoints NARROW_INT with std gate; "
"GNHPR endpoints heading_valid/quality 4|5; both streams bracket LiDAR time"
),
"heading_std_limit_deg": a.heading_std_limit_deg,
"max_bracket_ms": a.max_bracket_ms,
"warning": "gt_valid is a quality gate, not independent proof of +/-3 cm absolute accuracy",
tools/prepare_multisensor_station_dataset.py
+94 -46
View File
@@ -1,5 +1,5 @@
#!/usr/bin/env python3
"""Prepare one static LiDAR frame and one yaw-only RTK reference pose per NPZ segment."""
"""Prepare one static LiDAR frame and one RTK reference pose per NPZ segment."""
from __future__ import annotations
@@ -14,6 +14,13 @@ from typing import Any
import numpy as np
from rtk_attitude import (
heading_to_enu_yaw,
parse_pitch_roll_from_heading_raw,
rotation_to_quat_xyzw,
rtk_body_rotation,
)
POSE_FIELDS = ["time", "x", "y", "z", "qx", "qy", "qz", "qw"]
@@ -53,26 +60,30 @@ def ecef_to_enu(ecef: np.ndarray, origin: np.ndarray, lat_deg: float, lon_deg: f
return rotation @ (ecef - origin)
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]])
def heading_to_enu_yaw(raw_heading_deg: float, heading_offset_deg: float) -> tuple[float, float]:
"""Convert GNHPR navigation heading to mathematical ENU yaw.
``heading_offset_deg`` is added in the receiver's clockwise-from-north
heading convention. It is therefore not interchangeable with a ROS yaw
offset, whose sign and zero axis depend on the ROS frame definition.
"""
corrected_heading = (raw_heading_deg + heading_offset_deg) % 360.0
return corrected_heading, math.radians(90.0 - corrected_heading)
def scalar(data: np.lib.npyio.NpzFile, name: str) -> float:
def scalar(data: np.lib.npyio.NpzFile, name: str, default: float | None = None) -> float:
if name not in data.files:
if default is None:
raise KeyError(name)
return float(default)
return float(np.asarray(data[name]).reshape(-1)[0])
def frame_pitch_roll(data: np.lib.npyio.NpzFile) -> tuple[float, float]:
pitch = scalar(data, "rtk_pitch_deg", math.nan)
roll = scalar(data, "rtk_roll_deg", math.nan)
if math.isfinite(pitch) and math.isfinite(roll):
return pitch, roll
raw = None
if "rtk_heading_raw_utf8" in data.files:
raw = bytes(np.asarray(data["rtk_heading_raw_utf8"]).reshape(-1))
parsed_pitch, parsed_roll = parse_pitch_roll_from_heading_raw(raw)
if not math.isfinite(pitch):
pitch = float(parsed_pitch) if parsed_pitch is not None else 0.0
if not math.isfinite(roll):
roll = float(parsed_roll) if parsed_roll is not None else 0.0
return pitch, roll
def parse_args() -> argparse.Namespace:
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("--combined-root", type=Path, required=True)
@@ -84,6 +95,12 @@ def parse_args() -> argparse.Namespace:
parser.add_argument("--heading-std-limit-deg", type=float, default=0.5)
parser.add_argument("--min-stations", type=int, default=30)
parser.add_argument("--expected-stations", type=int, default=0)
parser.add_argument(
"--orientation-model",
choices=("heading_pitch_roll", "yaw_only"),
default="heading_pitch_roll",
help="heading_pitch_roll uses GNHPR/UNIHEADINGA pitch+roll; yaw_only forces roll=pitch=0",
)
parser.add_argument("--overwrite", action="store_true")
return parser.parse_args()
@@ -113,9 +130,10 @@ def main() -> int:
for row in good:
path = args.combined_root / Path(row["output"])
with np.load(path, allow_pickle=False) as data:
pitch, roll = frame_pitch_roll(data)
samples.append((scalar(data, "rtk_lat_deg"), scalar(data, "rtk_lon_deg"),
scalar(data, "rtk_altitude_m"), scalar(data, "rtk_raw_heading_deg"),
scalar(data, "rtk_pitch_deg"), scalar(data, "rtk_heading_stddev_deg")))
pitch, roll, scalar(data, "rtk_heading_stddev_deg", math.nan)))
values = np.asarray(samples, dtype=float)
heading_std = circular_std_deg(values[:, 3])
if heading_std > args.heading_std_limit_deg:
@@ -123,16 +141,23 @@ def main() -> int:
continue
frame = good[len(good) // 2]
source = args.combined_root / Path(frame["output"])
reported_std = values[:, 5]
reported_std = values[:, 6]
reported_std_mean = float(np.nanmean(reported_std)) if np.isfinite(reported_std).any() else None
selected.append({"station": segment, "source": source, "time": int(frame["lidar_time_ns"]) / 1e9,
"lat": float(np.mean(values[:, 0])), "lon": float(np.mean(values[:, 1])),
"alt": float(np.mean(values[:, 2])), "heading": circular_mean_deg(values[:, 3])})
summaries.append({"station": segment, "frames": len(group), "valid_fixed_frames": len(good),
"heading_mean_deg": circular_mean_deg(values[:, 3]),
"heading_circular_std_deg": heading_std, "rtk_pitch_mean_deg": float(np.mean(values[:, 4])),
"reported_heading_std_mean_deg": reported_std_mean,
"altitude_std_m": float(np.std(values[:, 2])), "selected_source": str(source)})
selected.append({
"station": segment, "source": source, "time": int(frame["lidar_time_ns"]) / 1e9,
"lat": float(np.mean(values[:, 0])), "lon": float(np.mean(values[:, 1])),
"alt": float(np.mean(values[:, 2])), "heading": circular_mean_deg(values[:, 3]),
"pitch": float(np.mean(values[:, 4])), "roll": float(np.mean(values[:, 5])),
})
summaries.append({
"station": segment, "frames": len(group), "valid_fixed_frames": len(good),
"heading_mean_deg": circular_mean_deg(values[:, 3]),
"heading_circular_std_deg": heading_std,
"rtk_pitch_mean_deg": float(np.mean(values[:, 4])),
"rtk_roll_mean_deg": float(np.mean(values[:, 5])),
"reported_heading_std_mean_deg": reported_std_mean,
"altitude_std_m": float(np.std(values[:, 2])), "selected_source": str(source),
})
if args.expected_stations and len(selected) != args.expected_stations:
raise RuntimeError(f"expected {args.expected_stations} usable stations, got {len(selected)}; rejected={rejected}")
@@ -145,6 +170,7 @@ def main() -> int:
origin = selected[0]
origin_ecef = geodetic_to_ecef(origin["lat"], origin["lon"], origin["alt"])
lever = np.asarray(args.antenna_lever, dtype=float)
use_attitude = args.orientation_model == "heading_pitch_roll"
pose_rows = []
for index, item in enumerate(selected, 1):
destination = frames / f"station_{index:02d}.npz"
@@ -152,32 +178,54 @@ def main() -> int:
antenna = ecef_to_enu(geodetic_to_ecef(item["lat"], item["lon"], item["alt"]), origin_ecef,
origin["lat"], origin["lon"])
corrected_heading, yaw = heading_to_enu_yaw(item["heading"], args.heading_offset_deg)
reference_position = antenna - yaw_rotation(yaw) @ lever
pose_rows.append(dict(zip(POSE_FIELDS, [item["time"], *reference_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})
pitch = float(item["pitch"]) if use_attitude else 0.0
roll = float(item["roll"]) if use_attitude else 0.0
rotation = rtk_body_rotation(
item["heading"], args.heading_offset_deg, pitch_deg=pitch, roll_deg=roll
)
reference_position = antenna - rotation @ lever
quat = rotation_to_quat_xyzw(rotation)
pose_rows.append(dict(zip(POSE_FIELDS, [item["time"], *reference_position, *quat])))
summaries[index - 1].update({
"sequence": index, "prepared_frame": destination.name,
"corrected_heading_deg": corrected_heading,
"pose_yaw_enu_deg": math.degrees(yaw),
"pose_pitch_deg": pitch, "pose_roll_deg": roll,
})
pose_path = args.output / f"reference_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)
with (args.output / "station_summary.csv").open("w", encoding="utf-8", newline="") as stream:
fields = sorted({key for row in summaries for key in row})
writer = csv.DictWriter(stream, fieldnames=fields); writer.writeheader(); writer.writerows(summaries)
document = {"source_combined_root": str(args.combined_root.resolve()), "station_count": len(selected),
"rejected": rejected, "pose_csv": pose_path.name,
"selection_policy": "middle LiDAR frame among fixed-position and valid-heading associations",
"reference_pose_configuration": {"raw_heading_offset_deg": args.heading_offset_deg,
"antenna_lever_body_m": args.antenna_lever,
"heading_offset_semantics": (
"added to clockwise-from-north GNHPR heading before ENU yaw conversion"
),
"orientation_model": "yaw-only, identical to the previous calibration workflow"},
"stations": [{"sequence": i + 1, "source_station": item["station"],
"source_frame": str(item["source"]), "prepared_frame": f"station_{i + 1:02d}.npz"}
for i, item in enumerate(selected)]}
document = {
"source_combined_root": str(args.combined_root.resolve()), "station_count": len(selected),
"rejected": rejected, "pose_csv": pose_path.name,
"selection_policy": "middle LiDAR frame among fixed-position and valid-heading associations",
"reference_pose_configuration": {
"raw_heading_offset_deg": args.heading_offset_deg,
"antenna_lever_body_m": args.antenna_lever,
"heading_offset_semantics": (
"added to clockwise-from-north GNHPR heading before ENU yaw conversion"
),
"orientation_model": args.orientation_model,
"orientation_composition": (
"R_W_body = Rz(yaw_raw) Ry(-pitch) Rx(roll) Rz(-heading_offset); "
"yaw_raw from rawHeading, pitch/roll stay in baseline frame"
),
"pitch_roll_note": (
"pitch/roll come from dual-antenna GNHPR/UNIHEADINGA (baseline elevation / reported roll). "
"This is not a fused IMU vehicle attitude; G90 roll is often ~0."
),
},
"stations": [{"sequence": i + 1, "source_station": item["station"],
"source_frame": str(item["source"]), "prepared_frame": f"station_{i + 1:02d}.npz"}
for i, item in enumerate(selected)],
}
(args.output / "manifest.json").write_text(json.dumps(document, ensure_ascii=False, indent=2), encoding="utf-8")
print(json.dumps({"prepared": str(args.output.resolve()), "stations": len(selected),
"rejected": rejected, "pose_csv": pose_path.name}, ensure_ascii=False, indent=2))
"rejected": rejected, "pose_csv": pose_path.name,
"orientation_model": args.orientation_model}, ensure_ascii=False, indent=2))
return 0
tools/rscap_v2/pipeline_common.py
+1
View File
@@ -120,6 +120,7 @@ def parse_heading(line: str) -> dict:
"baseline_length_m": safe_float(fields[2]),
"raw_heading_deg": raw_heading,
"pitch_deg": safe_float(fields[4]),
"roll_deg": 0.0,
"heading_stddev_deg": safe_float(fields[6]),
"pitch_stddev_deg": safe_float(fields[7]) if len(fields) > 7 else None,
"station_id": fields[8].strip('"') if len(fields) > 8 else "",
tools/rtk_attitude.py
+127
View File
@@ -0,0 +1,127 @@
#!/usr/bin/env python3
"""RTK dual-antenna attitude helpers shared by prepare and SLAM delivery."""
from __future__ import annotations
import math
import re
import numpy as np
# GNHPR / UNIHEADINGA pitch is baseline elevation (far antenna higher ⇒ +pitch).
# Build the baseline-frame attitude first, then apply the fixed body yaw offset:
# R_W_body = Rz(yaw_raw) Ry(-pitch) Rx(roll) Rz(-heading_offset)
# so pitch/roll stay about the physical baseline, even when delivering vehicle-forward.
def heading_to_enu_yaw(raw_heading_deg: float, heading_offset_deg: float = 0.0) -> tuple[float, float]:
"""Convert clockwise-from-north heading to mathematical ENU yaw (rad)."""
corrected_heading = (raw_heading_deg + heading_offset_deg) % 360.0
return corrected_heading, math.radians(90.0 - corrected_heading)
def _rz(yaw_rad: float) -> np.ndarray:
c, s = math.cos(yaw_rad), math.sin(yaw_rad)
return np.array([[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]], dtype=float)
def _ry(pitch_rad: float) -> np.ndarray:
c, s = math.cos(pitch_rad), math.sin(pitch_rad)
return np.array([[c, 0.0, s], [0.0, 1.0, 0.0], [-s, 0.0, c]], dtype=float)
def _rx(roll_rad: float) -> np.ndarray:
c, s = math.cos(roll_rad), math.sin(roll_rad)
return np.array([[1.0, 0.0, 0.0], [0.0, c, -s], [0.0, s, c]], dtype=float)
def attitude_rotation(
yaw_rad: float,
pitch_deg: float = 0.0,
roll_deg: float = 0.0,
) -> np.ndarray:
"""ENU←baseline rotation: Rz(yaw) Ry(-pitch) Rx(roll).
Positive ``pitch_deg`` elevates baseline X (slave higher than master).
"""
return _rz(float(yaw_rad)) @ _ry(-math.radians(float(pitch_deg))) @ _rx(math.radians(float(roll_deg)))
def rtk_body_rotation(
raw_heading_deg: float,
heading_offset_deg: float = 0.0,
pitch_deg: float = 0.0,
roll_deg: float = 0.0,
) -> np.ndarray:
"""ENU←delivered RTK body frame.
Pitch/roll are applied in the raw baseline frame; ``heading_offset_deg`` then
rotates that frame into the delivered body (0 = baseline X, -90 = vehicle
forward when baseline points vehicle-right on this vehicle).
"""
_, yaw_baseline = heading_to_enu_yaw(raw_heading_deg, 0.0)
return attitude_rotation(yaw_baseline, pitch_deg, roll_deg) @ _rz(-math.radians(float(heading_offset_deg)))
def rotation_to_quat_xyzw(rotation: np.ndarray) -> np.ndarray:
r = np.asarray(rotation, dtype=float)
tr = float(np.trace(r))
if tr > 0.0:
s = math.sqrt(tr + 1.0) * 2.0
q = np.array(
[(r[2, 1] - r[1, 2]) / s, (r[0, 2] - r[2, 0]) / s, (r[1, 0] - r[0, 1]) / s, 0.25 * s],
dtype=float,
)
else:
i = int(np.argmax(np.diag(r)))
if i == 0:
s = math.sqrt(1.0 + r[0, 0] - r[1, 1] - r[2, 2]) * 2.0
q = np.array(
[0.25 * s, (r[0, 1] + r[1, 0]) / s, (r[0, 2] + r[2, 0]) / s, (r[2, 1] - r[1, 2]) / s],
dtype=float,
)
elif i == 1:
s = math.sqrt(1.0 + r[1, 1] - r[0, 0] - r[2, 2]) * 2.0
q = np.array(
[(r[0, 1] + r[1, 0]) / s, 0.25 * s, (r[1, 2] + r[2, 1]) / s, (r[0, 2] - r[2, 0]) / s],
dtype=float,
)
else:
s = math.sqrt(1.0 + r[2, 2] - r[0, 0] - r[1, 1]) * 2.0
q = np.array(
[(r[0, 2] + r[2, 0]) / s, (r[1, 2] + r[2, 1]) / s, 0.25 * s, (r[1, 0] - r[0, 1]) / s],
dtype=float,
)
if q[3] < 0.0:
q = -q
return q / np.linalg.norm(q)
def parse_pitch_roll_from_heading_raw(raw_utf8: bytes | str | None) -> tuple[float | None, float | None]:
"""Best-effort pitch/roll from a stored GNHPR/UNIHEADINGA raw line."""
if raw_utf8 is None:
return None, None
text = raw_utf8.decode("ascii", "ignore") if isinstance(raw_utf8, (bytes, bytearray)) else str(raw_utf8)
text = text.strip()
if "GNHPR" in text:
parts = text.split(",")
if len(parts) >= 5:
try:
return float(parts[3]), float(parts[4])
except ValueError:
return None, None
if "UNIHEADINGA" in text.upper() or "HEADINGA" in text.upper():
payload = text.split(";", 1)[-1]
fields = payload.split(",")
if len(fields) >= 5:
try:
return float(fields[4]), 0.0
except ValueError:
return None, None
match = re.search(r",(-?\d+(?:\.\d+)?),(-?\d+(?:\.\d+)?),\d,", text)
if match:
try:
return float(match.group(1)), float(match.group(2))
except ValueError:
return None, None
return None, None
雷达与RTK标定说明书.md
+6 -6
View File
@@ -1,7 +1,7 @@
# 雷达与 RTK 标定说明书
本文说明如何用本仓库完成 **双天线 RTK ↔ 3D 激光雷达** 外参标定，得到可直接使用的 `T_RTK_lidar`。
默认交付坐标系为 **基线系**（`HeadingOffsetDeg = 0`）。更完整的指标与本次结果见根目录 [`README.md`](README.md)。
默认交付坐标系为 **车头向前**（`HeadingOffsetDeg = -90`）。更完整的指标与本次结果见根目录 [`README.md`](README.md)。
---
@@ -14,8 +14,8 @@ p_RTK = T_RTK_lidar · p_lidar
| 项目 | 说明 |
|---|---|
| 输出文件 | `final_T_RTK_lidar.json` |
| 坐标系 | **基线系**：GGA 原点 + `rawHeading` 基线方向为 X（不是车体后轮轴系） |
| 不用到的量 | 车体航向偏置、天线 XY 杆臂、IMU 姿态 |
| 坐标系 | **车头向前**：GGA 原点 + 车头 X（本车 `HeadingOffsetDeg=-90`） |
| 不用到的量 | 车体航向偏置、天线 XY 杆臂、IMU 融合姿态（双天线 pitch/roll 默认进入参考位姿） |
| 必须提供 | RTK 参考点（通常 ANT1）**相位中心**离地高度 |
| pair 配准 | **禁止**使用外参 seed |
| 求解初值 | 可用 `run/rtk_lidar_mechanical_initial.json`（仅 AX=XB） |
@@ -92,7 +92,7 @@ powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_full_pipe
-ImuCapture "$Raw\captures\imu.rscap" `
-OutputRoot $Out `
-RtkReferenceHeightAboveGroundM 1.9165 `
-HeadingOffsetDeg 0 `
-HeadingOffsetDeg -90 `
-ExpectedStations 27 `
-GroundZMin -2.5 `
-GroundZMax -1.5
@@ -101,7 +101,7 @@ powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_full_pipe
| 参数 | 含义 |
|---|---|
| `-RtkReferenceHeightAboveGroundM` | 相位中心离地高（m），**必填**；本车 1.9165 |
| `-HeadingOffsetDeg` | 默认 0 = 基线系 |
| `-HeadingOffsetDeg` | 默认 **-90** = 车头向前（本车主从装反） |
| `-GroundZMin/Max` | 约 2 m 雷达用 `[-2.5,-1.5]` |
| `-ExpectedStations` / `-MinStations` | 本批 27 / 20 |
@@ -113,7 +113,7 @@ powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_direct_rt
-WorkRoot "D:\data\rtk_lidar_run\prepared_baseline_h19165" `
-OutputRoot "D:\data\rtk_lidar_run\outputs_baseline_h19165" `
-RtkReferenceHeightAboveGroundM 1.9165 `
-HeadingOffsetDeg 0 `
-HeadingOffsetDeg -90 `
-ExpectedStations 27 `
-GroundZMin -2.5 `
-GroundZMax -1.5