File size: 17,947 Bytes
dffbc1f
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
"""T5 samples: anchors, 3-class grasp-state labels and contact-event strata.

Definitions (camera-ready, v1.1 labels)
---------------------------------------
All streams are 100 Hz on disk and are decimated by ``downsample`` (5 -> 20 Hz)
by taking every 5th frame from frame 0.

* **Grasp-verb mask** (100 Hz): a frame is "grasp" if it lies inside any v1.1
  segment whose ``action_name`` is in ``grasp_verbs`` (grasp, hold, pick_up,
  move, place, put_down, pull, rotate, insert, remove).  Segment ``[start,
  end]`` covers frames ``round(100 start) .. round(100 end) - 1``, clipped to
  the loaded frames.  Every verb must belong to the frozen v1.1 label space
  (``taxonomy_v1.1.json``, including the dropped ``support``); anything else
  raises.  Nouns are not used.
* **Contact** (20 Hz): at least ``contact_min_cells`` (3) of the 50 pressure
  cells (both hands) read more than ``contact_cell_threshold_g`` (10 g).
* **Anchors** every ``anchor_stride_sec`` (0.25 s = 5 frames) at 20 Hz frame
  ``a`` with ``T_obs <= a <= T - T_fut``.  The input is the past window
  ``[a - T_obs, a)`` (1 s = 20 frames); labels use only the future window
  ``[a, a + T_fut)`` (0.5 s = 10 frames).
* **Label**: NoGrasp (0) unless at least ``majority_threshold`` (50 %) of the
  future window is grasp-verb; then SustainedGrasp (2) if the longest
  contiguous contact run in the future window is at least
  ``sustained_min_sec`` (0.3 s = 6 frames), else AttemptedGrasp (1).
* **Event stratum** (Table 10): past / future window "in contact" if more than
  50 % of its frames are contact frames: non-contact (no/no), pre-contact
  (no/yes), steady-grip (yes/yes), release (yes/no).  SustainedGrasp anchors
  can only occur in pre-contact and steady-grip.

Sample set
----------
A configuration names its model ``inputs`` and its ``sample_streams`` (a
superset that always contains ``pressure``, which defines the labels).  All
sample streams are loaded and cut to a common length, so rows that differ only
in their inputs (EMG vs EMG + pressure vs pressure only) see exactly the same
anchors and labels.

Release re-trim (v2s6, v13s2, v13s3)
------------------------------------
In v1.1 the EMG, eye-tracking and pressure files of these three recordings
were re-trimmed to the MoCap capture window ``[front, front + n)`` recorded in
``alignment_metadata.json`` (``mocap_padding.mocap_sq``), while IMU and MoCap
keep the full alignment timeline, and the annotations are on the full
timeline.  When a loaded stream has exactly ``n`` rows, the full-timeline
streams are sliced to ``[front, front + n)`` and the annotation times are
shifted by ``front`` frames, so frame ``i`` of every array and of the label
timeline is the same instant (recorded per recording as ``timeline_offset``).

Normalisation
-------------
Per-channel z-score with the mean / SD (population) of the training frames,
each frame weighted by the number of kept training anchors whose observation
window contains it (the statistics of the training windows, as in the original
pipeline).  Inner-validation and test data use the training statistics.
"""
from __future__ import annotations

import hashlib
from dataclasses import dataclass, field
from typing import Any, Dict, List, Optional, Sequence, Tuple

import numpy as np

from ..data.annotations import load_segments
from ..data.io import load_alignment_metadata, load_modality
from ..errors import DataQualityError
from ..paths import DataRoot
from ..taxonomy import Taxonomy, TaxonomyError

SR_HZ = 100
CLASS_KEYS: Tuple[str, ...] = ("nograsp", "attempted", "sustained")
CLASS_NAMES: Tuple[str, ...] = ("NoGrasp", "AttemptedGrasp", "SustainedGrasp")
EVENT_NAMES: Tuple[str, ...] = ("non-contact", "pre-contact", "steady-grip", "release")
#: Events in which SustainedGrasp anchors can exist (future window in contact).
SUSTAINED_EVENTS: Tuple[str, ...] = ("pre-contact", "steady-grip")
DEFAULT_GRASP_VERBS: Tuple[str, ...] = ("grasp", "hold", "pick_up", "move", "place", "put_down",
                                        "pull", "rotate", "insert", "remove")


@dataclass(frozen=True)
class LabelSpec:
    downsample: int = 5
    t_obs_sec: float = 1.0
    t_fut_sec: float = 0.5
    anchor_stride_sec: float = 0.25
    majority_threshold: float = 0.5
    grasp_verbs: Tuple[str, ...] = DEFAULT_GRASP_VERBS
    contact_cell_threshold_g: float = 10.0
    contact_min_cells: int = 3
    sustained_min_sec: float = 0.3

    @classmethod
    def from_config(cls, cfg: Dict[str, Any]) -> "LabelSpec":
        d = dict(cfg or {})
        if "grasp_verbs" in d:
            d["grasp_verbs"] = tuple(d["grasp_verbs"])
        unknown = set(d) - set(cls.__dataclass_fields__)
        if unknown:
            raise ValueError(f"unknown T5 label settings: {sorted(unknown)}")
        return cls(**d)

    @property
    def sr(self) -> int:
        if SR_HZ % self.downsample:
            raise ValueError("downsample must divide 100")
        return SR_HZ // self.downsample

    @property
    def t_obs(self) -> int:
        return int(round(self.t_obs_sec * self.sr))

    @property
    def t_fut(self) -> int:
        return int(round(self.t_fut_sec * self.sr))

    @property
    def stride(self) -> int:
        return max(1, int(round(self.anchor_stride_sec * self.sr)))

    @property
    def sustained_frames(self) -> int:
        return int(round(self.sustained_min_sec * self.sr))

    def check_against(self, tax: Taxonomy) -> None:
        bad = [v for v in self.grasp_verbs if v not in tax.verbs]
        if bad:
            raise TaxonomyError(f"grasp verbs {bad} are not v1.1 verbs")


# ---------------------------------------------------------------------------
# label derivation (pure functions, unit-tested)
# ---------------------------------------------------------------------------

def grasp_mask(segments, n_frames: int, grasp_verbs: Sequence[str], offset_frames: int = 0,
               tax: Optional[Taxonomy] = None) -> Tuple[np.ndarray, Dict[str, int]]:
    """100 Hz bool mask of grasp-verb frames for frames ``0 .. n_frames - 1``.

    ``offset_frames``: timeline frame of loaded frame 0 (release re-trim).
    """
    gv = set(grasp_verbs)
    mask = np.zeros(int(n_frames), dtype=bool)
    st = {"segments": 0, "grasp_segments": 0, "grasp_clipped": 0, "grasp_outside": 0}
    for s in segments:
        st["segments"] += 1
        if tax is not None and s.verb not in tax.verbs and s.verb not in tax.dropped_verbs:
            raise TaxonomyError(f"{s.rid} segment {s.index}: verb {s.verb!r} is not a v1.1 verb")
        if s.verb not in gv:
            continue
        st["grasp_segments"] += 1
        i0 = int(round(s.start_sec * SR_HZ)) - offset_frames
        i1 = int(round(s.end_sec * SR_HZ)) - offset_frames
        j0, j1 = max(0, i0), min(int(n_frames), i1)
        if j1 <= j0:
            st["grasp_outside"] += 1
            continue
        if (j0, j1) != (i0, i1):
            st["grasp_clipped"] += 1
        mask[j0:j1] = True
    return mask, st


def contact_frames(pressure_ds: np.ndarray, cell_threshold_g: float, min_cells: int) -> np.ndarray:
    return (np.asarray(pressure_ds) > cell_threshold_g).sum(axis=1) >= min_cells


def longest_true_run(windows: np.ndarray) -> np.ndarray:
    """Longest run of True per row of a ``(N, T)`` bool array."""
    w = np.asarray(windows, dtype=bool)
    cur = np.zeros(w.shape[0], dtype=np.int64)
    best = np.zeros(w.shape[0], dtype=np.int64)
    for t in range(w.shape[1]):
        cur = np.where(w[:, t], cur + 1, 0)
        best = np.maximum(best, cur)
    return best


def derive_anchors(mask_ds: np.ndarray, contact_ds: np.ndarray, spec: LabelSpec
                   ) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
    """Return ``(anchors, labels, events)`` for one recording at the decimated rate."""
    mask_ds = np.asarray(mask_ds, dtype=bool)
    contact_ds = np.asarray(contact_ds, dtype=bool)
    if mask_ds.shape != contact_ds.shape:
        raise ValueError("mask and contact must have the same length")
    T = mask_ds.shape[0]
    to, tf = spec.t_obs, spec.t_fut
    if T - tf < to:
        e = np.zeros(0, dtype=np.int64)
        return e, e.copy(), e.copy()
    anchors = np.arange(to, T - tf + 1, spec.stride, dtype=np.int64)
    fut = anchors[:, None] + np.arange(tf)[None, :]
    past = anchors[:, None] - to + np.arange(to)[None, :]
    is_grasp = mask_ds[fut].mean(axis=1) >= spec.majority_threshold
    sustained = longest_true_run(contact_ds[fut]) >= spec.sustained_frames
    labels = np.where(is_grasp, np.where(sustained, 2, 1), 0).astype(np.int64)
    past_high = contact_ds[past].mean(axis=1) > 0.5
    fut_high = contact_ds[fut].mean(axis=1) > 0.5
    events = np.select([~past_high & ~fut_high, ~past_high & fut_high, past_high & fut_high],
                       [0, 1, 2], default=3).astype(np.int64)
    return anchors, labels, events


# ---------------------------------------------------------------------------
# release re-trim (v2s6, v13s2, v13s3)
# ---------------------------------------------------------------------------

def release_trim_window(meta: dict) -> Optional[Tuple[int, int]]:
    """``(front, n)`` from ``mocap_padding`` if it covers part of the timeline, else None."""
    n_total = meta.get("n_samples")
    pads = meta.get("mocap_padding") or {}
    pad = pads.get("mocap_sq") or pads.get("mocap") or {}
    front, n = pad.get("front_samples"), pad.get("n_frames")
    if not n_total or front is None or n is None:
        return None
    front, n, n_total = int(front), int(n), int(n_total)
    if n <= 0 or n >= n_total or front + n > n_total:
        return None
    return front, n


def align_release_trim(rid: str, arrays: Dict[str, np.ndarray], meta: dict
                       ) -> Tuple[Dict[str, np.ndarray], int]:
    """Put every stream on the same instants; return (arrays, timeline offset)."""
    win = release_trim_window(meta)
    if win is None:
        return arrays, 0
    front, n = win
    trimmed = [m for m, a in arrays.items() if a.shape[0] == n]
    if not trimmed:
        return arrays, 0
    out = dict(arrays)
    for m, a in arrays.items():
        if a.shape[0] == n:
            continue
        if a.shape[0] < front + n:
            raise DataQualityError(
                f"{rid}: {m} has {a.shape[0]} rows; cannot align it with the re-trimmed "
                f"streams {trimmed} (window {front}+{n})", "release_trim_mismatch")
        out[m] = a[front:front + n]
    return out, front


# ---------------------------------------------------------------------------
# recordings and partitions
# ---------------------------------------------------------------------------

@dataclass
class RecordingSamples:
    rid: str
    arrays: Dict[str, np.ndarray]     # model inputs, decimated, (T_ds, F) float32, raw units
    anchors: np.ndarray               # decimated frame index of each anchor
    labels: np.ndarray
    events: np.ndarray
    n_frames: int                     # common length at 100 Hz
    timeline_offset: int              # 100 Hz frames (release re-trim), 0 otherwise
    label_stats: Dict[str, int]
    annotation_md5: str


def load_recording_samples(dr: DataRoot, rid: str, inputs: Sequence[str],
                           sample_streams: Sequence[str], spec: LabelSpec,
                           tax: Optional[Taxonomy] = None) -> RecordingSamples:
    if "pressure" not in sample_streams:
        raise ValueError("T5 sample_streams must contain 'pressure' (it defines the labels)")
    missing = [m for m in inputs if m not in sample_streams]
    if missing:
        raise ValueError(f"inputs {missing} are not in sample_streams")
    raw = {m: load_modality(dr, rid, m).data for m in sample_streams}   # raises if absent
    raw, offset = align_release_trim(rid, raw, load_alignment_metadata(dr, rid))
    n = min(a.shape[0] for a in raw.values())
    ann_path = dr.annotation_path(rid, must_exist=True)
    segs = load_segments(dr, rid)
    mask, st = grasp_mask(segs, n, spec.grasp_verbs, offset_frames=offset, tax=tax)
    ds = spec.downsample
    mask_ds = mask[:n:ds]
    contact = contact_frames(raw["pressure"][:n:ds], spec.contact_cell_threshold_g,
                             spec.contact_min_cells)
    anchors, labels, events = derive_anchors(mask_ds, contact, spec)
    arrays = {m: np.ascontiguousarray(raw[m][:n:ds], dtype=np.float32) for m in inputs}
    st = dict(st, n_anchors=int(anchors.size))
    return RecordingSamples(rid, arrays, anchors, labels, events, int(n), int(offset), st,
                            hashlib.md5(ann_path.read_bytes()).hexdigest())


@dataclass
class Partition:
    """All anchors of one partition, windows gathered from concatenated arrays."""

    name: str
    rids: List[str]
    inputs: List[str]
    data: Dict[str, np.ndarray]       # modality -> (sum T_ds, F) float32
    rec_index: np.ndarray             # (N,) recording index of each anchor
    start: np.ndarray                 # (N,) global row of the first frame of the window
    labels: np.ndarray
    events: np.ndarray
    t_obs: int
    recordings: Dict[str, Dict[str, Any]] = field(default_factory=dict)

    @property
    def n(self) -> int:
        return int(self.labels.size)

    def subset(self, idx: np.ndarray) -> "Partition":
        return Partition(self.name, self.rids, self.inputs, self.data, self.rec_index[idx],
                         self.start[idx], self.labels[idx], self.events[idx], self.t_obs,
                         self.recordings)

    def class_counts(self, n_classes: int = 3) -> List[int]:
        return np.bincount(self.labels, minlength=n_classes).astype(int).tolist()

    def event_counts(self) -> Dict[str, int]:
        c = np.bincount(self.events, minlength=len(EVENT_NAMES))
        return {e: int(c[i]) for i, e in enumerate(EVENT_NAMES)}

    def window_index(self, idx: np.ndarray) -> np.ndarray:
        return self.start[idx][:, None] + np.arange(self.t_obs)[None, :]


def build_partition(name: str, recs: Sequence[RecordingSamples], inputs: Sequence[str],
                    spec: LabelSpec) -> Partition:
    data: Dict[str, List[np.ndarray]] = {m: [] for m in inputs}
    rec_index, start, labels, events = [], [], [], []
    info: Dict[str, Dict[str, Any]] = {}
    offset = 0
    dims: Dict[str, int] = {}
    for i, r in enumerate(recs):
        T = next(iter(r.arrays.values())).shape[0]
        for m in inputs:
            a = r.arrays[m]
            if a.shape[0] != T:
                raise ValueError(f"{r.rid}: {m} has {a.shape[0]} frames, expected {T}")
            if dims.setdefault(m, a.shape[1]) != a.shape[1]:
                raise ValueError(f"{r.rid}: {m} has {a.shape[1]} features, others {dims[m]}")
            data[m].append(a)
        rec_index.append(np.full(r.anchors.size, i, dtype=np.int64))
        start.append(offset + r.anchors - spec.t_obs)
        labels.append(r.labels)
        events.append(r.events)
        info[r.rid] = {"n_frames_100hz": r.n_frames, "n_frames_ds": int(T),
                       "timeline_offset": r.timeline_offset,
                       "class_counts": np.bincount(r.labels, minlength=3).astype(int).tolist(),
                       **r.label_stats}
        offset += T
    cat = lambda xs, dt=np.int64: np.concatenate(xs).astype(dt) if xs else np.zeros(0, dt)  # noqa: E731
    return Partition(name, [r.rid for r in recs], list(inputs),
                     {m: np.concatenate(v, axis=0) if v else np.zeros((0, 0), np.float32)
                      for m, v in data.items()},
                     cat(rec_index), cat(start), cat(labels), cat(events), spec.t_obs, info)


def cap_per_class(p: Partition, per_class_max: Optional[int], seed: int,
                  n_classes: int = 3) -> Tuple[Partition, Dict[str, Any]]:
    """Keep at most ``per_class_max`` anchors per class (uniform without replacement)."""
    before = p.class_counts(n_classes)
    if not per_class_max:
        return p, {"per_class_max": None, "before": before, "after": before}
    rng = np.random.default_rng([int(seed), 5])
    keep = []
    for c in range(n_classes):
        idx = np.flatnonzero(p.labels == c)
        if idx.size > per_class_max:
            idx = np.sort(rng.choice(idx, size=per_class_max, replace=False))
        keep.append(idx)
    sel = np.sort(np.concatenate(keep))
    q = p.subset(sel)
    return q, {"per_class_max": int(per_class_max), "before": before, "after": q.class_counts(n_classes)}


def window_stats(p: Partition) -> Dict[str, Tuple[np.ndarray, np.ndarray]]:
    """Mean / SD of the frames of ``p``'s observation windows (with multiplicity)."""
    total = next(iter(p.data.values())).shape[0]
    cover = np.zeros(total + 1, dtype=np.int64)
    np.add.at(cover, p.start, 1)
    np.add.at(cover, p.start + p.t_obs, -1)
    w = np.cumsum(cover)[:total].astype(np.float64)
    W = w.sum()
    if W <= 0:
        raise ValueError(f"{p.name}: no windows to compute statistics from")
    chunk = 1 << 16
    out = {}
    for m, x in p.data.items():
        s1 = np.zeros(x.shape[1], np.float64)
        for i in range(0, total, chunk):
            s1 += w[i:i + chunk] @ x[i:i + chunk].astype(np.float64)
        mu = s1 / W
        s2 = np.zeros(x.shape[1], np.float64)
        for i in range(0, total, chunk):
            s2 += w[i:i + chunk] @ (x[i:i + chunk].astype(np.float64) - mu) ** 2
        sd = np.sqrt(s2 / W)
        sd = np.where(sd < 1e-6, 1.0, sd)
        out[m] = (mu.astype(np.float32), sd.astype(np.float32))
    return out


def normalise(p: Partition, stats: Dict[str, Tuple[np.ndarray, np.ndarray]]) -> None:
    for m in p.data:
        mu, sd = stats[m]
        p.data[m] = np.nan_to_num((p.data[m] - mu) / sd, nan=0.0, posinf=0.0,
                                  neginf=0.0).astype(np.float32)


def class_weights(counts: Sequence[int]) -> np.ndarray:
    ny = np.asarray(counts, dtype=np.float64)
    return ny.sum() / (len(ny) * np.maximum(ny, 1))