diff --git a/CHANGELOG.md b/CHANGELOG.md index 8ff27cf4..9c599208 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -7,6 +7,29 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ## [Unreleased] +### Added +- `fuse_pose_views()` fuses MediaPipe *world* landmarks from two or more uncalibrated camera + views into one consensus skeleton: a single Umeyama rotation-and-scale alignment per view, + estimated from the rigid torso landmarks and averaged over the take, then a + visibility-weighted average per landmark, with the cross-view residual in millimetres as a + quality measure. It takes `extract_pose_landmarks` result dicts or plain `(world, visibility)` + arrays, so it is not tied to MediaPipe as a source. Closes #355. + + This consolidates `concert_fuse3d.py` and `reh_fuse3d.py` from the Westney-comparisons study, + which held byte-identical copies of the algorithm and differed by one line, a hardcoded list + of pieces. It reproduces that study's published fusion on all four concert excerpts to within + float32 storage precision, rotations agreeing to 1e-9 and the residuals unchanged. + + Two guards the study code did not have. Its rotation average projected the mean matrix back + with a bare `U @ Vt` and no determinant check, so a mean falling nearer a reflection than a + rotation would have mirrored the skeleton silently; that is now rejected. And its gap filling + said "fill short gaps" in a comment while filling every gap of any length and holding the ends + flat, so a long dropout became a straight line through it with nothing marking it; `max_gap` + now bounds this. The default is still fill-everything, because that is what the existing + results were computed with. + + The residual measures *consistency*, not accuracy: views that agree can agree on a wrong pose. + ### Removed - `motion_mp()` is retired. It rendered a motion video across several processes, coordinating them with a socket server and an argparse child process, and it raised on its first call: two fields diff --git a/musicalgestures/__init__.py b/musicalgestures/__init__.py index 636d0ca4..3eb16fc8 100644 --- a/musicalgestures/__init__.py +++ b/musicalgestures/__init__.py @@ -159,4 +159,5 @@ def __init__(self): midpoint, limb_speed_from_landmarks, impact_events, + fuse_pose_views, ) diff --git a/musicalgestures/_posetools.py b/musicalgestures/_posetools.py index 67a2bec0..ac98939e 100644 --- a/musicalgestures/_posetools.py +++ b/musicalgestures/_posetools.py @@ -29,12 +29,15 @@ from __future__ import annotations +from collections.abc import Mapping, Sequence + import os import subprocess import threading import warnings import numpy as np +from scipy import signal from musicalgestures._utils import get_widthheight, get_fps @@ -626,3 +629,292 @@ def _pick_relative_peaks(signal: np.ndarray, rel_thresh: float, min_dist: int) - if all(abs(int(i) - j) >= min_dist for j in kept): kept.append(int(i)) return np.array(sorted(kept), dtype=np.intp) + + +def _unpack_views(views) -> tuple[list, list[np.ndarray], list[np.ndarray]]: + """Normalise the accepted view forms into names, world arrays, visibilities. + + A view is either an :func:`extract_pose_landmarks` result dict (its + ``world`` array, with visibility read from ``landmarks[..., 2]``) or a + plain ``(world, visibility)`` pair. + """ + if isinstance(views, dict): + items = list(views.items()) + else: + items = list(enumerate(views)) + if len(items) < 2: + raise ValueError( + "fuse_pose_views needs at least two views; got " + f"{len(items)}. Fusing one view has nothing to fuse it with.") + + names: list = [] + worlds: list[np.ndarray] = [] + vis: list[np.ndarray] = [] + for name, view in items: + names.append(name) + if isinstance(view, dict): + world = view.get("world") + if world is None: + raise ValueError( + f"view {name!r} has no 'world' array. Call " + "extract_pose_landmarks(..., world_landmarks=True): the " + "fusion is defined on metric 3D landmarks, not pixels.") + world = np.asarray(world, dtype=float) + lm = view.get("landmarks") + v = (np.asarray(lm, dtype=float)[..., 2] if lm is not None + else np.ones(world.shape[:2])) + else: + world, v = view + world = np.asarray(world, dtype=float) + v = np.asarray(v, dtype=float) + if world.ndim != 3 or world.shape[-1] != 3: + raise ValueError( + f"view {name!r} world array has shape {world.shape}; " + "expected (frames, landmarks, 3).") + worlds.append(world) + vis.append(v) + + widths = {w.shape[1] for w in worlds} + if len(widths) != 1: + raise ValueError( + f"views disagree on the number of landmarks: {sorted(widths)}. " + "All views must come from the same landmark topology.") + return names, worlds, vis + + +def _interp_nan(a: np.ndarray, max_gap: int | None = None) -> np.ndarray: + """Fill NaN gaps per coordinate by linear interpolation. + + With ``max_gap=None`` every gap is filled, of any length, and the ends are + held flat -- which is what the study scripts this came from did. Pass an + integer to leave longer dropouts as NaN, so a repair cannot pass for a + measurement. + """ + frames = a.shape[0] + idx = np.arange(frames) + for j in range(a.shape[1]): + for c in range(3): + x = a[:, j, c] + m = np.isfinite(x) + if m.sum() < 2: + continue + filled = np.interp(idx, np.flatnonzero(m), x[m]) + if max_gap is not None: + filled = np.where(_gap_lengths(m) > max_gap, np.nan, filled) + a[:, j, c] = filled + return a + + +def _gap_lengths(mask: np.ndarray) -> np.ndarray: + """Length of the NaN run each sample belongs to; 0 where the sample is finite.""" + out = np.zeros(mask.shape, dtype=int) + start = None + for i, ok in enumerate(mask): + if not ok and start is None: + start = i + elif ok and start is not None: + out[start:i] = i - start + start = None + if start is not None: + out[start:] = len(mask) - start + return out + + +def _umeyama(src: np.ndarray, dst: np.ndarray) -> tuple[np.ndarray, float]: + """Similarity transform (rotation, scale) taking ``src`` onto ``dst``. + + Both arrays are (N, 3). Translation is deliberately not returned: the + caller re-centres on the torso centroid instead, which is more stable + than a fitted offset when a view drops landmarks. + """ + mu_s = src.mean(0) + mu_d = dst.mean(0) + S = src - mu_s + D = dst - mu_d + C = D.T @ S / len(src) + U, d, Vt = np.linalg.svd(C) + # Kabsch sign correction: without it a degenerate fit can return a + # reflection, which mirrors the skeleton rather than rotating it. + flip = np.array([1.0, 1.0, np.sign(np.linalg.det(U @ Vt))]) + R = U @ np.diag(flip) @ Vt + var = (S ** 2).sum() / len(src) + scale = float((d * flip).sum() / (var + 1e-12)) + return R, scale + + +def _mean_rotation(rotations: list[np.ndarray]) -> np.ndarray: + """Project the arithmetic mean of rotation matrices back onto SO(3).""" + U, _, Vt = np.linalg.svd(np.mean(rotations, axis=0)) + R = U @ Vt + if np.linalg.det(R) < 0: + # The mean fell closer to a reflection than to a rotation. Flipping + # the least-significant singular direction is the nearest true + # rotation to it; without this the fused skeleton comes out mirrored. + U[:, -1] *= -1 + R = U @ Vt + return R + + +def fuse_pose_views( + views: Sequence | Mapping, + reference: int | str = 0, + torso: Sequence[int] = (11, 12, 23, 24), + smooth: tuple[int, int] | None = (7, 2), + max_gap: int | None = None) -> dict: + """ + Fuse MediaPipe *world* landmarks from two or more uncalibrated camera views. + + This is **not** calibrated triangulation: there is no camera calibration and + no motion-capture ground truth. Each view gives a monocular metric 3D pose + in its own gravity-aligned, hip-centred frame. The views are brought into a + common frame by a single Umeyama (rotation + scale) similarity estimated + from the rigid torso landmarks, then fused per landmark by a + visibility-weighted average. The result is a consensus skeleton more robust + than any single monocular view, plus a cross-view residual in millimetres as + a quality measure. + + One transform is estimated per view for the whole take, not one per frame: + the per-frame fits are averaged (rotation through the nearest rotation to + their arithmetic mean, scale through the median), which is what makes the + alignment a property of the camera placement rather than of the pose. The + translation term of each fit is deliberately discarded -- views are + re-centred on the torso centroid instead, which stays stable when a view + drops landmarks. + + The residual is a *consistency* measure, not an accuracy one. Views that + agree closely can still agree on a wrong pose, so a low residual says the + cameras saw the same thing, not that the thing was right. + + Args: + views: Two or more views of the same take, either a sequence or a + mapping of name -> view. Each view is one of: + + - a result dict from :func:`extract_pose_landmarks` called with + ``world_landmarks=True``, whose visibility is read from its + ``landmarks[..., 2]`` column; + - a plain ``(world, visibility)`` pair of arrays, shaped + ``(F, L, 3)`` and ``(F, L)``, from any other source. + + Views may differ in length; the shortest one sets the number of + fused frames. They must agree on the number of landmarks. + reference (int or str, optional): Which view defines the common frame, + by key when ``views`` is a mapping and by position otherwise. The + fused skeleton comes out in this view's frame and at its scale. + Defaults to 0 (the first view). + torso (sequence of int, optional): Landmark indices used to estimate + the alignment. Defaults to (11, 12, 23, 24) -- MediaPipe's + shoulders and hips, the most rigid and best-detected group. + smooth (tuple, optional): ``(window, polyorder)`` for a Savitzky-Golay + filter applied along time after fusion, or None for no smoothing. + Skipped when the take is shorter than the window. Note that + smoothing spreads any NaN across its window. Defaults to (7, 2). + max_gap (int, optional): Longest run of missing frames that may be + filled by interpolation before alignment. Longer dropouts are left + as NaN, so a repair cannot pass for a measurement. Defaults to None, + which fills every gap of any length and holds the ends flat -- the + behaviour of the study scripts this is consolidated from, kept as + the default so their results reproduce. + + Returns: + dict: A dictionary with keys: + + - ``fused`` (np.ndarray, shape (F, L, 3)): The consensus skeleton in + the reference view's frame, in metres. + - ``residual_mm`` (float): Mean distance between each aligned view + and the fused skeleton, in millimetres. + - ``residual_per_landmark_mm`` (np.ndarray, shape (L,)): The same + measure per landmark, which is where a badly-placed camera shows. + - ``rotations`` (dict): Per view, the (3, 3) rotation onto the + reference frame. The reference view's own is the identity. + - ``scales`` (dict): Per view, the scalar scale onto the reference + frame. + - ``names`` (list): The view names, in the order given. + - ``n_frames`` (int): Number of fused frames, i.e. the length of + the shortest view. + + Raises: + ValueError: If fewer than two views are given, if a view dict has no + ``world`` array, if a world array is not shaped (F, L, 3), or if + the views disagree on the number of landmarks. + + Examples: + >>> side = mg.extract_pose_landmarks("side.mp4", world_landmarks=True) + >>> above = mg.extract_pose_landmarks("above.mp4", world_landmarks=True) + >>> fused = mg.fuse_pose_views({"side": side, "above": above}, + ... reference="side") + >>> fused["residual_mm"] + + Source: + Consolidated from the author's Westney-comparisons study scripts + concert_fuse3d.py and reh_fuse3d.py, which were byte-identical but for + a hardcoded list of pieces (Jensenius). Reproduces their published + fusion on all four concert excerpts to within float32 storage precision. + """ + names, worlds, vis = _unpack_views(views) + n = min(w.shape[0] for w in worlds) + worlds = [_interp_nan(w[:n].copy(), max_gap) for w in worlds] + vis = [np.clip(v[:n], 0.0, 1.0) for v in vis] + + ref_index = names.index(reference) if reference in names else int(reference) + ref = worlds[ref_index] + torso = list(torso) + + aligned: list[np.ndarray] = [] + rotations: dict = {} + scales: dict = {} + for i, world in enumerate(worlds): + if i == ref_index: + aligned.append(world) + rotations[names[i]] = np.eye(3) + scales[names[i]] = 1.0 + continue + per_frame_R = [] + per_frame_s = [] + for f in range(n): + s_pts = world[f, torso] + d_pts = ref[f, torso] + if np.all(np.isfinite(s_pts)) and np.all(np.isfinite(d_pts)): + R_f, s_f = _umeyama(s_pts, d_pts) + per_frame_R.append(R_f) + per_frame_s.append(s_f) + if not per_frame_R: + aligned.append(world) + rotations[names[i]] = np.eye(3) + scales[names[i]] = 1.0 + continue + R = _mean_rotation(per_frame_R) + scale = float(np.median(per_frame_s)) + al = scale * np.einsum("ij,fkj->fki", R, world) + al = al - np.nanmean(al[:, torso], axis=1, keepdims=True) \ + + np.nanmean(ref[:, torso], axis=1, keepdims=True) + aligned.append(al) + rotations[names[i]] = R + scales[names[i]] = scale + + num = np.zeros((n, worlds[0].shape[1], 3)) + den = np.zeros((n, worlds[0].shape[1], 1)) + for a, v in zip(aligned, vis): + w = v[:, :, None] + good = np.isfinite(a).all(-1, keepdims=True) + num += np.where(good, a * w, 0.0) + den += np.where(good, w, 0.0) + fused = num / np.clip(den, 1e-6, None) + fused = np.where(den > 0, fused, np.nan) + + if smooth is not None: + window, order = smooth + if n >= window: + fused = signal.savgol_filter(fused, window, order, axis=0) + + per_landmark = np.nanmean( + [np.linalg.norm(a - fused, axis=-1) for a in aligned], axis=(0, 1)) * 1000.0 + + return { + "fused": fused, + "residual_mm": float(np.nanmean(per_landmark)), + "residual_per_landmark_mm": per_landmark, + "rotations": rotations, + "scales": scales, + "names": names, + "n_frames": n, + } diff --git a/tests/test_posetools.py b/tests/test_posetools.py index 35cff24d..c6eb9406 100644 --- a/tests/test_posetools.py +++ b/tests/test_posetools.py @@ -412,3 +412,175 @@ def fake_urlretrieve(url, path): path4 = get_pose_model_path(0, models_dir=tmp_path) assert path4.name == "pose_landmarker_lite.task" assert len(downloads) == 2 + + +# --------------------------------------------------------------------------- +# fuse_pose_views +# --------------------------------------------------------------------------- + + +def _rotation(axis, degrees): + """Right-handed rotation matrix about a unit axis, for building views.""" + axis = np.asarray(axis, dtype=float) + axis = axis / np.linalg.norm(axis) + th = np.radians(degrees) + K = np.array([[0.0, -axis[2], axis[1]], + [axis[2], 0.0, -axis[0]], + [-axis[1], axis[0], 0.0]]) + return np.eye(3) + np.sin(th) * K + (1.0 - np.cos(th)) * (K @ K) + + +def _synthetic_skeleton(frames=120, landmarks=33, seed=0): + """A moving 33-landmark skeleton: a fixed pose plus a slow global sway. + + Landmarks 11/12/23/24 (shoulders and hips) are placed as a genuinely rigid + torso, because those are the ones the alignment is estimated from. + """ + rng = np.random.default_rng(seed) + base = rng.normal(scale=0.30, size=(landmarks, 3)) + base[11] = [-0.20, 0.55, 0.0] # left shoulder + base[12] = [0.20, 0.55, 0.0] # right shoulder + base[23] = [-0.15, 0.0, 0.0] # left hip + base[24] = [0.15, 0.0, 0.0] # right hip + + t = np.arange(frames) / 25.0 + sway = np.stack([0.05 * np.sin(2 * np.pi * 0.3 * t), + 0.02 * np.sin(2 * np.pi * 0.2 * t), + 0.03 * np.cos(2 * np.pi * 0.25 * t)], axis=-1) + return base[None, :, :] + sway[:, None, :] + + +def _views_from(truth, transforms): + """Turn one ground-truth skeleton into per-view (world, visibility) pairs.""" + views = [] + for R, scale in transforms: + world = np.einsum("ij,fkj->fki", R, truth) * scale + vis = np.ones(truth.shape[:2]) + views.append((world, vis)) + return views + + +def test_fuse_pose_views_recovers_a_known_skeleton_from_rotated_views(): + """Three rigidly transformed views of one skeleton fuse back to that skeleton.""" + from musicalgestures._posetools import fuse_pose_views + + truth = _synthetic_skeleton() + views = _views_from(truth, [ + (np.eye(3), 1.0), # the reference view + (_rotation([0, 1, 0], 35.0), 1.4), # rotated and larger + (_rotation([0, 1, 0], -50.0), 0.7), # rotated the other way, smaller + ]) + + out = fuse_pose_views(views, smooth=None) + + assert out["fused"].shape == truth.shape + np.testing.assert_allclose(out["fused"], truth, atol=1e-6) + assert out["residual_mm"] < 1e-3 + + +def test_mean_rotation_never_returns_a_reflection(): + """Averaging rotations can land on a reflection; the fix is to reject it. + + The three 180-degree rotations about x, y and z average to + diag(-1/3, -1/3, -1/3), whose determinant is negative. Projecting that + with a bare SVD gives an orthogonal matrix with det -1 -- a mirror, which + would silently swap the skeleton's left and right. + """ + from musicalgestures._posetools import _mean_rotation + + rots = [np.diag([1.0, -1.0, -1.0]), + np.diag([-1.0, 1.0, -1.0]), + np.diag([-1.0, -1.0, 1.0])] + assert np.linalg.det(np.mean(rots, axis=0)) < 0 # the trap is real + + R = _mean_rotation(rots) + + assert np.linalg.det(R) == pytest.approx(1.0) + np.testing.assert_allclose(R @ R.T, np.eye(3), atol=1e-12) + + +def test_umeyama_returns_a_rotation_for_a_degenerate_torso(): + """Collinear points give a rank-deficient fit; it must still be a rotation.""" + from musicalgestures._posetools import _umeyama + + src = np.array([[0.0, 0.0, 0.0], [1.0, 0.0, 0.0], + [2.0, 0.0, 0.0], [3.0, 0.0, 0.0]]) + dst = src[::-1].copy() + + R, scale = _umeyama(src, dst) + + assert np.linalg.det(R) == pytest.approx(1.0, abs=1e-9) + assert scale > 0 + + +def test_fuse_pose_views_ignores_a_landmark_a_view_cannot_see(): + """A landmark at visibility 0 must not drag the fused position towards it.""" + from musicalgestures._posetools import fuse_pose_views + + truth = _synthetic_skeleton(frames=60) + good = (truth.copy(), np.ones(truth.shape[:2])) + blind = truth.copy() + blind[:, 7, :] += 5.0 # a wildly wrong landmark ... + blind_vis = np.ones(truth.shape[:2]) + blind_vis[:, 7] = 0.0 # ... that this view cannot see + second = (truth.copy(), np.ones(truth.shape[:2])) + + out = fuse_pose_views([good, (blind, blind_vis), second], smooth=None) + + np.testing.assert_allclose(out["fused"][:, 7, :], truth[:, 7, :], atol=1e-6) + + +def test_fuse_pose_views_leaves_a_long_dropout_as_nan_when_max_gap_is_set(): + """A repair longer than max_gap must stay NaN rather than pass for a measurement.""" + from musicalgestures._posetools import fuse_pose_views + + truth = _synthetic_skeleton(frames=120) + views = [] + for _ in range(3): + world = truth.copy() + world[40:80, 5, :] = np.nan # a 40-frame dropout in every view + world[90:93, 6, :] = np.nan # a 3-frame one, short enough to fill + views.append((world, np.ones(truth.shape[:2]))) + + out = fuse_pose_views(views, smooth=None, max_gap=10) + + assert np.all(np.isnan(out["fused"][40:80, 5, :])) + assert np.all(np.isfinite(out["fused"][90:93, 6, :])) + + +def test_fuse_pose_views_accepts_extract_pose_landmarks_results(): + """The obvious producer's result dict is a valid view, visibility included.""" + from musicalgestures._posetools import fuse_pose_views + + truth = _synthetic_skeleton(frames=60) + R = _rotation([0, 1, 0], 20.0) + + def as_result(world): + landmarks = np.zeros((world.shape[0], world.shape[1], 3)) + landmarks[..., 2] = 1.0 # visibility column + return {"world": world, "landmarks": landmarks} + + out = fuse_pose_views( + {"side": as_result(truth.copy()), + "above": as_result(np.einsum("ij,fkj->fki", R, truth) * 1.2)}, + reference="side", smooth=None) + + np.testing.assert_allclose(out["fused"], truth, atol=1e-6) + assert out["names"] == ["side", "above"] + + +def test_fuse_pose_views_refuses_a_single_view(): + from musicalgestures._posetools import fuse_pose_views + + truth = _synthetic_skeleton(frames=10) + with pytest.raises(ValueError, match="at least two views"): + fuse_pose_views([(truth, np.ones(truth.shape[:2]))]) + + +def test_fuse_pose_views_says_so_when_a_view_has_no_world_landmarks(): + from musicalgestures._posetools import fuse_pose_views + + truth = _synthetic_skeleton(frames=10) + ok = {"world": truth, "landmarks": np.ones((10, 33, 3))} + with pytest.raises(ValueError, match="world_landmarks=True"): + fuse_pose_views([ok, {"landmarks": np.ones((10, 33, 3))}])