First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix: unify path smoothing geometry metrics

Browse Source
This commit is contained in:
梁薄云
2026-07-30 16:39:08 +08:00
parent d7ceb761b7
commit d6b34f88ce
16 changed files with 259 additions and 115 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/RawPathBaselineBuilder.cs
+18 -100
View File
@@ -1,19 +1,16 @@
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>
/// <summary>通过候选路径同一几何分析器和验证器构建安全、可公平比较的原始基线。</summary>
internal static class RawPathBaselineBuilder
{
private const double MinimumArcDeltaMeters = 1e-12d;
internal static bool TryCreate(
PathSmoothingRequest request,
PreparedPath preparedPath,
PathGeometryAnalyzer analyzer,
double outputSpacingMeters,
SmoothedPathValidator validator,
double maximumCollisionCheckStepMeters,
out RawPathBaseline baseline,
@@ -21,17 +18,18 @@ internal static class RawPathBaselineBuilder
{
baseline = null;
reason = string.Empty;
if (request == null || preparedPath == null || validator == null)
if (request == null || preparedPath == null || analyzer == null || validator == null)
{
reason = "原始粗路径基线缺少请求、预处理路径或安全验证器。";
reason = "原始粗路径基线缺少请求、预处理路径、几何分析器或安全验证器。";
return false;
}
IReadOnlyList<SmoothedPathPoint> candidatePath = CreatePoints(request.CoarsePath);
IReadOnlyList<SmoothedPathSegment> candidateSegments = CreateSegments(request.Segments);
if (!analyzer.TryAnalyze(preparedPath.Segments, outputSpacingMeters, out PathGeometryAnalysis analysis, out reason))
return false;
if (!validator.TryValidate(
candidatePath,
candidateSegments,
analysis.Path,
analysis.Segments,
preparedPath,
request.Map,
request.Vehicle,
@@ -43,102 +41,22 @@ internal static class RawPathBaselineBuilder
return false;
}
baseline = new RawPathBaseline(safePath, candidateSegments, CreateMetrics(safePath, candidateSegments, minimumClearanceMeters));
baseline = new RawPathBaseline(safePath, analysis.Segments, CreateMetrics(analysis, 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,
PathGeometryAnalysis analysis,
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,
analysis.PathLengthMeters,
analysis.MaximumAbsoluteVehicleCurvaturePerMeter,
analysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
analysis.RootMeanSquareVehicleCurvaturePerMeter,
analysis.TotalAbsoluteCurvatureVariationPerMeter,
analysis.CurvatureVariationEnergy,
minimumClearanceMeters,
0d,
0d,