"""Lap division, splitting, and alignment utilities.
Ported from vnav_lap_analyzer.ipynb cells 8 (lap detection) and 16
(splitting + alignment).
"""
from __future__ import annotations
import numpy as np
import pandas as pd
import plotly.graph_objects as go
from scipy.optimize import minimize
from scipy.spatial import cKDTree
from ._utils import detect_time_divisor, downsample_indices, get_time_array
# ---------------------------------------------------------------------------
# Geometry helpers
# ---------------------------------------------------------------------------
def _is_crossing_line(
p1: tuple[float, float],
p2: tuple[float, float],
line_start: tuple[float, float],
line_end: tuple[float, float],
) -> bool:
"""True if segment p1->p2 crosses the segment line_start->line_end."""
p1 = np.asarray(p1, dtype=np.float64)
p2 = np.asarray(p2, dtype=np.float64)
ls = np.asarray(line_start, dtype=np.float64)
le = np.asarray(line_end, dtype=np.float64)
d_line = le - ls
d_seg = p2 - p1
det = d_line[0] * d_seg[1] - d_line[1] * d_seg[0]
if det == 0:
return False
t = ((p1[0] - ls[0]) * d_seg[1] - (p1[1] - ls[1]) * d_seg[0]) / det
u = ((p1[0] - ls[0]) * d_line[1] - (p1[1] - ls[1]) * d_line[0]) / det
return 0 <= t <= 1 and 0 <= u <= 1
def _cumulative_distance(posn: np.ndarray, pose: np.ndarray) -> np.ndarray:
ds = np.sqrt(np.diff(posn) ** 2 + np.diff(pose) ** 2)
return np.concatenate(([0.0], np.cumsum(ds)))
_ALIGN_BINS = 2048
def _bin_xy(xy: np.ndarray, n_bins: int = _ALIGN_BINS) -> np.ndarray:
"""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])
# ---------------------------------------------------------------------------
# Lat/lon -> XY conversion for S/F line
# ---------------------------------------------------------------------------
R_EARTH = 6_371_000
def _latlon_to_xy(
lat: float,
lon: float,
lat_ref_rad: float,
lon_ref_rad: float,
) -> tuple[float, float]:
n = (np.radians(lat) - lat_ref_rad) * R_EARTH
e = (np.radians(lon) - lon_ref_rad) * R_EARTH * np.cos(lat_ref_rad)
return (n, e)
# ---------------------------------------------------------------------------
# Public API
# ---------------------------------------------------------------------------
[docs]
def detect_lap_crossings(
df: pd.DataFrame,
sf_line: tuple[tuple[float, float], tuple[float, float]],
time_col: str = "time_s",
pos_cols: tuple[str, str] = ("posN", "posE"),
min_lap_time_s: float = 20.0,
start_time: float | None = None,
end_time: float | None = None,
) -> pd.DataFrame:
"""Detect start/finish line crossings and label laps.
Works in local cartesian coordinates (``posN``, ``posE``).
The ``sf_line`` is a pair of (N, E) tuples defining the S/F segment.
If ``sf_line`` contains lat/lon tuples instead, convert them first
with ``gps_to_local_cartesian`` and pick coordinates from the map plot.
Parameters
----------
df : pd.DataFrame
Must contain *pos_cols* and *time_col*.
sf_line : ((n1, e1), (n2, e2))
Start/finish line endpoints in local cartesian.
time_col : str
pos_cols : (str, str)
(North column, East column).
min_lap_time_s : float
Minimum time between crossings to accept as a real lap.
start_time, end_time : float or None
Time window to search within. ``None`` uses full range.
Returns
-------
pd.DataFrame
Copy of *df* with a ``Lap`` column added (0 = before first
crossing, 1 = first lap, etc.).
"""
print(
"""
DEPRECATION WARNING: detect_lap_crossings is deprecated and will be removed in a future release.
Please use suboptimumg.log_analysis.gps_laps.detect_lap_crossings instead, which implements this functionality
without needing to convert to pandas DataFrames.
"""
)
df = df.copy()
df["Lap"] = 0
pn_col, pe_col = pos_cols
pn = df[pn_col].values
pe = df[pe_col].values
t = get_time_array(df, time_col)
divisor = detect_time_divisor(np.asarray([t[0], t[-1]], dtype=np.float64))
if start_time is None:
start_time = t[0]
if end_time is None:
end_time = t[-1]
lap_count = 0
prev_crossing_t = start_time
for i in range(len(df) - 1):
if t[i] < start_time or t[i] > end_time:
continue
if _is_crossing_line(
(pn[i], pe[i]),
(pn[i + 1], pe[i + 1]),
sf_line[0],
sf_line[1],
):
elapsed = (t[i] - prev_crossing_t) / divisor
if elapsed < min_lap_time_s:
print(
f" Ignoring crossing at t={t[i]:.0f}: "
f"{elapsed:.2f}s < min_lap_time ({min_lap_time_s}s)"
)
prev_crossing_t = t[i]
continue
print(
f" Lap {lap_count} -> {lap_count + 1} at t={t[i]:.0f} ({elapsed:.2f}s)"
)
prev_crossing_t = t[i]
lap_count += 1
df.iat[i, df.columns.get_loc("Lap")] = lap_count
# Fill the last row
df.iat[-1, df.columns.get_loc("Lap")] = lap_count
print(f"[laps] Detected {lap_count} lap(s)")
print(f"[laps] Added column: Lap ({len(df)} rows)")
return df
[docs]
def split_laps(
df: pd.DataFrame,
buffer_distance_m: float = 5.0,
min_lap_distance_m: float = 20.0,
pos_cols: tuple[str, str] = ("posN", "posE"),
time_col: str = "time_s",
) -> dict[int, pd.DataFrame]:
"""Split a DataFrame by ``Lap`` column with arc-length buffer.
Each lap DataFrame gets ``LapTime`` and ``LapDist`` columns added.
Parameters
----------
df : pd.DataFrame
Must have ``Lap``, *pos_cols*, and *time_col* columns.
buffer_distance_m : float
Extra path distance before/after each lap boundary.
min_lap_distance_m : float
Discard laps shorter than this.
pos_cols, time_col : str
Returns
-------
dict[int, pd.DataFrame]
Keyed by lap number (>0).
"""
print(
"""
DEPRECATION WARNING: split_laps is deprecated and will be removed in a future release.
Please use suboptimumg.log_analysis.gps_laps.split_laps_from_gps instead, which implements this functionality
without needing to convert to pandas DataFrames. Please also check the latest PERDA documentation. PERDA provides
split_single_run_data and trim_single_run_data to natively manipulate SingleRunData objects.
"""
)
pn_col, pe_col = pos_cols
full_cumdist = _cumulative_distance(df[pn_col].values, df[pe_col].values)
lap_numbers = sorted(df.loc[df["Lap"] > 0, "Lap"].unique())
lap_dfs: dict[int, pd.DataFrame] = {}
t_arr = get_time_array(df, time_col)
divisor = detect_time_divisor(np.asarray([t_arr[0], t_arr[-1]], dtype=np.float64))
for lap_num in lap_numbers:
mask = df["Lap"] == lap_num
positions = np.nonzero(mask.values)[0]
si, ei = positions[0], positions[-1]
buf_si = np.searchsorted(
full_cumdist,
max(full_cumdist[si] - buffer_distance_m, 0.0),
side="left",
)
buf_ei = (
np.searchsorted(
full_cumdist,
min(full_cumdist[ei] + buffer_distance_m, full_cumdist[-1]),
side="right",
)
- 1
)
buf_ei = min(buf_ei, len(df) - 1)
lap_df = df.iloc[buf_si : buf_ei + 1].copy()
t_vals = get_time_array(lap_df, time_col)
lap_df["LapTime"] = (t_vals - t_vals[0]) / divisor
lap_df["LapDist"] = _cumulative_distance(
lap_df[pn_col].values, lap_df[pe_col].values
)
total_dist = lap_df["LapDist"].iloc[-1]
if total_dist < min_lap_distance_m:
print(
f" Dropping Lap {lap_num}: {total_dist:.1f}m < {min_lap_distance_m}m"
)
continue
lap_dfs[lap_num] = lap_df
print(
f" Lap {lap_num}: {len(lap_df)} pts, "
f"dist={total_dist:.1f}m, time={lap_df['LapTime'].iloc[-1]:.2f}s"
)
print(f"[laps] Split into {len(lap_dfs)} lap(s)")
print(f"[laps] Added columns: LapTime, LapDist")
return lap_dfs
[docs]
def align_laps(
lap_dfs: dict[int, pd.DataFrame],
baseline_lap: int | None = None,
sf_line: tuple[tuple[float, float], tuple[float, float]] | None = None,
pos_cols: tuple[str, str] = ("posN", "posE"),
time_col: str = "time_s",
) -> dict[int, pd.DataFrame]:
"""Align all laps to a baseline via translation-only optimisation.
Uses cKDTree + Nelder-Mead to minimise mean log-nearest-neighbour
distance after a rigid translation (no rotation, preserving yaw).
If ``sf_line`` is given, laps are re-trimmed at the S/F line after
alignment.
Parameters
----------
lap_dfs : dict[int, DataFrame]
As returned by :func:`split_laps`.
baseline_lap : int or None
Lap number to align to. ``None`` picks the fastest (shortest
``LapTime``).
sf_line : ((n1,e1), (n2,e2)) or None
S/F line for re-trimming.
pos_cols, time_col : str
Returns
-------
dict[int, pd.DataFrame]
Aligned (and optionally trimmed) lap DataFrames.
"""
print(
"""
DEPRECATION WARNING: align_laps is deprecated and will be removed in a future release.
Please use suboptimumg.log_analysis.gps_laps.align_laps instead, which implements this functionality
without needing to convert to pandas DataFrames.
"""
)
pn_col, pe_col = pos_cols
if baseline_lap is None:
baseline_lap = min(
lap_dfs,
key=lambda k: lap_dfs[k]["LapTime"].iloc[-1],
)
print(f"[laps] Aligning to Lap {baseline_lap}")
bl_xy = np.column_stack(
[
lap_dfs[baseline_lap][pn_col].values,
lap_dfs[baseline_lap][pe_col].values,
]
)
bl_binned = _bin_xy(bl_xy)
bl_tree = cKDTree(bl_binned)
def _loss(params, lap_xy):
shifted = lap_xy + np.array(params)
dists, _ = bl_tree.query(shifted)
return np.mean(np.log(dists + 1.0))
result_dfs: dict[int, pd.DataFrame] = {}
for lap_num, lap_df in lap_dfs.items():
lap_df = lap_df.copy()
if lap_num != baseline_lap:
lap_xy = np.column_stack([lap_df[pn_col].values, lap_df[pe_col].values])
lap_binned = _bin_xy(lap_xy)
res = minimize(
_loss,
x0=[0.0, 0.0],
args=(lap_binned,),
method="Nelder-Mead",
options={"xatol": 1e-4, "fatol": 1e-6, "maxiter": 5000},
)
dn, de = res.x
lap_df[pn_col] = lap_df[pn_col] + dn
lap_df[pe_col] = lap_df[pe_col] + de
print(f" Lap {lap_num}: dN={dn:+.4f}m, dE={de:+.4f}m, loss={res.fun:.6f}")
else:
print(f" Lap {lap_num}: baseline — no alignment")
# Re-trim at S/F line if provided
if sf_line is not None:
pn = lap_df[pn_col].values
pe = lap_df[pe_col].values
crossing_idxs = [
j
for j in range(len(lap_df) - 1)
if _is_crossing_line(
(pn[j], pe[j]),
(pn[j + 1], pe[j + 1]),
sf_line[0],
sf_line[1],
)
]
if len(crossing_idxs) >= 2:
lap_df = lap_df.iloc[crossing_idxs[0] : crossing_idxs[-1] + 2].copy()
else:
print(
f" Warning: Lap {lap_num} has {len(crossing_idxs)} S/F "
f"crossing(s) after alignment — keeping full segment"
)
lap_df = lap_df.copy()
t_vals = get_time_array(lap_df, time_col)
div = detect_time_divisor(
np.asarray([t_vals[0], t_vals[-1]], dtype=np.float64)
)
lap_df["LapTime"] = (t_vals - t_vals[0]) / div
lap_df["LapDist"] = _cumulative_distance(
lap_df[pn_col].values, lap_df[pe_col].values
)
result_dfs[lap_num] = lap_df
print(
f" Lap {lap_num}: {len(lap_df)} pts, "
f"dist={lap_df['LapDist'].iloc[-1]:.1f}m"
)
return result_dfs
[docs]
def plot_lap_overlay(
lap_dfs: dict[int, pd.DataFrame],
baseline_lap: int | None = None,
sf_line: tuple[tuple[float, float], tuple[float, float]] | None = None,
pos_cols: tuple[str, str] = ("posN", "posE"),
time_col: str = "time_s",
max_display_hz: float = 100.0,
) -> go.Figure:
"""Overlay all laps on a 2D scatter plot.
Parameters
----------
lap_dfs : dict[int, DataFrame]
baseline_lap : int or None
sf_line : S/F line endpoints or None
pos_cols : (north_col, east_col)
time_col : str
Time column used to estimate sample rate for downsampling.
max_display_hz : float
Target display sample rate. ``0`` or ``None`` disables.
Returns
-------
go.Figure
"""
pn_col, pe_col = pos_cols
if baseline_lap is None:
baseline_lap = min(
lap_dfs,
key=lambda k: lap_dfs[k]["LapTime"].iloc[-1],
)
colors = [
"#1f77b4",
"#ff7f0e",
"#2ca02c",
"#9467bd",
"#8c564b",
"#e377c2",
"#7f7f7f",
"#bcbd22",
"#17becf",
]
baseline_color = "#d62728"
fig = go.Figure()
def _lap_idx(tdf: pd.DataFrame) -> np.ndarray:
t = get_time_array(tdf, time_col)
div = detect_time_divisor(np.asarray([t[0], t[-1]], dtype=np.float64))
dur = (t[-1] - t[0]) / div
return downsample_indices(len(tdf), dur, max_display_hz)
ci = 0
for lap_num, tdf in sorted(lap_dfs.items()):
if lap_num == baseline_lap:
continue
li = _lap_idx(tdf)
fig.add_trace(
go.Scattergl(
x=tdf[pe_col].values[li],
y=tdf[pn_col].values[li],
mode="markers",
marker=dict(size=3, color=colors[ci % len(colors)]),
name=f"Lap {lap_num}",
)
)
ci += 1
bl = lap_dfs[baseline_lap]
bli = _lap_idx(bl)
fig.add_trace(
go.Scattergl(
x=bl[pe_col].values[bli],
y=bl[pn_col].values[bli],
mode="markers",
marker=dict(size=4, color=baseline_color),
name=f"Lap {baseline_lap} (baseline)",
)
)
if sf_line is not None:
fig.add_trace(
go.Scattergl(
x=[sf_line[0][1], sf_line[1][1]],
y=[sf_line[0][0], sf_line[1][0]],
mode="lines+markers",
line=dict(color="red", width=2),
marker=dict(size=6, color="red"),
name="S/F Line",
)
)
fig.update_layout(
title="Lap Overlay",
xaxis_title="East (m)",
yaxis_title="North (m)",
height=700,
width=800,
)
fig.update_yaxes(scaleanchor="x", scaleratio=1)
return fig