新增雷达到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,14 @@
+# run目录
+
+根README包含完整复现命令；这里仅列入口职责。
+
+| 脚本 | 用途 |
+|---|---|
+| `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运动对对比并打印数值增量 |
+
+所有路径均为命令行参数；默认生成目录`work/`和`outputs/`不会提交Git。
@@ -0,0 +1,89 @@
+param(
+    [Parameter(Mandatory = $true)][string]$DataRoot,
+    [Parameter(Mandatory = $true)][string]$OutputRoot,
+    [Parameter(Mandatory = $true)][string]$RtkCapture,
+    [Parameter(Mandatory = $true)][string]$ImuCapture,
+    [string]$LidarObject = "frontlidar",
+    [string]$Timezone = "+08:00",
+    [string[]]$StationNames = @(),
+    [int]$Stride = 1,
+    [double]$RtkMaxDtMs = 150.0,
+    [double]$ImuBeforeMs = 100.0,
+    [double]$ImuAfterMs = 100.0,
+    [switch]$SkipLidarExport,
+    [switch]$SkipSerialParsing
+)
+
+$ErrorActionPreference = "Stop"
+$RepoRoot = Split-Path -Parent $PSScriptRoot
+$Exporter = Join-Path $RepoRoot "tools\frontlidar_dlog_export.py"
+$Builder = Join-Path $RepoRoot "tools\build_multisensor_npz.py"
+$Parser = Join-Path $RepoRoot "tools\rscap_v2\parse_rtk_imu_v2.py"
+$Auditor = Join-Path $RepoRoot "tools\rscap_v2\audit_capture_v2.py"
+$ExportRoot = Join-Path $OutputRoot "export"
+$ParsedRoot = Join-Path $OutputRoot "parsed"
+$CombinedRoot = Join-Path $OutputRoot "combined"
+
+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 @($DataRoot, $RtkCapture, $ImuCapture)) {
+    if (-not (Test-Path -LiteralPath $Path)) { throw "Input does not exist: $Path" }
+}
+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
+if (-not $SkipSerialParsing) {
+    New-Item -ItemType Directory -Force -Path $ParsedRoot | Out-Null
+    Run-Python "capture audit" @($Auditor, $RtkCapture, $ImuCapture, "--out", (Join-Path $OutputRoot "capture_audit.json"))
+    Run-Python "RTK/IMU parse" @($Parser, "--rtk", $RtkCapture, "--imu", $ImuCapture, "--out", $ParsedRoot)
+}
+
+foreach ($Station in $Stations) {
+    $StationOut = Join-Path $ExportRoot $Station.Name
+    if (-not $SkipLidarExport) {
+        Run-Python "LiDAR station $($Station.Name)" @(
+            $Exporter, "--dlog", $Station.FullName, "--out", $StationOut,
+            "--object", $LidarObject, "--format", "npz", "--timezone", $Timezone,
+            "--stride", "$Stride", "--compress", "--skip-rtk", "--write-reports", "--resume"
+        )
+    }
+    if (-not (Test-Path -LiteralPath (Join-Path $StationOut "frames"))) {
+        throw "Exported frame directory is absent for station $($Station.Name): $StationOut"
+    }
+}
+
+$BuildArgs = @($Builder)
+foreach ($Station in $Stations) {
+    $Frames = Join-Path (Join-Path $ExportRoot $Station.Name) "frames"
+    $BuildArgs += @("--lidar", "$($Station.Name)=$Frames")
+}
+$BuildArgs += @(
+    "--rtk", (Join-Path $ParsedRoot "rtk.jsonl"),
+    "--imu", (Join-Path $ParsedRoot "imu.jsonl"),
+    "--out", $CombinedRoot,
+    "--rtk-max-dt-ms", "$RtkMaxDtMs",
+    "--imu-before-ms", "$ImuBeforeMs",
+    "--imu-after-ms", "$ImuAfterMs",
+    "--overwrite"
+)
+Run-Python "LiDAR/RTK/IMU association" $BuildArgs
+
+Write-Host "Completed stations: $($Stations.Count)"
+Write-Host "Combined NPZ: $CombinedRoot"
@@ -0,0 +1,22 @@
+param(
+    [Parameter(Mandatory = $true)][string]$CombinedRoot,
+    [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,
+    [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_multisensor_station_dataset.py"), "--combined-root", $CombinedRoot,
+    "--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" }
+& python @Args
+if ($LASTEXITCODE -ne 0) { throw "Multisensor dataset preparation failed" }
@@ -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')"
@@ -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')"
@@ -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"
@@ -0,0 +1,19 @@
+param(
+    [Parameter(Mandatory = $true)][string]$Frames,
+    [Parameter(Mandatory = $true)][string]$Pairs,
+    [Parameter(Mandatory = $true)][string]$Extrinsic,
+    [int]$PairIndex = 0,
+    [double]$LeftRollDeg = 0.0,
+    [double]$LeftPitchDeg = 0.0,
+    [double]$LeftYawDeg = 0.0
+)
+
+$ErrorActionPreference = "Stop"
+$Repo = Split-Path -Parent $PSScriptRoot
+foreach ($Path in @($Frames, $Pairs, $Extrinsic)) {
+    if (-not (Test-Path -LiteralPath $Path)) { throw "Input does not exist: $Path" }
+}
+& python (Join-Path $Repo "code\visualize_pair_3d.py") `
+    --frames $Frames --pairs $Pairs --extrinsic $Extrinsic --pair-index $PairIndex `
+    --left-rpy-deg $LeftRollDeg $LeftPitchDeg $LeftYawDeg
+if ($LASTEXITCODE -ne 0) { throw "Visualization failed with Python exit code $LASTEXITCODE" }