fix: preserve trusted raw path curvature
This commit is contained in:
梁薄云
@@ -150,6 +150,110 @@ function New-EmptyGeometryMap {
|
||||
return $map
|
||||
}
|
||||
|
||||
function Invoke-GeometryValidation($Analysis, [object[]]$Segments, $Vehicle) {
|
||||
$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))
|
||||
$arguments = [object[]]@(
|
||||
$Analysis.Path, $Analysis.Segments, $preparedPath, (New-EmptyGeometryMap), $Vehicle, [double]0.05,
|
||||
$null, [double]0.0, $null)
|
||||
$accepted = $validateMethod.Invoke($validator, $arguments)
|
||||
return [pscustomobject]@{ Accepted = $accepted; Reason = $arguments[8] }
|
||||
}
|
||||
|
||||
function New-CircularDirectionSegment(
|
||||
$Direction,
|
||||
[double]$Curvature,
|
||||
[int]$Intervals,
|
||||
[double]$ChordLength) {
|
||||
$radius = 1.0 / $Curvature
|
||||
$headingStep = 2.0 * [Math]::Asin($Curvature * $ChordLength / 2.0)
|
||||
$points = New-Object System.Collections.Generic.List[object]
|
||||
for ($index = 0; $index -le $Intervals; $index++) {
|
||||
$theta = $headingStep * $index
|
||||
$heading = if ($Direction.ToString() -eq 'Forward') { $theta } else { $theta + [Math]::PI }
|
||||
[void]$points.Add((New-GeometryPoint `
|
||||
(1.0 + $radius * [Math]::Sin($theta)) `
|
||||
(1.0 + $radius * (1.0 - [Math]::Cos($theta))) `
|
||||
($index * $ChordLength) $heading $heading))
|
||||
}
|
||||
return New-DirectionSegment 0 $Direction $points.ToArray()
|
||||
}
|
||||
|
||||
function Get-OldPolylineCurvatureMaximum($Analysis) {
|
||||
$maximum = 0.0
|
||||
$path = $Analysis.Path
|
||||
for ($index = 0; $index -lt $path.Count; $index++) {
|
||||
if ($path.Count -eq 1) {
|
||||
$curvature = 0.0
|
||||
}
|
||||
elseif ($index -eq 0) {
|
||||
$curvature = ($path[1].UnwrappedHeading - $path[0].UnwrappedHeading) /
|
||||
($path[1].ArcLength - $path[0].ArcLength)
|
||||
}
|
||||
elseif ($index -eq $path.Count - 1) {
|
||||
$curvature = ($path[$index].UnwrappedHeading - $path[$index - 1].UnwrappedHeading) /
|
||||
($path[$index].ArcLength - $path[$index - 1].ArcLength)
|
||||
}
|
||||
else {
|
||||
$curvature = ($path[$index + 1].UnwrappedHeading - $path[$index - 1].UnwrappedHeading) /
|
||||
($path[$index + 1].ArcLength - $path[$index - 1].ArcLength)
|
||||
}
|
||||
$maximum = [Math]::Max($maximum, [Math]::Abs($curvature))
|
||||
}
|
||||
return $maximum
|
||||
}
|
||||
|
||||
function Get-QuinticAnalyticCurvatureMaximum(
|
||||
[double]$C2,
|
||||
[double]$C3,
|
||||
[double]$C4,
|
||||
[double]$C5,
|
||||
[int]$ReferenceSamples = 20000) {
|
||||
$maximum = 0.0
|
||||
for ($index = 0; $index -lt $ReferenceSamples; $index++) {
|
||||
$t = $index / [double]($ReferenceSamples - 1)
|
||||
$firstDerivative = 2.0 * $C2 * $t + 3.0 * $C3 * $t * $t +
|
||||
4.0 * $C4 * $t * $t * $t + 5.0 * $C5 * $t * $t * $t * $t
|
||||
$secondDerivative = 2.0 * $C2 + 6.0 * $C3 * $t +
|
||||
12.0 * $C4 * $t * $t + 20.0 * $C5 * $t * $t * $t
|
||||
$curvature = [Math]::Abs($secondDerivative / [Math]::Pow(1.0 + $firstDerivative * $firstDerivative, 1.5))
|
||||
$maximum = [Math]::Max($maximum, $curvature)
|
||||
}
|
||||
return $maximum
|
||||
}
|
||||
|
||||
function New-QuinticDirectionSegment(
|
||||
[double]$C2,
|
||||
[double]$C3,
|
||||
[double]$C4,
|
||||
[double]$C5,
|
||||
[double]$DistributionPower,
|
||||
[int]$Samples = 400) {
|
||||
$points = New-Object System.Collections.Generic.List[object]
|
||||
$arcLength = 0.0
|
||||
$previousX = 0.0
|
||||
$previousY = 0.0
|
||||
for ($index = 0; $index -lt $Samples; $index++) {
|
||||
$t = [Math]::Pow($index / [double]($Samples - 1), $DistributionPower)
|
||||
$x = 1.0 + $t
|
||||
$y = $C2 * $t * $t + $C3 * $t * $t * $t + $C4 * $t * $t * $t * $t + $C5 * $t * $t * $t * $t * $t
|
||||
if ($index -gt 0) {
|
||||
$arcLength += [Math]::Sqrt(($x - $previousX) * ($x - $previousX) + ($y - $previousY) * ($y - $previousY))
|
||||
}
|
||||
$heading = [Math]::Atan2(
|
||||
2.0 * $C2 * $t + 3.0 * $C3 * $t * $t + 4.0 * $C4 * $t * $t * $t + 5.0 * $C5 * $t * $t * $t * $t,
|
||||
1.0)
|
||||
[void]$points.Add((New-GeometryPoint $x $y $arcLength $heading $heading))
|
||||
$previousX = $x
|
||||
$previousY = $y
|
||||
}
|
||||
return New-DirectionSegment 0 $forward $points.ToArray()
|
||||
}
|
||||
|
||||
$root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.'
|
||||
$processing = $root + 'Processing.'
|
||||
$coarsePath = 'MultiWheelC.TrajectoryPlanning.CoarsePath.'
|
||||
@@ -179,6 +283,7 @@ $vehicleType = Get-RequiredType ($coarsePath + 'VehicleParameters')
|
||||
$boundsType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.MapBoundsMm'
|
||||
$mapRequestType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapRequest'
|
||||
$mapFactoryType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapFactory'
|
||||
$validatorType = Get-RequiredType ($root + 'Validation.SmoothedPathValidator')
|
||||
|
||||
Assert-True ($null -ne $preparedPathType) 'PreparedPath must be discoverable for smoothing algorithms.'
|
||||
Assert-True ($null -ne $preprocessorType) 'PathSmoothingPreprocessor must be discoverable for request preparation.'
|
||||
@@ -190,12 +295,62 @@ $analyzer = [Activator]::CreateInstance($analyzerType)
|
||||
$analyzeMethod = $analyzerType.GetMethod('TryAnalyze')
|
||||
Assert-True ($null -ne $analyzeMethod) 'PathGeometryAnalyzer must expose TryAnalyze.'
|
||||
Assert-Equal 4 $analyzeMethod.GetParameters().Length 'TryAnalyze must accept segments, spacing, analysis, and reason.'
|
||||
$validator = [Activator]::CreateInstance($validatorType)
|
||||
$validateMethod = $validatorType.GetMethod('TryValidate')
|
||||
Assert-True ($null -ne $validateMethod) 'SmoothedPathValidator must expose TryValidate.'
|
||||
Assert-Equal 9 $validateMethod.GetParameters().Length 'SmoothedPathValidator.TryValidate must retain its public contract.'
|
||||
|
||||
$forward = [Enum]::Parse($directionType, 'Forward')
|
||||
$reverse = [Enum]::Parse($directionType, 'Reverse')
|
||||
$anchor = [Enum]::Parse($sourceType, 'Anchor')
|
||||
$coarseAnchor = [Enum]::Parse($coarseSourceType, 'Start')
|
||||
|
||||
# The chord-corrected estimator must retain an exact circular curvature limit for both travel directions.
|
||||
$maximumAllowedCurvature = 5.0 / 6.0
|
||||
$circleVehicle = [Activator]::CreateInstance($vehicleType)
|
||||
$circleVehicle.LengthMeters = 0.20
|
||||
$circleVehicle.WidthMeters = 0.20
|
||||
$circleVehicle.SafetyMarginMeters = 0.0
|
||||
$circleVehicle.MaximumCurvaturePerMeter = $maximumAllowedCurvature
|
||||
foreach ($direction in @($forward, $reverse)) {
|
||||
$exactLimitCircle = New-CircularDirectionSegment $direction $maximumAllowedCurvature 20 0.05
|
||||
$exactLimitAnalysis = Invoke-Analysis @($exactLimitCircle)
|
||||
foreach ($point in $exactLimitAnalysis.Path) {
|
||||
Assert-True ([Math]::Abs($point.VehicleCurvature) -le $maximumAllowedCurvature + 1.0e-9) `
|
||||
('An exact-limit ' + $direction + ' circle must not exceed the curvature limit.')
|
||||
}
|
||||
$validation = Invoke-GeometryValidation $exactLimitAnalysis @($exactLimitCircle) $circleVehicle
|
||||
Assert-True $validation.Accepted ('The validator must accept an analyzed exact-limit ' + $direction + ' circle. Reason=' + $validation.Reason)
|
||||
}
|
||||
|
||||
# An analyzed over-limit circle must remain detectable by the unchanged validator threshold.
|
||||
$overLimitCircle = New-CircularDirectionSegment $forward ($maximumAllowedCurvature + 0.01) 20 0.05
|
||||
$overLimitAnalysis = Invoke-Analysis @($overLimitCircle)
|
||||
Assert-True ($overLimitAnalysis.MaximumAbsoluteVehicleCurvaturePerMeter -gt $maximumAllowedCurvature + 1.0e-6) `
|
||||
'An over-limit circle must exceed the vehicle curvature limit by more than the validator tolerance.'
|
||||
$overLimitValidation = Invoke-GeometryValidation $overLimitAnalysis @($overLimitCircle) $circleVehicle
|
||||
Assert-False $overLimitValidation.Accepted 'The validator must reject an analyzed over-limit circle.'
|
||||
|
||||
# The chord-corrected estimator must not under-estimate these smooth references more than the former polyline estimator.
|
||||
foreach ($quinticCase in @(
|
||||
[pscustomobject]@{ Name = 'SBend'; C2 = 0.0; C3 = 0.30; C4 = -0.45; C5 = 0.18; Power = 1.0 },
|
||||
[pscustomobject]@{ Name = 'EndpointPeak'; C2 = 0.18; C3 = -0.12; C4 = 0.0; C5 = 0.0; Power = 1.0 },
|
||||
[pscustomobject]@{ Name = 'NonUniformFinalInterval'; C2 = -0.12; C3 = 0.36; C4 = -0.30; C5 = 0.08; Power = 1.7 })) {
|
||||
$quintic = New-QuinticDirectionSegment $quinticCase.C2 $quinticCase.C3 $quinticCase.C4 $quinticCase.C5 $quinticCase.Power
|
||||
$quinticAnalysis = Invoke-Analysis @($quintic)
|
||||
$analyticMaximum = Get-QuinticAnalyticCurvatureMaximum $quinticCase.C2 $quinticCase.C3 $quinticCase.C4 $quinticCase.C5
|
||||
$newDeficit = [Math]::Max(0.0, $analyticMaximum - $quinticAnalysis.MaximumAbsoluteVehicleCurvaturePerMeter)
|
||||
$oldDeficit = [Math]::Max(0.0, $analyticMaximum - (Get-OldPolylineCurvatureMaximum $quinticAnalysis))
|
||||
Assert-True ($newDeficit -le $oldDeficit + 1.0e-6) `
|
||||
($quinticCase.Name + ' must not have greater one-sided curvature under-estimation than the old polyline estimator.')
|
||||
}
|
||||
|
||||
# A half-turn over a single chord has no unambiguous geometric curvature estimate.
|
||||
$ambiguousTurn = New-DirectionSegment 0 $forward @(
|
||||
(New-GeometryPoint 0.0 0.0 0.0 0.0 0.0),
|
||||
(New-GeometryPoint 1.0 0.0 1.0 ([Math]::PI) ([Math]::PI)))
|
||||
Assert-AnalysisRejected @($ambiguousTurn) 'A two-point heading turn of π must be rejected as ambiguous.'
|
||||
|
||||
# Forward straight: resampling is exactly 0.05 m, preserves the exact endpoint, and has zero curvature.
|
||||
$straight = New-DirectionSegment 0 $forward @(
|
||||
(New-GeometryPoint 0.0 0.0 0.0 0.0 0.0),
|
||||
|
||||
Reference in New Issue
Block a user