调整雷达到RTK标定分支为独立根目录结构

This commit is contained in:
lichun.qu
2026-07-24 08:42:16 +08:00
parent d2aae6177e
commit 6d87b6ba9c
132 changed files with 214 additions and 197832 deletions
+10 -23
View File
@@ -1,27 +1,14 @@
# run
# run目录
本目录是 PowerShell 运行入口;完整流程、参数解释、验收方法和代码职责统一见仓库根目录 `README.md`
根README包含完整复现命令;这里仅列入口职责
| 脚本 | 使用时机 |
| 脚本 | 用途 |
|---|---|
| `export_legacy_stations.ps1` | 第一、第二批式逐站 dlog 导出。 |
| `export_multisensor_stations.ps1` | LiDAR dlog + 独立 RTK/IMU rscap 导出和时间关联。 |
| `prepare_legacy_dataset.ps1` | 旧式导出结果生成 prepared。 |
| `prepare_multisensor_dataset.ps1` | 多传感器 combined 结果生成 prepared。 |
| `run_single_dataset.ps1` | 单批数据运行 small_gicp、Open3D、consensus 和 X 求解 |
| `run_all.ps1` | 第二批求解、第一批辅助复核的历史流程。 |
| `run_consensus_finish.ps1` | 从已有两个后端 B 重做 consensus。 |
| `run_sensitivity_scan.ps1` | 指定 Pair 的外参角度灵敏度扫描。 |
| `view_result.ps1` | 传入 frames、B、X 查看 3D 配准及增量。 |
| `run_full_pipeline.ps1` | 从逐站LiDAR dlog、RTK rscap、IMU rscap一直运行到最终`T_RTK_lidar` |
| `export_multisensor_stations.ps1` | 解析原始三传感器数据并按LiDAR帧生成combined NPZ |
| `prepare_multisensor_dataset.ps1` | 每站选一帧,生成yaw-only RTK参考轨迹和`frames_all` |
| `run_direct_rtk_lidar.ps1` | combined数据运行RTK直接标定和最终结果封装 |
| `run_single_dataset.ps1` | 执行地面、两个GICP后端、精筛、共识和AX=XB求解 |
| `view_result.ps1` | 打开3D运动对对比并打印数值增量 |
最常用的 3D 入口:
```powershell
powershell.exe -NoProfile -ExecutionPolicy Bypass -File ".\run\view_result.ps1" `
-Frames "D:\你的数据目录\prepared\frames_all" `
-Pairs "D:\你的结果目录\B_consensus.npz" `
-Extrinsic "D:\你的结果目录\extrinsic.json" `
-PairIndex 0
```
`frames_all` 必须与生成 B 时的站点顺序一致。
所有路径均为命令行参数;默认生成目录`work/``outputs/`不会提交Git。
-34
View File
@@ -1,34 +0,0 @@
param(
[Parameter(Mandatory = $true)][string]$DataRoot,
[Parameter(Mandatory = $true)][string]$OutputRoot,
[string]$LidarObject = "frontlidar",
[string]$Timezone = "+08:00",
[string[]]$StationNames = @(),
[int]$Stride = 1,
[double]$RtkMaxDtMs = 150.0
)
$ErrorActionPreference = "Stop"
$Repo = Split-Path -Parent $PSScriptRoot
$Exporter = Join-Path $Repo "tools\frontlidar_dlog_export.py"
if (-not (Test-Path -LiteralPath $DataRoot)) { throw "DataRoot does not exist: $DataRoot" }
if ($Stride -lt 1) { throw "Stride must be at least 1" }
if ($StationNames.Count -gt 0) {
$Stations = @($StationNames | ForEach-Object { Get-Item -LiteralPath (Join-Path $DataRoot $_) })
} else {
$Stations = @(Get-ChildItem -LiteralPath $DataRoot -Directory | Where-Object {
(Test-Path -LiteralPath (Join-Path $_.FullName "dobject")) -and
(Test-Path -LiteralPath (Join-Path $_.FullName "dobject_recording"))
} | Sort-Object Name)
}
if ($Stations.Count -eq 0) { throw "No station directory containing dobject and dobject_recording was found" }
New-Item -ItemType Directory -Force -Path $OutputRoot | Out-Null
foreach ($Station in $Stations) {
$Out = Join-Path $OutputRoot $Station.Name
Write-Host "[legacy station $($Station.Name)]"
& python $Exporter --dlog $Station.FullName --out $Out --object $LidarObject --format npz `
--timezone $Timezone --stride $Stride --compress --rtk-sidecars --write-reports --resume `
--rtk-max-dt-ms $RtkMaxDtMs
if ($LASTEXITCODE -ne 0) { throw "Export failed for station $($Station.Name)" }
}
Write-Host "Exported stations: $($Stations.Count)"
-21
View File
@@ -1,21 +0,0 @@
param(
[Parameter(Mandatory = $true)][string]$ExportRoot,
[Parameter(Mandatory = $true)][string]$Output,
[Parameter(Mandatory = $true)][double]$HeadingOffsetDeg,
[Parameter(Mandatory = $true)][double[]]$AntennaLever,
[int]$MinStations = 30,
[int]$ExpectedStations = 0,
[double]$HeadingStdLimitDeg = [double]::PositiveInfinity,
[switch]$Overwrite
)
$ErrorActionPreference = "Stop"
if ($AntennaLever.Count -ne 3) { throw "AntennaLever must contain X,Y,Z in body coordinates" }
$Repo = Split-Path -Parent $PSScriptRoot
$Args = @((Join-Path $Repo "tools\prepare_station_dataset.py"), "--export-root", $ExportRoot,
"--output", $Output, "--heading-offset-deg", "$HeadingOffsetDeg", "--antenna-lever") +
@($AntennaLever | ForEach-Object { "$_" }) + @("--min-stations", "$MinStations",
"--expected-stations", "$ExpectedStations", "--heading-std-limit-deg", "$HeadingStdLimitDeg")
if ($Overwrite) { $Args += "--overwrite" }
& python @Args
if ($LASTEXITCODE -ne 0) { throw "Legacy dataset preparation failed" }
+2 -1
View File
@@ -3,6 +3,7 @@ param(
[Parameter(Mandatory = $true)][string]$Output,
[Parameter(Mandatory = $true)][double]$HeadingOffsetDeg,
[Parameter(Mandatory = $true)][double[]]$AntennaLever,
[string]$PoseName = "rtk_gga_raw_heading",
[int]$MinStations = 30,
[int]$ExpectedStations = 0,
[double]$HeadingStdLimitDeg = 0.5,
@@ -13,7 +14,7 @@ $ErrorActionPreference = "Stop"
if ($AntennaLever.Count -ne 3) { throw "AntennaLever must contain X,Y,Z in body coordinates" }
$Repo = Split-Path -Parent $PSScriptRoot
$Args = @((Join-Path $Repo "tools\prepare_multisensor_station_dataset.py"), "--combined-root", $CombinedRoot,
"--output", $Output, "--heading-offset-deg", "$HeadingOffsetDeg", "--antenna-lever") +
"--output", $Output, "--pose-name", $PoseName, "--heading-offset-deg", "$HeadingOffsetDeg", "--antenna-lever") +
@($AntennaLever | ForEach-Object { "$_" }) + @("--min-stations", "$MinStations",
"--expected-stations", "$ExpectedStations", "--heading-std-limit-deg", "$HeadingStdLimitDeg")
if ($Overwrite) { $Args += "--overwrite" }
-74
View File
@@ -1,74 +0,0 @@
param(
[Parameter(Mandatory = $true)][string]$Batch1Prepared,
[Parameter(Mandatory = $true)][string]$Batch2Prepared,
[string]$OutputRoot = ""
)
$ErrorActionPreference = "Stop"
$Repo = Split-Path -Parent $PSScriptRoot
if ([string]::IsNullOrWhiteSpace($OutputRoot)) {
$OutputRoot = Join-Path $Repo "results\01_previous_two_batches"
}
$Code = Join-Path $Repo "code\rigorous_calibration.py"
$Refine = Join-Path $Repo "code\refine_pairs.py"
$Summary = Join-Path $Repo "code\summarize_results.py"
$B1Frames = Join-Path $Batch1Prepared "frames_all"
$B2Frames = Join-Path $Batch2Prepared "frames_all"
$B1Body = Join-Path $Batch1Prepared "body_poses_rear_gga_raw_rear_to_front.csv"
$B2Body = Join-Path $Batch2Prepared "body_poses_rear_gga_raw_rear_to_front.csv"
function Run-Python {
param([string]$Stage, [string[]]$Arguments)
Write-Host "[$Stage]"
& python @Arguments
if ($LASTEXITCODE -ne 0) { throw "$Stage failed with Python exit code $LASTEXITCODE" }
}
foreach ($Path in @($B1Frames, $B2Frames, $B1Body, $B2Body)) {
if (-not (Test-Path -LiteralPath $Path)) { throw "Input does not exist: $Path" }
}
$Common = Join-Path $OutputRoot "common"
$Small = Join-Path $OutputRoot "small_gicp"
$Open = Join-Path $OutputRoot "open3d_gicp"
New-Item -ItemType Directory -Force -Path $Common,$Small,$Open | Out-Null
Run-Python "batch2 ground planes" @($Code, "ground", "--frames", $B2Frames, "--output", (Join-Path $Common "ground_planes_batch2.csv"))
foreach ($Backend in @("small_gicp", "open3d")) {
$Directory = if ($Backend -eq "small_gicp") { $Small } else { $Open }
$Extra = @()
if ($Backend -eq "open3d") { $Extra = @("--max-gap", "3", "--multistart", "1", "--iterations", "40") }
$B2Raw = Join-Path $Directory "B_batch2_estimation.npz"
$B2QualityJson = Join-Path $Directory "B_batch2_quality.json"
$B2QualityCsv = Join-Path $Directory "B_batch2_quality.csv"
Run-Python "$Backend batch2 pairs" (@($Code, "pairs", "--backend", $Backend, "--frames", $B2Frames, "--body", $B2Body,
"--output", $B2Raw, "--quality-json", $B2QualityJson, "--quality-csv", $B2QualityCsv) + $Extra)
$B2Refined = Join-Path $Directory "B_batch2_refined.npz"
Run-Python "$Backend batch2 refinement" @($Refine, "--pairs", $B2Raw, "--quality-json", $B2QualityJson, "--output", $B2Refined)
$Extrinsic = Join-Path $Directory "extrinsic_batch2_refined.json"
Run-Python "$Backend batch2 calibration" @($Code, "calibrate", "--pairs", $B2Refined,
"--ground-planes", (Join-Path $Common "ground_planes_batch2.csv"), "--output", $Extrinsic)
$B1Raw = Join-Path $Directory "B_batch1_auxiliary.npz"
$B1QualityJson = Join-Path $Directory "B_batch1_quality.json"
$B1QualityCsv = Join-Path $Directory "B_batch1_quality.csv"
Run-Python "$Backend batch1 auxiliary pairs" @($Code, "pairs", "--backend", $Backend, "--frames", $B1Frames, "--body", $B1Body,
"--output", $B1Raw, "--quality-json", $B1QualityJson, "--quality-csv", $B1QualityCsv,
"--max-gap", "3", "--multistart", "1", "--iterations", "40")
$B1Refined = Join-Path $Directory "B_batch1_auxiliary_refined.npz"
Run-Python "$Backend batch1 refinement" @($Refine, "--pairs", $B1Raw, "--quality-json", $B1QualityJson, "--output", $B1Refined)
Run-Python "$Backend batch1 auxiliary check" @($Code, "validate", "--pairs", $B1Refined,
"--extrinsic", $Extrinsic, "--output", (Join-Path $Directory "batch1_auxiliary_check.json"))
}
Run-Python "backend summary" @($Summary,
"--open3d", (Join-Path $Open "extrinsic_batch2_refined.json"),
"--small", (Join-Path $Small "extrinsic_batch2_refined.json"),
"--open3d-quality", (Join-Path $Open "B_batch2_quality.json"),
"--small-quality", (Join-Path $Small "B_batch2_quality.json"),
"--open3d-check", (Join-Path $Open "batch1_auxiliary_check.json"),
"--small-check", (Join-Path $Small "batch1_auxiliary_check.json"),
"--output", (Join-Path $OutputRoot "comparison_summary.json"),
"--recommended-output", (Join-Path $OutputRoot "final_extrinsic_recommended.json"))
Write-Host "Two-batch results: $OutputRoot"
-34
View File
@@ -1,34 +0,0 @@
param([string]$ResultRoot = "")
$ErrorActionPreference = "Stop"
$Repo = Split-Path -Parent $PSScriptRoot
if ([string]::IsNullOrWhiteSpace($ResultRoot)) { $ResultRoot = Join-Path $Repo "results\01_previous_two_batches" }
$Code = Join-Path $Repo "code\rigorous_calibration.py"
$Filter = Join-Path $Repo "code\cross_backend_filter.py"
$Finalize = Join-Path $Repo "code\finalize_consensus.py"
function Run-Python {
param([string[]]$Arguments)
& python @Arguments
if ($LASTEXITCODE -ne 0) { throw "Python failed with exit code $LASTEXITCODE" }
}
Run-Python @($Filter, "--open3d-pairs", (Join-Path $ResultRoot "open3d_gicp\B_batch2_refined.npz"),
"--small-pairs", (Join-Path $ResultRoot "small_gicp\B_batch2_refined.npz"),
"--output", (Join-Path $ResultRoot "consensus\B_batch2_consensus.npz"))
Run-Python @($Filter, "--open3d-pairs", (Join-Path $ResultRoot "open3d_gicp\B_batch1_auxiliary_refined.npz"),
"--small-pairs", (Join-Path $ResultRoot "small_gicp\B_batch1_auxiliary_refined.npz"),
"--output", (Join-Path $ResultRoot "consensus\B_batch1_consensus.npz"), "--min-pairs", "15")
Run-Python @($Code, "calibrate", "--pairs", (Join-Path $ResultRoot "consensus\B_batch2_consensus.npz"),
"--ground-planes", (Join-Path $ResultRoot "common\ground_planes_batch2.csv"),
"--output", (Join-Path $ResultRoot "consensus\extrinsic_batch2_consensus.json"))
Run-Python @($Code, "validate", "--pairs", (Join-Path $ResultRoot "consensus\B_batch1_consensus.npz"),
"--extrinsic", (Join-Path $ResultRoot "consensus\extrinsic_batch2_consensus.json"),
"--output", (Join-Path $ResultRoot "consensus\batch1_auxiliary_check.json"))
Run-Python @($Finalize,
"--consensus-extrinsic", (Join-Path $ResultRoot "consensus\extrinsic_batch2_consensus.json"),
"--consensus-check", (Join-Path $ResultRoot "consensus\batch1_auxiliary_check.json"),
"--open3d-extrinsic", (Join-Path $ResultRoot "open3d_gicp\extrinsic_batch2_refined.json"),
"--small-extrinsic", (Join-Path $ResultRoot "small_gicp\extrinsic_batch2_refined.json"),
"--output", (Join-Path $ResultRoot "final_extrinsic_recommended.json"),
"--summary", (Join-Path $ResultRoot "final_summary.json"))
+36
View File
@@ -0,0 +1,36 @@
param(
[Parameter(Mandatory = $true)][string]$CombinedRoot,
[string]$OutputRoot = "",
[string]$WorkRoot = "",
[double]$RtkReferenceHeightAboveGroundM = 0.8535,
[int]$ExpectedStations = 34,
[int]$MinPairs = 20,
[int]$Bootstrap = 200
)
$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" }
$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
if ($LASTEXITCODE -ne 0) { throw "RTK-direct dataset preparation failed" }
& (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
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"
)
& python @Finalize
if ($LASTEXITCODE -ne 0) { throw "Final result packaging failed" }
Write-Host "Final T_RTK_lidar: $(Join-Path $OutputRoot 'final_T_RTK_lidar.json')"
+32
View File
@@ -0,0 +1,32 @@
param(
[Parameter(Mandatory = $true)][string]$DataRoot,
[Parameter(Mandatory = $true)][string]$RtkCapture,
[Parameter(Mandatory = $true)][string]$ImuCapture,
[Parameter(Mandatory = $true)][string]$OutputRoot,
[string]$LidarObject = "frontlidar",
[string]$Timezone = "+08:00",
[double]$RtkReferenceHeightAboveGroundM = 0.8535,
[int]$ExpectedStations = 34,
[int]$MinPairs = 20,
[int]$Bootstrap = 200
)
$ErrorActionPreference = "Stop"
$ExportRoot = Join-Path $OutputRoot "exported"
$PreparedRoot = Join-Path $OutputRoot "prepared_rtk_direct"
$CalibrationRoot = Join-Path $OutputRoot "calibration"
& (Join-Path $PSScriptRoot "export_multisensor_stations.ps1") `
-DataRoot $DataRoot -RtkCapture $RtkCapture -ImuCapture $ImuCapture `
-OutputRoot $ExportRoot -LidarObject $LidarObject -Timezone $Timezone
if ($LASTEXITCODE -ne 0) { throw "Raw-data export failed" }
& (Join-Path $PSScriptRoot "run_direct_rtk_lidar.ps1") `
-CombinedRoot (Join-Path $ExportRoot "combined") `
-WorkRoot $PreparedRoot -OutputRoot $CalibrationRoot `
-RtkReferenceHeightAboveGroundM $RtkReferenceHeightAboveGroundM `
-ExpectedStations $ExpectedStations -MinPairs $MinPairs -Bootstrap $Bootstrap
if ($LASTEXITCODE -ne 0) { throw "RTK-LiDAR calibration failed" }
Write-Host "Final result: $(Join-Path $CalibrationRoot 'final_T_RTK_lidar.json')"
Write-Host "Prepared frames: $(Join-Path $PreparedRoot 'frames_all')"
-22
View File
@@ -1,22 +0,0 @@
param(
[int]$PairIndex = 0,
[string]$ResultRoot = ""
)
$ErrorActionPreference = "Stop"
$Repo = Split-Path -Parent $PSScriptRoot
if ([string]::IsNullOrWhiteSpace($ResultRoot)) { $ResultRoot = Join-Path $Repo "results\01_previous_two_batches" }
$Code = Join-Path $Repo "code\scan_extrinsic_sensitivity.py"
$X = Join-Path $ResultRoot "final_extrinsic_recommended.json"
$Ground = Join-Path $ResultRoot "common\ground_planes_batch2.csv"
$Out = Join-Path $ResultRoot "diagnostics"
foreach ($Backend in @("open3d_gicp", "small_gicp")) {
$Label = if ($Backend -eq "open3d_gicp") { "open3d_refined" } else { "small_gicp_refined" }
& python $Code --pairs (Join-Path $ResultRoot "$Backend\B_batch2_refined.npz") --extrinsic $X `
--ground-planes $Ground --pair-index $PairIndex --output (Join-Path $Out "${Label}_pair${PairIndex}_left_rpy_scan.json")
if ($LASTEXITCODE -ne 0) { throw "$Backend sensitivity scan failed" }
}
& python $Code --pairs (Join-Path $ResultRoot "consensus\B_batch2_consensus.npz") --extrinsic $X `
--ground-planes $Ground --pair-index $PairIndex --output (Join-Path $Out "consensus_pair${PairIndex}_left_rpy_scan.json")
if ($LASTEXITCODE -ne 0) { throw "Consensus sensitivity scan failed" }
+7 -7
View File
@@ -1,8 +1,8 @@
param(
[Parameter(Mandatory = $true)][string]$Prepared,
[Parameter(Mandatory = $true)][string]$OutputRoot,
[string]$PoseName = "rear_gga_raw_rear_to_front",
[double]$BodyHeight = 0.2335,
[string]$ReferencePoseFile = "reference_poses_rtk_gga_raw_heading.csv",
[double]$ReferenceHeight = 0.8535,
[int]$MinPairs = 20,
[int]$Bootstrap = 100
)
@@ -13,7 +13,7 @@ $Code = Join-Path $Repo "code\rigorous_calibration.py"
$Refine = Join-Path $Repo "code\refine_pairs.py"
$Consensus = Join-Path $Repo "code\cross_backend_filter.py"
$Frames = Join-Path $Prepared "frames_all"
$Body = Join-Path $Prepared "body_poses_$PoseName.csv"
$ReferencePoses = Join-Path $Prepared $ReferencePoseFile
$Common = Join-Path $OutputRoot "common"
$Open = Join-Path $OutputRoot "open3d_gicp"
$Small = Join-Path $OutputRoot "small_gicp"
@@ -26,7 +26,7 @@ function Run-Python {
if ($LASTEXITCODE -ne 0) { throw "$Stage failed with Python exit code $LASTEXITCODE" }
}
foreach ($Path in @($Frames, $Body)) {
foreach ($Path in @($Frames, $ReferencePoses)) {
if (-not (Test-Path -LiteralPath $Path)) { throw "Input does not exist: $Path" }
}
New-Item -ItemType Directory -Force -Path $Common,$Open,$Small,$ConsensusOut | Out-Null
@@ -39,7 +39,7 @@ foreach ($Backend in @("small_gicp", "open3d")) {
$Raw = Join-Path $Directory "B_estimation.npz"
$QualityJson = Join-Path $Directory "B_quality.json"
$QualityCsv = Join-Path $Directory "B_quality.csv"
$PairArgs = @($Code, "pairs", "--backend", $Backend, "--frames", $Frames, "--body", $Body,
$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") }
@@ -50,7 +50,7 @@ foreach ($Backend in @("small_gicp", "open3d")) {
)
Run-Python "$Backend calibration" @(
$Code, "calibrate", "--pairs", (Join-Path $Directory "B_refined.npz"),
"--ground-planes", $Ground, "--body-height", "$BodyHeight",
"--ground-planes", $Ground, "--reference-height", "$ReferenceHeight",
"--bootstrap", "$Bootstrap", "--output", (Join-Path $Directory "extrinsic.json")
)
}
@@ -63,7 +63,7 @@ Run-Python "cross-backend consensus" @(
)
Run-Python "consensus calibration" @(
$Code, "calibrate", "--pairs", $ConsensusPairs, "--ground-planes", $Ground,
"--body-height", "$BodyHeight", "--bootstrap", "$Bootstrap",
"--reference-height", "$ReferenceHeight", "--bootstrap", "$Bootstrap",
"--output", (Join-Path $ConsensusOut "extrinsic.json")
)