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

feat: compare and rank smoothing methods

Browse Source
This commit is contained in:
梁薄云
2026-07-29 15:30:50 +08:00
parent a693cbf323
commit 09953ab1f1
8 changed files with 968 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Comparison/PathSmoothingComparisonEntry.cs
+120
View File
@@ -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);
}
}
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Comparison/PathSmoothingComparisonRequest.cs
+60
View File
@@ -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);
}
}
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Comparison/PathSmoothingComparisonResult.cs
+49
View File
@@ -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);
}
}
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Comparison/SmoothingMethodRanker.cs
+60
View File
@@ -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;
}
}
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Comparison/SmoothingTimingSummary.cs
+90
View File
@@ -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;
}
}
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Comparison/StableGeometryDigest.cs
+111
View File
@@ -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)));
}
}
}