修正基线系标定默认：机械初值、地面ROI与航向偏移可配，并补充G90窗导出与契约测试
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
lichun.qu
Cursor
@@ -26,3 +26,25 @@ python tools\export_raw_to_combined.py --stations-root ... --rtk-rscap ... --imu
|
||||
- `-TimeBasis host`：旧「已解码点云」dlog + 主机接收时间关联
|
||||
|
||||
所有路径均为命令行参数。标定入口要求显式传入RTK/GGA参考点离地高度，避免静默使用与实车不符的默认值；默认生成目录`work/`和`outputs/`不会提交Git。
|
||||
|
||||
## G90 + H32 连续录制数据按站导出
|
||||
|
||||
如果各站不是独立目录，而是记录在多个 Medulla DLog ZIP 和 G90 `.rscap` 中，使用：
|
||||
|
||||
```powershell
|
||||
python tools\export_g90_h32_windows_to_combined.py `
|
||||
--segments-csv <rtk_lidar_station_segments.csv> `
|
||||
--lidar-dlog <dump_1.zip> --lidar-dlog <dump_2.zip> `
|
||||
--rtk-rscap <g90_1.rscap> --rtk-rscap <g90_2.rscap> `
|
||||
--out <output_root> --expected-stations 27 --frame-stride 5
|
||||
```
|
||||
|
||||
该入口使用 MSOP 的主机接收 UTC 与 G90 串口主机接收 UTC 做近邻关联，同时保留雷达包内设备时间作为审计字段。它只读取原始文件，生成 `export/`、`combined/` 和摘要，不使用 IMU。`--frame-stride` 仅控制参与标定的点云帧密度，不改变站点时间窗。
|
||||
|
||||
如果雷达 ZIP 扫描已经完成但后续步骤中断，可保留 `export/` 并续跑：
|
||||
|
||||
```powershell
|
||||
python tools\export_g90_h32_windows_to_combined.py --reuse-export --segments-csv <csv> --rtk-rscap <g90.rscap> --out <output_root> --expected-stations 27
|
||||
```
|
||||
|
||||
在尚未测得 GGA/ANT1 相位中心离地高度时，可先生成 `combined/`，再运行 `prepare_multisensor_dataset.ps1` 得到 `prepared/`。最终外参求解仍需显式传入真实离地高度，不应留空或猜测。
|
||||
|
||||
@@ -0,0 +1,23 @@
|
||||
{
|
||||
"schema_version": 1,
|
||||
"convention": "T_RTK_lidar maps raw LiDAR points into the RTK baseline frame (X = rawHeading baseline, Y left, Z up; heading_offset_deg = 0)",
|
||||
"translation_m": [
|
||||
0.414179474,
|
||||
0.210859360,
|
||||
0.004000001
|
||||
],
|
||||
"rotation_rpy_deg_xyz": [
|
||||
0.0,
|
||||
0.0,
|
||||
90.0
|
||||
],
|
||||
"matrix_4x4": [
|
||||
[0.0, -1.0, 0.0, 0.414179474],
|
||||
[1.0, 0.0, 0.0, 0.210859360],
|
||||
[0.0, 0.0, 1.0, 0.004000001],
|
||||
[0.0, 0.0, 0.0, 1.0]
|
||||
],
|
||||
"use": "Final AX=XB solver initialization only; never use for LiDAR pair registration",
|
||||
"yaw_note": "≈90 deg yaw is expected when LiDAR X is vehicle-forward and the dual-antenna baseline is left-right",
|
||||
"z_note": "CAD/mechanical z only; final z is constrained by measured GGA/ANT1 phase-center height above ground"
|
||||
}
|
||||
@@ -3,32 +3,66 @@ param(
|
||||
[Parameter(Mandatory = $true)][double]$RtkReferenceHeightAboveGroundM,
|
||||
[string]$OutputRoot = "",
|
||||
[string]$WorkRoot = "",
|
||||
[int]$ExpectedStations = 34,
|
||||
[int]$ExpectedStations = 27,
|
||||
[int]$MinStations = 20,
|
||||
[int]$MinPairs = 20,
|
||||
[int]$Bootstrap = 200
|
||||
[int]$Bootstrap = 200,
|
||||
# Roof-mounted H32 (~2 m): ground points are near z≈-2 in the LiDAR frame (Z-up).
|
||||
# The old [-1.4, -0.4] window fits walls on this vehicle and must not be reused.
|
||||
[double]$GroundZMin = -2.5,
|
||||
[double]$GroundZMax = -1.5,
|
||||
[int]$SmallGicpMaxGap = 26,
|
||||
[int]$Open3DMaxGap = 26,
|
||||
[double]$MaxReferenceTranslationM = 8.0,
|
||||
# Baseline frame: rawHeading as RTK X. Use 90 only when deliberately targeting vehicle-forward.
|
||||
[double]$HeadingOffsetDeg = 0.0,
|
||||
[string]$SolverInitialExtrinsic = "",
|
||||
[double]$RefineMinInlierRatio = 0.63,
|
||||
[double]$RefineMaxInlierRmseM = 0.14
|
||||
)
|
||||
|
||||
$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" }
|
||||
if ([string]::IsNullOrWhiteSpace($SolverInitialExtrinsic)) {
|
||||
$SolverInitialExtrinsic = Join-Path $PSScriptRoot "rtk_lidar_mechanical_initial.json"
|
||||
}
|
||||
|
||||
$PoseName = if ([math]::Abs($HeadingOffsetDeg) -le 1e-12) {
|
||||
"rtk_gga_raw_heading"
|
||||
} else {
|
||||
"rtk_vehicle_heading"
|
||||
}
|
||||
$ReferencePoseFile = "reference_poses_${PoseName}.csv"
|
||||
$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
|
||||
-CombinedRoot $CombinedRoot -Output $Prepared -HeadingOffsetDeg $HeadingOffsetDeg `
|
||||
-AntennaLever @(0.0,0.0,0.0) -PoseName $PoseName -MinStations $MinStations `
|
||||
-ExpectedStations $ExpectedStations -Overwrite
|
||||
if ($LASTEXITCODE -ne 0) { throw "RTK-direct dataset preparation failed" }
|
||||
|
||||
# Pair registration intentionally has no --initial-extrinsic (B must stay X-independent).
|
||||
# SolverInitialExtrinsic is applied only in the final AX=XB calibrate stage.
|
||||
& (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
|
||||
-ReferencePoseFile $ReferencePoseFile `
|
||||
-ReferenceHeight $RtkReferenceHeightAboveGroundM `
|
||||
-MinStations $MinStations -MinPairs $MinPairs -Bootstrap $Bootstrap `
|
||||
-GroundZMin $GroundZMin -GroundZMax $GroundZMax `
|
||||
-SmallGicpMaxGap $SmallGicpMaxGap -Open3DMaxGap $Open3DMaxGap `
|
||||
-MaxReferenceTranslationM $MaxReferenceTranslationM `
|
||||
-SolverInitialExtrinsic $SolverInitialExtrinsic `
|
||||
-RefineMinInlierRatio $RefineMinInlierRatio `
|
||||
-RefineMaxInlierRmseM $RefineMaxInlierRmseM
|
||||
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"
|
||||
"--reference-height", "$RtkReferenceHeightAboveGroundM",
|
||||
"--heading-offset-deg", "$HeadingOffsetDeg"
|
||||
)
|
||||
& python @Finalize
|
||||
if ($LASTEXITCODE -ne 0) { throw "Final result packaging failed" }
|
||||
|
||||
@@ -9,8 +9,11 @@
|
||||
[string]$Timezone = "+08:00",
|
||||
[ValidateSet("device_gnss", "host")][string]$TimeBasis = "device_gnss",
|
||||
[int]$ExpectedStations = 34,
|
||||
[int]$MinStations = 20,
|
||||
[int]$MinPairs = 20,
|
||||
[int]$Bootstrap = 200
|
||||
[int]$Bootstrap = 200,
|
||||
[double]$GroundZMin = -1.4,
|
||||
[double]$GroundZMax = -0.4
|
||||
)
|
||||
|
||||
$ErrorActionPreference = "Stop"
|
||||
@@ -28,7 +31,9 @@ if ($LASTEXITCODE -ne 0) { throw "Raw-data export failed" }
|
||||
-CombinedRoot (Join-Path $ExportRoot "combined") `
|
||||
-WorkRoot $PreparedRoot -OutputRoot $CalibrationRoot `
|
||||
-RtkReferenceHeightAboveGroundM $RtkReferenceHeightAboveGroundM `
|
||||
-ExpectedStations $ExpectedStations -MinPairs $MinPairs -Bootstrap $Bootstrap
|
||||
-MinStations $MinStations `
|
||||
-ExpectedStations $ExpectedStations -MinPairs $MinPairs -Bootstrap $Bootstrap `
|
||||
-GroundZMin $GroundZMin -GroundZMax $GroundZMax
|
||||
if ($LASTEXITCODE -ne 0) { throw "RTK-LiDAR calibration failed" }
|
||||
|
||||
Write-Host "Final result: $(Join-Path $CalibrationRoot 'final_T_RTK_lidar.json')"
|
||||
|
||||
@@ -3,8 +3,18 @@ param(
|
||||
[Parameter(Mandatory = $true)][string]$OutputRoot,
|
||||
[Parameter(Mandatory = $true)][double]$ReferenceHeight,
|
||||
[string]$ReferencePoseFile = "reference_poses_rtk_gga_raw_heading.csv",
|
||||
[int]$MinStations = 20,
|
||||
[int]$MinPairs = 20,
|
||||
[int]$Bootstrap = 100
|
||||
[int]$Bootstrap = 100,
|
||||
# Roof-mounted H32 (~2 m): ground near z≈-2. Old [-1.4,-0.4] fits walls on this vehicle.
|
||||
[double]$GroundZMin = -2.5,
|
||||
[double]$GroundZMax = -1.5,
|
||||
[int]$SmallGicpMaxGap = 26,
|
||||
[int]$Open3DMaxGap = 26,
|
||||
[double]$MaxReferenceTranslationM = 8.0,
|
||||
[string]$SolverInitialExtrinsic = "",
|
||||
[double]$RefineMinInlierRatio = 0.63,
|
||||
[double]$RefineMaxInlierRmseM = 0.14
|
||||
)
|
||||
|
||||
$ErrorActionPreference = "Stop"
|
||||
@@ -32,27 +42,38 @@ foreach ($Path in @($Frames, $ReferencePoses)) {
|
||||
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)
|
||||
Run-Python "ground planes" @($Code, "ground", "--frames", $Frames, "--output", $Ground,
|
||||
"--z-min", "$GroundZMin", "--z-max", "$GroundZMax")
|
||||
|
||||
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"
|
||||
$MaxGap = if ($Backend -eq "open3d") { $Open3DMaxGap } else { $SmallGicpMaxGap }
|
||||
$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") }
|
||||
"--min-stations", "$MinStations", "--min-pairs", "$MinPairs", "--max-gap", "$MaxGap")
|
||||
if ($MaxReferenceTranslationM -gt 0) {
|
||||
$PairArgs += @("--max-reference-translation", "$MaxReferenceTranslationM")
|
||||
}
|
||||
if ($Backend -eq "open3d") { $PairArgs += @("--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"
|
||||
"--output", (Join-Path $Directory "B_refined.npz"), "--min-pairs", "$MinPairs",
|
||||
"--min-inlier-ratio", "$RefineMinInlierRatio",
|
||||
"--max-inlier-rmse", "$RefineMaxInlierRmseM"
|
||||
)
|
||||
Run-Python "$Backend calibration" @(
|
||||
$CalibrationArgs = @(
|
||||
$Code, "calibrate", "--pairs", (Join-Path $Directory "B_refined.npz"),
|
||||
"--ground-planes", $Ground, "--reference-height", "$ReferenceHeight",
|
||||
"--bootstrap", "$Bootstrap", "--output", (Join-Path $Directory "extrinsic.json")
|
||||
)
|
||||
if (-not [string]::IsNullOrWhiteSpace($SolverInitialExtrinsic)) {
|
||||
$CalibrationArgs += @("--initial-extrinsic", $SolverInitialExtrinsic)
|
||||
}
|
||||
Run-Python "$Backend calibration" $CalibrationArgs
|
||||
}
|
||||
|
||||
$ConsensusPairs = Join-Path $ConsensusOut "B_consensus.npz"
|
||||
@@ -61,10 +82,14 @@ Run-Python "cross-backend consensus" @(
|
||||
"--small-pairs", (Join-Path $Small "B_refined.npz"),
|
||||
"--output", $ConsensusPairs, "--min-pairs", "$MinPairs"
|
||||
)
|
||||
Run-Python "consensus calibration" @(
|
||||
$ConsensusCalibrationArgs = @(
|
||||
$Code, "calibrate", "--pairs", $ConsensusPairs, "--ground-planes", $Ground,
|
||||
"--reference-height", "$ReferenceHeight", "--bootstrap", "$Bootstrap",
|
||||
"--output", (Join-Path $ConsensusOut "extrinsic.json")
|
||||
)
|
||||
if (-not [string]::IsNullOrWhiteSpace($SolverInitialExtrinsic)) {
|
||||
$ConsensusCalibrationArgs += @("--initial-extrinsic", $SolverInitialExtrinsic)
|
||||
}
|
||||
Run-Python "consensus calibration" $ConsensusCalibrationArgs
|
||||
|
||||
Write-Host "Calibration results: $OutputRoot"
|
||||
|
||||
Reference in New Issue
Block a user