补充data4与data5联合标定流程并显式配置RTK高度

README.md

@@ -92,7 +92,7 @@ powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_full_pipe
   -RtkCapture "$Raw\captures\rtk.rscap" `
   -ImuCapture "$Raw\captures\imu.rscap" `
   -OutputRoot $Out `
-  -RtkReferenceHeightAboveGroundM 0.8535 `
+  -RtkReferenceHeightAboveGroundM 0.758 `
   -ExpectedStations 34
 ```
 
@@ -122,7 +122,7 @@ powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_direct_rt
   -CombinedRoot "E:\calibration_output\exported\combined" `
   -WorkRoot "E:\calibration_output\prepared_rtk_direct" `
   -OutputRoot "E:\calibration_output\calibration" `
-  -RtkReferenceHeightAboveGroundM 0.8535 `
+  -RtkReferenceHeightAboveGroundM 0.758 `
   -ExpectedStations 34
 ```
 
@@ -147,27 +147,68 @@ powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\view_result.p
 
 模式3和4应让同一墙面、立柱、路缘和地面尽量重合。终端同时打印 `B^-1(X^-1AX)` 的平移和旋转增量。应查看多对，不能只挑视觉效果最好的一对。
 
-## 7. data4参考结果
+## 7. data4与data4+data5结果对比
 
-仓库保留了精简参考产物，见[`results/reference_data4`](results/reference_data4/README.md)：
+data5补充了30个有效静态站点及两个法向方向不同的固定平面板。当前联合流程只合并data4、data5各自的批内运动对，不构造跨批次运动，因此两次采集的时间、ENU原点和绝对位置不同不会直接影响共享外参；前提是传感器安装未改变，并且两批数据使用相同的RTK坐标定义和LiDAR原始坐标定义。
+
+两块平面板在现有代码中作为点云场景结构参与GICP配准，但没有作为已知RTK/ENU平面方程单独加入优化；若后续能测得板面方程，才可新增绝对平面约束。
+
+为公平比较，下面两组结果都使用ANT1参考点离地高度`0.758 m`重新求解：
+
+| 指标 | data4单独 | data4+data5联合 | 变化 |
+|---|---:|---:|---:|
+| 有效站点 | 34 | 64 | +30 |
+| 共识运动对 | 25 | 36 | +11 |
+| 平移残差RMS | 0.100394 m | 0.086902 m | -13.4% |
+| 平移残差中位数 | 0.062075 m | 0.053647 m | -13.6% |
+| 平移残差P95 | 0.123039 m | 0.125166 m | +1.7% |
+| 平移残差最大值 | 0.353438 m | 0.355864 m | +0.7% |
+| 旋转残差RMS | 1.252391° | 1.115207° | -11.0% |
+| 旋转残差中位数 | 0.747183° | 0.685164° | -8.3% |
+| 旋转残差P90 | 1.825297° | 1.481369° | -18.8% |
+| 旋转残差P95 | 1.965290° | 1.875860° | -4.6% |
+| Weighted Jacobian condition | 7.713973 | 8.379217 | +8.6% |
+| bootstrap z标准差 | 0.003147 m | 0.001957 m | -37.8% |
+| bootstrap roll标准差 | 0.099319° | 0.062245° | -37.3% |
+| bootstrap pitch标准差 | 0.096049° | 0.064370° | -33.0% |
+
+联合结果为：
 
 ```text
-translation_m = [1.638179350, -0.240844799, 0.084481236]
-RPY_deg_xyz   = [-0.817167459, 1.323288119, -22.104163318]
+translation_m = [1.642932528, -0.242302311, 0.180599708]
+RPY_deg_xyz   = [-0.886210651, 1.372780243, -22.112054052]
 
-站点：34
-共识运动对：25
-AX Translation RMS：0.100207 m
-AX Rotation RMS：1.252794°
-Weighted Jacobian condition：7.739413
-Open3D vs small_gicp：0.003889 m / 0.188431°
+T_RTK_lidar =
+ 0.926183553  0.376030869  0.028014474  1.642932528
+-0.376311142  0.926478119  0.005312208 -0.242302311
+-0.023957243 -0.015462238  0.999593402  0.180599708
+ 0.000000000  0.000000000  0.000000000  1.000000000
 ```
 
-唯一建议下游读取的参考结果是[`final_T_RTK_lidar.json`](results/reference_data4/final_T_RTK_lidar.json)。
+与相同高度下的data4单独结果相比，联合外参相差`5.25 mm / 0.064°`。data5使RMS、中位数、旋转P90和bootstrap稳定性改善，但平移P95及最大值没有改善，说明少数高残差运动对仍然存在；不应仅为降低最大值而按最终外参残差删边。
+
+已经分别得到各批次的共识运动对和地面平面时，可运行：
+
+```powershell
+$Names = @("data4", "data5")
+$Pairs = @("E:\data4\consensus\B_consensus.npz", "E:\data5\consensus\B_consensus.npz")
+$Planes = @("E:\data4\common\ground_planes.csv", "E:\data5\common\ground_planes.csv")
+
+powershell.exe -NoProfile -ExecutionPolicy Bypass -File "$Repo\run\run_joint_rtk_lidar.ps1" `
+  -BatchNames $Names -Pairs $Pairs -GroundPlanes $Planes `
+  -OutputRoot "E:\calibration_output\data4_data5_joint" `
+  -RtkReferenceHeightAboveGroundM 0.758 -Bootstrap 200
+```
+
+脚本会先分别拟合各批次外参；任一批与首批相差超过`0.25 m`或`5°`时中止，提示检查RTK航向/坐标定义和传感器安装。阈值可通过`-MaxBatchTranslationDifferenceM`和`-MaxBatchRotationDifferenceDeg`显式调整。
+
+仓库内[`results/reference_data4`](results/reference_data4/README.md)是历史data4参考产物，使用旧高度配置，不应与上表直接比较，也不应继续作为当前联合外参下发。
 
 ## 8. z与精度限制
 
-平面阿克曼运动不能独立观测z。参考结果使用34站地面平面和RTK参考点离地`0.8535 m`约束z；该高度必须量到实际GGA参考点/天线相位中心。更改参考高度后必须重新求解。
+平面阿克曼运动不能独立观测z。当前联合结果使用64站地面平面和ANT1参考点离地`0.758 m`约束z；其中`0.758 m`来自本次现场粗测的天线底部安装参考高度`0.710 m`，加上天线标签给出的L1/L2 PCO高度`46/50 mm`的中值`48 mm`。该值仍是测量输入，不是手眼运动方程自行估计出来的量。
+
+旧联合结果曾使用`0.8535 m = 0.2335 m + 0.620 m`，得到`z = 0.085093 m`；改用`0.758 m`并重新求解后得到`z = 0.180600 m`，z增加约`0.095507 m`，而x、y和旋转基本不变。所有标定入口现均要求显式提供参考高度，更改高度后必须重新求解，不能只手工修改输出JSON中的z。
 
 AX残差、Hessian/Jacobian条件数、bootstrap和双后端一致性只证明内部一致性，不能单独证明逐帧GT达到±3 cm。当前关联仍以LiDAR和串口主机接收时间为主；GNSS周/周内时间和IMU设备时间被保留，但没有联合估计时钟偏移与漂移。用于连续GT pose前，应补做严格设备时间同步和独立轨迹验证。
 
@@ -180,7 +221,7 @@ AX残差、Hessian/Jacobian条件数、bootstrap和双后端一致性只证明
 | [`code/`](code/) | GICP、运动对质量评价、AX=XB求解、结果封装和3D可视化 |
 | [`tools/`](tools/) | 原始dlog/rscap解析、按LiDAR帧关联及静态站点prepared生成 |
 | [`run/`](run/) | PowerShell入口；所有数据和输出路径都通过参数传入 |
-| [`results/reference_data4/`](results/reference_data4/) | 可提交Git的精简参考结果，不包含点云和本机过程目录 |
+| [`results/reference_data4/`](results/reference_data4/) | 历史data4精简参考结果，不包含点云和本机过程目录 |
 | `work/`、`outputs/` | 本地运行生成物，已由`.gitignore`排除 |
 
 各代码文件职责见[`code/README.md`](code/README.md)，命令索引见[`run/README.md`](run/README.md)，工具说明见[`tools/README.md`](tools/README.md)。

code/README.md

@@ -8,5 +8,6 @@
 | `finalize_direct_rtk_lidar.py` | 将三路求解结果封装为明确方向的`T_RTK_lidar`，选择consensus为最终结果 |
 | `visualize_pair_3d.py` | 交互显示原始、RTK初值、GICP B和`X^-1AX`，并打印增量 |
 | `compare_extrinsics.py` | 计算两套外参的SE(3)平移/旋转差异 |
+| `build_joint_rtk_lidar_inputs.py` | 合并多个独立批次的批内A/B运动对和地面平面，并保留批次索引与汇总信息 |
 
 核心约定：`A=T_Ri_Rj`、`B=T_Li_Lj`、`X=T_RTK_lidar`，满足`A X = X B`。点云配准以i为target、j为source，B将j帧点云变换到i帧。

code/build_joint_rtk_lidar_inputs.py

@@ -0,0 +1,119 @@
+#!/usr/bin/env python3
+"""Combine independent RTK-direct hand-eye batches for a shared extrinsic.
+
+Each batch contributes only its within-batch A/B motion pairs and LiDAR ground
+planes.  No cross-batch motion pair is created, so different ENU origins and
+capture locations are valid as long as every batch uses the same RTK-direct
+frame definition and unchanged physical sensor installation.
+"""
+
+from __future__ import annotations
+
+import argparse
+import csv
+import json
+from pathlib import Path
+
+import numpy as np
+
+
+def parse_args() -> argparse.Namespace:
+    parser = argparse.ArgumentParser(description=__doc__)
+    parser.add_argument("--batch-name", action="append", required=True)
+    parser.add_argument("--pairs", action="append", required=True, type=Path)
+    parser.add_argument("--ground-planes", action="append", required=True, type=Path)
+    parser.add_argument("--output-pairs", required=True, type=Path)
+    parser.add_argument("--output-ground-planes", required=True, type=Path)
+    parser.add_argument("--summary", required=True, type=Path)
+    return parser.parse_args()
+
+
+def load_planes(path: Path, batch_name: str) -> list[dict[str, str]]:
+    with path.open(encoding="utf-8-sig", newline="") as stream:
+        rows = list(csv.DictReader(stream))
+    if not rows:
+        raise ValueError(f"no ground planes in {path}")
+    for row in rows:
+        for key in ("nx", "ny", "nz", "d"):
+            if key not in row or row[key] in (None, ""):
+                raise ValueError(f"missing {key} in {path}")
+        row["source_batch"] = batch_name
+    return rows
+
+
+def main() -> int:
+    args = parse_args()
+    count = len(args.batch_name)
+    if count < 2 or len(args.pairs) != count or len(args.ground_planes) != count:
+        raise ValueError("provide the same number of --batch-name, --pairs, and --ground-planes (at least two)")
+
+    pair_parts: list[dict[str, np.ndarray]] = []
+    plane_rows: list[dict[str, str]] = []
+    batch_summaries: list[dict[str, object]] = []
+    for index, (name, pairs_path, planes_path) in enumerate(zip(args.batch_name, args.pairs, args.ground_planes)):
+        with np.load(pairs_path, allow_pickle=False) as source:
+            required = ("A", "B", "meta", "station_times", "rtk_nearest_dt_s")
+            missing = [key for key in required if key not in source]
+            if missing:
+                raise ValueError(f"{pairs_path} missing {missing}")
+            a = np.asarray(source["A"], float)
+            b = np.asarray(source["B"], float)
+            meta = np.asarray(source["meta"], float)
+            times = np.asarray(source["station_times"], float)
+            rtk_dt = np.asarray(source["rtk_nearest_dt_s"], float)
+        if len(a) == 0 or len(a) != len(b) or len(a) != len(meta):
+            raise ValueError(f"invalid A/B/meta sizes in {pairs_path}")
+        pair_parts.append({"A": a, "B": b, "meta": meta, "station_times": times, "rtk_dt": rtk_dt})
+        rows = load_planes(planes_path, name)
+        plane_rows.extend(rows)
+        batch_summaries.append({
+            "name": name,
+            "pairs_path": str(pairs_path.resolve()),
+            "ground_planes_path": str(planes_path.resolve()),
+            "pairs": len(a),
+            "stations": len(times),
+            "ground_planes": len(rows),
+            "pair_offset": sum(item["A"].shape[0] for item in pair_parts[:-1]),
+        })
+
+    output_pairs = args.output_pairs
+    output_pairs.parent.mkdir(parents=True, exist_ok=True)
+    batch_index = np.concatenate([np.full(len(part["A"]), index, np.int32) for index, part in enumerate(pair_parts)])
+    np.savez_compressed(
+        output_pairs,
+        A=np.concatenate([part["A"] for part in pair_parts]),
+        B=np.concatenate([part["B"] for part in pair_parts]),
+        meta=np.concatenate([part["meta"] for part in pair_parts]),
+        station_times=np.concatenate([part["station_times"] for part in pair_parts]),
+        rtk_nearest_dt_s=np.concatenate([part["rtk_dt"] for part in pair_parts]),
+        batch_index=batch_index,
+        batch_names=np.asarray(args.batch_name),
+        backend=np.asarray("independent_batch_consensus"),
+    )
+
+    output_planes = args.output_ground_planes
+    output_planes.parent.mkdir(parents=True, exist_ok=True)
+    fieldnames = ["nx", "ny", "nz", "d", "source_batch"]
+    with output_planes.open("w", encoding="utf-8", newline="") as stream:
+        writer = csv.DictWriter(stream, fieldnames=fieldnames)
+        writer.writeheader()
+        for row in plane_rows:
+            writer.writerow({key: row[key] for key in fieldnames})
+
+    summary = {
+        "schema_version": 1,
+        "convention": "Shared T_RTK_lidar; only within-batch A_ij and B_ij are combined.",
+        "batches": batch_summaries,
+        "total_pairs": int(len(batch_index)),
+        "total_ground_planes": len(plane_rows),
+        "output_pairs": str(output_pairs.resolve()),
+        "output_ground_planes": str(output_planes.resolve()),
+    }
+    args.summary.parent.mkdir(parents=True, exist_ok=True)
+    args.summary.write_text(json.dumps(summary, ensure_ascii=False, indent=2), encoding="utf-8")
+    print(json.dumps(summary, ensure_ascii=False, indent=2))
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

code/rigorous_calibration.py

@@ -748,7 +748,11 @@ def build_parser():
     calibrate.add_argument("--rotation-sigma", type=float, default=0.5)
     calibrate.add_argument("--plane-normal-sigma", type=float, default=0.02)
     calibrate.add_argument("--plane-height-sigma", type=float, default=0.03)
-    calibrate.add_argument("--reference-height", "--body-height", dest="reference_height", type=float, default=0.8535)
+    calibrate.add_argument(
+        "--reference-height", "--body-height", dest="reference_height",
+        type=float, required=True,
+        help="measured RTK/GGA reference-origin height above the local ground in metres",
+    )
     calibrate.add_argument("--solver-multistart", type=int, default=12)
     calibrate.add_argument("--start-translation-sigma", type=float, default=1.0)
     calibrate.add_argument("--start-rotation-sigma", type=float, default=20.0)

run/README.md

@@ -9,6 +9,7 @@
 | `prepare_multisensor_dataset.ps1` | 每站选一帧，生成yaw-only RTK参考轨迹和`frames_all` |
 | `run_direct_rtk_lidar.ps1` | 从combined数据运行RTK直接标定和最终结果封装 |
 | `run_single_dataset.ps1` | 执行地面、两个GICP后端、精筛、共识和AX=XB求解 |
+| `run_joint_rtk_lidar.ps1` | 合并多个独立批次的批内共识运动对和地面平面，求解共享外参 |
 | `view_result.ps1` | 打开3D运动对对比并打印数值增量 |
 
-所有路径均为命令行参数；默认生成目录`work/`和`outputs/`不会提交Git。
+所有路径均为命令行参数。标定入口要求显式传入RTK/GGA参考点离地高度，避免静默使用与实车不符的默认值；默认生成目录`work/`和`outputs/`不会提交Git。

run/run_direct_rtk_lidar.ps1

@@ -1,8 +1,8 @@
 param(
     [Parameter(Mandatory = $true)][string]$CombinedRoot,
+    [Parameter(Mandatory = $true)][double]$RtkReferenceHeightAboveGroundM,
     [string]$OutputRoot = "",
     [string]$WorkRoot = "",
-    [double]$RtkReferenceHeightAboveGroundM = 0.8535,
     [int]$ExpectedStations = 34,
     [int]$MinPairs = 20,
     [int]$Bootstrap = 200

run/run_full_pipeline.ps1

@@ -4,8 +4,8 @@
     [Parameter(Mandatory = $true)][string]$ImuCapture,
     [Parameter(Mandatory = $true)][string]$OutputRoot,
     [string]$LidarObject = "frontlidar",
+    [Parameter(Mandatory = $true)][double]$RtkReferenceHeightAboveGroundM,
     [string]$Timezone = "+08:00",
-    [double]$RtkReferenceHeightAboveGroundM = 0.8535,
     [int]$ExpectedStations = 34,
     [int]$MinPairs = 20,
     [int]$Bootstrap = 200

run/run_joint_rtk_lidar.ps1

@@ -0,0 +1,103 @@
+param(
+    [Parameter(Mandatory = $true)][string[]]$BatchNames,
+    [Parameter(Mandatory = $true)][string[]]$Pairs,
+    [Parameter(Mandatory = $true)][string[]]$GroundPlanes,
+    [Parameter(Mandatory = $true)][string]$OutputRoot,
+    [Parameter(Mandatory = $true)][double]$RtkReferenceHeightAboveGroundM,
+    [int]$Bootstrap = 200,
+    [double]$MaxBatchTranslationDifferenceM = 0.25,
+    [double]$MaxBatchRotationDifferenceDeg = 5.0
+)
+
+$ErrorActionPreference = "Stop"
+$Repo = Split-Path -Parent $PSScriptRoot
+
+if ($BatchNames.Count -lt 2) {
+    throw "At least two independent batches are required"
+}
+if (($Pairs.Count -ne $BatchNames.Count) -or ($GroundPlanes.Count -ne $BatchNames.Count)) {
+    throw "BatchNames, Pairs, and GroundPlanes must have the same number of entries"
+}
+if ($RtkReferenceHeightAboveGroundM -le 0.0) {
+    throw "RtkReferenceHeightAboveGroundM must be greater than zero"
+}
+foreach ($Path in @($Pairs + $GroundPlanes)) {
+    if (-not (Test-Path -LiteralPath $Path)) {
+        throw "Input does not exist: $Path"
+    }
+}
+
+$PreflightRoot = Join-Path $OutputRoot "preflight"
+New-Item -ItemType Directory -Force -Path $PreflightRoot | Out-Null
+$BatchExtrinsics = @()
+for ($Index = 0; $Index -lt $BatchNames.Count; $Index++) {
+    $SafeName = $BatchNames[$Index] -replace '[^A-Za-z0-9_.-]', '_'
+    $BatchExtrinsic = Join-Path $PreflightRoot "$SafeName.json"
+    Write-Host "[preflight batch: $($BatchNames[$Index])]"
+    & python (Join-Path $Repo "code\rigorous_calibration.py") calibrate `
+        --pairs $Pairs[$Index] `
+        --ground-planes $GroundPlanes[$Index] `
+        --reference-height $RtkReferenceHeightAboveGroundM `
+        --bootstrap 0 `
+        --output $BatchExtrinsic | Out-Null
+    if ($LASTEXITCODE -ne 0) { throw "Batch preflight failed: $($BatchNames[$Index])" }
+    $BatchExtrinsics += $BatchExtrinsic
+}
+
+for ($Index = 1; $Index -lt $BatchNames.Count; $Index++) {
+    $SafeName = $BatchNames[$Index] -replace '[^A-Za-z0-9_.-]', '_'
+    $ComparisonPath = Join-Path $PreflightRoot "$SafeName-vs-batch0.json"
+    & python (Join-Path $Repo "code\compare_extrinsics.py") `
+        --reference $BatchExtrinsics[0] `
+        --candidate $BatchExtrinsics[$Index] `
+        --output $ComparisonPath | Out-Null
+    if ($LASTEXITCODE -ne 0) { throw "Batch comparison failed: $($BatchNames[$Index])" }
+
+    $Comparison = Get-Content -LiteralPath $ComparisonPath -Raw | ConvertFrom-Json
+    $TranslationDifference = [double]$Comparison.relative_translation_norm_m
+    $RotationDifference = [double]$Comparison.relative_rotation_deg
+    Write-Host ("[preflight consistency] {0} vs {1}: {2:F4} m / {3:F3} deg" -f `
+        $BatchNames[$Index], $BatchNames[0], $TranslationDifference, $RotationDifference)
+    if (($TranslationDifference -gt $MaxBatchTranslationDifferenceM) -or
+        ($RotationDifference -gt $MaxBatchRotationDifferenceDeg)) {
+        throw ("Batch extrinsics are inconsistent: {0} vs {1} = {2:F4} m / {3:F3} deg; " +
+            "check RTK heading/frame convention and sensor installation") -f `
+            $BatchNames[$Index], $BatchNames[0], $TranslationDifference, $RotationDifference
+    }
+}
+
+$InputRoot = Join-Path $OutputRoot "inputs"
+$JointPairs = Join-Path $InputRoot "joint_consensus_pairs.npz"
+$JointGroundPlanes = Join-Path $InputRoot "joint_ground_planes.csv"
+$InputSummary = Join-Path $InputRoot "joint_input_summary.json"
+$Extrinsic = Join-Path $OutputRoot "shared_T_RTK_lidar.json"
+
+$BuildArgs = @((Join-Path $Repo "code\build_joint_rtk_lidar_inputs.py"))
+for ($Index = 0; $Index -lt $BatchNames.Count; $Index++) {
+    $BuildArgs += @(
+        "--batch-name", $BatchNames[$Index],
+        "--pairs", $Pairs[$Index],
+        "--ground-planes", $GroundPlanes[$Index]
+    )
+}
+$BuildArgs += @(
+    "--output-pairs", $JointPairs,
+    "--output-ground-planes", $JointGroundPlanes,
+    "--summary", $InputSummary
+)
+
+Write-Host "[combine independent batches]"
+& python @BuildArgs
+if ($LASTEXITCODE -ne 0) { throw "Combining independent batches failed" }
+
+Write-Host "[solve shared T_RTK_lidar]"
+& python (Join-Path $Repo "code\rigorous_calibration.py") calibrate `
+    --pairs $JointPairs `
+    --ground-planes $JointGroundPlanes `
+    --reference-height $RtkReferenceHeightAboveGroundM `
+    --bootstrap $Bootstrap `
+    --output $Extrinsic
+if ($LASTEXITCODE -ne 0) { throw "Shared RTK-LiDAR calibration failed" }
+
+Write-Host "Shared T_RTK_lidar: $Extrinsic"
+Write-Host "Joint input summary: $InputSummary"

run/run_single_dataset.ps1

@@ -1,8 +1,8 @@
 param(
     [Parameter(Mandatory = $true)][string]$Prepared,
     [Parameter(Mandatory = $true)][string]$OutputRoot,
+    [Parameter(Mandatory = $true)][double]$ReferenceHeight,
     [string]$ReferencePoseFile = "reference_poses_rtk_gga_raw_heading.csv",
-    [double]$ReferenceHeight = 0.8535,
     [int]$MinPairs = 20,
     [int]$Bootstrap = 100
 )