fix: unify path smoothing geometry metrics

2026-07-30 16:39:08 +08:00
parent d7ceb761b7
commit d6b34f88ce
16 changed files with 259 additions and 115 deletions
@@ -37,6 +37,7 @@ public sealed class PathGeometryAnalyzer
        double previousOutputUnwrappedHeading = 0d;
        bool hasPreviousOutputHeading = false;
        double maximumAbsoluteVehicleCurvature = 0d;
+        double maximumAbsoluteVehicleCurvatureDerivative = 0d;
        double curvatureSquareSum = 0d;
        int curvatureSampleCount = 0;
        double totalCurvatureVariation = 0d;
@@ -61,6 +62,7 @@ public sealed class PathGeometryAnalyzer
                    ref hasPreviousOutputHeading,
                    outputPath,
                    out double segmentMaximumCurvature,
+                    out double segmentMaximumCurvatureDerivative,
                    out double segmentCurvatureSquareSum,
                    out int segmentCurvatureSampleCount,
                    out double segmentVariation,
@@ -72,6 +74,9 @@ public sealed class PathGeometryAnalyzer
            }

            maximumAbsoluteVehicleCurvature = Math.Max(maximumAbsoluteVehicleCurvature, segmentMaximumCurvature);
+            maximumAbsoluteVehicleCurvatureDerivative = Math.Max(
+                maximumAbsoluteVehicleCurvatureDerivative,
+                segmentMaximumCurvatureDerivative);
            curvatureSquareSum += segmentCurvatureSquareSum;
            curvatureSampleCount += segmentCurvatureSampleCount;
            totalCurvatureVariation += segmentVariation;
@@ -94,6 +99,7 @@ public sealed class PathGeometryAnalyzer
            outputSegments,
            cumulativeArcLength,
            maximumAbsoluteVehicleCurvature,
+            maximumAbsoluteVehicleCurvatureDerivative,
            rmsCurvature,
            totalCurvatureVariation,
            curvatureVariationEnergy,
@@ -110,6 +116,7 @@ public sealed class PathGeometryAnalyzer
        ref bool hasPreviousOutputHeading,
        List<SmoothedPathPoint> output,
        out double maximumAbsoluteVehicleCurvature,
+        out double maximumAbsoluteVehicleCurvatureDerivative,
        out double curvatureSquareSum,
        out int curvatureSampleCount,
        out double totalCurvatureVariation,
@@ -118,6 +125,7 @@ public sealed class PathGeometryAnalyzer
        out string reason)
    {
        maximumAbsoluteVehicleCurvature = 0d;
+        maximumAbsoluteVehicleCurvatureDerivative = 0d;
        curvatureSquareSum = 0d;
        curvatureSampleCount = 0;
        totalCurvatureVariation = 0d;
@@ -129,6 +137,8 @@ public sealed class PathGeometryAnalyzer
        var headings = new double[count];
        var unwrappedHeadings = new double[count];
        var geometricCurvatures = new double[count];
+        var vehicleCurvatures = new double[count];
+        var curvatureDerivatives = new double[count];

        for (int index = 1; index < count; index++)
        {
@@ -214,9 +224,53 @@ public sealed class PathGeometryAnalyzer

        for (int index = 0; index < count; index++)
        {
-            SmoothingPoint2D sample = samples[index];
            double directionSign = segment.Direction == TravelDirection.Forward ? 1d : -1d;
-            double vehicleCurvature = directionSign * geometricCurvatures[index];
+            vehicleCurvatures[index] = directionSign * geometricCurvatures[index];
+        }
+
+        if (segment.StartVehicleCurvaturePerMeter.HasValue)
+        {
+            double directionSign = segment.Direction == TravelDirection.Forward ? 1d : -1d;
+            vehicleCurvatures[0] = segment.StartVehicleCurvaturePerMeter.Value;
+            geometricCurvatures[0] = directionSign * vehicleCurvatures[0];
+        }
+
+        for (int index = 0; index < count; index++)
+        {
+            if (count == 1)
+            {
+                curvatureDerivatives[index] = 0d;
+            }
+            else if (index == 0)
+            {
+                curvatureDerivatives[index] =
+                    (vehicleCurvatures[1] - vehicleCurvatures[0]) /
+                    (localArcLengths[1] - localArcLengths[0]);
+            }
+            else if (index == count - 1)
+            {
+                curvatureDerivatives[index] =
+                    (vehicleCurvatures[index] - vehicleCurvatures[index - 1]) /
+                    (localArcLengths[index] - localArcLengths[index - 1]);
+            }
+            else
+            {
+                curvatureDerivatives[index] =
+                    (vehicleCurvatures[index + 1] - vehicleCurvatures[index - 1]) /
+                    (localArcLengths[index + 1] - localArcLengths[index - 1]);
+            }
+
+            if (!NumericGuard.IsFinite(curvatureDerivatives[index]))
+            {
+                reason = "候选路径曲率导数计算产生了非法数值。";
+                return false;
+            }
+        }
+
+        for (int index = 0; index < count; index++)
+        {
+            SmoothingPoint2D sample = samples[index];
+            double vehicleCurvature = vehicleCurvatures[index];
            double arcLength = cumulativeArcLength + localArcLengths[index];
            bool isGearSwitch = index == 0 && segment.StartsAtGearSwitch;
            SmoothedPathPointSource source = isGearSwitch ? SmoothedPathPointSource.GearSwitch : sample.Source;
@@ -229,11 +283,15 @@ public sealed class PathGeometryAnalyzer
                segment.Direction,
                geometricCurvatures[index],
                vehicleCurvature,
+                curvatureDerivatives[index],
                sample.BodyClearance,
                isGearSwitch,
                source));

            maximumAbsoluteVehicleCurvature = Math.Max(maximumAbsoluteVehicleCurvature, Math.Abs(vehicleCurvature));
+            maximumAbsoluteVehicleCurvatureDerivative = Math.Max(
+                maximumAbsoluteVehicleCurvatureDerivative,
+                Math.Abs(curvatureDerivatives[index]));
            curvatureSquareSum += vehicleCurvature * vehicleCurvature;
            curvatureSampleCount++;
            minimumClearance = Math.Min(minimumClearance, sample.BodyClearance);