修正基线系标定默认：机械初值、地面ROI与航向偏移可配，并补充G90窗导出与契约测试

Co-authored-by: Cursor <cursoragent@cursor.com>

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),
     }

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)

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 <rtk_lidar_station_segments.csv> `
+  --lidar-dlog <dump_1.zip> --lidar-dlog <dump_2.zip> `
+  --rtk-rscap <g90_1.rscap> --rtk-rscap <g90_2.rscap> `
+  --out <output_root> --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 <csv> --rtk-rscap <g90.rscap> --out <output_root> --expected-stations 27
+```
+
+在尚未测得 GGA/ANT1 相位中心离地高度时，可先生成 `combined/`，再运行 `prepare_multisensor_dataset.ps1` 得到 `prepared/`。最终外参求解仍需显式传入真实离地高度，不应留空或猜测。

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"
+}

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" }

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')"

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"

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)

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

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,

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("<i", read_exact(stream, 4))[0]
+                if length < 0 or length > 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())

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"}

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)

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)