新增雷达到RTK直接手眼标定流程

2026-07-24 00:06:50 +08:00
parent f72fcb71cc
commit d2aae6177e
43 changed files with 9113 additions and 0 deletions
@@ -0,0 +1,70 @@
+param(
+    [Parameter(Mandatory = $true)][string]$Prepared,
+    [Parameter(Mandatory = $true)][string]$OutputRoot,
+    [string]$ReferencePoseFile = "reference_poses_rtk_gga_raw_heading.csv",
+    [double]$ReferenceHeight = 0.8535,
+    [int]$MinPairs = 20,
+    [int]$Bootstrap = 100
+)
+
+$ErrorActionPreference = "Stop"
+$Repo = Split-Path -Parent $PSScriptRoot
+$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"
+$ReferencePoses = Join-Path $Prepared $ReferencePoseFile
+$Common = Join-Path $OutputRoot "common"
+$Open = Join-Path $OutputRoot "open3d_gicp"
+$Small = Join-Path $OutputRoot "small_gicp"
+$ConsensusOut = Join-Path $OutputRoot "consensus"
+
+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 @($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
+
+$Ground = Join-Path $Common "ground_planes.csv"
+Run-Python "ground planes" @($Code, "ground", "--frames", $Frames, "--output", $Ground)
+
+foreach ($Backend in @("small_gicp", "open3d")) {
+    $Directory = if ($Backend -eq "small_gicp") { $Small } else { $Open }
+    $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, "--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") }
+    Run-Python "$Backend pairs" $PairArgs
+    Run-Python "$Backend X-independent refinement" @(
+        $Refine, "--pairs", $Raw, "--quality-json", $QualityJson,
+        "--output", (Join-Path $Directory "B_refined.npz"), "--min-pairs", "$MinPairs"
+    )
+    Run-Python "$Backend calibration" @(
+        $Code, "calibrate", "--pairs", (Join-Path $Directory "B_refined.npz"),
+        "--ground-planes", $Ground, "--reference-height", "$ReferenceHeight",
+        "--bootstrap", "$Bootstrap", "--output", (Join-Path $Directory "extrinsic.json")
+    )
+}
+
+$ConsensusPairs = Join-Path $ConsensusOut "B_consensus.npz"
+Run-Python "cross-backend consensus" @(
+    $Consensus, "--open3d-pairs", (Join-Path $Open "B_refined.npz"),
+    "--small-pairs", (Join-Path $Small "B_refined.npz"),
+    "--output", $ConsensusPairs, "--min-pairs", "$MinPairs"
+)
+Run-Python "consensus calibration" @(
+    $Code, "calibrate", "--pairs", $ConsensusPairs, "--ground-planes", $Ground,
+    "--reference-height", "$ReferenceHeight", "--bootstrap", "$Bootstrap",
+    "--output", (Join-Path $ConsensusOut "extrinsic.json")
+)
+
+Write-Host "Calibration results: $OutputRoot"