feat: detect local curvature transition regions
This commit is contained in:
梁薄云
@@ -0,0 +1,51 @@
|
|||||||
|
using System;
|
||||||
|
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||||
|
|
||||||
|
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
||||||
|
|
||||||
|
/// <summary>同一方向原语边界两侧车辆曲率的离散跳变。</summary>
|
||||||
|
internal sealed class CurvatureTransition
|
||||||
|
{
|
||||||
|
internal CurvatureTransition(
|
||||||
|
int segmentIndex,
|
||||||
|
int leftCoarsePathIndex,
|
||||||
|
int rightCoarsePathIndex,
|
||||||
|
double localArcLengthMeters,
|
||||||
|
double x,
|
||||||
|
double y,
|
||||||
|
double vehicleHeadingRadians,
|
||||||
|
double leftVehicleCurvaturePerMeter,
|
||||||
|
double rightVehicleCurvaturePerMeter)
|
||||||
|
{
|
||||||
|
if (segmentIndex < 0 || leftCoarsePathIndex < 0 || rightCoarsePathIndex != leftCoarsePathIndex + 1)
|
||||||
|
throw new ArgumentOutOfRangeException(nameof(leftCoarsePathIndex));
|
||||||
|
if (!NumericGuard.IsFinite(localArcLengthMeters) || localArcLengthMeters < 0d ||
|
||||||
|
!NumericGuard.IsFinite(x) || !NumericGuard.IsFinite(y) || !NumericGuard.IsFinite(vehicleHeadingRadians) ||
|
||||||
|
!NumericGuard.IsFinite(leftVehicleCurvaturePerMeter) || !NumericGuard.IsFinite(rightVehicleCurvaturePerMeter))
|
||||||
|
{
|
||||||
|
throw new ArgumentOutOfRangeException(nameof(localArcLengthMeters));
|
||||||
|
}
|
||||||
|
|
||||||
|
SegmentIndex = segmentIndex;
|
||||||
|
LeftCoarsePathIndex = leftCoarsePathIndex;
|
||||||
|
RightCoarsePathIndex = rightCoarsePathIndex;
|
||||||
|
LocalArcLengthMeters = localArcLengthMeters;
|
||||||
|
X = x;
|
||||||
|
Y = y;
|
||||||
|
VehicleHeadingRadians = vehicleHeadingRadians;
|
||||||
|
LeftVehicleCurvaturePerMeter = leftVehicleCurvaturePerMeter;
|
||||||
|
RightVehicleCurvaturePerMeter = rightVehicleCurvaturePerMeter;
|
||||||
|
CurvatureJumpPerMeter = Math.Abs(rightVehicleCurvaturePerMeter - leftVehicleCurvaturePerMeter);
|
||||||
|
}
|
||||||
|
|
||||||
|
internal int SegmentIndex { get; }
|
||||||
|
internal int LeftCoarsePathIndex { get; }
|
||||||
|
internal int RightCoarsePathIndex { get; }
|
||||||
|
internal double LocalArcLengthMeters { get; }
|
||||||
|
internal double X { get; }
|
||||||
|
internal double Y { get; }
|
||||||
|
internal double VehicleHeadingRadians { get; }
|
||||||
|
internal double LeftVehicleCurvaturePerMeter { get; }
|
||||||
|
internal double RightVehicleCurvaturePerMeter { get; }
|
||||||
|
internal double CurvatureJumpPerMeter { get; }
|
||||||
|
}
|
||||||
@@ -0,0 +1,255 @@
|
|||||||
|
using System;
|
||||||
|
using System.Collections.Generic;
|
||||||
|
using System.Collections.ObjectModel;
|
||||||
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||||
|
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||||
|
|
||||||
|
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
||||||
|
|
||||||
|
/// <summary>从原始粗路径的同向相邻点中识别恒曲率原语边界。</summary>
|
||||||
|
internal sealed class CurvatureTransitionDetector
|
||||||
|
{
|
||||||
|
internal bool TryDetect(
|
||||||
|
PathSmoothingRequest request,
|
||||||
|
double maximumVehicleCurvaturePerMeter,
|
||||||
|
LocalG2OptionsSnapshot options,
|
||||||
|
out IReadOnlyList<CurvatureTransition> transitions,
|
||||||
|
out string reason)
|
||||||
|
{
|
||||||
|
transitions = Empty<CurvatureTransition>();
|
||||||
|
reason = string.Empty;
|
||||||
|
if (request == null || request.CoarsePath == null || request.Segments == null || options == null ||
|
||||||
|
!NumericGuard.IsPositiveFinite(maximumVehicleCurvaturePerMeter))
|
||||||
|
{
|
||||||
|
reason = "局部 G2 曲率事件检测输入无效。";
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
if (!ValidatePath(request.CoarsePath, out reason) || !ValidateSegments(request.CoarsePath, request.Segments, out reason))
|
||||||
|
return false;
|
||||||
|
|
||||||
|
double threshold = Math.Max(
|
||||||
|
options.AbsoluteCurvatureJumpFloorPerMeter,
|
||||||
|
options.CurvatureJumpRatioOfMaximum * maximumVehicleCurvaturePerMeter);
|
||||||
|
if (!NumericGuard.IsPositiveFinite(threshold))
|
||||||
|
{
|
||||||
|
reason = "局部 G2 曲率事件阈值无效。";
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
var detected = new List<CurvatureTransition>();
|
||||||
|
for (int segmentPosition = 0; segmentPosition < request.Segments.Count; segmentPosition++)
|
||||||
|
{
|
||||||
|
PathSegment segment = request.Segments[segmentPosition];
|
||||||
|
double segmentStartArc = request.CoarsePath[segment.StartIndex].ArcLength;
|
||||||
|
for (int leftIndex = segment.StartIndex; leftIndex < segment.EndIndex; leftIndex++)
|
||||||
|
{
|
||||||
|
CoarsePathPoint left = request.CoarsePath[leftIndex];
|
||||||
|
CoarsePathPoint right = request.CoarsePath[leftIndex + 1];
|
||||||
|
double delta = right.VehicleCurvature - left.VehicleCurvature;
|
||||||
|
if (Math.Abs(delta) >= threshold)
|
||||||
|
{
|
||||||
|
detected.Add(new CurvatureTransition(
|
||||||
|
segment.SegmentIndex,
|
||||||
|
leftIndex,
|
||||||
|
leftIndex + 1,
|
||||||
|
left.ArcLength - segmentStartArc,
|
||||||
|
left.X,
|
||||||
|
left.Y,
|
||||||
|
left.Heading,
|
||||||
|
left.VehicleCurvature,
|
||||||
|
right.VehicleCurvature));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
transitions = new ReadOnlyCollection<CurvatureTransition>(detected);
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>反射脚本使用的确定性检测与窗口规划接缝。</summary>
|
||||||
|
public static class TestHooks
|
||||||
|
{
|
||||||
|
public static DetectionTestSnapshot Execute(string scenario)
|
||||||
|
{
|
||||||
|
if (string.IsNullOrWhiteSpace(scenario)) throw new ArgumentException("A scenario is required.", nameof(scenario));
|
||||||
|
switch (scenario)
|
||||||
|
{
|
||||||
|
case "SingleTransition": return DetectSingleTransition();
|
||||||
|
case "GearSwitch": return DetectGearSwitch();
|
||||||
|
case "Noise": return DetectNoise();
|
||||||
|
case "Overlap": return PlanOverlap();
|
||||||
|
case "NearStart": return PlanNearStart();
|
||||||
|
default: throw new ArgumentOutOfRangeException(nameof(scenario));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public sealed class DetectionTestSnapshot
|
||||||
|
{
|
||||||
|
internal DetectionTestSnapshot(int transitionCount, double maximumJump, int regionCount,
|
||||||
|
int transitionCountInFirstRegion, double startArcLength, double leftWindowLength, double rightWindowLength)
|
||||||
|
{
|
||||||
|
TransitionCount = transitionCount;
|
||||||
|
MaximumJump = maximumJump;
|
||||||
|
RegionCount = regionCount;
|
||||||
|
TransitionCountInFirstRegion = transitionCountInFirstRegion;
|
||||||
|
StartArcLength = startArcLength;
|
||||||
|
LeftWindowLength = leftWindowLength;
|
||||||
|
RightWindowLength = rightWindowLength;
|
||||||
|
}
|
||||||
|
|
||||||
|
public int TransitionCount { get; }
|
||||||
|
public double MaximumJump { get; }
|
||||||
|
public int RegionCount { get; }
|
||||||
|
public int TransitionCountInFirstRegion { get; }
|
||||||
|
public double StartArcLength { get; }
|
||||||
|
public double LeftWindowLength { get; }
|
||||||
|
public double RightWindowLength { get; }
|
||||||
|
}
|
||||||
|
|
||||||
|
private static DetectionTestSnapshot DetectSingleTransition()
|
||||||
|
{
|
||||||
|
IReadOnlyList<CurvatureTransition> transitions = Detect(CreateRequest(
|
||||||
|
new[] { Point(0d, 0d), Point(0.1d, 0.4167d), Point(0.2d, 0.4167d) },
|
||||||
|
new[] { new PathSegment(0, TravelDirection.Forward, 0, 2, false, false) }));
|
||||||
|
return Snapshot(transitions);
|
||||||
|
}
|
||||||
|
|
||||||
|
private static DetectionTestSnapshot DetectGearSwitch()
|
||||||
|
{
|
||||||
|
IReadOnlyList<CurvatureTransition> transitions = Detect(CreateRequest(
|
||||||
|
new[]
|
||||||
|
{
|
||||||
|
Point(0d, 0d, TravelDirection.Forward), Point(0.2d, 0d, TravelDirection.Forward),
|
||||||
|
Point(0.2d, 0.4167d, TravelDirection.Reverse, true), Point(0.4d, 0.4167d, TravelDirection.Reverse),
|
||||||
|
},
|
||||||
|
new[]
|
||||||
|
{
|
||||||
|
new PathSegment(0, TravelDirection.Forward, 0, 1, false, true),
|
||||||
|
new PathSegment(1, TravelDirection.Reverse, 2, 3, true, false),
|
||||||
|
}));
|
||||||
|
return Snapshot(transitions);
|
||||||
|
}
|
||||||
|
|
||||||
|
private static DetectionTestSnapshot DetectNoise()
|
||||||
|
{
|
||||||
|
IReadOnlyList<CurvatureTransition> transitions = Detect(CreateRequest(
|
||||||
|
new[] { Point(0d, 0d), Point(0.1d, 0.01d), Point(0.2d, 0.01d) },
|
||||||
|
new[] { new PathSegment(0, TravelDirection.Forward, 0, 2, false, false) }));
|
||||||
|
return Snapshot(transitions);
|
||||||
|
}
|
||||||
|
|
||||||
|
private static DetectionTestSnapshot PlanOverlap()
|
||||||
|
{
|
||||||
|
LocalG2OptionsSnapshot options = CreateOptions();
|
||||||
|
var transitions = new[]
|
||||||
|
{
|
||||||
|
new CurvatureTransition(0, 1, 2, 0.4d, 0.4d, 0d, 0d, 0d, 0.5d),
|
||||||
|
new CurvatureTransition(0, 2, 3, 0.7d, 0.7d, 0d, 0d, 0.5d, 0d),
|
||||||
|
};
|
||||||
|
var planner = new LocalG2WindowPlanner();
|
||||||
|
if (!planner.TryPlan(CreatePreparedPath(1.4d), transitions, options, out IReadOnlyList<LocalG2SmoothingRegion> regions, out string reason))
|
||||||
|
throw new InvalidOperationException(reason);
|
||||||
|
return new DetectionTestSnapshot(2, 0.5d, regions.Count, regions[0].Transitions.Count, 0d, 0d, 0d);
|
||||||
|
}
|
||||||
|
|
||||||
|
private static DetectionTestSnapshot PlanNearStart()
|
||||||
|
{
|
||||||
|
LocalG2OptionsSnapshot options = CreateOptions();
|
||||||
|
var transitions = new[] { new CurvatureTransition(0, 0, 1, 0.1d, 0.1d, 0d, 0d, 0d, 0.5d) };
|
||||||
|
var planner = new LocalG2WindowPlanner();
|
||||||
|
if (!planner.TryPlan(CreatePreparedPath(1d), transitions, options, out IReadOnlyList<LocalG2SmoothingRegion> regions, out string reason))
|
||||||
|
throw new InvalidOperationException(reason);
|
||||||
|
LocalG2WindowVariant first = regions[0].WindowVariants[0];
|
||||||
|
return new DetectionTestSnapshot(1, 0.5d, regions.Count, 1,
|
||||||
|
first.StartArcLengthMeters, first.LeftWindowLengthMeters, first.RightWindowLengthMeters);
|
||||||
|
}
|
||||||
|
|
||||||
|
private static IReadOnlyList<CurvatureTransition> Detect(PathSmoothingRequest request)
|
||||||
|
{
|
||||||
|
var detector = new CurvatureTransitionDetector();
|
||||||
|
if (!detector.TryDetect(request, 0.8333d, CreateOptions(), out IReadOnlyList<CurvatureTransition> transitions, out string reason))
|
||||||
|
throw new InvalidOperationException(reason);
|
||||||
|
return transitions;
|
||||||
|
}
|
||||||
|
|
||||||
|
private static DetectionTestSnapshot Snapshot(IReadOnlyList<CurvatureTransition> transitions)
|
||||||
|
{
|
||||||
|
double maximum = 0d;
|
||||||
|
for (int index = 0; index < transitions.Count; index++) maximum = Math.Max(maximum, transitions[index].CurvatureJumpPerMeter);
|
||||||
|
return new DetectionTestSnapshot(transitions.Count, maximum, 0, 0, 0d, 0d, 0d);
|
||||||
|
}
|
||||||
|
|
||||||
|
private static PathSmoothingRequest CreateRequest(IReadOnlyList<CoarsePathPoint> points, IReadOnlyList<PathSegment> segments)
|
||||||
|
{
|
||||||
|
return new PathSmoothingRequest(points, segments, null, null, new PathSmoothingConfiguration());
|
||||||
|
}
|
||||||
|
|
||||||
|
private static CoarsePathPoint Point(double arcLength, double curvature, TravelDirection direction = TravelDirection.Forward, bool gearSwitch = false)
|
||||||
|
{
|
||||||
|
return new CoarsePathPoint(arcLength, 0d, 0d, 0d, arcLength, direction, curvature, 1d, gearSwitch,
|
||||||
|
CoarsePathPointSource.MotionPrimitive);
|
||||||
|
}
|
||||||
|
|
||||||
|
private static LocalG2OptionsSnapshot CreateOptions() => new LocalG2OptionsSnapshot(new PathSmoothingConfiguration());
|
||||||
|
|
||||||
|
private static Processing.PreparedPath CreatePreparedPath(double length)
|
||||||
|
{
|
||||||
|
var points = new[]
|
||||||
|
{
|
||||||
|
new Processing.SmoothingPoint2D(0d, 0d, 0d, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
|
||||||
|
new Processing.SmoothingPoint2D(length, 0d, length, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
|
||||||
|
};
|
||||||
|
var segment = new Processing.PreparedDirectionSegment(0, TravelDirection.Forward, points, false, false);
|
||||||
|
return new Processing.PreparedPath(new[] { segment });
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
private static bool ValidatePath(IReadOnlyList<CoarsePathPoint> path, out string reason)
|
||||||
|
{
|
||||||
|
reason = string.Empty;
|
||||||
|
if (path.Count == 0)
|
||||||
|
{
|
||||||
|
reason = "局部 G2 曲率事件检测需要粗路径点。";
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
double previousArc = -1d;
|
||||||
|
for (int index = 0; index < path.Count; index++)
|
||||||
|
{
|
||||||
|
CoarsePathPoint point = path[index];
|
||||||
|
if (point == null || !NumericGuard.IsFinite(point.X) || !NumericGuard.IsFinite(point.Y) ||
|
||||||
|
!NumericGuard.IsFinite(point.Heading) || !NumericGuard.IsFinite(point.ArcLength) || point.ArcLength < 0d ||
|
||||||
|
!NumericGuard.IsFinite(point.VehicleCurvature) || point.ArcLength < previousArc)
|
||||||
|
{
|
||||||
|
reason = "局部 G2 曲率事件检测要求有限且非递减的粗路径。";
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
previousArc = point.ArcLength;
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
private static bool ValidateSegments(IReadOnlyList<CoarsePathPoint> path, IReadOnlyList<PathSegment> segments, out string reason)
|
||||||
|
{
|
||||||
|
reason = string.Empty;
|
||||||
|
for (int position = 0; position < segments.Count; position++)
|
||||||
|
{
|
||||||
|
PathSegment segment = segments[position];
|
||||||
|
if (segment == null || segment.SegmentIndex != position || segment.StartIndex < 0 ||
|
||||||
|
segment.EndIndex < segment.StartIndex || segment.EndIndex >= path.Count)
|
||||||
|
{
|
||||||
|
reason = "局部 G2 曲率事件检测的方向分段无效。";
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
for (int index = segment.StartIndex; index <= segment.EndIndex; index++)
|
||||||
|
{
|
||||||
|
if (path[index].Direction != segment.Direction)
|
||||||
|
{
|
||||||
|
reason = "局部 G2 曲率事件检测的方向分段包含换向点。";
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
private static IReadOnlyList<T> Empty<T>() => new ReadOnlyCollection<T>(new List<T>());
|
||||||
|
}
|
||||||
@@ -0,0 +1,57 @@
|
|||||||
|
using System;
|
||||||
|
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||||
|
|
||||||
|
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
||||||
|
|
||||||
|
/// <summary>局部 G2 预平滑一次运行使用的已校验不可变选项。</summary>
|
||||||
|
internal sealed class LocalG2OptionsSnapshot
|
||||||
|
{
|
||||||
|
internal LocalG2OptionsSnapshot(PathSmoothingConfiguration configuration)
|
||||||
|
{
|
||||||
|
if (configuration == null) throw new ArgumentNullException(nameof(configuration));
|
||||||
|
LocalG2QuinticOptions source = configuration.LocalG2Quintic;
|
||||||
|
if (source == null) throw new ArgumentOutOfRangeException(nameof(configuration));
|
||||||
|
|
||||||
|
MinimumWindowLengthMeters = source.MinimumWindowLengthMeters;
|
||||||
|
PreferredWindowLengthMeters = source.PreferredWindowLengthMeters;
|
||||||
|
MaximumWindowLengthMeters = source.MaximumWindowLengthMeters;
|
||||||
|
MaximumDeviationMeters = source.MaximumDeviationMeters;
|
||||||
|
AbsoluteCurvatureJumpFloorPerMeter = source.AbsoluteCurvatureJumpFloorPerMeter;
|
||||||
|
CurvatureJumpRatioOfMaximum = source.CurvatureJumpRatioOfMaximum;
|
||||||
|
MinimumPeakGradientImprovementRatio = source.MinimumPeakGradientImprovementRatio;
|
||||||
|
MaximumVariationCostRegressionRatio = source.MaximumVariationCostRegressionRatio;
|
||||||
|
MaximumCandidatesPerRegion = source.MaximumCandidatesPerRegion;
|
||||||
|
|
||||||
|
if (!NumericGuard.IsPositiveFinite(MinimumWindowLengthMeters))
|
||||||
|
throw new ArgumentOutOfRangeException(nameof(MinimumWindowLengthMeters));
|
||||||
|
if (!NumericGuard.IsFinite(PreferredWindowLengthMeters) || PreferredWindowLengthMeters < MinimumWindowLengthMeters)
|
||||||
|
throw new ArgumentOutOfRangeException(nameof(PreferredWindowLengthMeters));
|
||||||
|
if (!NumericGuard.IsFinite(MaximumWindowLengthMeters) || MaximumWindowLengthMeters < PreferredWindowLengthMeters)
|
||||||
|
throw new ArgumentOutOfRangeException(nameof(MaximumWindowLengthMeters));
|
||||||
|
if (!NumericGuard.IsPositiveFinite(MaximumDeviationMeters))
|
||||||
|
throw new ArgumentOutOfRangeException(nameof(MaximumDeviationMeters));
|
||||||
|
if (!NumericGuard.IsPositiveFinite(AbsoluteCurvatureJumpFloorPerMeter))
|
||||||
|
throw new ArgumentOutOfRangeException(nameof(AbsoluteCurvatureJumpFloorPerMeter));
|
||||||
|
if (!NumericGuard.IsFinite(CurvatureJumpRatioOfMaximum) || CurvatureJumpRatioOfMaximum <= 0d || CurvatureJumpRatioOfMaximum > 1d)
|
||||||
|
throw new ArgumentOutOfRangeException(nameof(CurvatureJumpRatioOfMaximum));
|
||||||
|
if (!NumericGuard.IsFinite(MinimumPeakGradientImprovementRatio) ||
|
||||||
|
MinimumPeakGradientImprovementRatio <= 0d || MinimumPeakGradientImprovementRatio >= 1d)
|
||||||
|
{
|
||||||
|
throw new ArgumentOutOfRangeException(nameof(MinimumPeakGradientImprovementRatio));
|
||||||
|
}
|
||||||
|
if (!NumericGuard.IsFinite(MaximumVariationCostRegressionRatio) || MaximumVariationCostRegressionRatio < 0d)
|
||||||
|
throw new ArgumentOutOfRangeException(nameof(MaximumVariationCostRegressionRatio));
|
||||||
|
if (MaximumCandidatesPerRegion < 1)
|
||||||
|
throw new ArgumentOutOfRangeException(nameof(MaximumCandidatesPerRegion));
|
||||||
|
}
|
||||||
|
|
||||||
|
internal double MinimumWindowLengthMeters { get; }
|
||||||
|
internal double PreferredWindowLengthMeters { get; }
|
||||||
|
internal double MaximumWindowLengthMeters { get; }
|
||||||
|
internal double MaximumDeviationMeters { get; }
|
||||||
|
internal double AbsoluteCurvatureJumpFloorPerMeter { get; }
|
||||||
|
internal double CurvatureJumpRatioOfMaximum { get; }
|
||||||
|
internal double MinimumPeakGradientImprovementRatio { get; }
|
||||||
|
internal double MaximumVariationCostRegressionRatio { get; }
|
||||||
|
internal int MaximumCandidatesPerRegion { get; }
|
||||||
|
}
|
||||||
@@ -0,0 +1,67 @@
|
|||||||
|
using System;
|
||||||
|
using System.Collections.Generic;
|
||||||
|
using System.Collections.ObjectModel;
|
||||||
|
|
||||||
|
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
||||||
|
|
||||||
|
/// <summary>一个局部 G2 候选可替换的弧长窗口。</summary>
|
||||||
|
internal sealed class LocalG2WindowVariant
|
||||||
|
{
|
||||||
|
internal LocalG2WindowVariant(int candidateIndex, double startArcLengthMeters, double endArcLengthMeters,
|
||||||
|
double leftWindowLengthMeters, double rightWindowLengthMeters)
|
||||||
|
{
|
||||||
|
if (candidateIndex < 0 || startArcLengthMeters < 0d || endArcLengthMeters < startArcLengthMeters ||
|
||||||
|
leftWindowLengthMeters < 0d || rightWindowLengthMeters < 0d)
|
||||||
|
{
|
||||||
|
throw new ArgumentOutOfRangeException(nameof(candidateIndex));
|
||||||
|
}
|
||||||
|
CandidateIndex = candidateIndex;
|
||||||
|
StartArcLengthMeters = startArcLengthMeters;
|
||||||
|
EndArcLengthMeters = endArcLengthMeters;
|
||||||
|
LeftWindowLengthMeters = leftWindowLengthMeters;
|
||||||
|
RightWindowLengthMeters = rightWindowLengthMeters;
|
||||||
|
}
|
||||||
|
|
||||||
|
internal int CandidateIndex { get; }
|
||||||
|
internal double StartArcLengthMeters { get; }
|
||||||
|
internal double EndArcLengthMeters { get; }
|
||||||
|
internal double LeftWindowLengthMeters { get; }
|
||||||
|
internal double RightWindowLengthMeters { get; }
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>因最大合法窗口相交而合并的一组曲率事件。</summary>
|
||||||
|
internal sealed class LocalG2SmoothingRegion
|
||||||
|
{
|
||||||
|
internal LocalG2SmoothingRegion(
|
||||||
|
int segmentIndex,
|
||||||
|
IReadOnlyList<CurvatureTransition> transitions,
|
||||||
|
double maximumStartArcLengthMeters,
|
||||||
|
double maximumEndArcLengthMeters,
|
||||||
|
IReadOnlyList<LocalG2WindowVariant> windowVariants)
|
||||||
|
{
|
||||||
|
if (segmentIndex < 0 || transitions == null || transitions.Count == 0 ||
|
||||||
|
maximumStartArcLengthMeters < 0d || maximumEndArcLengthMeters < maximumStartArcLengthMeters ||
|
||||||
|
windowVariants == null)
|
||||||
|
{
|
||||||
|
throw new ArgumentOutOfRangeException(nameof(transitions));
|
||||||
|
}
|
||||||
|
SegmentIndex = segmentIndex;
|
||||||
|
Transitions = Copy(transitions);
|
||||||
|
MaximumStartArcLengthMeters = maximumStartArcLengthMeters;
|
||||||
|
MaximumEndArcLengthMeters = maximumEndArcLengthMeters;
|
||||||
|
WindowVariants = Copy(windowVariants);
|
||||||
|
}
|
||||||
|
|
||||||
|
internal int SegmentIndex { get; }
|
||||||
|
internal IReadOnlyList<CurvatureTransition> Transitions { get; }
|
||||||
|
internal double MaximumStartArcLengthMeters { get; }
|
||||||
|
internal double MaximumEndArcLengthMeters { get; }
|
||||||
|
internal IReadOnlyList<LocalG2WindowVariant> WindowVariants { get; }
|
||||||
|
|
||||||
|
private static IReadOnlyList<T> Copy<T>(IReadOnlyList<T> source)
|
||||||
|
{
|
||||||
|
var copy = new List<T>(source.Count);
|
||||||
|
for (int index = 0; index < source.Count; index++) copy.Add(source[index]);
|
||||||
|
return new ReadOnlyCollection<T>(copy);
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -0,0 +1,212 @@
|
|||||||
|
using System;
|
||||||
|
using System.Collections.Generic;
|
||||||
|
using System.Collections.ObjectModel;
|
||||||
|
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||||
|
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||||
|
|
||||||
|
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
||||||
|
|
||||||
|
/// <summary>按硬方向边界生成并合并局部 G2 曲率事件的候选窗口。</summary>
|
||||||
|
internal sealed class LocalG2WindowPlanner
|
||||||
|
{
|
||||||
|
private const double MergeToleranceMeters = 1e-9d;
|
||||||
|
|
||||||
|
internal bool TryPlan(
|
||||||
|
PreparedPath originalPath,
|
||||||
|
IReadOnlyList<CurvatureTransition> transitions,
|
||||||
|
LocalG2OptionsSnapshot options,
|
||||||
|
out IReadOnlyList<LocalG2SmoothingRegion> regions,
|
||||||
|
out string reason)
|
||||||
|
{
|
||||||
|
regions = Empty<LocalG2SmoothingRegion>();
|
||||||
|
reason = string.Empty;
|
||||||
|
if (originalPath == null || transitions == null || options == null)
|
||||||
|
{
|
||||||
|
reason = "局部 G2 窗口规划输入无效。";
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
if (!TryGetSegmentLengths(originalPath, out Dictionary<int, double> segmentLengths, out reason)) return false;
|
||||||
|
|
||||||
|
var ordered = new List<CurvatureTransition>(transitions.Count);
|
||||||
|
for (int index = 0; index < transitions.Count; index++)
|
||||||
|
{
|
||||||
|
CurvatureTransition transition = transitions[index];
|
||||||
|
if (transition == null || !segmentLengths.TryGetValue(transition.SegmentIndex, out double length) ||
|
||||||
|
!NumericGuard.IsFinite(transition.LocalArcLengthMeters) || transition.LocalArcLengthMeters < 0d ||
|
||||||
|
transition.LocalArcLengthMeters > length + MergeToleranceMeters)
|
||||||
|
{
|
||||||
|
reason = "局部 G2 曲率事件不属于有效方向分段。";
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
ordered.Add(transition);
|
||||||
|
}
|
||||||
|
ordered.Sort(CompareTransitions);
|
||||||
|
|
||||||
|
var planned = new List<LocalG2SmoothingRegion>();
|
||||||
|
int cursor = 0;
|
||||||
|
while (cursor < ordered.Count)
|
||||||
|
{
|
||||||
|
CurvatureTransition first = ordered[cursor];
|
||||||
|
double segmentLength = segmentLengths[first.SegmentIndex];
|
||||||
|
WindowRange merged = MaximumLegalRange(first.LocalArcLengthMeters, segmentLength, options.MaximumWindowLengthMeters);
|
||||||
|
var group = new List<CurvatureTransition> { first };
|
||||||
|
cursor++;
|
||||||
|
|
||||||
|
while (cursor < ordered.Count && ordered[cursor].SegmentIndex == first.SegmentIndex)
|
||||||
|
{
|
||||||
|
CurvatureTransition next = ordered[cursor];
|
||||||
|
WindowRange nextRange = MaximumLegalRange(next.LocalArcLengthMeters, segmentLength, options.MaximumWindowLengthMeters);
|
||||||
|
if (nextRange.StartArcLengthMeters > merged.EndArcLengthMeters + MergeToleranceMeters) break;
|
||||||
|
group.Add(next);
|
||||||
|
merged = new WindowRange(
|
||||||
|
Math.Min(merged.StartArcLengthMeters, nextRange.StartArcLengthMeters),
|
||||||
|
Math.Max(merged.EndArcLengthMeters, nextRange.EndArcLengthMeters));
|
||||||
|
cursor++;
|
||||||
|
}
|
||||||
|
|
||||||
|
planned.Add(new LocalG2SmoothingRegion(
|
||||||
|
first.SegmentIndex,
|
||||||
|
group,
|
||||||
|
merged.StartArcLengthMeters,
|
||||||
|
merged.EndArcLengthMeters,
|
||||||
|
BuildVariants(group, segmentLength, options)));
|
||||||
|
}
|
||||||
|
regions = new ReadOnlyCollection<LocalG2SmoothingRegion>(planned);
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
private static IReadOnlyList<LocalG2WindowVariant> BuildVariants(
|
||||||
|
IReadOnlyList<CurvatureTransition> transitions,
|
||||||
|
double segmentLength,
|
||||||
|
LocalG2OptionsSnapshot options)
|
||||||
|
{
|
||||||
|
var variants = new List<LocalG2WindowVariant>();
|
||||||
|
double firstEvent = transitions[0].LocalArcLengthMeters;
|
||||||
|
double lastEvent = transitions[transitions.Count - 1].LocalArcLengthMeters;
|
||||||
|
double anchor = (firstEvent + lastEvent) / 2d;
|
||||||
|
foreach (double target in BuildTargets(options, segmentLength))
|
||||||
|
{
|
||||||
|
if (variants.Count >= options.MaximumCandidatesPerRegion) break;
|
||||||
|
AddIfLegal(variants, target, 0.5d, anchor, firstEvent, lastEvent, segmentLength, true, options.MaximumCandidatesPerRegion);
|
||||||
|
AddIfLegal(variants, target, 0.4d, anchor, firstEvent, lastEvent, segmentLength, false, options.MaximumCandidatesPerRegion);
|
||||||
|
AddIfLegal(variants, target, 0.6d, anchor, firstEvent, lastEvent, segmentLength, false, options.MaximumCandidatesPerRegion);
|
||||||
|
}
|
||||||
|
return new ReadOnlyCollection<LocalG2WindowVariant>(variants);
|
||||||
|
}
|
||||||
|
|
||||||
|
private static void AddIfLegal(List<LocalG2WindowVariant> variants, double target, double leftRatio,
|
||||||
|
double anchor, double firstEvent, double lastEvent, double segmentLength, bool permitBoundaryShift, int maximumCount)
|
||||||
|
{
|
||||||
|
if (variants.Count >= maximumCount) return;
|
||||||
|
double left = target * leftRatio;
|
||||||
|
double right = target - left;
|
||||||
|
double availableLeft = anchor;
|
||||||
|
double availableRight = segmentLength - anchor;
|
||||||
|
if (permitBoundaryShift)
|
||||||
|
{
|
||||||
|
left = Math.Min(left, availableLeft);
|
||||||
|
right = Math.Min(right, availableRight);
|
||||||
|
double missing = target - left - right;
|
||||||
|
double addRight = Math.Min(missing, availableRight - right);
|
||||||
|
right += addRight;
|
||||||
|
left += Math.Min(missing - addRight, availableLeft - left);
|
||||||
|
}
|
||||||
|
else if (left > availableLeft + MergeToleranceMeters || right > availableRight + MergeToleranceMeters)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
double start = anchor - left;
|
||||||
|
double end = anchor + right;
|
||||||
|
if (start > firstEvent + MergeToleranceMeters || end + MergeToleranceMeters < lastEvent || end - start + MergeToleranceMeters < target)
|
||||||
|
return;
|
||||||
|
variants.Add(new LocalG2WindowVariant(variants.Count, start, end, left, right));
|
||||||
|
}
|
||||||
|
|
||||||
|
private static IReadOnlyList<double> BuildTargets(LocalG2OptionsSnapshot options, double segmentLength)
|
||||||
|
{
|
||||||
|
double[] requested =
|
||||||
|
{
|
||||||
|
options.PreferredWindowLengthMeters,
|
||||||
|
0.75d * options.PreferredWindowLengthMeters,
|
||||||
|
1.25d * options.PreferredWindowLengthMeters,
|
||||||
|
options.MinimumWindowLengthMeters,
|
||||||
|
options.MaximumWindowLengthMeters,
|
||||||
|
};
|
||||||
|
var targets = new List<double>(requested.Length);
|
||||||
|
for (int index = 0; index < requested.Length; index++)
|
||||||
|
{
|
||||||
|
double target = Math.Min(segmentLength,
|
||||||
|
Math.Max(options.MinimumWindowLengthMeters, Math.Min(options.MaximumWindowLengthMeters, requested[index])));
|
||||||
|
bool duplicate = false;
|
||||||
|
for (int prior = 0; prior < targets.Count; prior++)
|
||||||
|
{
|
||||||
|
if (Math.Abs(targets[prior] - target) <= MergeToleranceMeters)
|
||||||
|
{
|
||||||
|
duplicate = true;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (!duplicate) targets.Add(target);
|
||||||
|
}
|
||||||
|
return targets;
|
||||||
|
}
|
||||||
|
|
||||||
|
private static WindowRange MaximumLegalRange(double eventArcLength, double segmentLength, double maximumWindowLength)
|
||||||
|
{
|
||||||
|
double target = Math.Min(maximumWindowLength, segmentLength);
|
||||||
|
return new WindowRange(
|
||||||
|
Math.Max(0d, eventArcLength - target),
|
||||||
|
Math.Min(segmentLength, eventArcLength + target));
|
||||||
|
}
|
||||||
|
|
||||||
|
private static bool TryGetSegmentLengths(PreparedPath originalPath, out Dictionary<int, double> lengths, out string reason)
|
||||||
|
{
|
||||||
|
lengths = new Dictionary<int, double>();
|
||||||
|
reason = string.Empty;
|
||||||
|
for (int position = 0; position < originalPath.Segments.Count; position++)
|
||||||
|
{
|
||||||
|
PreparedDirectionSegment segment = originalPath.Segments[position];
|
||||||
|
if (segment == null || segment.SegmentIndex != position || segment.Points == null || segment.Points.Count == 0)
|
||||||
|
{
|
||||||
|
reason = "局部 G2 窗口规划的预处理方向分段无效。";
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
double previousArc = -1d;
|
||||||
|
for (int pointIndex = 0; pointIndex < segment.Points.Count; pointIndex++)
|
||||||
|
{
|
||||||
|
double arc = segment.Points[pointIndex].ArcLength;
|
||||||
|
if (!NumericGuard.IsFinite(arc) || arc < 0d || arc < previousArc)
|
||||||
|
{
|
||||||
|
reason = "局部 G2 窗口规划要求分段弧长有限且非递减。";
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
previousArc = arc;
|
||||||
|
}
|
||||||
|
lengths.Add(segment.SegmentIndex, previousArc);
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
private static int CompareTransitions(CurvatureTransition left, CurvatureTransition right)
|
||||||
|
{
|
||||||
|
int segment = left.SegmentIndex.CompareTo(right.SegmentIndex);
|
||||||
|
if (segment != 0) return segment;
|
||||||
|
int arc = left.LocalArcLengthMeters.CompareTo(right.LocalArcLengthMeters);
|
||||||
|
return arc != 0 ? arc : left.LeftCoarsePathIndex.CompareTo(right.LeftCoarsePathIndex);
|
||||||
|
}
|
||||||
|
|
||||||
|
private static IReadOnlyList<T> Empty<T>() => new ReadOnlyCollection<T>(new List<T>());
|
||||||
|
|
||||||
|
private readonly struct WindowRange
|
||||||
|
{
|
||||||
|
internal WindowRange(double startArcLengthMeters, double endArcLengthMeters)
|
||||||
|
{
|
||||||
|
StartArcLengthMeters = startArcLengthMeters;
|
||||||
|
EndArcLengthMeters = endArcLengthMeters;
|
||||||
|
}
|
||||||
|
|
||||||
|
internal double StartArcLengthMeters { get; }
|
||||||
|
internal double EndArcLengthMeters { get; }
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -0,0 +1,58 @@
|
|||||||
|
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)
|
||||||
|
}
|
||||||
|
|
||||||
|
$detectorType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2.CurvatureTransitionDetector'
|
||||||
|
$hooksType = $detectorType.GetNestedType('TestHooks', [Reflection.BindingFlags]'Public,NonPublic')
|
||||||
|
Assert-True ($null -ne $hooksType) 'CurvatureTransitionDetector must expose its narrowly scoped nested TestHooks helper.'
|
||||||
|
$executeMethod = $hooksType.GetMethod('Execute', [Reflection.BindingFlags]'Public,Static')
|
||||||
|
Assert-True ($null -ne $executeMethod) 'TestHooks must expose deterministic scenario execution for reflection tests.'
|
||||||
|
|
||||||
|
function Invoke-Scenario([string]$Scenario) {
|
||||||
|
return $executeMethod.Invoke($null, @($Scenario))
|
||||||
|
}
|
||||||
|
|
||||||
|
# Same direction: 0 -> 0.4167 is detected once.
|
||||||
|
$singleTransition = Invoke-Scenario 'SingleTransition'
|
||||||
|
Assert-Equal 1 $singleTransition.TransitionCount 'One primitive curvature jump must be detected.'
|
||||||
|
Assert-Near 0.4167 $singleTransition.MaximumJump 0.0001 'The jump magnitude must be retained.'
|
||||||
|
|
||||||
|
# Same pose at a forward/reverse boundary: no event crosses the stop.
|
||||||
|
$gearSwitch = Invoke-Scenario 'GearSwitch'
|
||||||
|
Assert-Equal 0 $gearSwitch.TransitionCount 'A stopped gear switch must not be a smoothing event.'
|
||||||
|
|
||||||
|
# A 0.01 1/m numerical change is below max(0.001, 5% of 0.8333).
|
||||||
|
$noise = Invoke-Scenario 'Noise'
|
||||||
|
Assert-Equal 0 $noise.TransitionCount 'Sub-threshold curvature noise must be ignored.'
|
||||||
|
|
||||||
|
# Two 0.50 m windows whose ranges overlap are merged.
|
||||||
|
$overlap = Invoke-Scenario 'Overlap'
|
||||||
|
Assert-Equal 1 $overlap.RegionCount 'Overlapping windows must form one joint region.'
|
||||||
|
Assert-Equal 2 $overlap.TransitionCountInFirstRegion 'The merged region must retain both events.'
|
||||||
|
|
||||||
|
# Near a segment start, the window becomes asymmetric without crossing the hard boundary.
|
||||||
|
$nearStart = Invoke-Scenario 'NearStart'
|
||||||
|
Assert-Near 0.0 $nearStart.StartArcLength 0.000000001 'A start window must be clamped to the segment.'
|
||||||
|
Assert-True ($nearStart.RightWindowLength -gt $nearStart.LeftWindowLength) `
|
||||||
|
'Unavailable left length must be shifted to the right.'
|
||||||
|
|
||||||
|
Write-Output 'Path smoothing Local G2 detection checks passed.'
|
||||||
Reference in New Issue
Block a user