import numpy as np
import numpy.typing as npt
from perda.core_data_structures import SingleRunData, split_single_run_data
from perda.core_data_structures.data_instance import (
DataInstance,
left_join_data_instances,
)
from scipy.optimize import minimize
from scipy.spatial import cKDTree
from .preprocess_gps import POS_X, POS_Y
type Point = tuple[float, float] # (East/x/lon, North/y/lat)
type LineSegment = tuple[Point, Point]
def _is_crossing_line(
p1: Point,
p2: Point,
line_start: Point,
line_end: Point,
) -> bool:
"""
Return True if [p1,p2] crosses [line_start,line_end].
Parametric form of the two line segments:
line_start.x + t*d_line.x = p1.x + u*d_seg.x
line_start.y + t*d_line.y = p1.y + u*d_seg.y
Rearranging gives a 2x2 linear system in t and u:
[ d_line.x -d_seg.x ] [ t ] [ p1.x - line_start.x ]
[ d_line.y -d_seg.y ] [ u ] = [ p1.y - line_start.y ]
"""
d_line = (line_end[0] - line_start[0], line_end[1] - line_start[1])
d_seg = (p2[0] - p1[0], p2[1] - p1[1])
det = d_line[0] * d_seg[1] - d_line[1] * d_seg[0]
if det == 0:
return False
dx = p1[0] - line_start[0]
dy = p1[1] - line_start[1]
t = (dx * d_seg[1] - dy * d_seg[0]) / det
u = (dx * d_line[1] - dy * d_line[0]) / det
return bool(0 <= t <= 1 and 0 <= u <= 1)
[docs]
def detect_lap_crossings(
pos_x: npt.NDArray[np.float64],
pos_y: npt.NDArray[np.float64],
timestamps: npt.NDArray[np.float64],
sf_line: LineSegment,
min_lap_time: float = 10.0,
) -> list[float]:
"""Detect start/finish line crossings and return their timestamps. Crossings separated
by less than ``min_lap_time`` are ignored to suppress false positives (e.g. GPS jitters).
Parameters
----------
pos_x, pos_y : NDArray[float64]
X and Y coordinates in meters, same length as timestamps.
timestamps : NDArray[float64]
sf_line : ((x1, y1), (x2, y2))
Start/finish line endpoints in local X/Y meters.
min_lap_time : float
Minimum elapsed time between accepted crossings (same unit as
timestamps).
Returns
-------
list[float]
Timestamps of each accepted S/F crossing, in order.
"""
crossings: list[float] = []
prev_t = timestamps[0]
for i in range(len(pos_x) - 1):
if _is_crossing_line(
(pos_x[i], pos_y[i]),
(pos_x[i + 1], pos_y[i + 1]),
sf_line[0],
sf_line[1],
):
t = timestamps[i]
elapsed = t - prev_t
prev_t = t
if elapsed < min_lap_time:
continue
crossings.append(float(t))
return crossings
[docs]
def split_laps_from_gps(
data: SingleRunData,
sf_line: LineSegment,
min_lap_time: float = 10.0,
) -> "dict[int, SingleRunData]":
"""Detect laps via GPS and split SingleRunData into per-lap segments.
Parameters
----------
data : SingleRunData
Must contain ``posX`` and ``posY``.
sf_line : ((x1, y1), (x2, y2))
Start/finish line endpoints in local X/Y meters.
min_lap_time : float
Minimum elapsed time between crossings (same unit as data timestamps).
Returns
-------
List[SingleRunData]
"""
missing = [c for c in (POS_X, POS_Y) if c not in data]
if missing:
raise KeyError(
f"split_laps_from_gps: missing variable(s) {missing}.\n"
f"Try running run preprocess_gps_data_instances first."
)
pos_x_di = data[POS_X]
ts = pos_x_di.timestamp_np
pos_x_vals = pos_x_di.value_np
pos_y_vals = data[POS_Y].value_np
crossings = detect_lap_crossings(
pos_x_vals, pos_y_vals, ts, sf_line, min_lap_time=min_lap_time
)
if len(crossings) < 2:
raise ValueError(
f"Fewer than 2 crossings detected ({len(crossings)}), cannot split laps."
)
return split_single_run_data(data, crossings)
def _bin_xy(xy: npt.NDArray[np.float64], n_bins: int = 2048) -> npt.NDArray[np.float64]:
"""Resample XY path to n_bins equally-spaced points along arc length."""
dists = np.concatenate(
([0.0], np.cumsum(np.linalg.norm(np.diff(xy, axis=0), axis=1)))
)
target = np.linspace(0, dists[-1], n_bins)
n_interp = np.interp(target, dists, xy[:, 0])
e_interp = np.interp(target, dists, xy[:, 1])
return np.column_stack([n_interp, e_interp])
[docs]
def align_laps(
laps: list[SingleRunData],
baseline_lap: int | None = None,
) -> int:
"""Align each lap's ``posX`` and ``posY`` DataInstances to a baseline
using cKDTree + Nelder-Mead to minimize mean log-nearest-neighbour distance.
Only considers translations of the GPS points.
Parameters
----------
laps : list[SingleRunData]
List of SingleRunData objects corresponding to each lap, typically generated by
split_laps_from_gps.
baseline_lap : int
Index into ``laps`` to use as the alignment reference. If None, dynamically picks
the fastest lap as the basline
Returns
-------
int
Index of the baseline lap used for alignment.
"""
for i, lap in enumerate(laps):
missing = [c for c in (POS_X, POS_Y) if c not in lap]
if missing:
raise KeyError(
f"align_laps: lap {i} is missing variable(s) {missing}.\n"
f"Try running preprocess_gps_data_instances first."
)
if len(laps) < 2:
raise ValueError(
f"align_laps: need at least 2 laps to align, but got {len(laps)}."
)
if baseline_lap is None:
lap_times = [
lap[POS_X].timestamp_np[-1] - lap[POS_X].timestamp_np[0] for lap in laps
]
baseline_lap = int(np.argmin(lap_times))
# Construct cKDTree for baseline lap
bl_lap = laps[baseline_lap]
bl_x = bl_lap[POS_X]
bl_y_aligned = left_join_data_instances(bl_x, [bl_lap[POS_Y]])[1]
bl_xy = np.column_stack([bl_x.value_np, bl_y_aligned.value_np])
bl_binned = _bin_xy(bl_xy)
bl_tree = cKDTree(bl_binned)
# Define loss function for a particular translation of the current lap
# being aligned: mean log-distance to nearest neighbour in baseline lap
def _loss(
# dX, dY translation, dimension (2,)
params: npt.NDArray[np.float64],
# Column-stacked GPS points, dimension (n_points, 2)
lap_xy: npt.NDArray[np.float64],
) -> float:
shifted = lap_xy + params
dists, _ = bl_tree.query(shifted)
return float(np.mean(np.log(dists + 1.0)))
for i, lap in enumerate(laps):
if i == baseline_lap:
continue
pos_x_di = lap[POS_X]
pos_y_aligned = left_join_data_instances(pos_x_di, [lap[POS_Y]])[1]
lap_xy = np.column_stack([pos_x_di.value_np, pos_y_aligned.value_np])
lap_binned = _bin_xy(lap_xy)
res = minimize(
_loss,
x0=np.zeros(2), # Initial value of params
args=(lap_binned,), # Extra arguments to _loss, unmodified lap_xy
method="Nelder-Mead",
options={"xatol": 1e-4, "fatol": 1e-6, "maxiter": 5000},
)
dx, dy = res.x
lap[POS_X] = DataInstance(
timestamp_np=pos_x_di.timestamp_np,
value_np=pos_x_di.value_np + dx,
label=pos_x_di.label,
cpp_name=POS_X,
)
lap[POS_Y] = DataInstance(
timestamp_np=pos_y_aligned.timestamp_np,
value_np=pos_y_aligned.value_np + dy,
label=pos_y_aligned.label,
cpp_name=POS_Y,
)
return baseline_lap