104 lines
4.4 KiB
PowerShell
104 lines
4.4 KiB
PowerShell
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"
|