feat: add reverse-safe Frenet transforms
This commit is contained in:
梁薄云
@@ -0,0 +1,33 @@
|
||||
using System;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>World pose projected onto one bounded direction segment.</summary>
|
||||
public sealed class FrenetProjection
|
||||
{
|
||||
public FrenetProjection(FrenetReferencePoint referencePoint, double lateralOffset, double headingError,
|
||||
double squaredDistanceMeters)
|
||||
{
|
||||
if (referencePoint == null)
|
||||
throw new ArgumentNullException(nameof(referencePoint));
|
||||
if (!IsFinite(lateralOffset) || !IsFinite(headingError) || !IsFinite(squaredDistanceMeters) || squaredDistanceMeters < 0d)
|
||||
throw new ArgumentOutOfRangeException(nameof(lateralOffset));
|
||||
|
||||
ReferencePoint = referencePoint;
|
||||
ReferenceS = referencePoint.ReferenceS;
|
||||
LateralOffset = lateralOffset;
|
||||
HeadingError = headingError;
|
||||
SquaredDistanceMeters = squaredDistanceMeters;
|
||||
}
|
||||
|
||||
public FrenetReferencePoint ReferencePoint { get; }
|
||||
public double ReferenceS { get; }
|
||||
public double LateralOffset { get; }
|
||||
public double HeadingError { get; }
|
||||
public double SquaredDistanceMeters { get; }
|
||||
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,131 @@
|
||||
using System;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Deterministically projects a world pose only inside the supplied direction segment and S window.</summary>
|
||||
public sealed class FrenetProjector
|
||||
{
|
||||
private const double TieTolerance = 1e-14d;
|
||||
|
||||
public bool TryProject(Pose2D worldPose, DirectionSegmentView segment, double minimumReferenceS,
|
||||
double maximumReferenceS, double maximumDistanceMeters, out FrenetProjection projection)
|
||||
{
|
||||
return TryProject(worldPose, segment, minimumReferenceS, maximumReferenceS, maximumDistanceMeters,
|
||||
minimumReferenceS, out projection);
|
||||
}
|
||||
|
||||
public bool TryProject(Pose2D worldPose, DirectionSegmentView segment, double minimumReferenceS,
|
||||
double maximumReferenceS, double maximumDistanceMeters, double seedReferenceS, out FrenetProjection projection)
|
||||
{
|
||||
projection = null;
|
||||
if (worldPose == null || segment == null || !IsFinite(worldPose.X) || !IsFinite(worldPose.Y) ||
|
||||
!IsFinite(worldPose.Heading) || !IsFinite(minimumReferenceS) || !IsFinite(maximumReferenceS) ||
|
||||
!IsFinite(maximumDistanceMeters) || !IsFinite(seedReferenceS) || maximumDistanceMeters < 0d)
|
||||
return false;
|
||||
|
||||
double lowerBound = Math.Max(0d, minimumReferenceS);
|
||||
double upperBound = Math.Min(segment.LengthMeters, maximumReferenceS);
|
||||
if (lowerBound > upperBound)
|
||||
return false;
|
||||
|
||||
Candidate best = null;
|
||||
for (int index = 0; index + 1 < segment.Points.Count; index++)
|
||||
{
|
||||
double startS = Math.Max(lowerBound, segment.Points[index].ArcLength);
|
||||
double endS = Math.Min(upperBound, segment.Points[index + 1].ArcLength);
|
||||
if (startS > endS)
|
||||
continue;
|
||||
|
||||
FrenetReferencePoint start = ReferencePathInterpolator.Interpolate(segment, startS);
|
||||
FrenetReferencePoint end = ReferencePathInterpolator.Interpolate(segment, endS);
|
||||
ConsiderLine(worldPose, start, end, seedReferenceS, ref best);
|
||||
}
|
||||
|
||||
if (best == null && Math.Abs(lowerBound - upperBound) <= TieTolerance)
|
||||
{
|
||||
FrenetReferencePoint point = ReferencePathInterpolator.Interpolate(segment, lowerBound);
|
||||
ConsiderPoint(worldPose, point, seedReferenceS, ref best);
|
||||
}
|
||||
|
||||
if (best == null || best.SquaredDistanceMeters > maximumDistanceMeters * maximumDistanceMeters)
|
||||
return false;
|
||||
|
||||
FrenetReferencePoint reference = ReferencePathInterpolator.Interpolate(segment, best.ReferenceS);
|
||||
double dx = worldPose.X - reference.X;
|
||||
double dy = worldPose.Y - reference.Y;
|
||||
double travelYaw = reference.TravelYaw;
|
||||
double lateralOffset = -dx * Math.Sin(travelYaw) + dy * Math.Cos(travelYaw);
|
||||
double egoTravelYaw = FrenetTransform.GetTravelYaw(worldPose.Heading, segment.Direction);
|
||||
double headingError = AngleMath.NormalizeRadians(egoTravelYaw - travelYaw);
|
||||
projection = new FrenetProjection(reference, lateralOffset, headingError, best.SquaredDistanceMeters);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static void ConsiderLine(Pose2D worldPose, FrenetReferencePoint start, FrenetReferencePoint end,
|
||||
double seedReferenceS, ref Candidate best)
|
||||
{
|
||||
double dx = end.X - start.X;
|
||||
double dy = end.Y - start.Y;
|
||||
double lengthSquared = dx * dx + dy * dy;
|
||||
if (lengthSquared <= TieTolerance)
|
||||
{
|
||||
ConsiderPoint(worldPose, start, seedReferenceS, ref best);
|
||||
ConsiderPoint(worldPose, end, seedReferenceS, ref best);
|
||||
return;
|
||||
}
|
||||
|
||||
double fraction = ((worldPose.X - start.X) * dx + (worldPose.Y - start.Y) * dy) / lengthSquared;
|
||||
fraction = Math.Max(0d, Math.Min(1d, fraction));
|
||||
double referenceS = start.ReferenceS + (end.ReferenceS - start.ReferenceS) * fraction;
|
||||
double x = start.X + dx * fraction;
|
||||
double y = start.Y + dy * fraction;
|
||||
Consider(worldPose, referenceS, x, y, seedReferenceS, ref best);
|
||||
}
|
||||
|
||||
private static void ConsiderPoint(Pose2D worldPose, FrenetReferencePoint point, double seedReferenceS,
|
||||
ref Candidate best)
|
||||
{
|
||||
Consider(worldPose, point.ReferenceS, point.X, point.Y, seedReferenceS, ref best);
|
||||
}
|
||||
|
||||
private static void Consider(Pose2D worldPose, double referenceS, double x, double y, double seedReferenceS,
|
||||
ref Candidate best)
|
||||
{
|
||||
double dx = worldPose.X - x;
|
||||
double dy = worldPose.Y - y;
|
||||
double squaredDistance = dx * dx + dy * dy;
|
||||
var candidate = new Candidate(referenceS, squaredDistance, Math.Abs(referenceS - seedReferenceS));
|
||||
if (best == null || candidate.IsPreferredTo(best))
|
||||
best = candidate;
|
||||
}
|
||||
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
}
|
||||
|
||||
private sealed class Candidate
|
||||
{
|
||||
public Candidate(double referenceS, double squaredDistanceMeters, double seedDistance)
|
||||
{
|
||||
ReferenceS = referenceS;
|
||||
SquaredDistanceMeters = squaredDistanceMeters;
|
||||
SeedDistance = seedDistance;
|
||||
}
|
||||
|
||||
public double ReferenceS { get; }
|
||||
public double SquaredDistanceMeters { get; }
|
||||
public double SeedDistance { get; }
|
||||
|
||||
public bool IsPreferredTo(Candidate other)
|
||||
{
|
||||
if (SquaredDistanceMeters < other.SquaredDistanceMeters - TieTolerance) return true;
|
||||
if (SquaredDistanceMeters > other.SquaredDistanceMeters + TieTolerance) return false;
|
||||
if (SeedDistance < other.SeedDistance - TieTolerance) return true;
|
||||
if (SeedDistance > other.SeedDistance + TieTolerance) return false;
|
||||
return ReferenceS < other.ReferenceS;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,58 @@
|
||||
using System;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Immutable interpolated reference sample in a single direction segment.</summary>
|
||||
public sealed class FrenetReferencePoint
|
||||
{
|
||||
public FrenetReferencePoint(double referenceS, double x, double y, double vehicleYaw, double unwrappedVehicleYaw,
|
||||
TravelDirection direction, double geometricCurvature, double vehicleCurvature,
|
||||
double vehicleCurvatureDerivative, double bodyClearance)
|
||||
{
|
||||
RequireFinite(referenceS, nameof(referenceS));
|
||||
RequireFinite(x, nameof(x));
|
||||
RequireFinite(y, nameof(y));
|
||||
RequireFinite(vehicleYaw, nameof(vehicleYaw));
|
||||
RequireFinite(unwrappedVehicleYaw, nameof(unwrappedVehicleYaw));
|
||||
RequireFinite(geometricCurvature, nameof(geometricCurvature));
|
||||
RequireFinite(vehicleCurvature, nameof(vehicleCurvature));
|
||||
RequireFinite(vehicleCurvatureDerivative, nameof(vehicleCurvatureDerivative));
|
||||
RequireFinite(bodyClearance, nameof(bodyClearance));
|
||||
|
||||
ReferenceS = referenceS;
|
||||
X = x;
|
||||
Y = y;
|
||||
VehicleYaw = AngleMath.NormalizeRadians(vehicleYaw);
|
||||
UnwrappedVehicleYaw = unwrappedVehicleYaw;
|
||||
Direction = direction;
|
||||
GeometricCurvature = geometricCurvature;
|
||||
VehicleCurvature = vehicleCurvature;
|
||||
VehicleCurvatureDerivative = vehicleCurvatureDerivative;
|
||||
BodyClearance = bodyClearance;
|
||||
}
|
||||
|
||||
public double ReferenceS { get; }
|
||||
public double X { get; }
|
||||
public double Y { get; }
|
||||
public double VehicleYaw { get; }
|
||||
public double UnwrappedVehicleYaw { get; }
|
||||
public TravelDirection Direction { get; }
|
||||
public double GeometricCurvature { get; }
|
||||
public double VehicleCurvature { get; }
|
||||
public double VehicleCurvatureDerivative { get; }
|
||||
public double BodyClearance { get; }
|
||||
|
||||
/// <summary>Unwrapped direction of travel, used internally for geometry.</summary>
|
||||
public double TravelYaw
|
||||
{
|
||||
get { return Direction == TravelDirection.Forward ? UnwrappedVehicleYaw : UnwrappedVehicleYaw + Math.PI; }
|
||||
}
|
||||
|
||||
private static void RequireFinite(double value, string name)
|
||||
{
|
||||
if (double.IsNaN(value) || double.IsInfinity(value))
|
||||
throw new ArgumentOutOfRangeException(name);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,45 @@
|
||||
using System;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Coordinate conversion that keeps Frenet lateral sign relative to travel direction.</summary>
|
||||
public static class FrenetTransform
|
||||
{
|
||||
public static bool TryReconstruct(FrenetReferencePoint referencePoint, double lateralOffset, double lateralDerivative,
|
||||
double minimumFrenetDenominator, out Pose2D pose)
|
||||
{
|
||||
pose = null;
|
||||
if (referencePoint == null || !IsFinite(lateralOffset) || !IsFinite(lateralDerivative) ||
|
||||
!IsFinite(minimumFrenetDenominator) || minimumFrenetDenominator <= 0d)
|
||||
return false;
|
||||
|
||||
double denominator = 1d - referencePoint.GeometricCurvature * lateralOffset;
|
||||
if (!IsFinite(denominator) || denominator < minimumFrenetDenominator)
|
||||
return false;
|
||||
|
||||
double travelYaw = referencePoint.TravelYaw;
|
||||
double x = referencePoint.X - lateralOffset * Math.Sin(travelYaw);
|
||||
double y = referencePoint.Y + lateralOffset * Math.Cos(travelYaw);
|
||||
double optimizedTravelYaw = travelYaw + Math.Atan2(lateralDerivative, denominator);
|
||||
double vehicleYaw = referencePoint.Direction == TravelDirection.Forward
|
||||
? AngleMath.NormalizeRadians(optimizedTravelYaw)
|
||||
: AngleMath.NormalizeRadians(optimizedTravelYaw + Math.PI);
|
||||
if (!IsFinite(x) || !IsFinite(y) || !IsFinite(vehicleYaw))
|
||||
return false;
|
||||
|
||||
pose = new Pose2D(x, y, vehicleYaw);
|
||||
return true;
|
||||
}
|
||||
|
||||
internal static double GetTravelYaw(double vehicleYaw, TravelDirection direction)
|
||||
{
|
||||
return direction == TravelDirection.Forward ? vehicleYaw : vehicleYaw + Math.PI;
|
||||
}
|
||||
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,64 @@
|
||||
using System;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Interpolates reference geometry inside exactly one direction segment.</summary>
|
||||
public static class ReferencePathInterpolator
|
||||
{
|
||||
private const double Epsilon = 1e-12d;
|
||||
|
||||
public static FrenetReferencePoint Interpolate(DirectionSegmentView segment, double referenceS)
|
||||
{
|
||||
if (segment == null)
|
||||
throw new ArgumentNullException(nameof(segment));
|
||||
if (!IsFinite(referenceS) || referenceS < -Epsilon || referenceS > segment.LengthMeters + Epsilon)
|
||||
throw new ArgumentOutOfRangeException(nameof(referenceS));
|
||||
|
||||
double clampedS = Math.Max(0d, Math.Min(segment.LengthMeters, referenceS));
|
||||
for (int index = 0; index < segment.Points.Count; index++)
|
||||
{
|
||||
SmoothedPathPoint upper = segment.Points[index];
|
||||
if (Math.Abs(upper.ArcLength - clampedS) <= Epsilon)
|
||||
return FromPoint(upper);
|
||||
if (upper.ArcLength > clampedS)
|
||||
{
|
||||
SmoothedPathPoint lower = segment.Points[index - 1];
|
||||
double span = upper.ArcLength - lower.ArcLength;
|
||||
if (span <= Epsilon)
|
||||
throw new ArgumentException("Reference points must have positive interpolation spans.", nameof(segment));
|
||||
double fraction = (clampedS - lower.ArcLength) / span;
|
||||
return new FrenetReferencePoint(
|
||||
clampedS,
|
||||
Lerp(lower.X, upper.X, fraction),
|
||||
Lerp(lower.Y, upper.Y, fraction),
|
||||
AngleMath.NormalizeRadians(Lerp(lower.UnwrappedHeading, upper.UnwrappedHeading, fraction)),
|
||||
Lerp(lower.UnwrappedHeading, upper.UnwrappedHeading, fraction),
|
||||
segment.Direction,
|
||||
Lerp(lower.GeometricCurvature, upper.GeometricCurvature, fraction),
|
||||
Lerp(lower.VehicleCurvature, upper.VehicleCurvature, fraction),
|
||||
Lerp(lower.VehicleCurvatureDerivative, upper.VehicleCurvatureDerivative, fraction),
|
||||
Lerp(lower.BodyClearance, upper.BodyClearance, fraction));
|
||||
}
|
||||
}
|
||||
return FromPoint(segment.Points[segment.Points.Count - 1]);
|
||||
}
|
||||
|
||||
private static FrenetReferencePoint FromPoint(SmoothedPathPoint point)
|
||||
{
|
||||
return new FrenetReferencePoint(point.ArcLength, point.X, point.Y, point.Heading, point.UnwrappedHeading,
|
||||
point.Direction, point.GeometricCurvature, point.VehicleCurvature, point.VehicleCurvatureDerivative,
|
||||
point.BodyClearance);
|
||||
}
|
||||
|
||||
private static double Lerp(double lower, double upper, double fraction)
|
||||
{
|
||||
return lower + (upper - lower) * fraction;
|
||||
}
|
||||
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user