fix: preserve raw path smoothing baselines

ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Facade/PathSmoothingComparisonService.cs

@@ -13,7 +13,6 @@ public sealed class PathSmoothingComparisonService
 {
     private readonly PathSmoothingService _smoothingService = new PathSmoothingService();
     private readonly PathSmoothingPreprocessor _preprocessor = new PathSmoothingPreprocessor();
-    private readonly PathGeometryAnalyzer _analyzer = new PathGeometryAnalyzer();
     private readonly SmoothedPathValidator _validator = new SmoothedPathValidator();
 
     /// <summary>比较所有请求方法；一个方法的失败不会阻止其他方法，取消会停止后续启动。</summary>
@@ -137,31 +136,23 @@ public sealed class PathSmoothingComparisonService
 
         if (!_preprocessor.TryPrepare(smoothingRequest, out PreparedPath preparedPath, out reason))
             return FailedBaseline(PathSmoothingStatus.InvalidInput, reason, out reason);
-        if (!_analyzer.TryAnalyze(preparedPath.Segments, configuration.OutputSpacingMeters, out PathGeometryAnalysis analysis, out reason))
+        if (!RawPathBaselineBuilder.TryCreate(
-            return FailedBaseline(PathSmoothingStatus.InvalidInput, reason, out reason);
+                smoothingRequest,
-        if (!_validator.TryValidate(
-                analysis.Path,
-                analysis.Segments,
                 preparedPath,
-                smoothingRequest.Map,
+                _validator,
-                smoothingRequest.Vehicle,
                 configuration.MaximumCollisionCheckStepMeters,
-                out IReadOnlyList<SmoothedPathPoint> safePath,
+                out RawPathBaseline rawPath,
-                out double minimumClearanceMeters,
                 out reason))
-        {
             return FailedBaseline(PathSmoothingStatus.InvalidInput, reason, out reason);
-        }
 
-        PathQualityMetrics metrics = CreateMetrics(analysis, minimumClearanceMeters);
+        string digest = StableGeometryDigest.Compute(PathSmoothingStatus.Success, null, rawPath.Path, rawPath.Segments);
-        string digest = StableGeometryDigest.Compute(PathSmoothingStatus.Success, null, safePath, analysis.Segments);
         return PathSmoothingComparisonEntry.CreateRawPathBaseline(
             PathSmoothingStatus.Success,
-            metrics,
+            rawPath.Metrics,
             digest,
             string.Empty,
-            safePath,
+            rawPath.Path,
-            analysis.Segments);
+            rawPath.Segments);
     }
 
     private static PathSmoothingComparisonEntry FailedBaseline(
@@ -199,22 +190,6 @@ public sealed class PathSmoothingComparisonService
             configuration);
     }
 
-    private static PathQualityMetrics CreateMetrics(PathGeometryAnalysis analysis, double minimumClearanceMeters)
-    {
-        return new PathQualityMetrics(
-            true,
-            analysis.PathLengthMeters,
-            analysis.MaximumAbsoluteVehicleCurvaturePerMeter,
-            analysis.RootMeanSquareVehicleCurvaturePerMeter,
-            analysis.TotalAbsoluteCurvatureVariationPerMeter,
-            analysis.CurvatureVariationEnergy,
-            minimumClearanceMeters,
-            0d,
-            0d,
-            0d,
-            0d);
-    }
-
     private static PathQualityMetrics NormalizeMetrics(
         PathQualityMetrics candidate,
         PathQualityMetrics raw)

ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Facade/PathSmoothingService.cs

@@ -16,7 +16,6 @@ public sealed class PathSmoothingService
 {
     private readonly PathSmoothingPreprocessor _preprocessor = new PathSmoothingPreprocessor();
     private readonly SmoothingAlgorithmRunner _runner = new SmoothingAlgorithmRunner();
-    private readonly PathGeometryAnalyzer _analyzer = new PathGeometryAnalyzer();
     private readonly SmoothedPathValidator _validator = new SmoothedPathValidator();
     private readonly IPathSmoother _bSpline = new CubicBSplineSmoother();
     private readonly IPathSmoother _bezier = new LocalCubicBezierSmoother();
@@ -37,15 +36,11 @@ public sealed class PathSmoothingService
             if (!_preprocessor.TryPrepare(request, out PreparedPath preparedPath, out reason))
                 return Failure(PathSmoothingStatus.InvalidInput, stopwatch, 0, 0d, reason);
 
-            if (!TryAnalyzeAndValidate(
+            if (!RawPathBaselineBuilder.TryCreate(
                     request,
                     preparedPath,
-                    preparedPath.Segments,
+                    _validator,
-                    configuration,
+                    configuration.MaximumCollisionCheckStepMeters,
-                    false,
-                    cancellationToken,
-                    out _,
-                    out _,
                     out _,
                     out reason))
             {
@@ -136,64 +131,20 @@ public sealed class PathSmoothingService
         if (!_preprocessor.TryPrepare(request, out PreparedPath revalidatedPath, out reason)) return false;
         cancellationToken.ThrowIfCancellationRequested();
 
-        return TryAnalyzeAndValidate(
+        if (!RawPathBaselineBuilder.TryCreate(
-            request,
+                request,
-            revalidatedPath,
+                revalidatedPath,
-            ToFallbackSegments(revalidatedPath),
+                _validator,
-            configuration,
-            true,
-            cancellationToken,
-            out fallbackPath,
-            out fallbackSegments,
-            out fallbackMetrics,
-            out reason);
-    }
-
-    private bool TryAnalyzeAndValidate(
-        PathSmoothingRequest request,
-        PreparedPath originalPath,
-        IReadOnlyList<PreparedDirectionSegment> candidateSegments,
-        PathSmoothingConfiguration configuration,
-        bool useFallbackSource,
-        CancellationToken cancellationToken,
-        out IReadOnlyList<SmoothedPathPoint> safePath,
-        out IReadOnlyList<SmoothedPathSegment> safeSegments,
-        out PathQualityMetrics metrics,
-        out string reason)
-    {
-        safePath = null;
-        safeSegments = null;
-        metrics = null;
-        reason = string.Empty;
-        if (!_analyzer.TryAnalyze(
-                candidateSegments,
-                configuration.OutputSpacingMeters,
-                out PathGeometryAnalysis analysis,
-                out reason))
-        {
-            return false;
-        }
-
-        IReadOnlyList<SmoothedPathPoint> pathForValidation = useFallbackSource
-            ? ToFallbackPoints(analysis.Path)
-            : analysis.Path;
-        cancellationToken.ThrowIfCancellationRequested();
-        if (!_validator.TryValidate(
-                pathForValidation,
-                analysis.Segments,
-                originalPath,
-                request.Map,
-                request.Vehicle,
                 configuration.MaximumCollisionCheckStepMeters,
-                out safePath,
+                out RawPathBaseline rawPath,
-                out double minimumClearanceMeters,
                 out reason))
         {
             return false;
         }
 
-        safeSegments = analysis.Segments;
+        fallbackPath = ToFallbackPoints(rawPath.Path);
-        metrics = CreateMetrics(analysis, minimumClearanceMeters);
+        fallbackSegments = rawPath.Segments;
+        fallbackMetrics = rawPath.Metrics;
         return true;
     }
 
@@ -268,36 +219,6 @@ public sealed class PathSmoothingService
         return NumericGuard.IsFinite(thresholdRadians) && thresholdRadians > 0d && thresholdRadians <= Math.PI;
     }
 
-    private static IReadOnlyList<PreparedDirectionSegment> ToFallbackSegments(PreparedPath path)
-    {
-        var segments = new List<PreparedDirectionSegment>(path.Segments.Count);
-        for (int segmentIndex = 0; segmentIndex < path.Segments.Count; segmentIndex++)
-        {
-            PreparedDirectionSegment segment = path.Segments[segmentIndex];
-            var points = new List<SmoothingPoint2D>(segment.Points.Count);
-            for (int pointIndex = 0; pointIndex < segment.Points.Count; pointIndex++)
-            {
-                SmoothingPoint2D point = segment.Points[pointIndex];
-                points.Add(new SmoothingPoint2D(
-                    point.X,
-                    point.Y,
-                    point.ArcLength,
-                    point.Heading,
-                    point.UnwrappedHeading,
-                    point.BodyClearance,
-                    point.IsGearSwitchPoint,
-                    SmoothedPathPointSource.CoarsePathFallback));
-            }
-            segments.Add(new PreparedDirectionSegment(
-                segment.SegmentIndex,
-                segment.Direction,
-                points,
-                segment.StartsAtGearSwitch,
-                segment.EndsAtGearSwitch));
-        }
-        return segments;
-    }
-
     private static IReadOnlyList<SmoothedPathPoint> ToFallbackPoints(IReadOnlyList<SmoothedPathPoint> path)
     {
         var points = new List<SmoothedPathPoint>(path.Count);
@@ -320,22 +241,6 @@ public sealed class PathSmoothingService
         return points;
     }
 
-    private static PathQualityMetrics CreateMetrics(PathGeometryAnalysis analysis, double minimumClearanceMeters)
-    {
-        return new PathQualityMetrics(
-            true,
-            analysis.PathLengthMeters,
-            analysis.MaximumAbsoluteVehicleCurvaturePerMeter,
-            analysis.RootMeanSquareVehicleCurvaturePerMeter,
-            analysis.TotalAbsoluteCurvatureVariationPerMeter,
-            analysis.CurvatureVariationEnergy,
-            minimumClearanceMeters,
-            0d,
-            0d,
-            0d,
-            0d);
-    }
-
     private static int GetRetryCount(IReadOnlyList<double> attemptedStrengths)
     {
         return attemptedStrengths == null || attemptedStrengths.Count == 0 ? 0 : attemptedStrengths.Count - 1;

ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathGeometryAnalyzer.cs

@@ -144,21 +144,14 @@ public sealed class PathGeometryAnalyzer
         for (int index = 0; index < count; index++)
         {
             double travelHeading;
-            if (count == 1)
+            if (index == 0 || index == count - 1)
             {
+                // Coarse-path endpoints encode the vehicle pose at the exact integration anchor.
+                // A chord across a finite integration step is not that pose's heading.
                 travelHeading = segment.Direction == TravelDirection.Forward
                     ? samples[index].Heading
                     : samples[index].Heading - Math.PI;
             }
-            else if (index == 0)
-            {
-                travelHeading = Math.Atan2(samples[1].Y - samples[0].Y, samples[1].X - samples[0].X);
-            }
-            else if (index == count - 1)
-            {
-                travelHeading = Math.Atan2(samples[index].Y - samples[index - 1].Y,
-                    samples[index].X - samples[index - 1].X);
-            }
             else
             {
                 travelHeading = Math.Atan2(samples[index + 1].Y - samples[index - 1].Y,

ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/RawPathBaselineBuilder.cs

@@ -0,0 +1,166 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;
+
+namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
+
+/// <summary>将已验证的粗路径转换为不改变任何锚点位姿或车辆曲率的安全比较基线。</summary>
+internal static class RawPathBaselineBuilder
+{
+    private const double MinimumArcDeltaMeters = 1e-12d;
+
+    internal static bool TryCreate(
+        PathSmoothingRequest request,
+        PreparedPath preparedPath,
+        SmoothedPathValidator validator,
+        double maximumCollisionCheckStepMeters,
+        out RawPathBaseline baseline,
+        out string reason)
+    {
+        baseline = null;
+        reason = string.Empty;
+        if (request == null || preparedPath == null || validator == null)
+        {
+            reason = "原始粗路径基线缺少请求、预处理路径或安全验证器。";
+            return false;
+        }
+
+        IReadOnlyList<SmoothedPathPoint> candidatePath = CreatePoints(request.CoarsePath);
+        IReadOnlyList<SmoothedPathSegment> candidateSegments = CreateSegments(request.Segments);
+        if (!validator.TryValidate(
+                candidatePath,
+                candidateSegments,
+                preparedPath,
+                request.Map,
+                request.Vehicle,
+                maximumCollisionCheckStepMeters,
+                out IReadOnlyList<SmoothedPathPoint> safePath,
+                out double minimumClearanceMeters,
+                out reason))
+        {
+            return false;
+        }
+
+        baseline = new RawPathBaseline(safePath, candidateSegments, CreateMetrics(safePath, candidateSegments, minimumClearanceMeters));
+        return true;
+    }
+
+    private static IReadOnlyList<SmoothedPathPoint> CreatePoints(IReadOnlyList<CoarsePathPoint> coarsePath)
+    {
+        var points = new List<SmoothedPathPoint>(coarsePath == null ? 0 : coarsePath.Count);
+        if (coarsePath != null)
+        {
+            for (int index = 0; index < coarsePath.Count; index++)
+            {
+                CoarsePathPoint point = coarsePath[index];
+                points.Add(new SmoothedPathPoint(
+                    point.X,
+                    point.Y,
+                    point.Heading,
+                    point.UnwrappedHeading,
+                    point.ArcLength,
+                    point.Direction,
+                    point.VehicleCurvature,
+                    point.VehicleCurvature,
+                    point.BodyClearance,
+                    point.IsGearSwitchPoint,
+                    point.IsGearSwitchPoint ? SmoothedPathPointSource.GearSwitch : SmoothedPathPointSource.Anchor));
+            }
+        }
+        return new ReadOnlyCollection<SmoothedPathPoint>(points);
+    }
+
+    private static IReadOnlyList<SmoothedPathSegment> CreateSegments(IReadOnlyList<PathSegment> coarseSegments)
+    {
+        var segments = new List<SmoothedPathSegment>(coarseSegments == null ? 0 : coarseSegments.Count);
+        if (coarseSegments != null)
+        {
+            for (int index = 0; index < coarseSegments.Count; index++)
+            {
+                PathSegment segment = coarseSegments[index];
+                segments.Add(new SmoothedPathSegment(
+                    segment.SegmentIndex,
+                    segment.Direction,
+                    segment.StartIndex,
+                    segment.EndIndex,
+                    segment.StartsAtGearSwitch,
+                    segment.EndsAtGearSwitch));
+            }
+        }
+        return new ReadOnlyCollection<SmoothedPathSegment>(segments);
+    }
+
+    private static PathQualityMetrics CreateMetrics(
+        IReadOnlyList<SmoothedPathPoint> path,
+        IReadOnlyList<SmoothedPathSegment> segments,
+        double minimumClearanceMeters)
+    {
+        double maximumAbsoluteVehicleCurvature = 0d;
+        double curvatureSquareSum = 0d;
+        int curvatureSampleCount = 0;
+        double totalAbsoluteCurvatureVariation = 0d;
+        double curvatureVariationEnergy = 0d;
+
+        for (int segmentIndex = 0; segmentIndex < segments.Count; segmentIndex++)
+        {
+            SmoothedPathSegment segment = segments[segmentIndex];
+            SmoothedPathPoint previous = null;
+            for (int pointIndex = segment.StartIndex; pointIndex <= segment.EndIndex; pointIndex++)
+            {
+                SmoothedPathPoint current = path[pointIndex];
+                double curvature = current.VehicleCurvature;
+                maximumAbsoluteVehicleCurvature = Math.Max(maximumAbsoluteVehicleCurvature, Math.Abs(curvature));
+                curvatureSquareSum += curvature * curvature;
+                curvatureSampleCount++;
+                if (previous != null)
+                {
+                    double arcDelta = current.ArcLength - previous.ArcLength;
+                    if (arcDelta > MinimumArcDeltaMeters)
+                    {
+                        double curvatureDelta = curvature - previous.VehicleCurvature;
+                        totalAbsoluteCurvatureVariation += Math.Abs(curvatureDelta);
+                        curvatureVariationEnergy += curvatureDelta * curvatureDelta / arcDelta;
+                    }
+                }
+                previous = current;
+            }
+        }
+
+        double rootMeanSquareVehicleCurvature = curvatureSampleCount == 0
+            ? 0d
+            : Math.Sqrt(curvatureSquareSum / curvatureSampleCount);
+        double pathLengthMeters = path.Count == 0 ? 0d : path[path.Count - 1].ArcLength;
+        return new PathQualityMetrics(
+            true,
+            pathLengthMeters,
+            maximumAbsoluteVehicleCurvature,
+            rootMeanSquareVehicleCurvature,
+            totalAbsoluteCurvatureVariation,
+            curvatureVariationEnergy,
+            minimumClearanceMeters,
+            0d,
+            0d,
+            0d,
+            0d);
+    }
+}
+
+/// <summary>已通过完整车体复核的原始粗路径及其比较指标。</summary>
+internal sealed class RawPathBaseline
+{
+    internal RawPathBaseline(
+        IReadOnlyList<SmoothedPathPoint> path,
+        IReadOnlyList<SmoothedPathSegment> segments,
+        PathQualityMetrics metrics)
+    {
+        Path = path;
+        Segments = segments;
+        Metrics = metrics;
+    }
+
+    internal IReadOnlyList<SmoothedPathPoint> Path { get; }
+    internal IReadOnlyList<SmoothedPathSegment> Segments { get; }
+    internal PathQualityMetrics Metrics { get; }
+}

ClumsyPilot/tests/verify_path_smoothing_geometry.ps1

@@ -196,6 +196,15 @@ $straightEnd = $straightAnalysis.Path[$straightAnalysis.Path.Count - 1]
 Assert-Near 1.0 $straightEnd.X 0.0 'Resampling must retain the exact final X coordinate.'
 Assert-Near 0.0 $straightEnd.Y 0.0 'Resampling must retain the exact final Y coordinate.'
 
+# Raw coarse anchors carry the vehicle pose, which may differ slightly from the chord tangent of a finite integration step.
+$poseAnchoredCurve = New-DirectionSegment 0 $forward @(
+    (New-GeometryPoint 0.0 0.0 0.0 0.0 0.0),
+    (New-GeometryPoint 1.0 0.2 1.0 0.4 0.4))
+$poseAnchoredAnalysis = Invoke-Analysis @($poseAnchoredCurve)
+Assert-Near 0.0 $poseAnchoredAnalysis.Path[0].Heading 0.000000000001 'Geometry analysis must retain the first coarse-anchor heading rather than replace it with a chord tangent.'
+$poseAnchoredEnd = $poseAnchoredAnalysis.Path[$poseAnchoredAnalysis.Path.Count - 1]
+Assert-Near 0.4 $poseAnchoredEnd.Heading 0.000000000001 'Geometry analysis must retain the final coarse-anchor heading rather than replace it with a chord tangent.'
+
 # A forward R=2 quarter circle has positive +0.5 1/m vehicle curvature.
 $forwardArcPoints = New-Object System.Collections.Generic.List[object]
 for ($index = 0; $index -le 32; $index++) {