import numpy as np
import numpy.typing as npt
from perda.core_data_structures.data_instance import (
DataInstance,
left_join_data_instances,
)
from perda.utils.filtering 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 .gps import *
from .models import *
from .track import Track
from .utils import *
[docs]
def from_corners(corner_input: CornerListInput):
"""
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 filtered curvature, posN, posE onto uniform distance grid.
5. Convert curvature to radius, clamped at max_radius.
6. Fit B-spline to uniformly-resampled XY.
"""
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
dX = np.diff(pos_x_vals, prepend=pos_x_vals[0])
dY = np.diff(pos_y_vals, prepend=pos_y_vals[0])
arc_steps = np.sqrt(dX**2 + dY**2)
arc_steps[0] = 0.0
cumulative_dist_raw: npt.NDArray[np.float64] = np.cumsum(arc_steps)
# 3. Spatial lowpass filter on curvature
curv_filtered_di: DataInstance = 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,
)
# 4. Resample onto uniform distance grid
total_dist = float(cumulative_dist_raw[-1])
n = int(np.ceil(total_dist / dx)) + 1
d_uniform: npt.NDArray[np.float64] = np.linspace(0.0, total_dist, n)
curv_uniform: npt.NDArray[np.float64] = np.interp(
d_uniform, cumulative_dist_raw, curv_filtered_di.value_np
)
x_m_uniform: npt.NDArray[np.float64] = np.interp(
d_uniform, cumulative_dist_raw, pos_x_vals # posX = x (lon)
)
y_m_uniform: npt.NDArray[np.float64] = np.interp(
d_uniform, cumulative_dist_raw, pos_y_vals # posY = y (lat)
)
# 5. Curvature -> radius, clamped
radius = np.full_like(curv_uniform, log_input.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, log_input.max_radius).astype(np.float64)
# 6. Fit B-spline (deduplicate consecutive identical XY points first)
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
)
track = Track(
dx=np.full(n, dx, dtype=np.float64),
radius=radius,
cumulative_dist=d_uniform,
x_m=x_m_uniform,
y_m=y_m_uniform,
distance_step=dx,
continuous=True,
tck=tck,
)
return track
[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