pyssms 0.1.0__py3-none-any.whl

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
pyssms/__init__.py ADDED
@@ -0,0 +1,32 @@
1
+ """pyssms: continuous surrogate safety indicators for road-user encounters.
2
+
3
+ TTC, Time Advantage, T2 and Time Gap for arbitrary approach angles, computed exactly on a
4
+ time-time conflict polygon.
5
+ """
6
+
7
+ from pyssms.disturbance import DisturbanceSet, disturbance_set, form_probability
8
+ from pyssms.geometry import rectangle, rectangle_corners
9
+ from pyssms.margins import lateral_margin
10
+ from pyssms.severity import delta_v, extended_delta_v
11
+ from pyssms.stories import Story, first_contact, profile
12
+ from pyssms.timetime import Indicators, conflict_polygon, indicators, indicators_rect
13
+
14
+ __version__ = "0.1.0"
15
+ __all__ = [
16
+ "DisturbanceSet",
17
+ "Indicators",
18
+ "Story",
19
+ "__version__",
20
+ "conflict_polygon",
21
+ "delta_v",
22
+ "disturbance_set",
23
+ "extended_delta_v",
24
+ "first_contact",
25
+ "form_probability",
26
+ "indicators",
27
+ "indicators_rect",
28
+ "lateral_margin",
29
+ "profile",
30
+ "rectangle",
31
+ "rectangle_corners",
32
+ ]
pyssms/bank.py ADDED
@@ -0,0 +1,360 @@
1
+ """Conflict windows: extract them from trajectories, store them, load the shipped ones.
2
+
3
+ A **conflict window** is the stretch of a pair's trajectories (by default 5 s each side) around the
4
+ moment its encounter was closest by one of the package's own indicators. A *bank* is a set of such
5
+ windows with a table that says where each came from and how it was chosen.
6
+
7
+ What the shipped bank is, and is not
8
+ ------------------------------------
9
+ ``load_conflict_bank("ngsim")`` returns windows cut from the FHWA **NGSIM** vehicle-trajectory data
10
+ (US-101, I-80, Lankershim Boulevard, Peachtree Street). NGSIM is licensed by the Federal Highway
11
+ Administration under a modified Creative Commons Attribution 2.0 licence (not public domain, not
12
+ CC0): reuse and redistribution are free **if credit is given to FHWA and the NGSIM program and if
13
+ any reuse states that it is subject to that licence** (``pyssms/data/NGSIM_LICENSE.txt``, copied
14
+ from the archives). The data are provided "as is". Positions were shifted to a local origin;
15
+ nothing else was changed (feet were converted to metres, the front-centre reference point to the
16
+ body centre, :func:`pyssms.io.read_ngsim`), and smoothing was applied as recorded in the table.
17
+
18
+ The windows are **chosen by the package's own indicators** (the most severe pairs by TTC for
19
+ collision courses, by Time Advantage for crossing courses, the closest pairs otherwise). They are
20
+ examples, test material and a starting point for method development. They are **not a random
21
+ sample**, so no frequency, rate or distribution may be read from them, and using them to compare
22
+ the indicators that selected them would be circular.
23
+
24
+ Data of the German drone data sets (inD, rounD, uniD, exiD, highD) are never shipped: only
25
+ aggregated counts and scales (:func:`load_summary`) that contain no trajectory.
26
+ """
27
+
28
+ from __future__ import annotations
29
+
30
+ import json
31
+ from dataclasses import dataclass
32
+ from importlib import resources
33
+ from pathlib import Path
34
+ from typing import Any
35
+
36
+ import numpy as np
37
+
38
+ from pyssms import process
39
+ from pyssms.geometry import FloatArray, IntArray
40
+ from pyssms.trajectories import Trajectories
41
+
42
+ __all__ = [
43
+ "ConflictBank",
44
+ "available_banks",
45
+ "concat_banks",
46
+ "extract_conflicts",
47
+ "load_conflict_bank",
48
+ "load_summary",
49
+ "save_bank",
50
+ ]
51
+
52
+ _COLS = ("t", "x", "y", "heading", "vx", "vy", "length", "width")
53
+ _TABLE_NUM = ("min_ttc", "min_tadv", "min_distance", "t_centre")
54
+ _TABLE_INT = ("id_a", "id_b")
55
+ _TABLE_STR = ("level", "site", "period", "selection")
56
+ LEVELS = ("A", "B", "C")
57
+ _BATCH = 3000 # pairs per batch of encounters
58
+
59
+
60
+ @dataclass(frozen=True)
61
+ class ConflictBank:
62
+ """A set of conflict windows.
63
+
64
+ Attributes:
65
+ name: Name of the bank.
66
+ table: One entry per window, as arrays of equal length: ``level`` (``'A'`` collision
67
+ course at some moment, ``'B'`` crossing courses only, ``'C'`` no overlap of the future
68
+ paths), ``min_ttc`` (s, NaN if never on a collision course), ``min_tadv`` (s),
69
+ ``min_distance`` (m), ``t_centre`` (s, in the time of the source file), ``id_a``,
70
+ ``id_b`` (road-user ids of the source file), ``site``, ``period`` and ``selection``
71
+ (the rule that picked the window).
72
+ rows: Long table of all windows stacked (``uid`` is ``2 * k`` and ``2 * k + 1`` for window
73
+ ``k``; ``t`` runs from 0 at the start of the window).
74
+ row_start: ``(n_windows + 1,)`` row offsets of the windows in ``rows``.
75
+ info: Free text about the origin, licence and preprocessing.
76
+ """
77
+
78
+ name: str
79
+ table: dict[str, Any]
80
+ rows: Trajectories
81
+ row_start: IntArray
82
+ info: str
83
+
84
+ def __len__(self) -> int:
85
+ """Number of windows."""
86
+ return int(self.row_start.size - 1)
87
+
88
+ def window(self, k: int) -> Trajectories:
89
+ """Trajectories of window ``k`` (two road users, ``uid`` 0 and 1, ``t`` from 0)."""
90
+ if not 0 <= k < len(self):
91
+ raise IndexError(f"window {k} out of range 0..{len(self) - 1}")
92
+ s = slice(int(self.row_start[k]), int(self.row_start[k + 1]))
93
+ r = self.rows
94
+ return Trajectories(
95
+ t=r.t[s], uid=r.uid[s] - 2 * k, x=r.x[s], y=r.y[s], heading=r.heading[s], vx=r.vx[s],
96
+ vy=r.vy[s], length=r.length[s], width=r.width[s], kind=r.kind[s],
97
+ source=f"{self.name} window {k}", meta={"window": k},
98
+ ) # fmt: skip
99
+
100
+ def select(self, level: str) -> list[int]:
101
+ """Indices of the windows of one level (``'A'``, ``'B'`` or ``'C'``)."""
102
+ lv = np.asarray(self.table["level"])
103
+ return [int(i) for i in np.flatnonzero(lv == level)]
104
+
105
+
106
+ def _rank(values: FloatArray) -> IntArray:
107
+ order: IntArray = np.argsort(values, kind="stable")
108
+ return order
109
+
110
+
111
+ def extract_conflicts(
112
+ traj: Trajectories,
113
+ n_collision: int = 8,
114
+ n_crossing: int = 4,
115
+ n_none: int = 2,
116
+ half_window: float = 5.0,
117
+ horizon: float = 10.0,
118
+ max_distance: float = 30.0,
119
+ name: str = "local",
120
+ site: str = "",
121
+ period: str = "",
122
+ drop_overlap: bool = True,
123
+ ) -> ConflictBank:
124
+ """Pick the most severe encounters of ``traj`` and cut a window around each.
125
+
126
+ Encounters (:func:`pyssms.process.encounters`) are ranked in three groups:
127
+
128
+ * ``A``: on a collision course at some moment, by the smallest TTC reached (window centred
129
+ there);
130
+ * ``B``: never on a collision course but with a finite Time Advantage, by the smallest TAdv;
131
+ * ``C``: the future paths never overlap, by the smallest centre distance.
132
+
133
+ ``drop_overlap`` removes pairs whose bodies overlap at some moment (TTC = 0), which in raw
134
+ video trajectories almost always means a measurement error, not a collision. The windows are
135
+ shifted so that the centre of the two road users at the central moment is the origin
136
+ (indicators do not depend on a translation).
137
+
138
+ Args:
139
+ traj: Trajectories (several road users over time, SI units).
140
+ n_collision: Number of windows of group ``A``.
141
+ n_crossing: Number of windows of group ``B``.
142
+ n_none: Number of windows of group ``C``.
143
+ half_window: Seconds kept on each side of the central moment.
144
+ horizon: Prediction horizon of the indicators (s).
145
+ max_distance: Candidate-pair distance (m), see :func:`pyssms.process.candidate_pairs`.
146
+ name: Name given to the bank.
147
+ site: Text stored in the table (where the data came from).
148
+ period: Text stored in the table (which recording).
149
+ drop_overlap: Skip pairs with overlapping bodies.
150
+
151
+ Returns:
152
+ A :class:`ConflictBank`; empty groups simply contribute no windows.
153
+ """
154
+ pairs = process.candidate_pairs(traj, max_distance=max_distance, horizon=horizon)
155
+ # keep only a score per pair (batches bound the memory), then recompute the chosen pairs
156
+ scored: dict[str, list[tuple[float, int, int, float]]] = {"A": [], "B": [], "C": []}
157
+ for s0 in range(0, len(pairs), _BATCH):
158
+ for e in process.encounters(traj, horizon=horizon, pairs=pairs[s0 : s0 + _BATCH]):
159
+ finite = np.isfinite(e.ttc)
160
+ if drop_overlap and finite.any() and float(np.nanmin(e.ttc)) <= 0.0:
161
+ continue
162
+ if (e.state == "A").any():
163
+ k = int(np.nanargmin(np.where(e.state == "A", e.ttc, np.inf)))
164
+ scored["A"].append((float(e.ttc[k]), e.a, e.b, float(e.t[k])))
165
+ elif np.isfinite(e.tadv).any():
166
+ k = int(np.nanargmin(e.tadv))
167
+ scored["B"].append((float(e.tadv[k]), e.a, e.b, float(e.t[k])))
168
+ else:
169
+ k = int(np.argmin(e.distance))
170
+ scored["C"].append((float(e.distance[k]), e.a, e.b, float(e.t[k])))
171
+ chosen: list[tuple[str, str, int, int, float]] = []
172
+ for lv, n, rule in (
173
+ ("A", n_collision, "smallest TTC"),
174
+ ("B", n_crossing, "smallest Time Advantage"),
175
+ ("C", n_none, "smallest distance"),
176
+ ):
177
+ pool = scored[lv]
178
+ for j in _rank(np.array([p[0] for p in pool]))[:n]:
179
+ chosen.append((lv, rule, pool[j][1], pool[j][2], pool[j][3]))
180
+ enc = process.encounters(traj, horizon=horizon, pairs=[(c[2], c[3]) for c in chosen])
181
+ by_pair = {(e.a, e.b): e for e in enc}
182
+ picks: list[tuple[str, str, int, int]] = []
183
+ enc_sel: list[process.Encounter] = []
184
+ for lv, rule, a_id, b_id, tc in chosen:
185
+ e = by_pair[(a_id, b_id)]
186
+ picks.append((lv, rule, len(enc_sel), int(np.flatnonzero(e.t == tc)[0])))
187
+ enc_sel.append(e)
188
+ enc = enc_sel
189
+ return _cut(traj, enc, picks, half_window, name, site, period)
190
+
191
+
192
+ def _cut(
193
+ traj: Trajectories,
194
+ enc: list[process.Encounter],
195
+ picks: list[tuple[str, str, int, int]],
196
+ half_window: float,
197
+ name: str,
198
+ site: str,
199
+ period: str,
200
+ ) -> ConflictBank:
201
+ cols: dict[str, list[FloatArray]] = {c: [] for c in (*_COLS, "uid")}
202
+ kinds: list[Any] = []
203
+ starts = [0]
204
+ tab: dict[str, list[Any]] = {k: [] for k in (*_TABLE_NUM, *_TABLE_INT, *_TABLE_STR)}
205
+ for w, (lv, rule, i, k) in enumerate(picks):
206
+ e = enc[i]
207
+ tc = float(e.t[k])
208
+ sel = (e.t >= tc - half_window) & (e.t <= tc + half_window)
209
+ times = e.t[sel]
210
+ origin = np.zeros(2)
211
+ for who, uid in enumerate((e.a, e.b)):
212
+ tr = traj.track(uid)
213
+ m = np.isin(tr.t, times)
214
+ if who == 0:
215
+ c = np.flatnonzero(tr.t == tc)[0]
216
+ origin[0], origin[1] = tr.x[c], tr.y[c]
217
+ for c_name in _COLS:
218
+ arr = getattr(tr, c_name)[m]
219
+ if c_name == "x":
220
+ arr = arr - origin[0]
221
+ elif c_name == "y":
222
+ arr = arr - origin[1]
223
+ elif c_name == "t":
224
+ arr = arr - times[0]
225
+ cols[c_name].append(arr.astype(float))
226
+ cols["uid"].append(np.full(int(m.sum()), 2 * w + who, dtype=float))
227
+ kinds.append(tr.kind[m])
228
+ starts.append(starts[-1] + 2 * int(sel.sum()))
229
+ finite = np.isfinite(e.ttc)
230
+ tab["level"].append(lv)
231
+ tab["selection"].append(rule)
232
+ tab["min_ttc"].append(float(np.nanmin(e.ttc)) if finite.any() else float("nan"))
233
+ tab["min_tadv"].append(e.min_tadv())
234
+ tab["min_distance"].append(e.min_distance())
235
+ tab["t_centre"].append(tc)
236
+ tab["id_a"].append(e.a)
237
+ tab["id_b"].append(e.b)
238
+ tab["site"].append(site)
239
+ tab["period"].append(period)
240
+ if not picks:
241
+ empty = np.zeros(0)
242
+ rows = Trajectories(
243
+ t=empty, uid=empty.astype(np.int64), x=empty, y=empty, heading=empty, vx=empty,
244
+ vy=empty, length=empty, width=empty, kind=np.zeros(0, dtype=str),
245
+ ) # fmt: skip
246
+ else:
247
+ cat = {c: np.concatenate(v) for c, v in cols.items()}
248
+ rows = Trajectories(
249
+ t=cat["t"], uid=cat["uid"].astype(np.int64), x=cat["x"], y=cat["y"],
250
+ heading=cat["heading"], vx=cat["vx"], vy=cat["vy"], length=cat["length"],
251
+ width=cat["width"], kind=np.concatenate(kinds).astype(str),
252
+ ) # fmt: skip
253
+ table: dict[str, Any] = {key: np.array(tab[key]) for key in tab}
254
+ for key in _TABLE_STR:
255
+ table[key] = np.array(tab[key], dtype=str)
256
+ return ConflictBank(
257
+ name=name, table=table, rows=rows, row_start=np.array(starts, dtype=np.int64),
258
+ info=f"{name}: {len(picks)} windows of +-{half_window:g} s; extracted by "
259
+ "pyssms.bank.extract_conflicts",
260
+ ) # fmt: skip
261
+
262
+
263
+ def concat_banks(banks: list[ConflictBank], name: str, info: str = "") -> ConflictBank:
264
+ """Stack several banks into one (window numbers and ``uid`` are renumbered)."""
265
+ if not banks:
266
+ raise ValueError("no banks to join")
267
+ rows_cols: dict[str, list[Any]] = {c: [] for c in (*_COLS, "uid", "kind")}
268
+ starts = [0]
269
+ shift = 0
270
+ for b in banks:
271
+ for c in _COLS:
272
+ rows_cols[c].append(getattr(b.rows, c))
273
+ rows_cols["uid"].append(b.rows.uid + 2 * shift)
274
+ rows_cols["kind"].append(b.rows.kind)
275
+ starts.extend((b.row_start[1:] + starts[-1]).tolist())
276
+ shift += len(b)
277
+ r = {c: np.concatenate(v) for c, v in rows_cols.items()}
278
+ rows = Trajectories(
279
+ t=r["t"], uid=r["uid"], x=r["x"], y=r["y"], heading=r["heading"], vx=r["vx"],
280
+ vy=r["vy"], length=r["length"], width=r["width"], kind=r["kind"], source=name,
281
+ ) # fmt: skip
282
+ table = {k: np.concatenate([np.asarray(b.table[k]) for b in banks]) for k in banks[0].table}
283
+ return ConflictBank(
284
+ name=name, table=table, rows=rows, row_start=np.array(starts, dtype=np.int64),
285
+ info=info or f"{name}: {shift} windows",
286
+ ) # fmt: skip
287
+
288
+
289
+ def save_bank(bank: ConflictBank, path: str | Path, info: str | None = None) -> Path:
290
+ """Write a bank to a compressed ``.npz`` file (no pickles)."""
291
+ r = bank.rows
292
+ out = {f"row_{c}": getattr(r, c) for c in _COLS}
293
+ out["row_uid"] = r.uid
294
+ out["row_kind"] = r.kind
295
+ out["row_start"] = bank.row_start
296
+ out.update({f"tab_{k}": np.asarray(v) for k, v in bank.table.items()})
297
+ out["name"] = np.array(bank.name)
298
+ out["info"] = np.array(info if info is not None else bank.info)
299
+ p = Path(path)
300
+ p.parent.mkdir(parents=True, exist_ok=True)
301
+ np.savez_compressed(p, **out)
302
+ return p
303
+
304
+
305
+ def _read_bank(z: Any) -> ConflictBank:
306
+ rows = Trajectories(
307
+ t=z["row_t"], uid=z["row_uid"], x=z["row_x"], y=z["row_y"], heading=z["row_heading"],
308
+ vx=z["row_vx"], vy=z["row_vy"], length=z["row_length"], width=z["row_width"],
309
+ kind=z["row_kind"], source=str(z["name"]),
310
+ ) # fmt: skip
311
+ table = {k[4:]: z[k] for k in z.files if k.startswith("tab_")}
312
+ return ConflictBank(
313
+ name=str(z["name"]), table=table, rows=rows, row_start=z["row_start"], info=str(z["info"])
314
+ )
315
+
316
+
317
+ def _data_dir() -> Any:
318
+ return resources.files("pyssms").joinpath("data")
319
+
320
+
321
+ def available_banks() -> list[str]:
322
+ """Names of the banks shipped with the package."""
323
+ names = []
324
+ for f in _data_dir().iterdir():
325
+ n = f.name
326
+ if n.startswith("conflict_bank_") and n.endswith(".npz"):
327
+ names.append(n[len("conflict_bank_") : -len(".npz")])
328
+ return sorted(names)
329
+
330
+
331
+ def load_conflict_bank(name: str = "ngsim") -> ConflictBank:
332
+ """Load a bank shipped with the package (see the module docstring for what it is).
333
+
334
+ Args:
335
+ name: One of :func:`available_banks` (currently ``"ngsim"``).
336
+
337
+ Raises:
338
+ KeyError: if ``name`` is not available. Banks of the LevelX data sets do not exist because
339
+ those data may not be redistributed.
340
+ """
341
+ avail = available_banks()
342
+ if name not in avail:
343
+ raise KeyError(f"no bank {name!r}; available: {avail}")
344
+ f = _data_dir().joinpath(f"conflict_bank_{name}.npz")
345
+ with f.open("rb") as fh, np.load(fh, allow_pickle=False) as z:
346
+ return _read_bank(z)
347
+
348
+
349
+ def load_summary() -> dict[str, Any]:
350
+ """Aggregated counts and prediction-error scales of the five German drone data sets.
351
+
352
+ Read from ``pyssms/data/summary_levelx.json``: for each data set and location the number of
353
+ candidate pairs per hierarchy level and the RMS prediction-error scales of unhindered
354
+ vehicles. **Counts only; no trajectory, no per-road-user value.** The counts depend on the
355
+ candidate-pair rule (``docs/diamond_*.md``), and they describe the recordings that were
356
+ processed, not the roads in general.
357
+ """
358
+ f = _data_dir().joinpath("summary_levelx.json")
359
+ out: dict[str, Any] = json.loads(f.read_text(encoding="utf-8"))
360
+ return out
pyssms/calibration.py ADDED
@@ -0,0 +1,158 @@
1
+ """Scales of the constant-velocity prediction error, estimated from unhindered road users.
2
+
3
+ The disturbance set of :mod:`pyssms.disturbance` needs two scales: ``sigma_delta`` (s), the spread
4
+ of the *timing* error of a constant-velocity prediction, and ``sigma_eps`` (m), the spread of the
5
+ *sideways* error. :func:`calibrate_scales` estimates both from trajectories in which nobody
6
+ interacts with the road user (an *unhindered* set, :func:`unhindered_ids`), as a function of the
7
+ prediction horizon ``tau``:
8
+
9
+ * take each moving road user at time ``t`` with position ``p``, velocity ``v`` and heading
10
+ ``u = v/|v|``;
11
+ * predict ``p + v tau`` and compare with the realised position ``p(t + tau)``;
12
+ * split the error into the part along ``u`` (``e_L``) and across it (``e_T``);
13
+ * timing error ``delta = -e_L / |v|`` (s; positive when the road user is later than predicted),
14
+ sideways error ``eps = e_T`` (m);
15
+ * report the root-mean-square (``robust=False``) or the median-absolute-deviation scale
16
+ (``robust=True``, 1.4826 x MAD about the median) of each over all samples.
17
+
18
+ This is one concrete definition of the scales, not a validated one: whether a spread estimated this
19
+ way is the right standardiser for a margin is the open question of ``docs/beta_proposal.md``. The
20
+ result depends on who is in the unhindered set; with data of a signalised intersection, stopping at
21
+ a red light is mistaken for a timing error unless those road users are excluded.
22
+ """
23
+
24
+ from __future__ import annotations
25
+
26
+ from collections.abc import Sequence
27
+ from dataclasses import dataclass
28
+
29
+ import numpy as np
30
+
31
+ from pyssms.geometry import FloatArray, IntArray
32
+ from pyssms.trajectories import Trajectories
33
+
34
+ __all__ = ["Scales", "calibrate_scales", "unhindered_ids"]
35
+
36
+
37
+ @dataclass(frozen=True)
38
+ class Scales:
39
+ """Scales of the prediction error per horizon.
40
+
41
+ Attributes:
42
+ tau: Prediction horizons (s).
43
+ sigma_delta: Timing scale (s) per horizon.
44
+ sigma_eps: Sideways scale (m) per horizon.
45
+ n: Number of samples per horizon.
46
+ """
47
+
48
+ tau: FloatArray
49
+ sigma_delta: FloatArray
50
+ sigma_eps: FloatArray
51
+ n: IntArray
52
+
53
+ def at(self, tau: float) -> tuple[float, float]:
54
+ """``(sigma_delta, sigma_eps)`` linearly interpolated at ``tau`` (clamped to the range)."""
55
+ ok = np.isfinite(self.sigma_delta) & np.isfinite(self.sigma_eps)
56
+ if not ok.any():
57
+ return float("nan"), float("nan")
58
+ return (
59
+ float(np.interp(tau, self.tau[ok], self.sigma_delta[ok])),
60
+ float(np.interp(tau, self.tau[ok], self.sigma_eps[ok])),
61
+ )
62
+
63
+
64
+ def unhindered_ids(
65
+ traj: Trajectories, min_gap: float = 15.0, min_duration: float = 2.0, min_speed: float = 1.0
66
+ ) -> IntArray:
67
+ """Ids of road users that never come within ``min_gap`` m (centres) of another one.
68
+
69
+ Only road users observed for at least ``min_duration`` seconds and moving faster than
70
+ ``min_speed`` m/s at least half of that time are returned. The rule is a proxy for "not
71
+ interacting"; it also removes dense traffic, so the set can be biased toward free flow.
72
+ """
73
+ ts = traj.times
74
+ key = np.searchsorted(ts, traj.t)
75
+ order = np.argsort(key, kind="stable")
76
+ bounds = np.searchsorted(key[order], np.arange(ts.size + 1))
77
+ near: set[int] = set()
78
+ for k in range(ts.size):
79
+ rows = order[bounds[k] : bounds[k + 1]]
80
+ if rows.size < 2:
81
+ continue
82
+ x, y = traj.x[rows], traj.y[rows]
83
+ d = np.hypot(x[:, None] - x[None, :], y[:, None] - y[None, :])
84
+ np.fill_diagonal(d, np.inf)
85
+ near.update(int(u) for u in traj.uid[rows[(d < min_gap).any(axis=1)]])
86
+ keep = []
87
+ for u in traj.ids:
88
+ if int(u) in near:
89
+ continue
90
+ tr = traj.track(int(u))
91
+ if len(tr) < 2 or tr.t[-1] - tr.t[0] < min_duration:
92
+ continue
93
+ if np.mean(np.hypot(tr.vx, tr.vy) > min_speed) < 0.5:
94
+ continue
95
+ keep.append(int(u))
96
+ return np.array(keep, dtype=np.int64)
97
+
98
+
99
+ def _spread(v: FloatArray, robust: bool) -> float:
100
+ if v.size < 2:
101
+ return float("nan")
102
+ if robust:
103
+ return float(1.4826 * np.median(np.abs(v - np.median(v))))
104
+ return float(np.sqrt(np.mean(v**2)))
105
+
106
+
107
+ def calibrate_scales(
108
+ traj: Trajectories,
109
+ ids: IntArray | Sequence[int] | None = None,
110
+ taus: Sequence[float] = (0.5, 1.0, 2.0, 3.0, 4.0),
111
+ min_speed: float = 1.0,
112
+ robust: bool = False,
113
+ ) -> Scales:
114
+ """Estimate ``sigma_delta(tau)`` and ``sigma_eps(tau)`` from (unhindered) trajectories.
115
+
116
+ Args:
117
+ traj: Trajectories sampled at a regular interval.
118
+ ids: Road users to use; default all (pass :func:`unhindered_ids` for free-flow scales).
119
+ taus: Prediction horizons (s); each is rounded to a whole number of frames.
120
+ min_speed: Samples with a lower speed are skipped (heading and timing are undefined).
121
+ robust: Use the MAD-based scale instead of the root-mean-square.
122
+
123
+ Returns:
124
+ :class:`Scales` (``NaN`` where fewer than two samples exist).
125
+ """
126
+ dt = traj.dt
127
+ use = traj.ids if ids is None else np.asarray(ids, dtype=np.int64)
128
+ e_long: dict[float, list[FloatArray]] = {tau: [] for tau in taus}
129
+ e_lat: dict[float, list[FloatArray]] = {tau: [] for tau in taus}
130
+ for u in use:
131
+ tr = traj.track(int(u))
132
+ sp = np.hypot(tr.vx, tr.vy)
133
+ for tau in taus:
134
+ k = int(round(tau / dt))
135
+ if k < 1 or len(tr) <= k:
136
+ continue
137
+ ex = tr.x[k:] - (tr.x[:-k] + tr.vx[:-k] * (k * dt))
138
+ ey = tr.y[k:] - (tr.y[:-k] + tr.vy[:-k] * (k * dt))
139
+ s = sp[:-k]
140
+ ok = s >= min_speed
141
+ ux, uy = tr.vx[:-k][ok] / s[ok], tr.vy[:-k][ok] / s[ok]
142
+ long = ex[ok] * ux + ey[ok] * uy
143
+ lat = -ex[ok] * uy + ey[ok] * ux
144
+ e_long[tau].append(-long / s[ok]) # timing error: behind prediction = later
145
+ e_lat[tau].append(lat)
146
+ sd, se, n = [], [], []
147
+ for tau in taus:
148
+ d = np.concatenate(e_long[tau]) if e_long[tau] else np.empty(0)
149
+ e = np.concatenate(e_lat[tau]) if e_lat[tau] else np.empty(0)
150
+ sd.append(_spread(d, robust))
151
+ se.append(_spread(e, robust))
152
+ n.append(int(d.size))
153
+ return Scales(
154
+ tau=np.array(taus, dtype=float),
155
+ sigma_delta=np.array(sd),
156
+ sigma_eps=np.array(se),
157
+ n=np.array(n, dtype=np.int64),
158
+ )