feat: validate lateral path geometry

2026-08-04 01:03:32 +08:00
parent f703d418ab
commit 0c48a7de7e
3 changed files with 681 additions and 0 deletions
@@ -5,6 +5,7 @@ using EMPlannerVerificationHost;
 using MultiWheelC.TrajectoryPlanning.CoarsePath;
 using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
 using MultiWheelC.TrajectoryPlanning.PathSmoothing;
+using MultiWheelC.TrajectoryPlanning.Utils;

 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

@@ -20,6 +21,8 @@ internal static class LateralModelChecks
        VerifiesAllNamedCostScales();
        VerifiesEmptyHardBoundIntersectionFailsBeforeSolve();
        VerifiesFakeSolverCapturesTheNeutralQpBoundary();
+        VerifiesNonlinearGeometryInBothDirections();
+        VerifiesIndependentGeometryValidationRejectsUnsafeOrTamperedPaths();
    }

    private static void VerifiesDeterministicVariableLayout()
@@ -275,6 +278,114 @@ internal static class LateralModelChecks
        Verification.NearlyEqual(2d, solver.LastWarmStart[1], "fake solver records a defensive warm-start copy");
    }

+    private static void VerifiesNonlinearGeometryInBothDirections()
+    {
+        double[] stations = { 0d, 0.7d, 2d };
+        LateralCandidate candidate = LateralCandidate.Integrate(stations, 0.1d, 0.1d, 0.05d,
+            new[] { 0.02d, -0.03d });
+        LateralGeometryEvaluator evaluator = CreateGeometryEvaluator();
+        LateralSolutionValidator validator = CreateGeometryValidator();
+
+        VerifyGeometryForDirection(TravelDirection.Forward, 0d, candidate, evaluator, validator);
+        VerifyGeometryForDirection(TravelDirection.Reverse, 0d, candidate, evaluator, validator);
+        VerifyGeometryForDirection(TravelDirection.Forward, 0.2d, candidate, evaluator, validator);
+        VerifyGeometryForDirection(TravelDirection.Reverse, 0.2d, candidate, evaluator, validator);
+    }
+
+    private static void VerifiesIndependentGeometryValidationRejectsUnsafeOrTamperedPaths()
+    {
+        LateralGeometryEvaluator evaluator = CreateGeometryEvaluator();
+        LateralSolutionValidator validator = CreateGeometryValidator();
+        double[] stations = { 0d, 1d, 2d };
+
+        LateralPlanningInput singularInput = CreateGeometryInput(TravelDirection.Forward, 1d, stations, 0.81d, 0d, 10d);
+        LateralCandidate singular = LateralCandidate.Integrate(stations, 0.81d, 0d, 0d, new[] { 0d, 0d });
+        Verification.True(!evaluator.TryEvaluate(singularInput, singular, out LateralPath singularPath, out string singularReason),
+            "Frenet denominator below 0.20 is rejected by reconstruction");
+        Verification.True(singularPath == null && singularReason.Length > 0, "singular reconstruction has no path");
+
+        LateralPlanningInput curvatureInput = CreateGeometryInput(TravelDirection.Forward, 0d, stations, 0d, 0d, 1d);
+        LateralCandidate excessiveCurvature = LateralCandidate.Integrate(stations, 0d, 0d, 2d, new[] { 0d, 0d });
+        Verification.True(evaluator.TryEvaluate(curvatureInput, excessiveCurvature, out LateralPath excessivePath,
+            out string excessiveReason), "curvature reconstruction remains geometric: " + excessiveReason);
+        Verification.True(!validator.TryValidate(curvatureInput, excessiveCurvature, excessivePath,
+            out LateralPath rejectedCurvature, out string curvatureReason), "curvature above vehicle limit is rejected");
+        Verification.True(rejectedCurvature == null && curvatureReason.Length > 0, "curvature failure has no validated path");
+
+        LateralCandidate valid = LateralCandidate.Integrate(stations, 0d, 0d, 0d, new[] { 0d, 0d });
+        Verification.True(evaluator.TryEvaluate(curvatureInput, valid, out LateralPath rawPath, out string rawReason),
+            "valid path reconstructs: " + rawReason);
+        var tamperedPoints = new List<LateralPathPoint>(rawPath.Points);
+        LateralPathPoint original = tamperedPoints[1];
+        tamperedPoints[1] = new LateralPathPoint(original.ReferenceS, original.PathS, original.L, original.DL,
+            original.DDL, original.DDDL, original.X + 0.01d, original.Y, original.VehicleYaw,
+            original.GeometricCurvature, original.VehicleCurvature, original.VehicleCurvatureDerivative);
+        Verification.True(!validator.TryValidate(curvatureInput, valid, new LateralPath(tamperedPoints, false),
+            out LateralPath rejectedTampered, out string tamperedReason),
+            "validator independently rejects a world-coordinate mismatch");
+        Verification.True(rejectedTampered == null && tamperedReason.Length > 0, "tampered path has no validated copy");
+
+        ExpectArgumentException(() => new LateralCandidate(stations, new[] { double.NaN, 0d, 0d },
+            new[] { 0d, 0d, 0d }, new[] { 0d, 0d, 0d }, new[] { 0d, 0d }), "non-finite lateral values are rejected");
+        ExpectArgumentException(() => new LateralPathPoint(0d, 0d, 0d, 0d, 0d, 0d, double.NaN, 0d, 0d, 0d, 0d, 0d),
+            "non-finite world geometry is rejected");
+    }
+
+    private static void VerifyGeometryForDirection(TravelDirection direction, double referenceCurvature,
+        LateralCandidate candidate,
+        LateralGeometryEvaluator evaluator, LateralSolutionValidator validator)
+    {
+        LateralPlanningInput input = CreateGeometryInput(direction, referenceCurvature, candidate.ReferenceStations,
+            candidate.L[0], candidate.DL[0], 10d);
+        Verification.True(evaluator.TryEvaluate(input, candidate, out LateralPath rawPath, out string evaluationReason),
+            direction + " geometry reconstructs: " + evaluationReason);
+        Verification.True(!rawPath.IsIndependentlyValidated, direction + " evaluator does not self-validate");
+        Verification.True(validator.TryValidate(input, candidate, rawPath, out LateralPath validatedPath,
+            out string validationReason), direction + " geometry validates: " + validationReason);
+        Verification.True(validatedPath.IsIndependentlyValidated, direction + " validation creates a marked immutable path");
+
+        double directionSign = direction == TravelDirection.Forward ? 1d : -1d;
+        Verification.NearlyEqual(0d, rawPath.Points[0].PathS, direction + " PathS starts at zero");
+        for (int index = 0; index < rawPath.Points.Count; index++)
+        {
+            LateralPathPoint point = rawPath.Points[index];
+            FrenetReferencePoint reference = ReferencePathInterpolator.Interpolate(input.ReferenceSegment, point.ReferenceS);
+            double denominator = 1d - reference.GeometricCurvature * point.L;
+            double expectedX = reference.X - point.L * Math.Sin(reference.TravelYaw);
+            double expectedY = reference.Y + point.L * Math.Cos(reference.TravelYaw);
+            double expectedTravelYaw = reference.TravelYaw + Math.Atan2(point.DL, denominator);
+            double expectedVehicleYaw = direction == TravelDirection.Forward
+                ? AngleMath.NormalizeRadians(expectedTravelYaw)
+                : AngleMath.NormalizeRadians(expectedTravelYaw + Math.PI);
+            double expectedGeometricCurvature = CalculateGeometricCurvature(reference, point.L, point.DL, point.DDL,
+                directionSign * reference.VehicleCurvatureDerivative);
+            Verification.NearlyEqual(expectedX, point.X, direction + " world x " + index);
+            Verification.NearlyEqual(expectedY, point.Y, direction + " world y " + index);
+            Verification.NearlyEqual(expectedVehicleYaw, point.VehicleYaw, direction + " vehicle yaw " + index);
+            Verification.NearlyEqual(expectedGeometricCurvature, point.GeometricCurvature,
+                direction + " full Frenet curvature " + index);
+            Verification.NearlyEqual(directionSign * point.GeometricCurvature, point.VehicleCurvature,
+                direction + " vehicle curvature sign " + index);
+            if (index > 0)
+            {
+                LateralPathPoint previous = rawPath.Points[index - 1];
+                double chord = Math.Sqrt((point.X - previous.X) * (point.X - previous.X) +
+                    (point.Y - previous.Y) * (point.Y - previous.Y));
+                Verification.NearlyEqual(previous.PathS + chord, point.PathS, direction + " actual chord PathS " + index);
+                Verification.True(point.PathS > previous.PathS, direction + " PathS strictly increases " + index);
+            }
+        }
+
+        LateralPathPoint check = rawPath.Points[1];
+        double signedSpeed = directionSign * 0.3d;
+        var trajectoryPoint = new EmTrajectoryPoint(check.X, check.Y, check.VehicleYaw, signedSpeed, 0d,
+            check.VehicleCurvature, 0, check.ReferenceS, check.PathS, direction, EmBoundaryType.None, 0d, 0d);
+        Verification.NearlyEqual(signedSpeed * check.VehicleCurvature, trajectoryPoint.YawRate,
+            direction + " yaw-rate identity");
+        Verification.NearlyEqual(0.3d * check.GeometricCurvature, trajectoryPoint.YawRate,
+            direction + " travel curvature yaw-rate identity");
+    }
+
    private static DirectionSegmentView CreateStraightSegment(double referenceCurvatureDerivative = 0d,
        double referenceCurvature = 0d)
    {
@@ -327,6 +438,57 @@ internal static class LateralModelChecks
            CreateVehicle(maximumVehicleCurvature), configuration, seed);
    }

+    private static LateralPlanningInput CreateGeometryInput(TravelDirection direction, double referenceCurvature,
+        IReadOnlyList<double> stations, double startL, double startDL, double maximumVehicleCurvature)
+    {
+        var points = new List<SmoothedPathPoint>(stations.Count);
+        for (int index = 0; index < stations.Count; index++)
+        {
+            double s = stations[index];
+            double travelYaw = referenceCurvature * s;
+            double x = Math.Abs(referenceCurvature) <= 1e-12d ? s : Math.Sin(travelYaw) / referenceCurvature;
+            double y = Math.Abs(referenceCurvature) <= 1e-12d ? 0d : (1d - Math.Cos(travelYaw)) / referenceCurvature;
+            double vehicleYaw = direction == TravelDirection.Forward ? travelYaw : travelYaw - Math.PI;
+            points.Add(new SmoothedPathPoint(x, y, AngleMath.NormalizeRadians(vehicleYaw), vehicleYaw, s, direction,
+                referenceCurvature, direction == TravelDirection.Forward ? referenceCurvature : -referenceCurvature,
+                0d, 1d, false, SmoothedPathPointSource.Anchor));
+        }
+        double end = stations[stations.Count - 1];
+        var segment = new DirectionSegmentView(0, direction, points,
+            new ReferenceBoundary(0, 0d, EmBoundaryType.None, 0d),
+            new ReferenceBoundary(0, end, EmBoundaryType.RollingSafetyStop, end), 0d);
+        var corridorStations = new List<LateralInterval>(stations.Count);
+        for (int index = 0; index < stations.Count; index++)
+            corridorStations.Add(new LateralInterval(stations[index], -1d, 1d, startL));
+        EmPlannerConfiguration configuration = CreateUnitScaleConfiguration();
+        configuration.Lateral.MaximumLateralSlope = 5d;
+        configuration.Lateral.MaximumLateralSecondDerivativePerMeter = 5d;
+        configuration.Lateral.MaximumLateralThirdDerivativePerSquareMeter = 5d;
+        double startDenominator = 1d - referenceCurvature * startL;
+        return new LateralPlanningInput(segment, new StaticCorridor(corridorStations),
+            new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, stations[0]), startL,
+                Math.Atan2(startDL, startDenominator), 0d), EmTerminalType.RollingSafetyStop,
+            CreateVehicle(maximumVehicleCurvature), configuration, Array.Empty<FrenetProjection>());
+    }
+
+    private static double CalculateGeometricCurvature(FrenetReferencePoint reference, double l, double dl, double ddl,
+        double referenceCurvatureDerivative)
+    {
+        double a = 1d - reference.GeometricCurvature * l;
+        return (a * a * reference.GeometricCurvature + a * ddl + referenceCurvatureDerivative * l * dl +
+            2d * reference.GeometricCurvature * dl * dl) / Math.Pow(a * a + dl * dl, 1.5d);
+    }
+
+    private static LateralGeometryEvaluator CreateGeometryEvaluator()
+    {
+        return new LateralGeometryEvaluator();
+    }
+
+    private static LateralSolutionValidator CreateGeometryValidator()
+    {
+        return new LateralSolutionValidator();
+    }
+
    private static LateralObjectiveBuilder CreateObjectiveBuilder()
    {
        return new LateralObjectiveBuilder();