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

2026-07-28 15:36:00 +08:00
parent e847cd4590
commit 24eaa8508e
9 changed files with 289 additions and 20 deletions
@@ -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"