feat: add reverse-safe Frenet transforms

2026-08-03 22:43:39 +08:00
parent aac48835f1
commit 06687e294c
7 changed files with 475 additions and 3 deletions
@@ -0,0 +1,33 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>World pose projected onto one bounded direction segment.</summary>
+public sealed class FrenetProjection
+{
+    public FrenetProjection(FrenetReferencePoint referencePoint, double lateralOffset, double headingError,
+        double squaredDistanceMeters)
+    {
+        if (referencePoint == null)
+            throw new ArgumentNullException(nameof(referencePoint));
+        if (!IsFinite(lateralOffset) || !IsFinite(headingError) || !IsFinite(squaredDistanceMeters) || squaredDistanceMeters < 0d)
+            throw new ArgumentOutOfRangeException(nameof(lateralOffset));
+
+        ReferencePoint = referencePoint;
+        ReferenceS = referencePoint.ReferenceS;
+        LateralOffset = lateralOffset;
+        HeadingError = headingError;
+        SquaredDistanceMeters = squaredDistanceMeters;
+    }
+
+    public FrenetReferencePoint ReferencePoint { get; }
+    public double ReferenceS { get; }
+    public double LateralOffset { get; }
+    public double HeadingError { get; }
+    public double SquaredDistanceMeters { get; }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}
@@ -0,0 +1,131 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Deterministically projects a world pose only inside the supplied direction segment and S window.</summary>
+public sealed class FrenetProjector
+{
+    private const double TieTolerance = 1e-14d;
+
+    public bool TryProject(Pose2D worldPose, DirectionSegmentView segment, double minimumReferenceS,
+        double maximumReferenceS, double maximumDistanceMeters, out FrenetProjection projection)
+    {
+        return TryProject(worldPose, segment, minimumReferenceS, maximumReferenceS, maximumDistanceMeters,
+            minimumReferenceS, out projection);
+    }
+
+    public bool TryProject(Pose2D worldPose, DirectionSegmentView segment, double minimumReferenceS,
+        double maximumReferenceS, double maximumDistanceMeters, double seedReferenceS, out FrenetProjection projection)
+    {
+        projection = null;
+        if (worldPose == null || segment == null || !IsFinite(worldPose.X) || !IsFinite(worldPose.Y) ||
+            !IsFinite(worldPose.Heading) || !IsFinite(minimumReferenceS) || !IsFinite(maximumReferenceS) ||
+            !IsFinite(maximumDistanceMeters) || !IsFinite(seedReferenceS) || maximumDistanceMeters < 0d)
+            return false;
+
+        double lowerBound = Math.Max(0d, minimumReferenceS);
+        double upperBound = Math.Min(segment.LengthMeters, maximumReferenceS);
+        if (lowerBound > upperBound)
+            return false;
+
+        Candidate best = null;
+        for (int index = 0; index + 1 < segment.Points.Count; index++)
+        {
+            double startS = Math.Max(lowerBound, segment.Points[index].ArcLength);
+            double endS = Math.Min(upperBound, segment.Points[index + 1].ArcLength);
+            if (startS > endS)
+                continue;
+
+            FrenetReferencePoint start = ReferencePathInterpolator.Interpolate(segment, startS);
+            FrenetReferencePoint end = ReferencePathInterpolator.Interpolate(segment, endS);
+            ConsiderLine(worldPose, start, end, seedReferenceS, ref best);
+        }
+
+        if (best == null && Math.Abs(lowerBound - upperBound) <= TieTolerance)
+        {
+            FrenetReferencePoint point = ReferencePathInterpolator.Interpolate(segment, lowerBound);
+            ConsiderPoint(worldPose, point, seedReferenceS, ref best);
+        }
+
+        if (best == null || best.SquaredDistanceMeters > maximumDistanceMeters * maximumDistanceMeters)
+            return false;
+
+        FrenetReferencePoint reference = ReferencePathInterpolator.Interpolate(segment, best.ReferenceS);
+        double dx = worldPose.X - reference.X;
+        double dy = worldPose.Y - reference.Y;
+        double travelYaw = reference.TravelYaw;
+        double lateralOffset = -dx * Math.Sin(travelYaw) + dy * Math.Cos(travelYaw);
+        double egoTravelYaw = FrenetTransform.GetTravelYaw(worldPose.Heading, segment.Direction);
+        double headingError = AngleMath.NormalizeRadians(egoTravelYaw - travelYaw);
+        projection = new FrenetProjection(reference, lateralOffset, headingError, best.SquaredDistanceMeters);
+        return true;
+    }
+
+    private static void ConsiderLine(Pose2D worldPose, FrenetReferencePoint start, FrenetReferencePoint end,
+        double seedReferenceS, ref Candidate best)
+    {
+        double dx = end.X - start.X;
+        double dy = end.Y - start.Y;
+        double lengthSquared = dx * dx + dy * dy;
+        if (lengthSquared <= TieTolerance)
+        {
+            ConsiderPoint(worldPose, start, seedReferenceS, ref best);
+            ConsiderPoint(worldPose, end, seedReferenceS, ref best);
+            return;
+        }
+
+        double fraction = ((worldPose.X - start.X) * dx + (worldPose.Y - start.Y) * dy) / lengthSquared;
+        fraction = Math.Max(0d, Math.Min(1d, fraction));
+        double referenceS = start.ReferenceS + (end.ReferenceS - start.ReferenceS) * fraction;
+        double x = start.X + dx * fraction;
+        double y = start.Y + dy * fraction;
+        Consider(worldPose, referenceS, x, y, seedReferenceS, ref best);
+    }
+
+    private static void ConsiderPoint(Pose2D worldPose, FrenetReferencePoint point, double seedReferenceS,
+        ref Candidate best)
+    {
+        Consider(worldPose, point.ReferenceS, point.X, point.Y, seedReferenceS, ref best);
+    }
+
+    private static void Consider(Pose2D worldPose, double referenceS, double x, double y, double seedReferenceS,
+        ref Candidate best)
+    {
+        double dx = worldPose.X - x;
+        double dy = worldPose.Y - y;
+        double squaredDistance = dx * dx + dy * dy;
+        var candidate = new Candidate(referenceS, squaredDistance, Math.Abs(referenceS - seedReferenceS));
+        if (best == null || candidate.IsPreferredTo(best))
+            best = candidate;
+    }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+
+    private sealed class Candidate
+    {
+        public Candidate(double referenceS, double squaredDistanceMeters, double seedDistance)
+        {
+            ReferenceS = referenceS;
+            SquaredDistanceMeters = squaredDistanceMeters;
+            SeedDistance = seedDistance;
+        }
+
+        public double ReferenceS { get; }
+        public double SquaredDistanceMeters { get; }
+        public double SeedDistance { get; }
+
+        public bool IsPreferredTo(Candidate other)
+        {
+            if (SquaredDistanceMeters < other.SquaredDistanceMeters - TieTolerance) return true;
+            if (SquaredDistanceMeters > other.SquaredDistanceMeters + TieTolerance) return false;
+            if (SeedDistance < other.SeedDistance - TieTolerance) return true;
+            if (SeedDistance > other.SeedDistance + TieTolerance) return false;
+            return ReferenceS < other.ReferenceS;
+        }
+    }
+}
@@ -0,0 +1,58 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Immutable interpolated reference sample in a single direction segment.</summary>
+public sealed class FrenetReferencePoint
+{
+    public FrenetReferencePoint(double referenceS, double x, double y, double vehicleYaw, double unwrappedVehicleYaw,
+        TravelDirection direction, double geometricCurvature, double vehicleCurvature,
+        double vehicleCurvatureDerivative, double bodyClearance)
+    {
+        RequireFinite(referenceS, nameof(referenceS));
+        RequireFinite(x, nameof(x));
+        RequireFinite(y, nameof(y));
+        RequireFinite(vehicleYaw, nameof(vehicleYaw));
+        RequireFinite(unwrappedVehicleYaw, nameof(unwrappedVehicleYaw));
+        RequireFinite(geometricCurvature, nameof(geometricCurvature));
+        RequireFinite(vehicleCurvature, nameof(vehicleCurvature));
+        RequireFinite(vehicleCurvatureDerivative, nameof(vehicleCurvatureDerivative));
+        RequireFinite(bodyClearance, nameof(bodyClearance));
+
+        ReferenceS = referenceS;
+        X = x;
+        Y = y;
+        VehicleYaw = AngleMath.NormalizeRadians(vehicleYaw);
+        UnwrappedVehicleYaw = unwrappedVehicleYaw;
+        Direction = direction;
+        GeometricCurvature = geometricCurvature;
+        VehicleCurvature = vehicleCurvature;
+        VehicleCurvatureDerivative = vehicleCurvatureDerivative;
+        BodyClearance = bodyClearance;
+    }
+
+    public double ReferenceS { get; }
+    public double X { get; }
+    public double Y { get; }
+    public double VehicleYaw { get; }
+    public double UnwrappedVehicleYaw { get; }
+    public TravelDirection Direction { get; }
+    public double GeometricCurvature { get; }
+    public double VehicleCurvature { get; }
+    public double VehicleCurvatureDerivative { get; }
+    public double BodyClearance { get; }
+
+    /// <summary>Unwrapped direction of travel, used internally for geometry.</summary>
+    public double TravelYaw
+    {
+        get { return Direction == TravelDirection.Forward ? UnwrappedVehicleYaw : UnwrappedVehicleYaw + Math.PI; }
+    }
+
+    private static void RequireFinite(double value, string name)
+    {
+        if (double.IsNaN(value) || double.IsInfinity(value))
+            throw new ArgumentOutOfRangeException(name);
+    }
+}
@@ -0,0 +1,45 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Coordinate conversion that keeps Frenet lateral sign relative to travel direction.</summary>
+public static class FrenetTransform
+{
+    public static bool TryReconstruct(FrenetReferencePoint referencePoint, double lateralOffset, double lateralDerivative,
+        double minimumFrenetDenominator, out Pose2D pose)
+    {
+        pose = null;
+        if (referencePoint == null || !IsFinite(lateralOffset) || !IsFinite(lateralDerivative) ||
+            !IsFinite(minimumFrenetDenominator) || minimumFrenetDenominator <= 0d)
+            return false;
+
+        double denominator = 1d - referencePoint.GeometricCurvature * lateralOffset;
+        if (!IsFinite(denominator) || denominator < minimumFrenetDenominator)
+            return false;
+
+        double travelYaw = referencePoint.TravelYaw;
+        double x = referencePoint.X - lateralOffset * Math.Sin(travelYaw);
+        double y = referencePoint.Y + lateralOffset * Math.Cos(travelYaw);
+        double optimizedTravelYaw = travelYaw + Math.Atan2(lateralDerivative, denominator);
+        double vehicleYaw = referencePoint.Direction == TravelDirection.Forward
+            ? AngleMath.NormalizeRadians(optimizedTravelYaw)
+            : AngleMath.NormalizeRadians(optimizedTravelYaw + Math.PI);
+        if (!IsFinite(x) || !IsFinite(y) || !IsFinite(vehicleYaw))
+            return false;
+
+        pose = new Pose2D(x, y, vehicleYaw);
+        return true;
+    }
+
+    internal static double GetTravelYaw(double vehicleYaw, TravelDirection direction)
+    {
+        return direction == TravelDirection.Forward ? vehicleYaw : vehicleYaw + Math.PI;
+    }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}
@@ -0,0 +1,64 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Interpolates reference geometry inside exactly one direction segment.</summary>
+public static class ReferencePathInterpolator
+{
+    private const double Epsilon = 1e-12d;
+
+    public static FrenetReferencePoint Interpolate(DirectionSegmentView segment, double referenceS)
+    {
+        if (segment == null)
+            throw new ArgumentNullException(nameof(segment));
+        if (!IsFinite(referenceS) || referenceS < -Epsilon || referenceS > segment.LengthMeters + Epsilon)
+            throw new ArgumentOutOfRangeException(nameof(referenceS));
+
+        double clampedS = Math.Max(0d, Math.Min(segment.LengthMeters, referenceS));
+        for (int index = 0; index < segment.Points.Count; index++)
+        {
+            SmoothedPathPoint upper = segment.Points[index];
+            if (Math.Abs(upper.ArcLength - clampedS) <= Epsilon)
+                return FromPoint(upper);
+            if (upper.ArcLength > clampedS)
+            {
+                SmoothedPathPoint lower = segment.Points[index - 1];
+                double span = upper.ArcLength - lower.ArcLength;
+                if (span <= Epsilon)
+                    throw new ArgumentException("Reference points must have positive interpolation spans.", nameof(segment));
+                double fraction = (clampedS - lower.ArcLength) / span;
+                return new FrenetReferencePoint(
+                    clampedS,
+                    Lerp(lower.X, upper.X, fraction),
+                    Lerp(lower.Y, upper.Y, fraction),
+                    AngleMath.NormalizeRadians(Lerp(lower.UnwrappedHeading, upper.UnwrappedHeading, fraction)),
+                    Lerp(lower.UnwrappedHeading, upper.UnwrappedHeading, fraction),
+                    segment.Direction,
+                    Lerp(lower.GeometricCurvature, upper.GeometricCurvature, fraction),
+                    Lerp(lower.VehicleCurvature, upper.VehicleCurvature, fraction),
+                    Lerp(lower.VehicleCurvatureDerivative, upper.VehicleCurvatureDerivative, fraction),
+                    Lerp(lower.BodyClearance, upper.BodyClearance, fraction));
+            }
+        }
+        return FromPoint(segment.Points[segment.Points.Count - 1]);
+    }
+
+    private static FrenetReferencePoint FromPoint(SmoothedPathPoint point)
+    {
+        return new FrenetReferencePoint(point.ArcLength, point.X, point.Y, point.Heading, point.UnwrappedHeading,
+            point.Direction, point.GeometricCurvature, point.VehicleCurvature, point.VehicleCurvatureDerivative,
+            point.BodyClearance);
+    }
+
+    private static double Lerp(double lower, double upper, double fraction)
+    {
+        return lower + (upper - lower) * fraction;
+    }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}
@@ -0,0 +1,136 @@
+using System;
+using System.Collections.Generic;
+using EMPlannerVerificationHost;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+internal static class FrenetChecks
+{
+    public static void Run()
+    {
+        VerifiesForwardProjectionAndReconstruction();
+        VerifiesReverseProjectionAcrossYawWrap();
+        VerifiesBoundedProjectionOnALoop();
+    }
+
+    private static void VerifiesForwardProjectionAndReconstruction()
+    {
+        DirectionSegmentView segment = CreateStraightSegment(TravelDirection.Forward, 0d, 0d);
+        var world = new Pose2D(1.5d, 0.2d, 0d);
+        var projector = new FrenetProjector();
+
+        Verification.True(projector.TryProject(world, segment, 0d, 4d, 0.5d, out FrenetProjection projection),
+            "forward projection succeeds");
+        Verification.NearlyEqual(1.5d, projection.ReferenceS, "forward reference s");
+        Verification.NearlyEqual(0.2d, projection.LateralOffset, "forward positive l is travel-left");
+        Verification.True(FrenetTransform.TryReconstruct(projection.ReferencePoint, projection.LateralOffset, 0d, 0.2d,
+            out Pose2D reconstructed), "forward reconstruction succeeds");
+        Verification.NearlyEqual(world.X, reconstructed.X, "forward reconstructed x");
+        Verification.NearlyEqual(world.Y, reconstructed.Y, "forward reconstructed y");
+        Verification.NearlyEqual(0d, reconstructed.Heading, "forward reconstructed vehicle yaw");
+    }
+
+    private static void VerifiesReverseProjectionAcrossYawWrap()
+    {
+        const double vehicleYaw = -Math.PI + 0.01d;
+        const double travelYaw = 0.01d;
+        DirectionSegmentView segment = CreateStraightSegment(TravelDirection.Reverse, vehicleYaw, travelYaw);
+        double expectedS = 1.5d;
+        double expectedL = 0.2d;
+        var world = new Pose2D(
+            expectedS * Math.Cos(travelYaw) - expectedL * Math.Sin(travelYaw),
+            expectedS * Math.Sin(travelYaw) + expectedL * Math.Cos(travelYaw),
+            vehicleYaw);
+        var projector = new FrenetProjector();
+
+        Verification.True(projector.TryProject(world, segment, 0d, 4d, 0.5d, out FrenetProjection projection),
+            "reverse projection succeeds");
+        Verification.NearlyEqual(expectedS, projection.ReferenceS, "reverse reference s");
+        Verification.NearlyEqual(expectedL, projection.LateralOffset, "reverse positive l is travel-left and body-right");
+        Verification.True(FrenetTransform.TryReconstruct(projection.ReferencePoint, projection.LateralOffset, 0d, 0.2d,
+            out Pose2D reconstructed), "reverse reconstruction succeeds");
+        Verification.NearlyEqual(world.X, reconstructed.X, "reverse reconstructed x");
+        Verification.NearlyEqual(world.Y, reconstructed.Y, "reverse reconstructed y");
+        Verification.NearlyEqual(AngleMath.NormalizeRadians(vehicleYaw), reconstructed.Heading,
+            "reverse reconstructed vehicle yaw");
+
+        DirectionSegmentView wrapped = CreateWrappedForwardSegment();
+        FrenetReferencePoint wrappedPoint = ReferencePathInterpolator.Interpolate(wrapped, 1.5d);
+        Verification.NearlyEqual(3.18d, wrappedPoint.UnwrappedVehicleYaw, "interpolated yaw remains unwrapped internally");
+        Verification.NearlyEqual(AngleMath.NormalizeRadians(3.18d), wrappedPoint.VehicleYaw,
+            "interpolated public yaw is normalized");
+        Verification.True(!FrenetTransform.TryReconstruct(new FrenetReferencePoint(0d, 0d, 0d, 0d, 0d,
+            TravelDirection.Forward, 1d, 0d, 0d, 0d), 1d, 0d, 0.2d, out _),
+            "singular Frenet reconstruction is rejected");
+    }
+
+    private static void VerifiesBoundedProjectionOnALoop()
+    {
+        DirectionSegmentView segment = CreateLoopSegment();
+        var projector = new FrenetProjector();
+        var world = new Pose2D(1d, 0.2d, Math.PI);
+
+        Verification.True(projector.TryProject(world, segment, 2.2d, 4.2d, 0.1d, out FrenetProjection projection),
+            "bounded loop projection succeeds");
+        Verification.NearlyEqual(3.2d, projection.ReferenceS, "bounded loop picks the local upper branch");
+        Verification.True(projection.ReferenceS >= 2.2d && projection.ReferenceS <= 4.2d,
+            "projection remains in supplied reference-s interval");
+        Verification.True(FrenetTransform.TryReconstruct(projection.ReferencePoint, projection.LateralOffset, 0d, 0.2d,
+            out Pose2D reconstructed), "loop reconstruction succeeds");
+        Verification.NearlyEqual(world.X, reconstructed.X, "loop reconstructed x");
+        Verification.NearlyEqual(world.Y, reconstructed.Y, "loop reconstructed y");
+    }
+
+    private static DirectionSegmentView CreateStraightSegment(TravelDirection direction, double vehicleYaw,
+        double travelYaw)
+    {
+        var points = new List<SmoothedPathPoint>
+        {
+            Point(0d, 0d, AngleMath.NormalizeRadians(vehicleYaw), vehicleYaw, 0d, direction),
+            Point(4d * Math.Cos(travelYaw), 4d * Math.Sin(travelYaw), AngleMath.NormalizeRadians(vehicleYaw),
+                vehicleYaw, 4d, direction),
+        };
+        return CreateSegment(direction, points);
+    }
+
+    private static DirectionSegmentView CreateWrappedForwardSegment()
+    {
+        var points = new List<SmoothedPathPoint>
+        {
+            Point(0d, 0d, AngleMath.NormalizeRadians(3.12d), 3.12d, 0d, TravelDirection.Forward),
+            Point(1d, 0d, AngleMath.NormalizeRadians(3.16d), 3.16d, 1d, TravelDirection.Forward),
+            Point(2d, 0d, AngleMath.NormalizeRadians(3.20d), 3.20d, 2d, TravelDirection.Forward),
+        };
+        return CreateSegment(TravelDirection.Forward, points);
+    }
+
+    private static DirectionSegmentView CreateLoopSegment()
+    {
+        var points = new List<SmoothedPathPoint>
+        {
+            Point(0d, 0d, 0d, 0d, 0d, TravelDirection.Forward),
+            Point(2d, 0d, 0d, 0d, 2d, TravelDirection.Forward),
+            Point(2d, 0.2d, Math.PI / 2d, Math.PI / 2d, 2.2d, TravelDirection.Forward),
+            Point(0d, 0.2d, Math.PI, Math.PI, 4.2d, TravelDirection.Forward),
+        };
+        return CreateSegment(TravelDirection.Forward, points);
+    }
+
+    private static DirectionSegmentView CreateSegment(TravelDirection direction, IReadOnlyList<SmoothedPathPoint> points)
+    {
+        double length = points[points.Count - 1].ArcLength;
+        return new DirectionSegmentView(0, direction, points,
+            new ReferenceBoundary(0, 0d, EmBoundaryType.None, 0d),
+            new ReferenceBoundary(0, length, EmBoundaryType.Goal, length), 0d);
+    }
+
+    private static SmoothedPathPoint Point(double x, double y, double heading, double unwrappedHeading, double s,
+        TravelDirection direction)
+    {
+        return new SmoothedPathPoint(x, y, heading, unwrappedHeading, s, direction, 0d, 0d, 0d, 1d,
+            false, SmoothedPathPointSource.Anchor);
+    }
+}
@@ -6,9 +6,9 @@ internal static class Program
 {
    private static int Main(string[] args)
    {
-        if (args.Length != 1 || (args[0] != "foundation" && args[0] != "segmentation"))
+        if (args.Length != 1 || (args[0] != "foundation" && args[0] != "segmentation" && args[0] != "frenet"))
        {
-            Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation|segmentation");
+            Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation|segmentation|frenet");
            return 2;
        }

@@ -19,11 +19,16 @@ internal static class Program
                MultiWheelC.TrajectoryPlanning.EMPlanner.FoundationChecks.Run();
                Console.WriteLine("PASS foundation");
            }
-            else
+            else if (args[0] == "segmentation")
            {
                MultiWheelC.TrajectoryPlanning.EMPlanner.SegmentationChecks.Run();
                Console.WriteLine("PASS segmentation");
            }
+            else
+            {
+                MultiWheelC.TrajectoryPlanning.EMPlanner.FrenetChecks.Run();
+                Console.WriteLine("PASS frenet");
+            }
            return 0;
        }
        catch (Exception exception)