First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix: harden local G2 candidate splicing

Browse Source
This commit is contained in:
梁薄云
2026-07-30 17:37:45 +08:00
parent 1c433458bc
commit 1662b68062
3 changed files with 107 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateBuilder.cs
+89 -1
View File
@@ -12,6 +12,7 @@ namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
internal sealed class LocalG2CandidateBuilder
{
private const double MinimumDerivativeNorm = 1e-10d;
private const int MinimumCurveEvaluationIntervals = 32;
private const int MaximumSubdivisionDepth = 32;
private static readonly double[] DerivativeScaleMultipliers = { 1d, 0.85d, 1.15d };
@@ -194,6 +195,13 @@ internal sealed class LocalG2CandidateBuilder
List<SmoothingPoint2D> output,
CancellationToken cancellationToken)
{
// A short endpoint chord does not prove that a fifth-degree curve has no interior cusp.
// Check a deterministic interior grid before the chord-driven subdivision below; every
// subsequently emitted subdivision parameter is checked again by TryEvaluate.
for (int index = 0; index <= MinimumCurveEvaluationIntervals; index++)
{
if (!TryEvaluate(curve, (double)index / MinimumCurveEvaluationIntervals, out _)) return false;
}
if (!TryEvaluate(curve, 0d, out CurveSample start) || !TryEvaluate(curve, 1d, out CurveSample end)) return false;
if (output.Count == 0 && !TryAppendPoint(start, left, right, segment, output)) return false;
return TrySubdivide(curve, left, right, segment, outputSpacingMeters, 0d, start, 1d, end, 0, output, cancellationToken);
@@ -361,6 +369,9 @@ internal sealed class LocalG2CandidateBuilder
case "Reverse": return BuildReverse();
case "Spliced": return BuildSpliced();
case "StartBoundary": return BuildStartBoundary();
case "InteriorStationaryCurve": return BuildInteriorStationaryCurve();
case "ExactSpliceEndpoints": return BuildExactSpliceEndpoints();
case "GearBoundary": return BuildGearBoundary();
default: throw new ArgumentOutOfRangeException(nameof(scenario));
}
}
@@ -371,7 +382,8 @@ internal sealed class LocalG2CandidateBuilder
double startCurvatureError, double endCurvatureError, bool containsLocalG2Source,
int outputRegionCount, bool internalConnectionsAreG2, string direction,
bool vehicleAndGeometricCurvatureSignsAreOpposite, bool noDuplicateNonGearPoints,
bool endpointsUnchanged)
bool endpointsUnchanged, bool rejected = false, bool endpointsAreExact = false,
bool gearBoundaryMarkerPreserved = false)
{
CandidateCount = candidateCount;
StartPositionError = startPositionError;
@@ -385,6 +397,9 @@ internal sealed class LocalG2CandidateBuilder
VehicleAndGeometricCurvatureSignsAreOpposite = vehicleAndGeometricCurvatureSignsAreOpposite;
NoDuplicateNonGearPoints = noDuplicateNonGearPoints;
EndpointsUnchanged = endpointsUnchanged;
Rejected = rejected;
EndpointsAreExact = endpointsAreExact;
GearBoundaryMarkerPreserved = gearBoundaryMarkerPreserved;
}
public int CandidateCount { get; }
public double StartPositionError { get; }
@@ -398,6 +413,9 @@ internal sealed class LocalG2CandidateBuilder
public bool VehicleAndGeometricCurvatureSignsAreOpposite { get; }
public bool NoDuplicateNonGearPoints { get; }
public bool EndpointsUnchanged { get; }
public bool Rejected { get; }
public bool EndpointsAreExact { get; }
public bool GearBoundaryMarkerPreserved { get; }
}
private static CandidateTestSnapshot BuildIsolated()
@@ -517,6 +535,73 @@ internal sealed class LocalG2CandidateBuilder
ContainsLocalG2(candidate.RegionPoints), 0, false, string.Empty, false, false, false);
}
private static CandidateTestSnapshot BuildInteriorStationaryCurve()
{
const double endpointX = 7d / 1500d;
if (!QuinticHermiteCurve2D.TryCreate(0d, 0d, 0.01d, 0d, 0d, 0d,
endpointX, 0d, 0.01d, 0d, 0d, 0d, out QuinticHermiteCurve2D curve, out string reason))
{
throw new InvalidOperationException(reason);
}
var points = new[]
{
new SmoothingPoint2D(0d, 0d, 0d, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
new SmoothingPoint2D(endpointX, 0d, endpointX, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
};
var segment = new PreparedDirectionSegment(0, TravelDirection.Forward, points, false, false);
var sampled = new List<SmoothingPoint2D>();
bool accepted = TryAppendCurveSamples(curve,
new BoundaryNode(0d, 0d, 0d, 0d, 0d),
new BoundaryNode(endpointX, endpointX, 0d, 0d, 0d),
segment, 0.025d, sampled, CancellationToken.None);
return new CandidateTestSnapshot(0, 0d, 0d, 0d, 0d, false, 0, false, string.Empty,
false, false, false, !accepted);
}
private static CandidateTestSnapshot BuildExactSpliceEndpoints()
{
PreparedDirectionSegment segment = CreateSegment(TravelDirection.Forward, null);
var candidate = new LocalG2CandidateGeometry(0, 0, 0.25d, 0.75d, 0.25d, 0.25d,
new[]
{
Point(0.2500000005d, 0d, 0.25d, false),
Point(0.5d, 0d, 0.5d, false),
Point(0.7499999995d, 0d, 0.75d, false),
}, 0d, 0d, 0d, 0d, true);
if (!new LocalG2PathSplicer().TryReplace(new PreparedPath(new[] { segment }), candidate,
out PreparedPath replaced, out string reason))
{
throw new InvalidOperationException(reason);
}
PathReferenceInterpolator.TryInterpolateByArcLength(segment.Points, 0.25d, out SmoothingPoint2D start, out _);
PathReferenceInterpolator.TryInterpolateByArcLength(segment.Points, 0.75d, out SmoothingPoint2D end, out _);
IReadOnlyList<SmoothingPoint2D> result = replaced.Segments[0].Points;
bool exact = result[1].X == start.X && result[1].Y == start.Y &&
result[result.Count - 2].X == end.X && result[result.Count - 2].Y == end.Y;
return new CandidateTestSnapshot(0, 0d, 0d, 0d, 0d, false, 0, false, string.Empty,
false, false, false, false, exact);
}
private static CandidateTestSnapshot BuildGearBoundary()
{
var sourcePoints = new[]
{
Point(0d, 0d, 0d, true), Point(0.25d, 0d, 0.25d, false), Point(0.5d, 0d, 0.5d, false),
};
var segment = new PreparedDirectionSegment(0, TravelDirection.Forward, sourcePoints, true, false);
var candidate = new LocalG2CandidateGeometry(0, 0, 0d, 0.5d, 0d, 0.5d,
new[] { Point(0d, 0d, 0d, false), Point(0.25d, 0d, 0.25d, false), Point(0.5d, 0d, 0.5d, false) },
0d, 0d, 0d, 0d, true);
if (!new LocalG2PathSplicer().TryReplace(new PreparedPath(new[] { segment }), candidate,
out PreparedPath replaced, out string reason))
{
throw new InvalidOperationException(reason);
}
bool preserved = replaced.Segments[0].StartsAtGearSwitch && replaced.Segments[0].Points[0].IsGearSwitchPoint;
return new CandidateTestSnapshot(0, 0d, 0d, 0d, 0d, false, 0, false, string.Empty,
false, false, false, false, false, preserved);
}
private static PreparedDirectionSegment CreateSegment(TravelDirection direction, double? startCurvature)
{
double heading = direction == TravelDirection.Forward ? 0d : Math.PI;
@@ -549,5 +634,8 @@ internal sealed class LocalG2CandidateBuilder
}
private static double Distance(SmoothingPoint2D left, SmoothingPoint2D right) =>
Math.Sqrt((right.X - left.X) * (right.X - left.X) + (right.Y - left.Y) * (right.Y - left.Y));
private static SmoothingPoint2D Point(double x, double y, double arcLength, bool isGearSwitch) =>
new SmoothingPoint2D(x, y, arcLength, 0d, 0d, 1d, isGearSwitch,
isGearSwitch ? SmoothedPathPointSource.GearSwitch : SmoothedPathPointSource.LocalG2Transition);
}
}
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2PathSplicer.cs
+6 -1
View File
@@ -40,7 +40,12 @@ internal sealed class LocalG2PathSplicer
SmoothingPoint2D point = source.Points[index];
if (point.ArcLength < candidate.StartArcLengthMeters) AddWithoutNonGearDuplicates(combined, point);
}
for (int index = 0; index < candidate.RegionPoints.Count; index++) AddWithoutNonGearDuplicates(combined, candidate.RegionPoints[index]);
// Candidate endpoints are only validated within a numerical tolerance. The replacement
// itself must use the exact source-window endpoints, including a possible gear marker.
AddWithoutNonGearDuplicates(combined, start);
for (int index = 1; index < candidate.RegionPoints.Count - 1; index++)
AddWithoutNonGearDuplicates(combined, candidate.RegionPoints[index]);
AddWithoutNonGearDuplicates(combined, end);
for (int index = 0; index < source.Points.Count; index++)
{
SmoothingPoint2D point = source.Points[index];
ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1
+12
View File
@@ -58,4 +58,16 @@ Assert-True ($startBoundary.CandidateCount -gt 0) 'A start-boundary event must b
Assert-Near 0.0 $startBoundary.StartCurvatureError 1e-8 `
'A start-boundary candidate must use the preserved physical start vehicle curvature.'
$interiorDerivative = Invoke-Scenario 'InteriorStationaryCurve'
Assert-True $interiorDerivative.Rejected `
'A curve with a stationary interior derivative must be rejected even when its endpoint chord is short.'
$exactSplice = Invoke-Scenario 'ExactSpliceEndpoints'
Assert-True $exactSplice.EndpointsAreExact `
'Splicing must write interpolated exact endpoints instead of accepting approximate candidate endpoints.'
$gearBoundary = Invoke-Scenario 'GearBoundary'
Assert-True $gearBoundary.GearBoundaryMarkerPreserved `
'A window touching a gear-switch segment boundary must preserve its point-level gear marker.'
Write-Output 'Path smoothing Local G2 candidate checks passed.'