feat: add local cubic bezier smoother
This commit is contained in:
梁薄云
@@ -0,0 +1,278 @@
|
||||
param([string]$AssemblyPath = (Join-Path $PSScriptRoot '..\bin\Debug\netstandard2.0\ClumsyPilot.dll'))
|
||||
|
||||
$ErrorActionPreference = 'Stop'
|
||||
$assembly = [Reflection.Assembly]::LoadFrom((Resolve-Path $AssemblyPath))
|
||||
|
||||
function Assert-True($Actual, [string]$Message) {
|
||||
if (-not $Actual) { throw $Message }
|
||||
}
|
||||
|
||||
function Assert-Equal($Expected, $Actual, [string]$Message) {
|
||||
if ($Expected -ne $Actual) { throw "$Message Expected=$Expected Actual=$Actual" }
|
||||
}
|
||||
|
||||
function Assert-Near([double]$Expected, [double]$Actual, [double]$Tolerance, [string]$Message) {
|
||||
if ([Math]::Abs($Expected - $Actual) -gt $Tolerance) {
|
||||
throw "$Message Expected=$Expected Actual=$Actual Tolerance=$Tolerance"
|
||||
}
|
||||
}
|
||||
|
||||
function Get-RequiredType([string]$Name) {
|
||||
return $assembly.GetType($Name, $true)
|
||||
}
|
||||
|
||||
function Get-PropertyValue($Instance, [string]$Name) {
|
||||
$property = $Instance.GetType().GetProperty($Name, [Reflection.BindingFlags]'Instance,Public,NonPublic')
|
||||
Assert-True ($null -ne $property) ("Missing property: " + $Name)
|
||||
return $property.GetValue($Instance)
|
||||
}
|
||||
|
||||
function Assert-PointBitwiseEqual($Expected, $Actual, [string]$Message) {
|
||||
foreach ($name in @('X', 'Y', 'ArcLength', 'Heading', 'UnwrappedHeading', 'BodyClearance')) {
|
||||
$expectedBits = [BitConverter]::DoubleToInt64Bits([double]$Expected.$name)
|
||||
$actualBits = [BitConverter]::DoubleToInt64Bits([double]$Actual.$name)
|
||||
Assert-Equal $expectedBits $actualBits ($Message + ' ' + $name)
|
||||
}
|
||||
Assert-Equal $Expected.IsGearSwitchPoint $Actual.IsGearSwitchPoint ($Message + ' IsGearSwitchPoint')
|
||||
Assert-Equal $Expected.Source.ToString() $Actual.Source.ToString() ($Message + ' Source')
|
||||
}
|
||||
|
||||
function New-Point(
|
||||
[double]$X,
|
||||
[double]$Y,
|
||||
[double]$ArcLength,
|
||||
[double]$Heading,
|
||||
[double]$BodyClearance = 0.10,
|
||||
[bool]$IsGearSwitch = $false) {
|
||||
return [Activator]::CreateInstance($pointType, @(
|
||||
$X, $Y, $ArcLength, $Heading, $Heading, $BodyClearance, $IsGearSwitch, $anchor))
|
||||
}
|
||||
|
||||
function New-DirectionSegment(
|
||||
[int]$Index,
|
||||
$Direction,
|
||||
[object[]]$Points,
|
||||
[bool]$StartsAtGearSwitch = $false,
|
||||
[bool]$EndsAtGearSwitch = $false) {
|
||||
$typedPoints = [Array]::CreateInstance($pointType, $Points.Count)
|
||||
for ($pointIndex = 0; $pointIndex -lt $Points.Count; $pointIndex++) {
|
||||
$typedPoints.SetValue($Points[$pointIndex], $pointIndex)
|
||||
}
|
||||
return [Activator]::CreateInstance($segmentType, @(
|
||||
$Index, $Direction, $typedPoints, $StartsAtGearSwitch, $EndsAtGearSwitch))
|
||||
}
|
||||
|
||||
function New-EmptyMap {
|
||||
$request = [Activator]::CreateInstance($mapRequestType)
|
||||
$request.Bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]5000, [single]0, [single]5000))
|
||||
$request.ResolutionMm = [single]50
|
||||
$request.AllowExplicitEmptyMap = $true
|
||||
$map = [Activator]::CreateInstance($mapFactoryType).Create($request).Map
|
||||
Assert-True ($null -ne $map) 'Bézier test must create an explicit empty planning map.'
|
||||
return $map
|
||||
}
|
||||
|
||||
function New-AlgorithmInput(
|
||||
[object[]]$Segments,
|
||||
[double]$ReserveMeters = 0.02,
|
||||
[double]$CornerThresholdRadians = ([Math]::PI / 18.0),
|
||||
[double]$MaximumWindowLengthMeters = 0.60,
|
||||
[double]$HandleLengthRatio = (1.0 / 3.0)) {
|
||||
$typedSegments = [Array]::CreateInstance($segmentType, $Segments.Count)
|
||||
for ($index = 0; $index -lt $Segments.Count; $index++) {
|
||||
$typedSegments.SetValue($Segments[$index], $index)
|
||||
}
|
||||
$preparedPath = [Activator]::CreateInstance($preparedPathType, [object[]]@(,$typedSegments))
|
||||
$vehicle = [Activator]::CreateInstance($vehicleType)
|
||||
$vehicle.LengthMeters = [double]0.20
|
||||
$vehicle.WidthMeters = [double]0.20
|
||||
$vehicle.SafetyMarginMeters = [double]0.0
|
||||
$vehicle.MaximumCurvaturePerMeter = [double]100.0
|
||||
$vehicle.MinimumTurningRadiusMeters = [double]0.01
|
||||
$configuration = [Activator]::CreateInstance($configurationType)
|
||||
$configuration.LocalCubicBezier.CornerHeadingThresholdRadians = $CornerThresholdRadians
|
||||
$configuration.LocalCubicBezier.MaximumWindowLengthMeters = $MaximumWindowLengthMeters
|
||||
$configuration.LocalCubicBezier.HandleLengthRatio = $HandleLengthRatio
|
||||
$options = $optionsConstructor.Invoke(@($configuration))
|
||||
return $inputConstructor.Invoke(@($preparedPath, (New-EmptyMap), $vehicle, [double]0.05, $ReserveMeters, $options))
|
||||
}
|
||||
|
||||
function Invoke-Candidate(
|
||||
[object[]]$Segments,
|
||||
[double]$ReserveMeters = 0.02,
|
||||
[double]$CornerThresholdRadians = ([Math]::PI / 18.0),
|
||||
[double]$MaximumWindowLengthMeters = 0.60,
|
||||
[double]$HandleLengthRatio = (1.0 / 3.0),
|
||||
[double]$Strength = 1.0) {
|
||||
return $smoothMethod.Invoke($smoother, @(
|
||||
(New-AlgorithmInput $Segments $ReserveMeters $CornerThresholdRadians $MaximumWindowLengthMeters $HandleLengthRatio),
|
||||
$Strength,
|
||||
[Threading.CancellationToken]::None))
|
||||
}
|
||||
|
||||
function Invoke-Smoothing(
|
||||
[object[]]$Segments,
|
||||
[double]$ReserveMeters = 0.02,
|
||||
[double]$CornerThresholdRadians = ([Math]::PI / 18.0),
|
||||
[double]$MaximumWindowLengthMeters = 0.60,
|
||||
[double]$HandleLengthRatio = (1.0 / 3.0),
|
||||
[double]$Strength = 1.0) {
|
||||
$candidate = Invoke-Candidate $Segments $ReserveMeters $CornerThresholdRadians $MaximumWindowLengthMeters $HandleLengthRatio $Strength
|
||||
Assert-True (Get-PropertyValue $candidate 'Succeeded') 'Bézier smoothing must produce a candidate for the deterministic fixture.'
|
||||
return @(Get-PropertyValue $candidate 'Segments')
|
||||
}
|
||||
|
||||
function Get-InterpolatedRunCount($Points) {
|
||||
$runCount = 0
|
||||
$inRun = $false
|
||||
foreach ($point in $Points) {
|
||||
$interpolated = $point.Source.ToString() -eq 'Interpolated'
|
||||
if ($interpolated -and -not $inRun) { $runCount++ }
|
||||
$inRun = $interpolated
|
||||
}
|
||||
return $runCount
|
||||
}
|
||||
|
||||
function Get-PointAtArcLength($Points, [double]$ArcLength) {
|
||||
foreach ($point in $Points) {
|
||||
if ([BitConverter]::DoubleToInt64Bits([double]$point.ArcLength) -eq
|
||||
[BitConverter]::DoubleToInt64Bits($ArcLength)) {
|
||||
return $point
|
||||
}
|
||||
}
|
||||
throw "No output point found at local arc length $ArcLength."
|
||||
}
|
||||
|
||||
function Get-FirstInterpolatedPoint($Points) {
|
||||
foreach ($point in $Points) {
|
||||
if ($point.Source.ToString() -eq 'Interpolated') { return $point }
|
||||
}
|
||||
throw 'Expected an interpolated Bézier point.'
|
||||
}
|
||||
|
||||
$root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.'
|
||||
$processing = $root + 'Processing.'
|
||||
$algorithms = $root + 'Algorithms.'
|
||||
$coarsePath = 'MultiWheelC.TrajectoryPlanning.CoarsePath.'
|
||||
|
||||
$smootherType = Get-RequiredType ($algorithms + 'LocalCubicBezierSmoother')
|
||||
$pointType = Get-RequiredType ($processing + 'SmoothingPoint2D')
|
||||
$segmentType = Get-RequiredType ($processing + 'PreparedDirectionSegment')
|
||||
$preparedPathType = Get-RequiredType ($processing + 'PreparedPath')
|
||||
$inputType = Get-RequiredType ($algorithms + 'SmoothingAlgorithmInput')
|
||||
$optionsType = Get-RequiredType ($algorithms + 'SmoothingOptionsSnapshot')
|
||||
$configurationType = Get-RequiredType ($root + 'PathSmoothingConfiguration')
|
||||
$vehicleType = Get-RequiredType ($coarsePath + 'VehicleParameters')
|
||||
$directionType = Get-RequiredType ($coarsePath + 'TravelDirection')
|
||||
$sourceType = Get-RequiredType ($root + 'SmoothedPathPointSource')
|
||||
$candidateStatusType = Get-RequiredType ($algorithms + 'SmoothingCandidateStatus')
|
||||
$boundsType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.MapBoundsMm'
|
||||
$mapType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningGridMap'
|
||||
$mapRequestType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapRequest'
|
||||
$mapFactoryType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapFactory'
|
||||
|
||||
$inputConstructor = $inputType.GetConstructor([Reflection.BindingFlags]'Instance,NonPublic', $null,
|
||||
@($preparedPathType, $mapType, $vehicleType, [double], [double], $optionsType), $null)
|
||||
Assert-True ($null -ne $inputConstructor) 'Bézier tests must construct algorithm input with immutable option values.'
|
||||
$optionsConstructor = $optionsType.GetConstructor([Reflection.BindingFlags]'Instance,NonPublic', $null, @($configurationType), $null)
|
||||
Assert-True ($null -ne $optionsConstructor) 'Bézier tests must create immutable option snapshots.'
|
||||
$smoother = [Activator]::CreateInstance($smootherType, $true)
|
||||
$smoothMethod = $smootherType.GetMethod('Smooth', [Reflection.BindingFlags]'Instance,Public')
|
||||
Assert-True ($null -ne $smoothMethod) 'LocalCubicBezierSmoother must implement the internal smoother contract.'
|
||||
Assert-Equal 'LocalCubicBezier' $smoother.Method.ToString() 'Bézier smoother must identify its public smoothing method.'
|
||||
|
||||
$forward = [Enum]::Parse($directionType, 'Forward')
|
||||
$reverse = [Enum]::Parse($directionType, 'Reverse')
|
||||
$anchor = [Enum]::Parse($sourceType, 'Anchor')
|
||||
|
||||
# A straight path must not create a local Bézier window or alter any sample.
|
||||
$straightSource = @(
|
||||
(New-Point 0.0 0.0 0.0 0.0),
|
||||
(New-Point 0.1 0.0 0.1 0.0),
|
||||
(New-Point 0.2 0.0 0.2 0.0),
|
||||
(New-Point 0.3 0.0 0.3 0.0))
|
||||
$straightOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $straightSource)))[0].Points
|
||||
Assert-Equal 0 (Get-InterpolatedRunCount $straightOutput) 'A straight line must create no Bézier replacement window.'
|
||||
Assert-Equal $straightSource.Count $straightOutput.Count 'A straight line must retain its original sample count.'
|
||||
for ($index = 0; $index -lt $straightSource.Count; $index++) {
|
||||
Assert-PointBitwiseEqual $straightSource[$index] $straightOutput[$index] 'Straight samples must remain bitwise unchanged.'
|
||||
}
|
||||
|
||||
# One corner is one local replacement: only the corner sample is evaluated while the window endpoints stay fixed.
|
||||
$cornerSource = @(
|
||||
(New-Point 0.0 0.0 0.0 0.0),
|
||||
(New-Point 0.1 0.0 0.1 0.0),
|
||||
(New-Point 0.2 0.0 0.2 0.0),
|
||||
(New-Point 0.2 0.1 0.3 ([Math]::PI / 2.0)),
|
||||
(New-Point 0.2 0.2 0.4 ([Math]::PI / 2.0)))
|
||||
$cornerOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)))[0].Points
|
||||
Assert-Equal 1 (Get-InterpolatedRunCount $cornerOutput) 'One corner must produce exactly one contiguous Bézier replacement.'
|
||||
Assert-Equal $cornerSource.Count $cornerOutput.Count 'One local replacement must preserve the segment sampling topology.'
|
||||
Assert-True (($cornerOutput[2].X -ne $cornerSource[2].X) -or ($cornerOutput[2].Y -ne $cornerSource[2].Y)) 'The corner sample must be replaced by cubic Bézier geometry.'
|
||||
Assert-PointBitwiseEqual $cornerSource[1] $cornerOutput[1] 'Bézier entry anchor must remain fixed.'
|
||||
Assert-PointBitwiseEqual $cornerSource[3] $cornerOutput[3] 'Bézier exit anchor must remain fixed.'
|
||||
|
||||
# Adjacent corner windows touch/overlap and must become one merged cubic replacement, not two sequential fits.
|
||||
$overlappingSource = @(
|
||||
(New-Point 0.0 0.0 0.0 0.0),
|
||||
(New-Point 0.1 0.0 0.1 0.0),
|
||||
(New-Point 0.2 0.0 0.2 0.0),
|
||||
(New-Point 0.2 0.1 0.3 ([Math]::PI / 2.0)),
|
||||
(New-Point 0.3 0.1 0.4 0.0),
|
||||
(New-Point 0.4 0.1 0.5 0.0))
|
||||
$overlappingOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $overlappingSource)))[0].Points
|
||||
Assert-Equal 1 (Get-InterpolatedRunCount $overlappingOutput) 'Touching local corner windows must merge into exactly one Bézier replacement.'
|
||||
Assert-PointBitwiseEqual $overlappingSource[1] $overlappingOutput[1] 'Merged Bézier entry anchor must remain fixed.'
|
||||
Assert-PointBitwiseEqual $overlappingSource[4] $overlappingOutput[4] 'Merged Bézier exit anchor must remain fixed.'
|
||||
Assert-True (($overlappingOutput[2].X -ne $overlappingSource[2].X) -or ($overlappingOutput[2].Y -ne $overlappingSource[2].Y)) 'Merged window must replace the first interior corner sample.'
|
||||
Assert-True (($overlappingOutput[3].X -ne $overlappingSource[3].X) -or ($overlappingOutput[3].Y -ne $overlappingSource[3].Y)) 'Merged window must replace the second interior corner sample.'
|
||||
|
||||
# Samples outside a local window must remain bitwise unchanged rather than be globally re-fit.
|
||||
$isolatedSource = @(
|
||||
(New-Point 0.0 0.0 0.0 0.0),
|
||||
(New-Point 0.1 0.0 0.1 0.0),
|
||||
(New-Point 0.2 0.0 0.2 0.0),
|
||||
(New-Point 0.3 0.0 0.3 0.0),
|
||||
(New-Point 0.3 0.1 0.4 ([Math]::PI / 2.0)),
|
||||
(New-Point 0.3 0.2 0.5 ([Math]::PI / 2.0)),
|
||||
(New-Point 0.3 0.3 0.6 ([Math]::PI / 2.0)))
|
||||
$isolatedOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $isolatedSource)))[0].Points
|
||||
foreach ($index in @(0, 1, 2, 5, 6)) {
|
||||
Assert-PointBitwiseEqual $isolatedSource[$index] (Get-PointAtArcLength $isolatedOutput $isolatedSource[$index].ArcLength) 'Samples outside a Bézier window must remain bitwise unchanged.'
|
||||
}
|
||||
|
||||
# Direction segments stay independent; segment endpoints and the gear-switch anchor are fixed.
|
||||
$reverseSource = @(
|
||||
(New-Point 0.2 0.2 0.0 ([Math]::PI / 2.0) 0.10 $true),
|
||||
(New-Point 0.2 0.1 0.1 ([Math]::PI / 2.0)),
|
||||
(New-Point 0.2 0.0 0.2 ([Math]::PI / 2.0)))
|
||||
$switchOutput = @(Invoke-Smoothing @(
|
||||
(New-DirectionSegment 0 $forward $cornerSource $false $true),
|
||||
(New-DirectionSegment 1 $reverse $reverseSource $true $false)))
|
||||
Assert-Equal 2 $switchOutput.Count 'Bézier smoothing must retain separate forward and reverse direction segments.'
|
||||
Assert-True $switchOutput[0].EndsAtGearSwitch 'The forward segment must retain its gear-switch boundary flag.'
|
||||
Assert-True $switchOutput[1].StartsAtGearSwitch 'The reverse segment must retain its gear-switch boundary flag.'
|
||||
Assert-PointBitwiseEqual $cornerSource[0] $switchOutput[0].Points[0] 'Segment start endpoint must remain fixed.'
|
||||
Assert-PointBitwiseEqual $cornerSource[$cornerSource.Count - 1] $switchOutput[0].Points[$switchOutput[0].Points.Count - 1] 'Segment end endpoint must remain fixed.'
|
||||
Assert-PointBitwiseEqual $reverseSource[0] $switchOutput[1].Points[0] 'Gear-switch point must remain fixed.'
|
||||
|
||||
# The immutable options each change only their own local behavior.
|
||||
$thresholdOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)) 0.02 1.70)[0].Points
|
||||
Assert-Equal 0 (Get-InterpolatedRunCount $thresholdOutput) 'A non-default heading threshold above the corner angle must suppress only corner detection.'
|
||||
$windowOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)) 0.02 ([Math]::PI / 18.0) 0.15)[0].Points
|
||||
Assert-Equal 0 (Get-InterpolatedRunCount $windowOutput) 'A non-default maximum window shorter than the local connection must suppress only that window.'
|
||||
$shortHandleOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)) 0.02 ([Math]::PI / 18.0) 0.60 0.10)[0].Points
|
||||
$longHandleOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)) 0.02 ([Math]::PI / 18.0) 0.60 0.60)[0].Points
|
||||
Assert-Equal 1 (Get-InterpolatedRunCount $shortHandleOutput) 'Changing handle ratio must not change detected window topology.'
|
||||
Assert-Equal 1 (Get-InterpolatedRunCount $longHandleOutput) 'Changing handle ratio must not change detected window topology.'
|
||||
$shortHandlePoint = Get-FirstInterpolatedPoint $shortHandleOutput
|
||||
$longHandlePoint = Get-FirstInterpolatedPoint $longHandleOutput
|
||||
Assert-True (($shortHandlePoint.X -ne $longHandlePoint.X) -or ($shortHandlePoint.Y -ne $longHandlePoint.Y)) 'A non-default handle ratio must change only the local Bézier geometry.'
|
||||
|
||||
# Parameter-matched local arc-length reference comparison must reject excess displacement as retryable and publish no geometry.
|
||||
$infeasible = Invoke-Candidate @((New-DirectionSegment 0 $forward $cornerSource)) 0.095
|
||||
Assert-Equal 'RetryableInfeasible' (Get-PropertyValue $infeasible 'Status').ToString() 'Exceeded local arc-length displacement must be retryable, not terminal.'
|
||||
Assert-True (-not (Get-PropertyValue $infeasible 'Succeeded')) 'An infeasible Bézier curve must not be executable.'
|
||||
Assert-Equal 0 (Get-PropertyValue $infeasible 'Segments').Count 'A retryable Bézier infeasibility must publish no executable geometry.'
|
||||
|
||||
Write-Output 'Path smoothing local cubic Bézier checks passed.'
|
||||
Reference in New Issue
Block a user