diff --git a/README.md b/README.md index d9847b1..d8b8f9d 100644 --- a/README.md +++ b/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)。 diff --git a/code/README.md b/code/README.md index 6f038d9..610555e 100644 --- a/code/README.md +++ b/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帧。 diff --git a/code/build_joint_rtk_lidar_inputs.py b/code/build_joint_rtk_lidar_inputs.py new file mode 100644 index 0000000..95e75ac --- /dev/null +++ b/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()) diff --git a/code/rigorous_calibration.py b/code/rigorous_calibration.py index 0974795..54e674b 100644 --- a/code/rigorous_calibration.py +++ b/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) diff --git a/run/README.md b/run/README.md index 120bf1c..ef3b4bd 100644 --- a/run/README.md +++ b/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。 diff --git a/run/run_direct_rtk_lidar.ps1 b/run/run_direct_rtk_lidar.ps1 index 7c837b2..0f31aad 100644 --- a/run/run_direct_rtk_lidar.ps1 +++ b/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 diff --git a/run/run_full_pipeline.ps1 b/run/run_full_pipeline.ps1 index 0ad9f17..200ebe6 100644 --- a/run/run_full_pipeline.ps1 +++ b/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 diff --git a/run/run_joint_rtk_lidar.ps1 b/run/run_joint_rtk_lidar.ps1 new file mode 100644 index 0000000..58b33fc --- /dev/null +++ b/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" diff --git a/run/run_single_dataset.ps1 b/run/run_single_dataset.ps1 index 3bf3e53..2f8f605 100644 --- a/run/run_single_dataset.ps1 +++ b/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 )