From 46d2fa1d692e5613290ab24488af83b3cdc01f14 Mon Sep 17 00:00:00 2001 From: "lichun.qu" <16975270+zzqlc@user.noreply.gitee.com> Date: Mon, 10 Aug 2026 22:28:25 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=AD=A3=E5=9F=BA=E7=BA=BF=E7=B3=BB?= =?UTF-8?q?=E6=A0=87=E5=AE=9A=E9=BB=98=E8=AE=A4=EF=BC=9A=E6=9C=BA=E6=A2=B0?= =?UTF-8?q?=E5=88=9D=E5=80=BC=E3=80=81=E5=9C=B0=E9=9D=A2ROI=E4=B8=8E?= =?UTF-8?q?=E8=88=AA=E5=90=91=E5=81=8F=E7=A7=BB=E5=8F=AF=E9=85=8D=EF=BC=8C?= =?UTF-8?q?=E5=B9=B6=E8=A1=A5=E5=85=85G90=E7=AA=97=E5=AF=BC=E5=87=BA?= =?UTF-8?q?=E4=B8=8E=E5=A5=91=E7=BA=A6=E6=B5=8B=E8=AF=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Cursor --- code/finalize_direct_rtk_lidar.py | 76 +++- code/rigorous_calibration.py | 116 +++++- run/README.md | 22 ++ run/rtk_lidar_mechanical_initial.json | 23 ++ run/run_direct_rtk_lidar.ps1 | 48 ++- run/run_full_pipeline.ps1 | 9 +- run/run_single_dataset.ps1 | 39 +- tests/test_g90_host_association.py | 116 ++++++ tests/test_rtk_lidar_coordinate_contract.py | 68 ++++ tools/build_multisensor_npz.py | 13 +- tools/export_g90_h32_windows_to_combined.py | 381 +++++++++++++++++++ tools/prepare_multisensor_station_dataset.py | 21 +- tools/rscap_v2/pipeline_common.py | 26 ++ tools/rscap_v2/pipeline_common_corrected.py | 12 +- 14 files changed, 932 insertions(+), 38 deletions(-) create mode 100644 run/rtk_lidar_mechanical_initial.json create mode 100644 tests/test_g90_host_association.py create mode 100644 tests/test_rtk_lidar_coordinate_contract.py create mode 100644 tools/export_g90_h32_windows_to_combined.py diff --git a/code/finalize_direct_rtk_lidar.py b/code/finalize_direct_rtk_lidar.py index a694cc0..9b70d64 100644 --- a/code/finalize_direct_rtk_lidar.py +++ b/code/finalize_direct_rtk_lidar.py @@ -34,7 +34,50 @@ def delta(a: np.ndarray, b: np.ndarray) -> dict: } -def corrected(raw: dict, backend: str, reference_height: float) -> dict: +def coordinate_contract_audit(raw: dict) -> dict: + """Compare the data-driven solution with the declared mechanical initial. + + 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. + """ + path_text = raw.get("solver_initial_extrinsic") + if not path_text: + return { + "status": "mechanical_initial_not_available", + "requires_physical_axis_confirmation": False, + } + path = Path(path_text) + if not path.exists(): + return { + "status": "mechanical_initial_file_missing", + "requires_physical_axis_confirmation": False, + "mechanical_initial_path": str(path), + } + initial_document = load(path) + initial = np.asarray(initial_document["matrix_4x4"], float) + solution = np.asarray(raw["matrix_4x4"], float) + comparison = delta(initial, solution) + near_180 = abs(comparison["rotation_deg"] - 180.0) <= 15.0 + return { + "status": "near_180_degree_axis_conflict" if near_180 else "no_near_180_degree_axis_conflict", + "requires_physical_axis_confirmation": near_180, + "mechanical_initial_path": str(path.resolve()), + "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." + ), + } + + +def corrected(raw: dict, backend: str, reference_height: float, heading_offset_deg: float) -> dict: + baseline_frame = abs(heading_offset_deg) <= 1e-12 + x_axis = ( + "horizontal projection of the rawHeading baseline direction reported by the receiver" + if baseline_frame else + "vehicle forward after applying the configured G90 heading offset" + ) return { "schema_version": 1, "success": bool(raw["success"]), @@ -43,20 +86,24 @@ def corrected(raw: dict, backend: str, reference_height: float) -> dict: "frames": { "RTK": { "origin": "GGA positioning reference point; confirm ANT1/reference antenna in receiver configuration", - "x_axis": "horizontal projection of the rawHeading baseline direction reported by the receiver", + "x_axis": x_axis, "y_axis": "left", "z_axis": "up", - "yaw_enu_deg": "90 - rawHeadingDeg", + "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", }, "backend": backend, - "measured_lidar_extrinsic_used_as_initial": False, - "body_heading_offset_used": False, + "measured_lidar_extrinsic_used_as_initial": bool(raw.get("measured_extrinsic_used_as_initial")), + "solver_initial_extrinsic": raw.get("solver_initial_extrinsic"), + "body_heading_offset_deg": heading_offset_deg, + "body_heading_offset_used": abs(heading_offset_deg) > 1e-12, "body_antenna_lever_xy_used": False, "translation_m": raw["translation_m"], "rotation_rpy_deg_xyz": raw["rotation_rpy_deg_xyz"], "quaternion_xyzw": raw["quaternion_xyzw"], + "coordinate_contract_audit": coordinate_contract_audit(raw), "matrix_4x4": raw["matrix_4x4"], "quality": { "stations": raw["estimation"]["stations"], @@ -80,6 +127,7 @@ def main() -> None: parser = argparse.ArgumentParser() parser.add_argument("--result-root", type=Path, required=True) parser.add_argument("--reference-height", type=float, required=True) + parser.add_argument("--heading-offset-deg", type=float, required=True) args = parser.parse_args() def solver_output(directory: str) -> Path: @@ -94,16 +142,26 @@ def main() -> None: } docs = {} for backend, path in paths.items(): - document = corrected(load(path), backend, args.reference_height) + document = corrected( + load(path), backend, args.reference_height, args.heading_offset_deg + ) write(path.with_name("extrinsic_rtk_lidar.json"), document) docs[backend] = document open_t = np.asarray(docs["open3d_gicp"]["matrix_4x4"], float) small_t = np.asarray(docs["small_gicp"]["matrix_4x4"], float) final = dict(docs["consensus"]) + needs_axis_confirmation = bool( + final["coordinate_contract_audit"]["requires_physical_axis_confirmation"] + ) final["selection"] = { - "recommended": True, - "reason": "Uses only motion pairs accepted independently by both Open3D GICP and small_gicp", + "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" + ), "open3d_vs_small_gicp": delta(open_t, small_t), } @@ -117,6 +175,8 @@ def main() -> None: "translation_rms_m": final["quality"]["residuals"]["translation_m"]["rms"], "rotation_rms_deg": final["quality"]["residuals"]["rotation_deg"]["rms"], "condition_number": final["quality"]["weighted_jacobian_condition_number"], + "coordinate_contract_status": final["coordinate_contract_audit"]["status"], + "recommended_for_deployment": final["selection"]["recommended"], }, "backend_difference": delta(open_t, small_t), } diff --git a/code/rigorous_calibration.py b/code/rigorous_calibration.py index 54e674b..f31cf58 100644 --- a/code/rigorous_calibration.py +++ b/code/rigorous_calibration.py @@ -80,6 +80,20 @@ def params_transform(params): return make_transform(params[:3], so3_exp(params[3:])) +def transform_params(transform): + from scipy.spatial.transform import Rotation + transform = np.asarray(transform, float) + return np.r_[transform[:3, 3], Rotation.from_matrix(transform[:3, :3]).as_rotvec()] + + +def load_extrinsic_matrix(path): + document = json.loads(Path(path).read_text(encoding="utf-8-sig")) + transform = np.asarray(document["matrix_4x4"], dtype=float) + if transform.shape != (4, 4): + raise ValueError("initial extrinsic matrix_4x4 must be 4x4") + return transform + + def inverse_transform(transform): answer = np.eye(4) answer[:3, :3] = transform[:3, :3].T @@ -135,7 +149,8 @@ def load_npz_xyz(path, min_range=1.0, max_range=50.0): if "points_raw" not in data: raise ValueError(f"{path}: points_raw is required; cart-frame points are forbidden") raw = np.asarray(data["points_raw"], dtype=np.float64) - timestamp = float(np.ravel(data["unix_time_ns"])[0]) / 1e9 + time_key = "lidar_association_time_ns" if "lidar_association_time_ns" in data else "unix_time_ns" + timestamp = float(np.ravel(data[time_key])[0]) / 1e9 counter = int(np.ravel(data["frame_counter"])[0]) distance = raw[:, 0] * 0.001 azimuth = np.deg2rad(raw[:, 1]) @@ -179,6 +194,63 @@ def make_o3d_cloud(points, voxel): return cloud.voxel_down_sample(voxel) +def make_global_features(points, voxel): + import open3d as o3d + cloud = make_o3d_cloud(points, voxel) + cloud.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid( + radius=voxel * 2.5, max_nn=50 + )) + features = o3d.pipelines.registration.compute_fpfh_feature( + cloud, + o3d.geometry.KDTreeSearchParamHybrid(radius=voxel * 5.0, max_nn=100), + ) + return cloud, features + + +def global_lidar_initialization(target_features, source_features, args, pair_seed): + """Estimate source-to-target motion from LiDAR geometry without RTK or an extrinsic.""" + import open3d as o3d + registration = o3d.pipelines.registration + target_cloud, target_fpfh = target_features + source_cloud, source_fpfh = source_features + attempts = [] + for attempt in range(args.global_ransac_attempts): + o3d.utility.random.seed(int(pair_seed + attempt)) + answer = registration.registration_ransac_based_on_feature_matching( + source_cloud, + target_cloud, + source_fpfh, + target_fpfh, + True, + args.global_correspondence, + registration.TransformationEstimationPointToPoint(False), + 4, + [ + registration.CorrespondenceCheckerBasedOnEdgeLength(0.9), + registration.CorrespondenceCheckerBasedOnDistance(args.global_correspondence), + ], + registration.RANSACConvergenceCriteria( + args.global_ransac_iterations, args.global_ransac_confidence + ), + ) + attempts.append({ + "transform": np.asarray(answer.transformation, float), + "fitness": float(answer.fitness), + "inlier_rmse_m": float(answer.inlier_rmse), + }) + best = max(attempts, key=lambda item: (item["fitness"], -item["inlier_rmse_m"])) + return { + "transform": best["transform"], + "method": "LiDAR-only FPFH RANSAC", + "fitness": best["fitness"], + "inlier_rmse_m": best["inlier_rmse_m"], + "attempts": [ + {key: value for key, value in item.items() if key != "transform"} + for item in attempts + ], + } + + def align_open3d(target, source, initial, voxels, correspondences, iterations): import open3d as o3d registration = o3d.pipelines.registration @@ -381,6 +453,8 @@ def cmd_pairs(args): reference_poses = np.asarray(reference_poses) split = [split_holdout(station[3], args.holdout_fraction, i) for i, station in enumerate(stations)] + global_features = [make_global_features(points[0], args.global_voxel) + for points in split] rng = np.random.default_rng(args.seed) accepted_a, accepted_b, accepted_meta, reports = [], [], [], [] accepted_transforms = {} @@ -389,9 +463,15 @@ def cmd_pairs(args): a_ij = inverse_transform(reference_poses[i]) @ reference_poses[j] translation = float(np.linalg.norm(a_ij[:2, 3])) rotation = rotation_angle_deg(a_ij[:3, :3]) + if args.max_reference_translation is not None and translation > args.max_reference_translation: + continue if translation < args.min_translation and rotation < args.min_rotation: continue - initial_b = a_ij.copy() # X0=I; no measured extrinsic. + global_initial = global_lidar_initialization( + global_features[i], global_features[j], args, + args.seed + i * 1009 + j * 9176, + ) + initial_b = global_initial["transform"] target_fit, target_holdout = split[i] source_fit, source_holdout = split[j] forward = align_backend(args.backend, target_fit, source_fit, initial_b, args) @@ -447,7 +527,10 @@ def cmd_pairs(args): "frame_counter_i": stations[i][1], "frame_counter_j": stations[j][1], "rtk_translation_m": translation, "rtk_rotation_deg": rotation, "nearest_rtk_dt_i_s": reference_dt[i], "nearest_rtk_dt_j_s": reference_dt[j], - "initial_B_source": "X0=identity; B0=A (no measured extrinsic)", + "initial_B_source": global_initial["method"], + "global_lidar_initialization": { + key: value for key, value in global_initial.items() if key != "transform" + }, "B_ij_4x4": forward["transform"].tolist(), "backend": args.backend, "backend_converged": forward["converged"], "backend_iterations": forward["iterations"], @@ -482,7 +565,11 @@ def cmd_pairs(args): "backend": args.backend, "transform_convention": "B_ij=T_Li_Lj maps station j points into station i", "raw_point_field": "points_raw", - "measured_extrinsic_used_as_initial": False, + "registration_initial_extrinsic": None, + "selection_is_X_independent": True, + "B_estimation_is_RTK_independent": True, + "candidate_pair_selection_uses_reference_motion": True, + "initialization_warning": None, "stations": len(stations), "candidate_pairs": len(reports), "accepted_pairs": len(accepted_a), "parameters": vars(args), @@ -585,9 +672,11 @@ def pair_metrics(a_array, b_array, x): def solve_extrinsic(a_array, b_array, planes, args): rng = np.random.default_rng(args.seed) - starts = [np.zeros(6)] + center = (transform_params(load_extrinsic_matrix(args.initial_extrinsic)) + if args.initial_extrinsic else np.zeros(6)) + starts = [center] for _ in range(args.solver_multistart - 1): - starts.append(np.r_[ + starts.append(center + np.r_[ rng.normal(0.0, args.start_translation_sigma, 3), np.deg2rad(rng.normal(0.0, args.start_rotation_sigma, 3)), ]) @@ -647,7 +736,10 @@ def cmd_calibrate(args): "message": best.message, "convention": "T_reference_lidar maps raw LiDAR points into the supplied reference frame", "equation": "A_ij X = X B_ij", - "measured_extrinsic_used_as_initial": False, + "measured_extrinsic_used_as_initial": bool(args.initial_extrinsic), + "solver_initial_extrinsic": ( + str(Path(args.initial_extrinsic).resolve()) if args.initial_extrinsic else None + ), "translation_m": x[:3, 3].tolist(), "rotation_rpy_deg_xyz": rpy_deg(x[:3, :3]), "quaternion_xyzw": rotation_to_quat(x[:3, :3]).tolist(), @@ -707,7 +799,8 @@ def build_parser(): ground = commands.add_parser("ground") ground.add_argument("--frames", required=True); ground.add_argument("--output", required=True) ground.add_argument("--min-range", type=float, default=1.0); ground.add_argument("--max-range", type=float, default=30.0) - ground.add_argument("--z-min", type=float, default=-1.4); ground.add_argument("--z-max", type=float, default=-0.4) + # Default ROI for ~2 m roof LiDAR (Z-up). Override for other mounting heights. + ground.add_argument("--z-min", type=float, default=-2.5); ground.add_argument("--z-max", type=float, default=-1.5) ground.add_argument("--voxel", type=float, default=0.08); ground.add_argument("--distance-threshold", type=float, default=0.025) ground.add_argument("--ransac-iterations", type=int, default=500); ground.add_argument("--min-inliers", type=int, default=500) ground.add_argument("--max-rms", type=float, default=0.025); ground.set_defaults(func=cmd_ground) @@ -720,10 +813,16 @@ def build_parser(): pairs.add_argument("--time-offset", type=float, default=0.0) pairs.add_argument("--min-stations", type=int, default=30); pairs.add_argument("--min-pairs", type=int, default=25) pairs.add_argument("--min-gap", type=int, default=1); pairs.add_argument("--max-gap", type=int, default=5) + pairs.add_argument("--max-reference-translation", type=float) pairs.add_argument("--min-translation", type=float, default=0.5); pairs.add_argument("--min-rotation", type=float, default=3.0) pairs.add_argument("--min-range", type=float, default=2.0); pairs.add_argument("--max-range", type=float, default=50.0) pairs.add_argument("--z-min", type=float, default=-0.60); pairs.add_argument("--z-max", type=float, default=5.0) pairs.add_argument("--min-roi-points", type=int, default=1000) + pairs.add_argument("--global-voxel", type=float, default=0.50) + pairs.add_argument("--global-correspondence", type=float, default=1.25) + pairs.add_argument("--global-ransac-attempts", type=int, default=3) + pairs.add_argument("--global-ransac-iterations", type=int, default=100000) + pairs.add_argument("--global-ransac-confidence", type=float, default=0.999) pairs.add_argument("--holdout-fraction", type=float, default=0.20) pairs.add_argument("--voxels", nargs="+", type=float, default=[0.30, 0.15, 0.08]) pairs.add_argument("--correspondences", nargs="+", type=float, default=[1.20, 0.50, 0.25]) @@ -744,6 +843,7 @@ def build_parser(): calibrate = commands.add_parser("calibrate") calibrate.add_argument("--pairs", required=True); calibrate.add_argument("--ground-planes", required=True) calibrate.add_argument("--output", required=True) + calibrate.add_argument("--initial-extrinsic") calibrate.add_argument("--translation-sigma", type=float, default=0.05) calibrate.add_argument("--rotation-sigma", type=float, default=0.5) calibrate.add_argument("--plane-normal-sigma", type=float, default=0.02) diff --git a/run/README.md b/run/README.md index 9088ef9..227911e 100644 --- a/run/README.md +++ b/run/README.md @@ -26,3 +26,25 @@ python tools\export_raw_to_combined.py --stations-root ... --rtk-rscap ... --imu - `-TimeBasis host`：旧「已解码点云」dlog + 主机接收时间关联 所有路径均为命令行参数。标定入口要求显式传入RTK/GGA参考点离地高度，避免静默使用与实车不符的默认值；默认生成目录`work/`和`outputs/`不会提交Git。 + +## G90 + H32 连续录制数据按站导出 + +如果各站不是独立目录，而是记录在多个 Medulla DLog ZIP 和 G90 `.rscap` 中，使用： + +```powershell +python tools\export_g90_h32_windows_to_combined.py ` + --segments-csv ` + --lidar-dlog --lidar-dlog ` + --rtk-rscap --rtk-rscap ` + --out --expected-stations 27 --frame-stride 5 +``` + +该入口使用 MSOP 的主机接收 UTC 与 G90 串口主机接收 UTC 做近邻关联，同时保留雷达包内设备时间作为审计字段。它只读取原始文件，生成 `export/`、`combined/` 和摘要，不使用 IMU。`--frame-stride` 仅控制参与标定的点云帧密度，不改变站点时间窗。 + +如果雷达 ZIP 扫描已经完成但后续步骤中断，可保留 `export/` 并续跑： + +```powershell +python tools\export_g90_h32_windows_to_combined.py --reuse-export --segments-csv --rtk-rscap --out --expected-stations 27 +``` + +在尚未测得 GGA/ANT1 相位中心离地高度时，可先生成 `combined/`，再运行 `prepare_multisensor_dataset.ps1` 得到 `prepared/`。最终外参求解仍需显式传入真实离地高度，不应留空或猜测。 diff --git a/run/rtk_lidar_mechanical_initial.json b/run/rtk_lidar_mechanical_initial.json new file mode 100644 index 0000000..be804cb --- /dev/null +++ b/run/rtk_lidar_mechanical_initial.json @@ -0,0 +1,23 @@ +{ + "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, + 0.210859360, + 0.004000001 + ], + "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.004000001], + [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" +} diff --git a/run/run_direct_rtk_lidar.ps1 b/run/run_direct_rtk_lidar.ps1 index 0f31aad..a0d8642 100644 --- a/run/run_direct_rtk_lidar.ps1 +++ b/run/run_direct_rtk_lidar.ps1 @@ -3,32 +3,66 @@ param( [Parameter(Mandatory = $true)][double]$RtkReferenceHeightAboveGroundM, [string]$OutputRoot = "", [string]$WorkRoot = "", - [int]$ExpectedStations = 34, + [int]$ExpectedStations = 27, + [int]$MinStations = 20, [int]$MinPairs = 20, - [int]$Bootstrap = 200 + [int]$Bootstrap = 200, + # Roof-mounted H32 (~2 m): ground points are near z≈-2 in the LiDAR frame (Z-up). + # The old [-1.4, -0.4] window fits walls on this vehicle and must not be reused. + [double]$GroundZMin = -2.5, + [double]$GroundZMax = -1.5, + [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, + [string]$SolverInitialExtrinsic = "", + [double]$RefineMinInlierRatio = 0.63, + [double]$RefineMaxInlierRmseM = 0.14 ) $ErrorActionPreference = "Stop" $Repo = Split-Path -Parent $PSScriptRoot if ([string]::IsNullOrWhiteSpace($OutputRoot)) { $OutputRoot = Join-Path $Repo "outputs\rtk_lidar_calibration" } if ([string]::IsNullOrWhiteSpace($WorkRoot)) { $WorkRoot = Join-Path $Repo "work\prepared_rtk_direct" } +if ([string]::IsNullOrWhiteSpace($SolverInitialExtrinsic)) { + $SolverInitialExtrinsic = Join-Path $PSScriptRoot "rtk_lidar_mechanical_initial.json" +} + +$PoseName = if ([math]::Abs($HeadingOffsetDeg) -le 1e-12) { + "rtk_gga_raw_heading" +} else { + "rtk_vehicle_heading" +} +$ReferencePoseFile = "reference_poses_${PoseName}.csv" $Prepared = $WorkRoot & (Join-Path $Repo "run\prepare_multisensor_dataset.ps1") ` - -CombinedRoot $CombinedRoot -Output $Prepared -HeadingOffsetDeg 0 ` - -AntennaLever @(0.0,0.0,0.0) -PoseName "rtk_gga_raw_heading" -MinStations 30 -ExpectedStations $ExpectedStations -Overwrite + -CombinedRoot $CombinedRoot -Output $Prepared -HeadingOffsetDeg $HeadingOffsetDeg ` + -AntennaLever @(0.0,0.0,0.0) -PoseName $PoseName -MinStations $MinStations ` + -ExpectedStations $ExpectedStations -Overwrite if ($LASTEXITCODE -ne 0) { throw "RTK-direct dataset preparation failed" } +# Pair registration intentionally has no --initial-extrinsic (B must stay X-independent). +# SolverInitialExtrinsic is applied only in the final AX=XB calibrate stage. & (Join-Path $Repo "run\run_single_dataset.ps1") ` -Prepared $Prepared -OutputRoot $OutputRoot ` - -ReferencePoseFile "reference_poses_rtk_gga_raw_heading.csv" ` - -ReferenceHeight $RtkReferenceHeightAboveGroundM -MinPairs $MinPairs -Bootstrap $Bootstrap + -ReferencePoseFile $ReferencePoseFile ` + -ReferenceHeight $RtkReferenceHeightAboveGroundM ` + -MinStations $MinStations -MinPairs $MinPairs -Bootstrap $Bootstrap ` + -GroundZMin $GroundZMin -GroundZMax $GroundZMax ` + -SmallGicpMaxGap $SmallGicpMaxGap -Open3DMaxGap $Open3DMaxGap ` + -MaxReferenceTranslationM $MaxReferenceTranslationM ` + -SolverInitialExtrinsic $SolverInitialExtrinsic ` + -RefineMinInlierRatio $RefineMinInlierRatio ` + -RefineMaxInlierRmseM $RefineMaxInlierRmseM if ($LASTEXITCODE -ne 0) { throw "RTK-direct calibration failed" } $Finalize = @( (Join-Path $Repo "code\finalize_direct_rtk_lidar.py"), "--result-root", $OutputRoot, - "--reference-height", "$RtkReferenceHeightAboveGroundM" + "--reference-height", "$RtkReferenceHeightAboveGroundM", + "--heading-offset-deg", "$HeadingOffsetDeg" ) & python @Finalize if ($LASTEXITCODE -ne 0) { throw "Final result packaging failed" } diff --git a/run/run_full_pipeline.ps1 b/run/run_full_pipeline.ps1 index 67e7cd2..f521675 100644 --- a/run/run_full_pipeline.ps1 +++ b/run/run_full_pipeline.ps1 @@ -9,8 +9,11 @@ [string]$Timezone = "+08:00", [ValidateSet("device_gnss", "host")][string]$TimeBasis = "device_gnss", [int]$ExpectedStations = 34, + [int]$MinStations = 20, [int]$MinPairs = 20, - [int]$Bootstrap = 200 + [int]$Bootstrap = 200, + [double]$GroundZMin = -1.4, + [double]$GroundZMax = -0.4 ) $ErrorActionPreference = "Stop" @@ -28,7 +31,9 @@ if ($LASTEXITCODE -ne 0) { throw "Raw-data export failed" } -CombinedRoot (Join-Path $ExportRoot "combined") ` -WorkRoot $PreparedRoot -OutputRoot $CalibrationRoot ` -RtkReferenceHeightAboveGroundM $RtkReferenceHeightAboveGroundM ` - -ExpectedStations $ExpectedStations -MinPairs $MinPairs -Bootstrap $Bootstrap + -MinStations $MinStations ` + -ExpectedStations $ExpectedStations -MinPairs $MinPairs -Bootstrap $Bootstrap ` + -GroundZMin $GroundZMin -GroundZMax $GroundZMax if ($LASTEXITCODE -ne 0) { throw "RTK-LiDAR calibration failed" } Write-Host "Final result: $(Join-Path $CalibrationRoot 'final_T_RTK_lidar.json')" diff --git a/run/run_single_dataset.ps1 b/run/run_single_dataset.ps1 index 2f8f605..40ab595 100644 --- a/run/run_single_dataset.ps1 +++ b/run/run_single_dataset.ps1 @@ -3,8 +3,18 @@ param( [Parameter(Mandatory = $true)][string]$OutputRoot, [Parameter(Mandatory = $true)][double]$ReferenceHeight, [string]$ReferencePoseFile = "reference_poses_rtk_gga_raw_heading.csv", + [int]$MinStations = 20, [int]$MinPairs = 20, - [int]$Bootstrap = 100 + [int]$Bootstrap = 100, + # Roof-mounted H32 (~2 m): ground near z≈-2. Old [-1.4,-0.4] fits walls on this vehicle. + [double]$GroundZMin = -2.5, + [double]$GroundZMax = -1.5, + [int]$SmallGicpMaxGap = 26, + [int]$Open3DMaxGap = 26, + [double]$MaxReferenceTranslationM = 8.0, + [string]$SolverInitialExtrinsic = "", + [double]$RefineMinInlierRatio = 0.63, + [double]$RefineMaxInlierRmseM = 0.14 ) $ErrorActionPreference = "Stop" @@ -32,27 +42,38 @@ foreach ($Path in @($Frames, $ReferencePoses)) { New-Item -ItemType Directory -Force -Path $Common,$Open,$Small,$ConsensusOut | Out-Null $Ground = Join-Path $Common "ground_planes.csv" -Run-Python "ground planes" @($Code, "ground", "--frames", $Frames, "--output", $Ground) +Run-Python "ground planes" @($Code, "ground", "--frames", $Frames, "--output", $Ground, + "--z-min", "$GroundZMin", "--z-max", "$GroundZMax") foreach ($Backend in @("small_gicp", "open3d")) { $Directory = if ($Backend -eq "small_gicp") { $Small } else { $Open } $Raw = Join-Path $Directory "B_estimation.npz" $QualityJson = Join-Path $Directory "B_quality.json" $QualityCsv = Join-Path $Directory "B_quality.csv" + $MaxGap = if ($Backend -eq "open3d") { $Open3DMaxGap } else { $SmallGicpMaxGap } $PairArgs = @($Code, "pairs", "--backend", $Backend, "--frames", $Frames, "--reference-poses", $ReferencePoses, "--output", $Raw, "--quality-json", $QualityJson, "--quality-csv", $QualityCsv, - "--min-pairs", "$MinPairs") - if ($Backend -eq "open3d") { $PairArgs += @("--max-gap", "3", "--multistart", "1", "--iterations", "40") } + "--min-stations", "$MinStations", "--min-pairs", "$MinPairs", "--max-gap", "$MaxGap") + if ($MaxReferenceTranslationM -gt 0) { + $PairArgs += @("--max-reference-translation", "$MaxReferenceTranslationM") + } + if ($Backend -eq "open3d") { $PairArgs += @("--multistart", "1", "--iterations", "40") } Run-Python "$Backend pairs" $PairArgs Run-Python "$Backend X-independent refinement" @( $Refine, "--pairs", $Raw, "--quality-json", $QualityJson, - "--output", (Join-Path $Directory "B_refined.npz"), "--min-pairs", "$MinPairs" + "--output", (Join-Path $Directory "B_refined.npz"), "--min-pairs", "$MinPairs", + "--min-inlier-ratio", "$RefineMinInlierRatio", + "--max-inlier-rmse", "$RefineMaxInlierRmseM" ) - Run-Python "$Backend calibration" @( + $CalibrationArgs = @( $Code, "calibrate", "--pairs", (Join-Path $Directory "B_refined.npz"), "--ground-planes", $Ground, "--reference-height", "$ReferenceHeight", "--bootstrap", "$Bootstrap", "--output", (Join-Path $Directory "extrinsic.json") ) + if (-not [string]::IsNullOrWhiteSpace($SolverInitialExtrinsic)) { + $CalibrationArgs += @("--initial-extrinsic", $SolverInitialExtrinsic) + } + Run-Python "$Backend calibration" $CalibrationArgs } $ConsensusPairs = Join-Path $ConsensusOut "B_consensus.npz" @@ -61,10 +82,14 @@ Run-Python "cross-backend consensus" @( "--small-pairs", (Join-Path $Small "B_refined.npz"), "--output", $ConsensusPairs, "--min-pairs", "$MinPairs" ) -Run-Python "consensus calibration" @( +$ConsensusCalibrationArgs = @( $Code, "calibrate", "--pairs", $ConsensusPairs, "--ground-planes", $Ground, "--reference-height", "$ReferenceHeight", "--bootstrap", "$Bootstrap", "--output", (Join-Path $ConsensusOut "extrinsic.json") ) +if (-not [string]::IsNullOrWhiteSpace($SolverInitialExtrinsic)) { + $ConsensusCalibrationArgs += @("--initial-extrinsic", $SolverInitialExtrinsic) +} +Run-Python "consensus calibration" $ConsensusCalibrationArgs Write-Host "Calibration results: $OutputRoot" diff --git a/tests/test_g90_host_association.py b/tests/test_g90_host_association.py new file mode 100644 index 0000000..9b19374 --- /dev/null +++ b/tests/test_g90_host_association.py @@ -0,0 +1,116 @@ +"""Regression tests for G90 GNHPR parsing and host-time LiDAR association.""" + +from __future__ import annotations + +import json +import sys +from pathlib import Path + +import numpy as np + +ROOT = Path(__file__).resolve().parents[1] +TOOLS = ROOT / "tools" +CODE = ROOT / "code" +sys.path.insert(0, str(TOOLS)) +sys.path.insert(0, str(TOOLS / "rscap_v2")) +sys.path.insert(0, str(CODE)) + +from build_multisensor_npz import build_combined # noqa: E402 +from pipeline_common import parse_gnhpr # noqa: E402 +from rigorous_calibration import load_npz_xyz # noqa: E402 + + +def test_parse_gnhpr_fixed_heading(): + row = parse_gnhpr("$GNHPR,070411.40,354.7437,000.2518,000.0000,4,26,0.00,0999*58") + assert row["type"] == "GNHPR" + assert row["raw_heading_deg"] == 354.7437 + assert row["pitch_deg"] == 0.2518 + assert row["roll_deg"] == 0.0 + assert row["heading_quality"] == 4 + assert row["satellites"] == 26 + assert row["heading_valid"] is True + + +def test_host_time_uses_lidar_receive_time_and_preserves_device_time(tmp_path: Path): + host_ns = 1_786_240_000_000_000_000 + device_ns = 1_500_000_000_000_000_000 + frame_dir = tmp_path / "lidar" + frame_dir.mkdir() + np.savez_compressed( + frame_dir / "frame.npz", + points=np.zeros((4, 4), dtype=np.float32), + unix_time_ns=np.asarray([device_ns], dtype=np.int64), + host_receive_utc_ns=np.asarray([host_ns], dtype=np.int64), + ) + + rtk = tmp_path / "rtk.jsonl" + rows = [ + { + "type": "GGA", + "checksum_valid": True, + "host_receive_utc_ns": host_ns + 20_000_000, + "lat_deg": 31.0, + "lon_deg": 121.0, + "altitude_m": 10.0, + "fix_quality": 4, + "satellites": 20, + "raw_line": "$GNGGA,...", + }, + { + "type": "GNHPR", + "checksum_valid": True, + "host_receive_utc_ns": host_ns - 10_000_000, + "raw_heading_deg": 90.0, + "pitch_deg": 1.0, + "roll_deg": 0.0, + "heading_quality": 4, + "heading_solution": "GNHPR_QUALITY_4", + "heading_valid": True, + "satellites": 22, + "raw_line": "$GNHPR,...", + }, + ] + rtk.write_text("".join(json.dumps(row) + "\n" for row in rows), encoding="utf-8") + imu = tmp_path / "imu.jsonl" + imu.write_text("", encoding="utf-8") + + out = tmp_path / "combined" + summary = build_combined( + [("STATION-01", frame_dir)], + [rtk], + [imu], + out, + time_basis="host", + rtk_max_dt_ms=100.0, + ) + + assert summary["frames"] == 1 + assert summary["rtk_valid"] == 1 + assert summary["heading_valid"] == 1 + assert summary["rtk_fixed"] == 1 + with np.load(next((out / "frames").glob("*.npz")), allow_pickle=False) as frame: + assert int(frame["lidar_association_time_ns"][0]) == host_ns + assert int(frame["unix_time_ns"][0]) == device_ns + assert int(frame["rtk_gga_dt_ns"][0]) == 20_000_000 + assert int(frame["rtk_heading_dt_ns"][0]) == -10_000_000 + + +def test_registration_prefers_lidar_association_time(tmp_path: Path): + host_ns = 1_786_240_000_000_000_000 + device_ns = 1_500_000_000_000_000_000 + source = tmp_path / "frame.npz" + np.savez_compressed( + source, + points_raw=np.asarray( + [[1000.0, 0.0, 0.0, 1.0], [2000.0, 90.0, 0.0, 1.0]], + dtype=np.float32, + ), + unix_time_ns=np.asarray([device_ns], dtype=np.int64), + lidar_association_time_ns=np.asarray([host_ns], dtype=np.int64), + frame_counter=np.asarray([7], dtype=np.int64), + ) + + timestamp, counter, xyz = load_npz_xyz(source) + assert timestamp == host_ns / 1e9 + assert counter == 7 + assert xyz.shape == (2, 3) diff --git a/tests/test_rtk_lidar_coordinate_contract.py b/tests/test_rtk_lidar_coordinate_contract.py new file mode 100644 index 0000000..40e9b1d --- /dev/null +++ b/tests/test_rtk_lidar_coordinate_contract.py @@ -0,0 +1,68 @@ +"""Regression tests for the RTK–LiDAR coordinate and initialization contract.""" + +from __future__ import annotations + +import math +import sys +from pathlib import Path + +import numpy as np + +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 prepare_multisensor_station_dataset import heading_to_enu_yaw # noqa: E402 +from rigorous_calibration import ( # noqa: E402 + build_parser, + load_extrinsic_matrix, + params_transform, + transform_params, +) + + +def test_left_baseline_heading_plus_90_points_vehicle_forward() -> None: + corrected, yaw = heading_to_enu_yaw(270.0, 90.0) + assert corrected == 0.0 + assert math.degrees(yaw) == 90.0 + + +def test_east_vehicle_heading_maps_to_zero_enu_yaw() -> None: + corrected, yaw = heading_to_enu_yaw(0.0, 90.0) + assert corrected == 90.0 + assert math.degrees(yaw) == 0.0 + + +def test_pair_registration_has_no_extrinsic_argument() -> None: + parser = build_parser() + pair_options = { + option + for action in parser._subparsers._group_actions[0].choices["pairs"]._actions + for option in action.option_strings + } + assert "--initial-extrinsic" not in pair_options + assert "--global-voxel" in pair_options + + +def test_mechanical_initial_round_trip() -> None: + path = ROOT / "run" / "rtk_lidar_mechanical_initial.json" + 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(params_transform(transform_params(transform)), transform, atol=1e-12) + + +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] + audit = coordinate_contract_audit({ + "solver_initial_extrinsic": str(initial_path), + "matrix_4x4": solution.tolist(), + }) + assert audit["status"] == "near_180_degree_axis_conflict" + assert audit["requires_physical_axis_confirmation"] is True diff --git a/tools/build_multisensor_npz.py b/tools/build_multisensor_npz.py index 80e0de8..83ccceb 100644 --- a/tools/build_multisensor_npz.py +++ b/tools/build_multisensor_npz.py @@ -25,6 +25,7 @@ import numpy as np GPS_EPOCH_UNIX_NS = 315964800 * 1_000_000_000 POSITION_TYPES = {"GGA", "PVTSLNA"} +HEADING_TYPES = {"UNIHEADINGA", "GNHPR"} def parse_named_path(text: str) -> tuple[str, Path]: @@ -230,7 +231,7 @@ def build_combined( heading = [] for row in rtk_rows: - if row.get("type") != "UNIHEADINGA" or not row.get("checksum_valid") or not row.get("heading_valid"): + if row.get("type") not in HEADING_TYPES or not row.get("checksum_valid") or not row.get("heading_valid"): continue assoc = association_time_ns(row, time_basis, gps_utc_leap_seconds) if assoc is None: @@ -258,7 +259,14 @@ def build_combined( for segment_index, source in enumerate(frame_paths): with np.load(source, allow_pickle=False) as frame: values = {key: np.asarray(frame[key]) for key in frame.files} - lidar_time_ns = int(scalar(values["unix_time_ns"])) + lidar_device_time_ns = int(scalar(values["unix_time_ns"])) + if time_basis == "host": + lidar_time_ns = int(scalar(values["host_receive_utc_ns"])) + if lidar_time_ns <= 0: + raise ValueError(f"host time requested but missing in {source}") + else: + lidar_time_ns = lidar_device_time_ns + values["lidar_association_time_ns"] = np.asarray([lidar_time_ns], dtype=np.int64) position_index = nearest_index(position_times, lidar_time_ns) heading_index = nearest_index(heading_times, lidar_time_ns) @@ -337,6 +345,7 @@ def build_combined( "output": str(output.relative_to(out)), "source_lidar": str(source.resolve()), "lidar_time_ns": lidar_time_ns, + "lidar_device_time_ns": lidar_device_time_ns, "rtk_gga_dt_ns": position_dt, "rtk_heading_dt_ns": heading_dt, "rtk_valid": position_ok, diff --git a/tools/export_g90_h32_windows_to_combined.py b/tools/export_g90_h32_windows_to_combined.py new file mode 100644 index 0000000..430db51 --- /dev/null +++ b/tools/export_g90_h32_windows_to_combined.py @@ -0,0 +1,381 @@ +#!/usr/bin/env python3 +"""Export audited H32/G90 static windows directly to RTK--LiDAR combined data. + +This adapter is for captures where multiple static stations live inside large +DLog archives instead of one directory per station. It uses the H32 packet +host-receive UTC ticks as the common software clock, parses G90 ``$GNGGA`` and +``$GNHPR`` from one or more V2 captures, and deliberately does not require IMU. +Raw inputs are opened read-only. +""" + +from __future__ import annotations + +import argparse +import bisect +import csv +import json +import shutil +import struct +import sys +import zipfile +from dataclasses import dataclass +from datetime import datetime, timedelta, timezone +from pathlib import Path +from typing import BinaryIO, Iterator + +import numpy as np + +ROOT = Path(__file__).resolve().parent +sys.path.insert(0, str(ROOT)) +sys.path.insert(0, str(ROOT / "rscap_v2")) + +from build_multisensor_npz import build_combined # noqa: E402 +from h32_dlog.difop import DifopAngles, parse_difop_angles # noqa: E402 +from h32_dlog.dotnet_bin import read_dotnet_string # noqa: E402 +from h32_dlog.payload_v1 import parse_difop_payload, parse_msop_batch_payload # noqa: E402 +from capture_format_v2 import read_capture # noqa: E402 +from h32_msop import iter_h32_frames_polar_from_packets # noqa: E402 +from pipeline_common_corrected import parse_rtk_capture, write_jsonl # noqa: E402 + +DOTNET_UNIX_EPOCH_TICKS = 621355968000000000 +TICKS_PER_SECOND = 10_000_000 +LOCAL_TZ = timezone(timedelta(hours=8)) +MSOP_OBJECT = "frontlidar-msop-raw" +DIFOP_OBJECT = "frontlidar-difop-raw" + + +@dataclass(frozen=True) +class Window: + station_id: str + start_ticks: int + end_ticks: int + + +def local_text_to_utc_ticks(text: str) -> int: + value = datetime.strptime(text.strip(), "%Y-%m-%d %H:%M:%S.%f").replace(tzinfo=LOCAL_TZ) + return int(round(value.timestamp() * TICKS_PER_SECOND)) + DOTNET_UNIX_EPOCH_TICKS + + +def read_exact(stream: BinaryIO, length: int) -> bytes: + value = stream.read(length) + if len(value) != length: + raise EOFError(f"expected {length} bytes, got {len(value)}") + return value + + +def iter_zip_dobject_payloads(path: Path) -> Iterator[tuple[str, bytes]]: + """Sequentially read DObject records from a standard Medulla DLog ZIP.""" + + with zipfile.ZipFile(path) as archive: + candidates = [ + name for name in archive.namelist() + if name.replace("\\", "/").endswith("dobject_recording/data.bin") + ] + if len(candidates) != 1: + raise ValueError(f"{path}: expected one dobject_recording/data.bin, got {candidates}") + with archive.open(candidates[0], "r") as stream: + while True: + try: + name = read_dotnet_string(stream) + except EOFError: + break + read_exact(stream, 8) # outer DObject tick + read_dotnet_string(stream) # record id + length = struct.unpack(" 128 * 1024 * 1024: + raise ValueError(f"{path}: invalid DObject payload length {length}") + yield name, read_exact(stream, length) + + +def load_windows(path: Path) -> list[Window]: + grouped: dict[str, list[tuple[int, int]]] = {} + with path.open("r", encoding="utf-8-sig", newline="") as stream: + for row in csv.DictReader(stream): + station = row["station_id"].strip() + grouped.setdefault(station, []).append( + (local_text_to_utc_ticks(row["local_start"]), local_text_to_utc_ticks(row["local_end"])) + ) + merged: list[Window] = [] + for station, ranges in grouped.items(): + current: list[list[int]] = [] + for start, end in sorted(ranges): + if current and start <= current[-1][1]: + current[-1][1] = max(current[-1][1], end) + else: + current.append([start, end]) + merged.extend(Window(station, start, end) for start, end in current) + merged.sort(key=lambda item: item.start_ticks) + for previous, current in zip(merged, merged[1:]): + if current.start_ticks <= previous.end_ticks and current.station_id != previous.station_id: + raise ValueError(f"overlapping stations: {previous} and {current}") + return merged + + +def station_lookup(windows: list[Window]): + starts = [item.start_ticks for item in windows] + + def lookup(ticks: int) -> str | None: + index = bisect.bisect_right(starts, ticks) - 1 + if index >= 0 and ticks <= windows[index].end_ticks: + return windows[index].station_id + return None + + return lookup + + +def save_frames( + station: str, + packet_items: dict[tuple[str, int], tuple[int, int, bytes]], + export_root: Path, + angles: DifopAngles, + source: Path, + *, + frame_stride: int, + seen_frame_keys: set[tuple[str, int, int]], +) -> int: + if not packet_items: + return 0 + ordered = sorted(packet_items.values(), key=lambda item: (item[0], item[1])) + frames = iter_h32_frames_polar_from_packets( + (item[2] for item in ordered), + host_utc_ticks=[item[0] for item in ordered], + frame_stride=frame_stride, + min_frame_points=100, + min_range_m=0.3, + max_range_m=120.0, + vertical_deg=angles.vertical_deg, + horizontal_deg=angles.horizontal_deg, + ) + frames_dir = export_root / station / "frames" + frames_dir.mkdir(parents=True, exist_ok=True) + written = 0 + for frame in frames: + # Overlapping archives contain identical revolutions. The host stamp + # and 0.1 s device bucket make the key stable without comparing points. + key = (station, int(round(frame.host_receive_utc_ns / 10_000_000)), int(round(frame.t_start_s * 10))) + if key in seen_frame_keys: + continue + seen_frame_keys.add(key) + device_ns = int(round(frame.t_start_s * 1_000_000_000)) + destination = frames_dir / f"h32_{frame.host_receive_utc_ns}_{device_ns}.npz" + np.savez_compressed( + destination, + points_raw=np.asarray(frame.points_raw, dtype=np.float32), + frame_counter=np.asarray([len(seen_frame_keys)], dtype=np.int32), + point_count=np.asarray([len(frame.points_raw)], dtype=np.int32), + unix_time_ns=np.asarray([device_ns], dtype=np.int64), + device_time_s=np.asarray([frame.t_start_s], dtype=np.float64), + device_time_end_s=np.asarray([frame.t_end_s], dtype=np.float64), + host_receive_utc_ns=np.asarray([frame.host_receive_utc_ns], dtype=np.int64), + source_file_utf8=np.frombuffer(str(source.resolve()).encode("utf-8"), dtype=np.uint8), + ) + written += 1 + return written + + +def scan_dlog_sources( + sources: list[Path], + windows: list[Window], + export_root: Path, + *, + frame_stride: int, +) -> dict[str, object]: + lookup = station_lookup(windows) + angles: DifopAngles | None = None + seen_packets: dict[str, set[tuple[str, int]]] = {} + seen_frames: set[tuple[str, int, int]] = set() + frame_counts: dict[str, int] = {} + source_stats: list[dict[str, object]] = [] + + for source_index, source in enumerate(sources, 1): + print(f"[dlog {source_index}/{len(sources)}] {source}", flush=True) + packets: dict[str, dict[tuple[str, int], tuple[int, int, bytes]]] = {} + msop_batches = difop_records = selected_packets = duplicates = 0 + for object_name, payload in iter_zip_dobject_payloads(source): + if object_name == DIFOP_OBJECT: + difop_records += 1 + if angles is None: + try: + angles = parse_difop_angles(parse_difop_payload(payload).raw) + except (EOFError, ValueError): + pass + continue + if object_name != MSOP_OBJECT: + continue + batch = parse_msop_batch_payload(payload) + msop_batches += 1 + for item in batch.packets: + station = lookup(item.host_receive_utc_ticks) + if station is None: + continue + packet_key = (batch.session_id, item.sequence) + station_seen = seen_packets.setdefault(station, set()) + if packet_key in station_seen: + duplicates += 1 + continue + station_seen.add(packet_key) + packets.setdefault(station, {})[packet_key] = ( + item.host_receive_utc_ticks, + item.sequence, + item.raw, + ) + selected_packets += 1 + if angles is None: + raise RuntimeError(f"no valid H32 DIFOP angles found before decoding {source}") + written = 0 + for station, items in packets.items(): + count = save_frames( + station, + items, + export_root, + angles, + source, + frame_stride=frame_stride, + seen_frame_keys=seen_frames, + ) + frame_counts[station] = frame_counts.get(station, 0) + count + written += count + source_stats.append( + { + "source": str(source.resolve()), + "msop_batches": msop_batches, + "difop_records": difop_records, + "selected_packets": selected_packets, + "duplicate_packets": duplicates, + "frames_written": written, + } + ) + print(f" selected_packets={selected_packets} frames={written} duplicates={duplicates}", flush=True) + return {"frame_counts": frame_counts, "sources": source_stats} + + +def parse_rtk_sources(paths: list[Path], parsed_root: Path) -> dict[str, object]: + rows = [] + source_stats = [] + for path in paths: + capture_rows = parse_rtk_capture( + read_capture(path), + accepted_prefixes=("$GNGGA", "$GPGGA", "$GNHPR"), + ) + for row in capture_rows: + row["capture_source"] = str(path.resolve()) + rows.extend(capture_rows) + source_stats.append( + { + "source": str(path.resolve()), + "rows": len(capture_rows), + "gga_valid": sum(row.get("type") == "GGA" and row.get("checksum_valid") for row in capture_rows), + "gnhpr_valid": sum( + row.get("type") == "GNHPR" and row.get("checksum_valid") and row.get("heading_valid") + for row in capture_rows + ), + } + ) + parsed_root.mkdir(parents=True, exist_ok=True) + write_jsonl(parsed_root / "rtk.jsonl", rows) + write_jsonl(parsed_root / "imu.jsonl", []) + return {"rows": len(rows), "sources": source_stats} + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--segments-csv", type=Path, required=True) + parser.add_argument("--lidar-dlog", type=Path, action="append", default=[]) + parser.add_argument("--rtk-rscap", type=Path, action="append", required=True) + parser.add_argument("--out", type=Path, required=True) + parser.add_argument("--expected-stations", type=int, default=0) + parser.add_argument("--frame-stride", type=int, default=5) + parser.add_argument("--rtk-max-dt-ms", type=float, default=200.0) + parser.add_argument("--reuse-export", action="store_true", help="Keep existing export/ and resume parsed/combined stages.") + parser.add_argument("--overwrite", action="store_true") + return parser.parse_args() + + +def main() -> int: + args = parse_args() + if args.frame_stride < 1: + raise SystemExit("--frame-stride must be >= 1") + if not args.reuse_export and not args.lidar_dlog: + raise SystemExit("at least one --lidar-dlog is required unless --reuse-export is used") + for source in [args.segments_csv, *args.lidar_dlog, *args.rtk_rscap]: + if not source.is_file(): + raise FileNotFoundError(source) + if args.reuse_export: + export_root = args.out / "export" + if not export_root.is_dir(): + raise FileNotFoundError(f"--reuse-export requested but missing {export_root}") + for name in ("parsed", "combined", "export_summary.json"): + target = args.out / name + if target.is_dir(): + shutil.rmtree(target) + elif target.exists(): + target.unlink() + elif args.out.exists() and any(args.out.iterdir()): + if not args.overwrite: + raise FileExistsError(f"{args.out} is non-empty; pass --overwrite") + for name in ("export", "parsed", "combined", "export_summary.json"): + target = args.out / name + if target.is_dir(): + shutil.rmtree(target) + elif target.exists(): + target.unlink() + args.out.mkdir(parents=True, exist_ok=True) + windows = load_windows(args.segments_csv) + expected_ids = sorted({item.station_id for item in windows}) + if args.reuse_export: + frame_counts = { + station.name: len(list((station / "frames").glob("*.npz"))) + for station in (args.out / "export").iterdir() + if station.is_dir() + } + lidar_summary = {"frame_counts": frame_counts, "sources": [], "reused_export": True} + else: + lidar_summary = scan_dlog_sources( + args.lidar_dlog, + windows, + args.out / "export", + frame_stride=args.frame_stride, + ) + frame_counts = lidar_summary["frame_counts"] + exported_ids = sorted(station for station, count in frame_counts.items() if count) + missing = sorted(set(expected_ids) - set(exported_ids)) + if missing: + raise RuntimeError(f"stations without decoded H32 frames: {missing}") + if args.expected_stations and len(exported_ids) != args.expected_stations: + raise RuntimeError(f"expected {args.expected_stations} stations, exported {len(exported_ids)}") + rtk_summary = parse_rtk_sources(args.rtk_rscap, args.out / "parsed") + lidar_segments = [(station, args.out / "export" / station / "frames") for station in exported_ids] + combined_summary = build_combined( + lidar_segments, + [args.out / "parsed" / "rtk.jsonl"], + [], + args.out / "combined", + rtk_max_dt_ms=args.rtk_max_dt_ms, + time_basis="host", + overwrite=True, + ) + summary = { + "role": "G90 GNGGA/GNHPR + H32 DLog static-window export", + "segments_csv": str(args.segments_csv.resolve()), + "time_basis": "H32 MSOP host_receive_utc_ticks <-> G90 rscap host_receive_utc_ns", + "imu_used": False, + "expected_station_ids": expected_ids, + "station_count": len(exported_ids), + "lidar": lidar_summary, + "rtk": rtk_summary, + "combined": combined_summary, + "outputs": { + "combined": str((args.out / "combined").resolve()), + "manifest": str((args.out / "combined" / "manifest.csv").resolve()), + }, + } + (args.out / "export_summary.json").write_text( + json.dumps(summary, ensure_ascii=False, indent=2) + "\n", + encoding="utf-8", + ) + print(json.dumps({"stations": len(exported_ids), "combined": combined_summary}, ensure_ascii=False, indent=2)) + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/tools/prepare_multisensor_station_dataset.py b/tools/prepare_multisensor_station_dataset.py index c47fdc5..290ef80 100644 --- a/tools/prepare_multisensor_station_dataset.py +++ b/tools/prepare_multisensor_station_dataset.py @@ -58,6 +58,17 @@ def yaw_rotation(yaw: float) -> np.ndarray: 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: return float(np.asarray(data[name]).reshape(-1)[0]) @@ -112,13 +123,15 @@ def main() -> int: continue frame = good[len(good) // 2] source = args.combined_root / Path(frame["output"]) + reported_std = values[:, 5] + 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": float(np.nanmean(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: @@ -138,8 +151,7 @@ def main() -> int: shutil.copy2(item["source"], destination) antenna = ecef_to_enu(geodetic_to_ecef(item["lat"], item["lon"], item["alt"]), origin_ecef, origin["lat"], origin["lon"]) - corrected_heading = (item["heading"] + args.heading_offset_deg) % 360.0 - yaw = math.radians(90.0 - corrected_heading) + 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)]))) @@ -156,6 +168,9 @@ def main() -> int: "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"} diff --git a/tools/rscap_v2/pipeline_common.py b/tools/rscap_v2/pipeline_common.py index 7bdf230..e9824f3 100644 --- a/tools/rscap_v2/pipeline_common.py +++ b/tools/rscap_v2/pipeline_common.py @@ -131,6 +131,30 @@ def parse_heading(line: str) -> dict: } +def parse_gnhpr(line: str) -> dict: + """Parse Wheeltec G90 ``$GNHPR`` heading/pitch output.""" + + fields = line[:line.rfind("*")].split(",") + if len(fields) < 7: + raise ValueError("GNHPR has too few fields") + quality = safe_int(fields[5], -1) + return { + "type": "GNHPR", + "position_time_utc": fields[1], + "raw_heading_deg": safe_float(fields[2]), + "pitch_deg": safe_float(fields[3]), + "roll_deg": safe_float(fields[4]), + "heading_quality": quality, + "satellites": safe_int(fields[6], -1), + "heading_solution": f"GNHPR_QUALITY_{quality}", + "baseline_length_m": None, + "heading_stddev_deg": None, + "pitch_stddev_deg": None, + "solution_satellites": safe_int(fields[6], -1), + "heading_valid": quality in {4, 5}, + } + + def parse_pvtslna(line: str) -> dict: """Parse Unicore/G90 ``#PVTSLNA`` into GGA-compatible position fields. @@ -221,6 +245,8 @@ def parse_rtk_capture(capture: CaptureFile) -> list[dict]: row.update(parse_pvtslna(line)) elif line.startswith("#UNIHEADINGA"): row.update(parse_heading(line)) + elif line.startswith("$GNHPR"): + row.update(parse_gnhpr(line)) except ValueError as ex: row["parse_error"] = str(ex) rows.append(row) diff --git a/tools/rscap_v2/pipeline_common_corrected.py b/tools/rscap_v2/pipeline_common_corrected.py index ae74c68..b3d1a9a 100644 --- a/tools/rscap_v2/pipeline_common_corrected.py +++ b/tools/rscap_v2/pipeline_common_corrected.py @@ -41,8 +41,14 @@ def source_for_span(chunks: list[RawChunk], start: int, end: int, segment_id: in } -def parse_rtk_capture(capture: CaptureFile) -> list[dict]: +def parse_rtk_capture( + capture: CaptureFile, + accepted_prefixes: tuple[str, ...] | None = None, +) -> list[dict]: rows = [] + accepted_prefix_bytes = ( + tuple(prefix.encode("ascii") for prefix in accepted_prefixes) if accepted_prefixes is not None else None + ) for segment_id, chunks in iter_contiguous_segments(capture.chunks): stream = b"".join(chunk.raw for chunk in chunks) cursor = 0 @@ -56,6 +62,8 @@ def parse_rtk_capture(capture: CaptureFile) -> list[dict]: cursor = end if not raw_line: continue + if accepted_prefix_bytes is not None and not raw_line.startswith(accepted_prefix_bytes): + continue line = raw_line.decode("ascii", "replace") row = {"type": "UNKNOWN", "raw_line": line, "checksum_valid": parse_checksum(line)} row.update(source_for_span(chunks, start, end, segment_id)) @@ -66,6 +74,8 @@ def parse_rtk_capture(capture: CaptureFile) -> list[dict]: row.update(parse_pvtslna(line)) elif line.startswith("#UNIHEADINGA"): row.update(parse_heading(line)) + elif line.startswith("$GNHPR"): + row.update(parse_gnhpr(line)) except ValueError as ex: row["parse_error"] = str(ex) rows.append(row)