feat: compare and rank smoothing methods
This commit is contained in:
梁薄云
+120
@@ -0,0 +1,120 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison;
|
||||
|
||||
/// <summary>原始基线或一种平滑方法的不可变比较条目。</summary>
|
||||
public sealed class PathSmoothingComparisonEntry
|
||||
{
|
||||
/// <summary>为测试、离线分析和排序创建不携带路径几何的候选条目。</summary>
|
||||
public PathSmoothingComparisonEntry(
|
||||
SmoothingMethod method,
|
||||
PathSmoothingStatus status,
|
||||
PathQualityMetrics metrics,
|
||||
SmoothingTimingSummary timing,
|
||||
string stableGeometryDigest,
|
||||
string diagnostic)
|
||||
: this(method, false, status, metrics, timing, stableGeometryDigest, diagnostic, Empty<SmoothedPathPoint>(), Empty<SmoothedPathSegment>())
|
||||
{
|
||||
}
|
||||
|
||||
private PathSmoothingComparisonEntry(
|
||||
SmoothingMethod? method,
|
||||
bool isRawPathBaseline,
|
||||
PathSmoothingStatus status,
|
||||
PathQualityMetrics metrics,
|
||||
SmoothingTimingSummary timing,
|
||||
string stableGeometryDigest,
|
||||
string diagnostic,
|
||||
IReadOnlyList<SmoothedPathPoint> path,
|
||||
IReadOnlyList<SmoothedPathSegment> segments)
|
||||
{
|
||||
Method = method;
|
||||
IsRawPathBaseline = isRawPathBaseline;
|
||||
Status = status;
|
||||
Metrics = metrics ?? new PathQualityMetrics();
|
||||
Timing = timing ?? new SmoothingTimingSummary(new double[] { 0d, 0d, 0d, 0d, 0d }, false, "未提供计时结果。");
|
||||
StableGeometryDigest = stableGeometryDigest ?? string.Empty;
|
||||
Diagnostic = diagnostic ?? string.Empty;
|
||||
Path = CopyReadOnly(path);
|
||||
Segments = CopyReadOnly(segments);
|
||||
}
|
||||
|
||||
/// <summary>候选所代表的方法；原始粗路径基线为空。</summary>
|
||||
public SmoothingMethod? Method { get; }
|
||||
|
||||
/// <summary>是否为单独分析的原始粗路径基线。</summary>
|
||||
public bool IsRawPathBaseline { get; }
|
||||
|
||||
/// <summary>本条目的最终状态。</summary>
|
||||
public PathSmoothingStatus Status { get; }
|
||||
|
||||
/// <summary>使用原始基线规范化后的质量指标。</summary>
|
||||
public PathQualityMetrics Metrics { get; }
|
||||
|
||||
/// <summary>方法的五次测量计时；基线不参与计时排名。</summary>
|
||||
public SmoothingTimingSummary Timing { get; }
|
||||
|
||||
/// <summary>由状态、分段元数据和完整路径 IEEE 754 位模式生成的 SHA-256 摘要。</summary>
|
||||
public string StableGeometryDigest { get; }
|
||||
|
||||
/// <summary>面向报告和诊断的稳定说明。</summary>
|
||||
public string Diagnostic { get; }
|
||||
|
||||
/// <summary>仅供比较与报告读取的正式路径；失败时为空。</summary>
|
||||
public IReadOnlyList<SmoothedPathPoint> Path { get; }
|
||||
|
||||
/// <summary>覆盖 <see cref="Path"/> 的方向段；失败时为空。</summary>
|
||||
public IReadOnlyList<SmoothedPathSegment> Segments { get; }
|
||||
|
||||
/// <summary>条目能否参与方法推荐。</summary>
|
||||
public bool IsEligibleForRecommendation =>
|
||||
!IsRawPathBaseline &&
|
||||
Status == PathSmoothingStatus.Success &&
|
||||
Metrics.IsFeasible &&
|
||||
Timing.IsDeterministic;
|
||||
|
||||
internal static PathSmoothingComparisonEntry CreateCandidate(
|
||||
SmoothingMethod method,
|
||||
PathSmoothingStatus status,
|
||||
PathQualityMetrics metrics,
|
||||
SmoothingTimingSummary timing,
|
||||
string stableGeometryDigest,
|
||||
string diagnostic,
|
||||
IReadOnlyList<SmoothedPathPoint> path,
|
||||
IReadOnlyList<SmoothedPathSegment> segments)
|
||||
{
|
||||
return new PathSmoothingComparisonEntry(
|
||||
method, false, status, metrics, timing, stableGeometryDigest, diagnostic, path, segments);
|
||||
}
|
||||
|
||||
internal static PathSmoothingComparisonEntry CreateRawPathBaseline(
|
||||
PathSmoothingStatus status,
|
||||
PathQualityMetrics metrics,
|
||||
string stableGeometryDigest,
|
||||
string diagnostic,
|
||||
IReadOnlyList<SmoothedPathPoint> path,
|
||||
IReadOnlyList<SmoothedPathSegment> segments)
|
||||
{
|
||||
return new PathSmoothingComparisonEntry(
|
||||
null, true, status, metrics,
|
||||
new SmoothingTimingSummary(new double[] { 0d, 0d, 0d, 0d, 0d }, true, string.Empty),
|
||||
stableGeometryDigest, diagnostic, path, segments);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<T> Empty<T>()
|
||||
{
|
||||
return new ReadOnlyCollection<T>(new List<T>());
|
||||
}
|
||||
|
||||
private static IReadOnlyList<T> CopyReadOnly<T>(IReadOnlyList<T> source)
|
||||
{
|
||||
var copy = new List<T>(source == null ? 0 : source.Count);
|
||||
if (source != null)
|
||||
{
|
||||
for (int index = 0; index < source.Count; index++) copy.Add(source[index]);
|
||||
}
|
||||
return new ReadOnlyCollection<T>(copy);
|
||||
}
|
||||
}
|
||||
+60
@@ -0,0 +1,60 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison;
|
||||
|
||||
/// <summary>同一粗路径的离线平滑比较请求。</summary>
|
||||
public sealed class PathSmoothingComparisonRequest
|
||||
{
|
||||
private static readonly SmoothingMethod[] DefaultMethods =
|
||||
{
|
||||
SmoothingMethod.CubicBSpline,
|
||||
SmoothingMethod.LocalCubicBezier,
|
||||
SmoothingMethod.PiecewiseQuintic,
|
||||
};
|
||||
|
||||
/// <summary>创建比较请求，并固定原始输入与方法顺序。</summary>
|
||||
public PathSmoothingComparisonRequest(
|
||||
PathSmoothingRequest smoothingRequest,
|
||||
IReadOnlyList<SmoothingMethod> methods = null)
|
||||
{
|
||||
SmoothingRequest = CopyRequest(smoothingRequest);
|
||||
Methods = CopyMethods(methods ?? DefaultMethods);
|
||||
}
|
||||
|
||||
/// <summary>所有方法共享的不可变粗路径、地图、车辆和配置快照。</summary>
|
||||
public PathSmoothingRequest SmoothingRequest { get; }
|
||||
|
||||
/// <summary>按调用方指定稳定顺序运行的方法集合。</summary>
|
||||
public IReadOnlyList<SmoothingMethod> Methods { get; }
|
||||
|
||||
private static PathSmoothingRequest CopyRequest(PathSmoothingRequest source)
|
||||
{
|
||||
if (source == null) return null;
|
||||
return new PathSmoothingRequest(
|
||||
source.CoarsePath,
|
||||
source.Segments,
|
||||
source.Map,
|
||||
source.Vehicle,
|
||||
source.Configuration);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<SmoothingMethod> CopyMethods(IReadOnlyList<SmoothingMethod> source)
|
||||
{
|
||||
var copy = new List<SmoothingMethod>(source == null ? 0 : source.Count);
|
||||
if (source != null)
|
||||
{
|
||||
for (int index = 0; index < source.Count; index++)
|
||||
{
|
||||
SmoothingMethod method = source[index];
|
||||
if (!Enum.IsDefined(typeof(SmoothingMethod), method))
|
||||
throw new ArgumentOutOfRangeException(nameof(source), "比较方法无效。");
|
||||
if (copy.Contains(method))
|
||||
throw new ArgumentException("比较方法不能重复。", nameof(source));
|
||||
copy.Add(method);
|
||||
}
|
||||
}
|
||||
return new ReadOnlyCollection<SmoothingMethod>(copy);
|
||||
}
|
||||
}
|
||||
+49
@@ -0,0 +1,49 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison;
|
||||
|
||||
/// <summary>一次离线比较的不可变基线、方法条目和推荐结论。</summary>
|
||||
public sealed class PathSmoothingComparisonResult
|
||||
{
|
||||
internal PathSmoothingComparisonResult(
|
||||
PathSmoothingComparisonEntry rawPathBaseline,
|
||||
IReadOnlyList<PathSmoothingComparisonEntry> entries,
|
||||
SmoothingMethod? recommendedMethod,
|
||||
bool isCancelled,
|
||||
string diagnostic)
|
||||
{
|
||||
RawPathBaseline = rawPathBaseline ?? throw new ArgumentNullException(nameof(rawPathBaseline));
|
||||
Entries = CopyReadOnly(entries);
|
||||
RecommendedMethod = recommendedMethod;
|
||||
IsCancelled = isCancelled;
|
||||
Diagnostic = diagnostic ?? string.Empty;
|
||||
}
|
||||
|
||||
/// <summary>独立分析的原始粗路径；不属于任何候选方法。</summary>
|
||||
public PathSmoothingComparisonEntry RawPathBaseline { get; }
|
||||
|
||||
/// <summary>每个请求方法恰有一个条目；取消时可能只包含已完成的方法。</summary>
|
||||
public IReadOnlyList<PathSmoothingComparisonEntry> Entries { get; }
|
||||
|
||||
/// <summary>按公开字典序选择的方法；没有合格方法或取消时为空。</summary>
|
||||
public SmoothingMethod? RecommendedMethod { get; }
|
||||
|
||||
/// <summary>比较是否在启动后续方法前被取消。</summary>
|
||||
public bool IsCancelled { get; }
|
||||
|
||||
/// <summary>整个比较的稳定状态说明。</summary>
|
||||
public string Diagnostic { get; }
|
||||
|
||||
private static IReadOnlyList<PathSmoothingComparisonEntry> CopyReadOnly(
|
||||
IReadOnlyList<PathSmoothingComparisonEntry> source)
|
||||
{
|
||||
var copy = new List<PathSmoothingComparisonEntry>(source == null ? 0 : source.Count);
|
||||
if (source != null)
|
||||
{
|
||||
for (int index = 0; index < source.Count; index++) copy.Add(source[index]);
|
||||
}
|
||||
return new ReadOnlyCollection<PathSmoothingComparisonEntry>(copy);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,60 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison;
|
||||
|
||||
/// <summary>按公开字典序选择唯一的推荐平滑方法。</summary>
|
||||
public static class SmoothingMethodRanker
|
||||
{
|
||||
/// <summary>从当前场景的可行且确定性候选中选择最佳方法；没有合格候选时返回空。</summary>
|
||||
public static SmoothingMethod? Rank(IReadOnlyList<PathSmoothingComparisonEntry> entries)
|
||||
{
|
||||
PathSmoothingComparisonEntry best = null;
|
||||
if (entries == null) return null;
|
||||
|
||||
for (int index = 0; index < entries.Count; index++)
|
||||
{
|
||||
PathSmoothingComparisonEntry candidate = entries[index];
|
||||
if (candidate == null || !candidate.IsEligibleForRecommendation) continue;
|
||||
if (best == null || Compare(candidate, best) < 0) best = candidate;
|
||||
}
|
||||
return best == null ? (SmoothingMethod?)null : best.Method;
|
||||
}
|
||||
|
||||
private static int Compare(PathSmoothingComparisonEntry left, PathSmoothingComparisonEntry right)
|
||||
{
|
||||
int comparison = CompareAscending(left.Metrics.CurvatureVariationEnergy, right.Metrics.CurvatureVariationEnergy);
|
||||
if (comparison != 0) return comparison;
|
||||
|
||||
comparison = CompareAscending(
|
||||
left.Metrics.MaximumAbsoluteVehicleCurvaturePerMeter,
|
||||
right.Metrics.MaximumAbsoluteVehicleCurvaturePerMeter);
|
||||
if (comparison != 0) return comparison;
|
||||
|
||||
comparison = CompareDescending(left.Metrics.MinimumBodyClearanceMeters, right.Metrics.MinimumBodyClearanceMeters);
|
||||
if (comparison != 0) return comparison;
|
||||
|
||||
comparison = CompareAscending(left.Metrics.LengthChangePercent, right.Metrics.LengthChangePercent);
|
||||
if (comparison != 0) return comparison;
|
||||
|
||||
comparison = CompareAscending(left.Timing.MedianElapsedMilliseconds, right.Timing.MedianElapsedMilliseconds);
|
||||
if (comparison != 0) return comparison;
|
||||
|
||||
return ((int)left.Method.Value).CompareTo((int)right.Method.Value);
|
||||
}
|
||||
|
||||
private static int CompareAscending(double left, double right)
|
||||
{
|
||||
return Normalize(left).CompareTo(Normalize(right));
|
||||
}
|
||||
|
||||
private static int CompareDescending(double left, double right)
|
||||
{
|
||||
return Normalize(right).CompareTo(Normalize(left));
|
||||
}
|
||||
|
||||
private static double Normalize(double value)
|
||||
{
|
||||
return double.IsNaN(value) || double.IsInfinity(value) ? double.PositiveInfinity : value;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,90 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison;
|
||||
|
||||
/// <summary>一个方法的固定五次计时样本和确定性结论。</summary>
|
||||
public sealed class SmoothingTimingSummary
|
||||
{
|
||||
/// <summary>创建一个只接受五个已测量样本的计时汇总。</summary>
|
||||
public SmoothingTimingSummary(
|
||||
IReadOnlyList<double> measuredElapsedMilliseconds,
|
||||
bool isDeterministic,
|
||||
string diagnostic)
|
||||
{
|
||||
if (measuredElapsedMilliseconds == null || measuredElapsedMilliseconds.Count != 5)
|
||||
throw new ArgumentException("计时汇总必须包含五个已测量样本。", nameof(measuredElapsedMilliseconds));
|
||||
|
||||
var copy = new List<double>(measuredElapsedMilliseconds.Count);
|
||||
for (int index = 0; index < measuredElapsedMilliseconds.Count; index++)
|
||||
{
|
||||
double value = measuredElapsedMilliseconds[index];
|
||||
if (double.IsNaN(value) || double.IsInfinity(value) || value < 0d)
|
||||
throw new ArgumentOutOfRangeException(nameof(measuredElapsedMilliseconds), "计时样本必须为有限非负数。");
|
||||
copy.Add(value);
|
||||
}
|
||||
|
||||
MeasuredElapsedMilliseconds = new ReadOnlyCollection<double>(copy);
|
||||
MedianElapsedMilliseconds = Median(copy);
|
||||
IsDeterministic = isDeterministic;
|
||||
Diagnostic = diagnostic ?? string.Empty;
|
||||
}
|
||||
|
||||
/// <summary>不含预热执行的五个实测耗时，单位 ms。</summary>
|
||||
public IReadOnlyList<double> MeasuredElapsedMilliseconds { get; }
|
||||
|
||||
/// <summary>五个实测耗时的稳定中位数，单位 ms。</summary>
|
||||
public double MedianElapsedMilliseconds { get; }
|
||||
|
||||
/// <summary>五次输出是否具有相同的状态和稳定几何摘要。</summary>
|
||||
public bool IsDeterministic { get; }
|
||||
|
||||
/// <summary>非确定性或测量失败的稳定诊断说明。</summary>
|
||||
public string Diagnostic { get; }
|
||||
|
||||
/// <summary>从五次测量结果中生成计时汇总，并拒绝状态或几何不稳定的输出。</summary>
|
||||
public static SmoothingTimingSummary FromMeasurements(
|
||||
IReadOnlyList<double> measuredElapsedMilliseconds,
|
||||
IReadOnlyList<PathSmoothingResult> measuredResults)
|
||||
{
|
||||
if (measuredResults == null || measuredResults.Count != 5)
|
||||
throw new ArgumentException("确定性检查必须包含五个测量结果。", nameof(measuredResults));
|
||||
for (int index = 0; index < measuredResults.Count; index++)
|
||||
{
|
||||
if (measuredResults[index] == null)
|
||||
throw new ArgumentException("确定性检查不能包含空测量结果。", nameof(measuredResults));
|
||||
}
|
||||
|
||||
PathSmoothingResult canonical = measuredResults[0];
|
||||
string canonicalDigest = StableGeometryDigest.Compute(canonical);
|
||||
for (int index = 1; index < measuredResults.Count; index++)
|
||||
{
|
||||
PathSmoothingResult measured = measuredResults[index];
|
||||
string digest = StableGeometryDigest.Compute(measured);
|
||||
if (measured.Status != canonical.Status ||
|
||||
measured.Path.Count != canonical.Path.Count ||
|
||||
measured.Segments.Count != canonical.Segments.Count ||
|
||||
!string.Equals(digest, canonicalDigest, StringComparison.Ordinal))
|
||||
{
|
||||
return new SmoothingTimingSummary(
|
||||
measuredElapsedMilliseconds,
|
||||
false,
|
||||
"五次测量的状态、点数、分段数或稳定几何摘要不一致。");
|
||||
}
|
||||
}
|
||||
|
||||
return new SmoothingTimingSummary(measuredElapsedMilliseconds, true, string.Empty);
|
||||
}
|
||||
|
||||
internal static double Median(IReadOnlyList<double> values)
|
||||
{
|
||||
var sorted = new double[values.Count];
|
||||
for (int index = 0; index < values.Count; index++) sorted[index] = values[index];
|
||||
Array.Sort(sorted);
|
||||
int middle = sorted.Length / 2;
|
||||
return sorted.Length % 2 == 1
|
||||
? sorted[middle]
|
||||
: (sorted[middle - 1] + sorted[middle]) / 2d;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,111 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.IO;
|
||||
using System.Security.Cryptography;
|
||||
using System.Text;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison;
|
||||
|
||||
/// <summary>为重复执行结果生成与进程无关的稳定几何 SHA-256 摘要。</summary>
|
||||
public static class StableGeometryDigest
|
||||
{
|
||||
/// <summary>计算正式平滑结果的状态、方法、分段和路径位模式摘要。</summary>
|
||||
public static string Compute(PathSmoothingResult result)
|
||||
{
|
||||
if (result == null) throw new ArgumentNullException(nameof(result));
|
||||
return Compute(result.Status, result.Method, result.Path, result.Segments);
|
||||
}
|
||||
|
||||
/// <summary>计算任意已分析路径的稳定摘要。</summary>
|
||||
public static string Compute(
|
||||
PathSmoothingStatus status,
|
||||
SmoothingMethod? method,
|
||||
IReadOnlyList<SmoothedPathPoint> path,
|
||||
IReadOnlyList<SmoothedPathSegment> segments)
|
||||
{
|
||||
using (var stream = new MemoryStream())
|
||||
{
|
||||
WriteInt32(stream, 1);
|
||||
WriteInt32(stream, (int)status);
|
||||
WriteBoolean(stream, method.HasValue);
|
||||
if (method.HasValue) WriteInt32(stream, (int)method.Value);
|
||||
|
||||
WriteInt32(stream, path == null ? 0 : path.Count);
|
||||
if (path != null)
|
||||
{
|
||||
for (int index = 0; index < path.Count; index++) WritePoint(stream, path[index]);
|
||||
}
|
||||
|
||||
WriteInt32(stream, segments == null ? 0 : segments.Count);
|
||||
if (segments != null)
|
||||
{
|
||||
for (int index = 0; index < segments.Count; index++) WriteSegment(stream, segments[index]);
|
||||
}
|
||||
|
||||
using (SHA256 sha256 = SHA256.Create())
|
||||
{
|
||||
byte[] hash = sha256.ComputeHash(stream.ToArray());
|
||||
var builder = new StringBuilder(hash.Length * 2);
|
||||
for (int index = 0; index < hash.Length; index++) builder.Append(hash[index].ToString("x2"));
|
||||
return builder.ToString();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private static void WritePoint(Stream stream, SmoothedPathPoint point)
|
||||
{
|
||||
if (point == null) throw new ArgumentException("稳定路径摘要不能包含空点。", nameof(point));
|
||||
WriteDouble(stream, point.X);
|
||||
WriteDouble(stream, point.Y);
|
||||
WriteDouble(stream, point.Heading);
|
||||
WriteDouble(stream, point.UnwrappedHeading);
|
||||
WriteDouble(stream, point.ArcLength);
|
||||
WriteInt32(stream, (int)point.Direction);
|
||||
WriteDouble(stream, point.GeometricCurvature);
|
||||
WriteDouble(stream, point.VehicleCurvature);
|
||||
WriteDouble(stream, point.BodyClearance);
|
||||
WriteBoolean(stream, point.IsGearSwitchPoint);
|
||||
WriteInt32(stream, (int)point.Source);
|
||||
}
|
||||
|
||||
private static void WriteSegment(Stream stream, SmoothedPathSegment segment)
|
||||
{
|
||||
if (segment == null) throw new ArgumentException("稳定路径摘要不能包含空方向段。", nameof(segment));
|
||||
WriteInt32(stream, segment.SegmentIndex);
|
||||
WriteInt32(stream, (int)segment.Direction);
|
||||
WriteInt32(stream, segment.StartIndex);
|
||||
WriteInt32(stream, segment.EndIndex);
|
||||
WriteBoolean(stream, segment.StartsAtGearSwitch);
|
||||
WriteBoolean(stream, segment.EndsAtGearSwitch);
|
||||
}
|
||||
|
||||
private static void WriteDouble(Stream stream, double value)
|
||||
{
|
||||
WriteInt64(stream, BitConverter.DoubleToInt64Bits(value));
|
||||
}
|
||||
|
||||
private static void WriteBoolean(Stream stream, bool value)
|
||||
{
|
||||
stream.WriteByte(value ? (byte)1 : (byte)0);
|
||||
}
|
||||
|
||||
private static void WriteInt32(Stream stream, int value)
|
||||
{
|
||||
unchecked
|
||||
{
|
||||
stream.WriteByte((byte)value);
|
||||
stream.WriteByte((byte)(value >> 8));
|
||||
stream.WriteByte((byte)(value >> 16));
|
||||
stream.WriteByte((byte)(value >> 24));
|
||||
}
|
||||
}
|
||||
|
||||
private static void WriteInt64(Stream stream, long value)
|
||||
{
|
||||
unchecked
|
||||
{
|
||||
ulong bits = (ulong)value;
|
||||
for (int index = 0; index < 8; index++) stream.WriteByte((byte)(bits >> (index * 8)));
|
||||
}
|
||||
}
|
||||
}
|
||||
+248
@@ -0,0 +1,248 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Diagnostics;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade;
|
||||
|
||||
/// <summary>以固定预热和五次测量隔离比较所有请求平滑方法的离线入口。</summary>
|
||||
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>
|
||||
public PathSmoothingComparisonResult Compare(
|
||||
PathSmoothingComparisonRequest request,
|
||||
CancellationToken cancellationToken = default)
|
||||
{
|
||||
PathSmoothingComparisonEntry baseline = CreateRawPathBaseline(request, out string baselineReason);
|
||||
var entries = new List<PathSmoothingComparisonEntry>();
|
||||
|
||||
if (cancellationToken.IsCancellationRequested)
|
||||
return Cancelled(baseline, entries);
|
||||
|
||||
if (request == null || request.SmoothingRequest == null)
|
||||
return new PathSmoothingComparisonResult(baseline, entries, null, false, baselineReason);
|
||||
|
||||
for (int methodIndex = 0; methodIndex < request.Methods.Count; methodIndex++)
|
||||
{
|
||||
if (cancellationToken.IsCancellationRequested)
|
||||
return Cancelled(baseline, entries);
|
||||
|
||||
SmoothingMethod method = request.Methods[methodIndex];
|
||||
if (!TryCompareMethod(
|
||||
request.SmoothingRequest,
|
||||
method,
|
||||
baseline.Metrics,
|
||||
cancellationToken,
|
||||
out PathSmoothingComparisonEntry entry))
|
||||
return Cancelled(baseline, entries);
|
||||
entries.Add(entry);
|
||||
}
|
||||
|
||||
return new PathSmoothingComparisonResult(
|
||||
baseline,
|
||||
entries,
|
||||
SmoothingMethodRanker.Rank(entries),
|
||||
false,
|
||||
baselineReason);
|
||||
}
|
||||
|
||||
private bool TryCompareMethod(
|
||||
PathSmoothingRequest sourceRequest,
|
||||
SmoothingMethod method,
|
||||
PathQualityMetrics rawMetrics,
|
||||
CancellationToken cancellationToken,
|
||||
out PathSmoothingComparisonEntry entry)
|
||||
{
|
||||
entry = null;
|
||||
try
|
||||
{
|
||||
PathSmoothingRequest methodRequest = CreateMethodRequest(sourceRequest, method);
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
PathSmoothingResult warmup = _smoothingService.Smooth(methodRequest, cancellationToken);
|
||||
if (warmup.Status == PathSmoothingStatus.Cancelled || cancellationToken.IsCancellationRequested) return false;
|
||||
|
||||
var timings = new List<double>(5);
|
||||
var measuredResults = new List<PathSmoothingResult>(5);
|
||||
for (int sampleIndex = 0; sampleIndex < 5; sampleIndex++)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
Stopwatch stopwatch = Stopwatch.StartNew();
|
||||
PathSmoothingResult result = _smoothingService.Smooth(methodRequest, cancellationToken);
|
||||
stopwatch.Stop();
|
||||
if (result.Status == PathSmoothingStatus.Cancelled || cancellationToken.IsCancellationRequested) return false;
|
||||
timings.Add(stopwatch.Elapsed.TotalMilliseconds);
|
||||
measuredResults.Add(result);
|
||||
}
|
||||
|
||||
PathSmoothingResult canonical = measuredResults[0];
|
||||
string digest = StableGeometryDigest.Compute(canonical);
|
||||
SmoothingTimingSummary timing = SmoothingTimingSummary.FromMeasurements(timings, measuredResults);
|
||||
string diagnostic = string.IsNullOrWhiteSpace(timing.Diagnostic)
|
||||
? canonical.Diagnostics.TerminationReason
|
||||
: timing.Diagnostic;
|
||||
|
||||
PathQualityMetrics metrics = canonical.Status == PathSmoothingStatus.Success
|
||||
? NormalizeMetrics(canonical.Diagnostics.Metrics, rawMetrics)
|
||||
: new PathQualityMetrics();
|
||||
entry = PathSmoothingComparisonEntry.CreateCandidate(
|
||||
method,
|
||||
canonical.Status,
|
||||
metrics,
|
||||
timing,
|
||||
digest,
|
||||
diagnostic,
|
||||
canonical.Path,
|
||||
canonical.Segments);
|
||||
return true;
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
entry = PathSmoothingComparisonEntry.CreateCandidate(
|
||||
method,
|
||||
PathSmoothingStatus.Failed,
|
||||
new PathQualityMetrics(),
|
||||
new SmoothingTimingSummary(new double[] { 0d, 0d, 0d, 0d, 0d }, false, exception.GetType().Name),
|
||||
string.Empty,
|
||||
exception.Message,
|
||||
null,
|
||||
null);
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
private PathSmoothingComparisonEntry CreateRawPathBaseline(
|
||||
PathSmoothingComparisonRequest request,
|
||||
out string reason)
|
||||
{
|
||||
reason = string.Empty;
|
||||
if (request == null || request.SmoothingRequest == null)
|
||||
return FailedBaseline(PathSmoothingStatus.InvalidInput, "比较请求为空。", out reason);
|
||||
|
||||
PathSmoothingRequest smoothingRequest = request.SmoothingRequest;
|
||||
PathSmoothingConfiguration configuration = smoothingRequest.Configuration;
|
||||
if (configuration == null || smoothingRequest.Map == null || smoothingRequest.Vehicle == null)
|
||||
return FailedBaseline(PathSmoothingStatus.InvalidInput, "比较请求缺少可用的地图、车辆或配置。", out reason);
|
||||
|
||||
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))
|
||||
return FailedBaseline(PathSmoothingStatus.InvalidInput, reason, out reason);
|
||||
if (!_validator.TryValidate(
|
||||
analysis.Path,
|
||||
analysis.Segments,
|
||||
preparedPath,
|
||||
smoothingRequest.Map,
|
||||
smoothingRequest.Vehicle,
|
||||
configuration.MaximumCollisionCheckStepMeters,
|
||||
out IReadOnlyList<SmoothedPathPoint> safePath,
|
||||
out double minimumClearanceMeters,
|
||||
out reason))
|
||||
{
|
||||
return FailedBaseline(PathSmoothingStatus.InvalidInput, reason, out reason);
|
||||
}
|
||||
|
||||
PathQualityMetrics metrics = CreateMetrics(analysis, minimumClearanceMeters);
|
||||
string digest = StableGeometryDigest.Compute(PathSmoothingStatus.Success, null, safePath, analysis.Segments);
|
||||
return PathSmoothingComparisonEntry.CreateRawPathBaseline(
|
||||
PathSmoothingStatus.Success,
|
||||
metrics,
|
||||
digest,
|
||||
string.Empty,
|
||||
safePath,
|
||||
analysis.Segments);
|
||||
}
|
||||
|
||||
private static PathSmoothingComparisonEntry FailedBaseline(
|
||||
PathSmoothingStatus status,
|
||||
string failureReason,
|
||||
out string reason)
|
||||
{
|
||||
reason = failureReason ?? string.Empty;
|
||||
return PathSmoothingComparisonEntry.CreateRawPathBaseline(
|
||||
status,
|
||||
new PathQualityMetrics(),
|
||||
StableGeometryDigest.Compute(status, null, null, null),
|
||||
reason,
|
||||
null,
|
||||
null);
|
||||
}
|
||||
|
||||
private static PathSmoothingComparisonResult Cancelled(
|
||||
PathSmoothingComparisonEntry baseline,
|
||||
IReadOnlyList<PathSmoothingComparisonEntry> entries)
|
||||
{
|
||||
return new PathSmoothingComparisonResult(baseline, entries, null, true, "路径平滑比较已取消。");
|
||||
}
|
||||
|
||||
private static PathSmoothingRequest CreateMethodRequest(PathSmoothingRequest source, SmoothingMethod method)
|
||||
{
|
||||
PathSmoothingConfiguration configuration = source.Configuration;
|
||||
configuration.Method = method;
|
||||
configuration.AllowFallbackToCoarsePath = false;
|
||||
return new PathSmoothingRequest(
|
||||
source.CoarsePath,
|
||||
source.Segments,
|
||||
source.Map,
|
||||
source.Vehicle,
|
||||
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)
|
||||
{
|
||||
if (candidate == null || raw == null || !candidate.IsFeasible)
|
||||
return new PathQualityMetrics();
|
||||
|
||||
return new PathQualityMetrics(
|
||||
true,
|
||||
candidate.PathLengthMeters,
|
||||
candidate.MaximumAbsoluteVehicleCurvaturePerMeter,
|
||||
candidate.RootMeanSquareVehicleCurvaturePerMeter,
|
||||
candidate.TotalAbsoluteCurvatureVariationPerMeter,
|
||||
candidate.CurvatureVariationEnergy,
|
||||
candidate.MinimumBodyClearanceMeters,
|
||||
RelativePercentOrAbsoluteDelta(candidate.PathLengthMeters, raw.PathLengthMeters),
|
||||
RelativePercentOrAbsoluteDelta(
|
||||
candidate.MaximumAbsoluteVehicleCurvaturePerMeter,
|
||||
raw.MaximumAbsoluteVehicleCurvaturePerMeter),
|
||||
RelativePercentOrAbsoluteDelta(
|
||||
candidate.TotalAbsoluteCurvatureVariationPerMeter,
|
||||
raw.TotalAbsoluteCurvatureVariationPerMeter),
|
||||
candidate.MinimumBodyClearanceMeters - raw.MinimumBodyClearanceMeters);
|
||||
}
|
||||
|
||||
private static double RelativePercentOrAbsoluteDelta(double candidate, double raw)
|
||||
{
|
||||
double delta = candidate - raw;
|
||||
return Math.Abs(raw) < 1e-12d ? delta : delta / raw * 100d;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user