diff --git a/README.md b/README.md index 1624b96..456ded6 100644 --- a/README.md +++ b/README.md @@ -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。 diff --git a/code/finalize_direct_rtk_lidar.py b/code/finalize_direct_rtk_lidar.py index 9b70d64..6ec9252 100644 --- a/code/finalize_direct_rtk_lidar.py +++ b/code/finalize_direct_rtk_lidar.py @@ -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": { diff --git a/run/README.md b/run/README.md index 62dd2ca..23d7885 100644 --- a/run/README.md +++ b/run/README.md @@ -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 ``` diff --git a/run/rtk_lidar_mechanical_initial.json b/run/rtk_lidar_mechanical_initial.json index be804cb..0ee0a3c 100644 --- a/run/rtk_lidar_mechanical_initial.json +++ b/run/rtk_lidar_mechanical_initial.json @@ -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" + } } diff --git a/run/run_direct_rtk_lidar.ps1 b/run/run_direct_rtk_lidar.ps1 index a0d8642..8a0e10f 100644 --- a/run/run_direct_rtk_lidar.ps1 +++ b/run/run_direct_rtk_lidar.ps1 @@ -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 diff --git a/run/run_full_pipeline.ps1 b/run/run_full_pipeline.ps1 index 6c434a5..7652d04 100644 --- a/run/run_full_pipeline.ps1 +++ b/run/run_full_pipeline.ps1 @@ -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" diff --git a/tests/test_rtk_lidar_coordinate_contract.py b/tests/test_rtk_lidar_coordinate_contract.py index 40e9b1d..39f07f8 100644 --- a/tests/test_rtk_lidar_coordinate_contract.py +++ b/tests/test_rtk_lidar_coordinate_contract.py @@ -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", + } diff --git a/tools/build_multisensor_npz.py b/tools/build_multisensor_npz.py index 83ccceb..6a43f6f 100644 --- a/tools/build_multisensor_npz.py +++ b/tools/build_multisensor_npz.py @@ -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) diff --git a/tools/build_slam_delivery.py b/tools/build_slam_delivery.py index a4a17e3..12cb645 100644 --- a/tools/build_slam_delivery.py +++ b/tools/build_slam_delivery.py @@ -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", diff --git a/tools/prepare_multisensor_station_dataset.py b/tools/prepare_multisensor_station_dataset.py index 290ef80..db638e7 100644 --- a/tools/prepare_multisensor_station_dataset.py +++ b/tools/prepare_multisensor_station_dataset.py @@ -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 diff --git a/tools/rscap_v2/pipeline_common.py b/tools/rscap_v2/pipeline_common.py index e9824f3..c5cd8f2 100644 --- a/tools/rscap_v2/pipeline_common.py +++ b/tools/rscap_v2/pipeline_common.py @@ -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 "", diff --git a/tools/rtk_attitude.py b/tools/rtk_attitude.py new file mode 100644 index 0000000..377db2c --- /dev/null +++ b/tools/rtk_attitude.py @@ -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 diff --git a/雷达与RTK标定说明书.md b/雷达与RTK标定说明书.md index ad3a7bd..4f7e1e4 100644 --- a/雷达与RTK标定说明书.md +++ b/雷达与RTK标定说明书.md @@ -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