import numpy as np
from numpy.typing import NDArray
from perda.core_data_structures import DataInstance, left_join_data_instances
from perda.utils import lowpass_filter_by_distance
from scipy.interpolate import splprep
from ..log_analysis.kinematics import CURVATURE_BODY
from ..log_analysis.preprocess_gps import POS_X, POS_Y
from .models import (
ContinuousTrackData,
CornerListInput,
DiscreteTrackData,
PerdaLogInput,
SampledTrackInput,
TrackData,
)
from .track import Track
from .utils import (
corners_to_cartesian,
scale_and_discretize_corner_lengths,
smooth_and_normalize_corner_radii,
unroll_corners_into_track,
)
[docs]
def from_corners(corner_input: CornerListInput) -> Track:
"""
Factory method to generate a Track class from a list of corners. Compatible with tracks saved in YAML.
Parameters
----------
corner_input : CornerListInput
Corner list input containing corners and track parameters
Returns
-------
Track
Track instance created from corner list
"""
# Convert corner length to step count. Optionally scale the length of a track.
discretized_corners = scale_and_discretize_corner_lengths(
corner_input.corners,
corner_input.shorten * (0.1 / corner_input.distance_step),
)
x_m, y_m, arcs = corners_to_cartesian(
discretized_corners,
corner_input.distance_step,
start_angle_deg=corner_input.ideal_rotation_angle,
)
if corner_input.clean_corners:
discretized_corners = smooth_and_normalize_corner_radii(discretized_corners)
dx, radius, cumulative_dist = unroll_corners_into_track(
discretized_corners, corner_input.distance_step
)
return Track(
dx,
radius,
cumulative_dist,
x_m,
y_m,
corner_input.distance_step,
False,
original_corners=corner_input.corners,
arcs=arcs,
)
[docs]
def from_perda_logs(log_input: PerdaLogInput) -> Track:
"""Build a QSS-compatible Track from a PERDA lap segment.
Parameters
----------
log_input : PerdaLogInput
Validated input holding a SingleRunData lap segment with required fields
(body.curvature, groundSpeed, posN, posE) and filter/resample parameters.
Returns
-------
Track
Continuous Track instance with precomputed radius and B-spline.
Notes
-----
Pipeline:
1. Align body.curvature and posE onto posN timestamp grid.
2. Compute cumulative arc-length distance DataInstance from posN/posE.
3. Apply PERDA spatial lowpass filter on aligned curvature.
4. Resample, clamp radius and fit the B-spline via _build_continuous_track.
"""
dx = log_input.distance_step
data = log_input.data
pos_x_di = data[POS_X]
pos_y_di = data[POS_Y]
curv_di = data[CURVATURE_BODY]
# 1. Align curvature and posY onto posX timestamp grid
pos_x_ref, pos_y_aligned, curv_aligned = left_join_data_instances(pos_x_di, [pos_y_di, curv_di])
pos_x_vals = pos_x_ref.value_np
pos_y_vals = pos_y_aligned.value_np
# 2. Compute cumulative arc-length distance
x_diff = np.diff(pos_x_vals, prepend=pos_x_vals[0])
y_diff = np.diff(pos_y_vals, prepend=pos_y_vals[0])
arc_steps = np.sqrt(x_diff**2 + y_diff**2)
arc_steps[0] = 0.0
cumulative_dist_raw: NDArray[np.float64] = np.cumsum(arc_steps)
# 3. Spatial lowpass filter on curvature
curv_filtered_result = lowpass_filter_by_distance(
curv_aligned,
distance_di=DataInstance(
timestamp_np=pos_x_ref.timestamp_np,
value_np=cumulative_dist_raw,
),
cutoff_freq_per_meter=log_input.cutoff_freq_per_meter,
order=log_input.filter_order,
)
assert isinstance(curv_filtered_result, DataInstance)
curv_filtered_di: DataInstance = curv_filtered_result
return _build_continuous_track(
cumulative_dist_raw,
curv_filtered_di.value_np,
pos_x_vals,
pos_y_vals,
dx,
log_input.max_radius,
)
[docs]
def from_sampled(sampled: SampledTrackInput, distance_step: float) -> Track:
"""
Factory method to rebuild a continuous Track from uniformly spaced samples saved in YAML.
Parameters
----------
sampled : SampledTrackInput
Radius and position samples, spaced by ``sampled.distance_step``.
distance_step : float
Spacing to resample the track onto, in meters.
Returns
-------
Track
Continuous Track instance with precomputed radius and B-spline.
"""
radius = np.asarray(sampled.radius, dtype=np.float64)
cumulative_dist_raw = np.arange(len(radius), dtype=np.float64) * sampled.distance_step
return _build_continuous_track(
cumulative_dist_raw,
1.0 / radius,
np.asarray(sampled.x_m, dtype=np.float64),
np.asarray(sampled.y_m, dtype=np.float64),
distance_step,
float(np.max(radius)),
)
def _build_continuous_track(
cumulative_dist_raw: NDArray[np.float64],
curvature_raw: NDArray[np.float64],
x_raw: NDArray[np.float64],
y_raw: NDArray[np.float64],
distance_step: float,
max_radius: float,
) -> Track:
"""
Resample curvature and position onto a uniform distance grid and fit a B-spline.
Parameters
----------
cumulative_dist_raw : NDArray[float64]
Distance along the path at each raw sample, in meters. Must be increasing.
curvature_raw : NDArray[float64]
Curvature at each raw sample, in 1/m.
x_raw, y_raw : NDArray[float64]
Position at each raw sample, in meters.
distance_step : float
Spacing of the uniform grid, in meters.
max_radius : float
Radius clamp for near-straight segments, in meters.
Returns
-------
Track
Continuous Track instance.
"""
total_dist = float(cumulative_dist_raw[-1])
n = int(np.ceil(total_dist / distance_step)) + 1
d_uniform: NDArray[np.float64] = np.linspace(0.0, total_dist, n)
curv_uniform: NDArray[np.float64] = np.interp(d_uniform, cumulative_dist_raw, curvature_raw)
x_m_uniform: NDArray[np.float64] = np.interp(d_uniform, cumulative_dist_raw, x_raw)
y_m_uniform: NDArray[np.float64] = np.interp(d_uniform, cumulative_dist_raw, y_raw)
radius = np.full_like(curv_uniform, max_radius, dtype=np.float64)
valid_curv_mask = curv_uniform != 0.0
radius[valid_curv_mask] = np.abs(1.0 / curv_uniform[valid_curv_mask])
radius = np.clip(radius, 0.0, max_radius).astype(np.float64)
# Deduplicate consecutive identical XY points before fitting the spline
seg = np.sqrt(np.diff(x_m_uniform) ** 2 + np.diff(y_m_uniform) ** 2)
unique = np.concatenate(([True], seg > 0))
tck, _ = splprep([x_m_uniform[unique], y_m_uniform[unique]], u=d_uniform[unique], s=0, k=3)
return Track(
dx=np.full(n, distance_step, dtype=np.float64),
radius=radius,
cumulative_dist=d_uniform,
x_m=x_m_uniform,
y_m=y_m_uniform,
distance_step=distance_step,
continuous=True,
tck=tck,
)
[docs]
def from_data(data: TrackData) -> Track:
"""
Factory method to create a Track from a TrackData model.
Parameters
----------
data : ContinuousTrackData | DiscreteTrackData
A TrackData instance containing all track fields including pre-computed seed indices
Returns
-------
Track
A new Track instance with all fields populated from the data model
"""
# Create a new Track instance without calling __init__
instance = Track.__new__(Track)
# Set common fields
instance.dx = data.dx
instance.radius = data.radius
instance.cumulative_dist = data.cumulative_dist
instance.x_m = data.x_m
instance.y_m = data.y_m
instance.distance_step = data.distance_step
instance.seed_idx = data.seed_idx
# Set type-specific fields
match data:
case ContinuousTrackData():
instance.continuous = True
instance.tck = data.tck
case DiscreteTrackData():
instance.continuous = False
instance.original_corners = data.original_corners
instance.arcs = data.arcs
return instance