File size: 3,571 Bytes
9f85448 | 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 | from __future__ import annotations
import math
import numpy as np
class _LowPass:
"""Exponential low-pass filter."""
def __init__(self) -> None:
self.y: np.ndarray | None = None
def __call__(self, x: np.ndarray, alpha: float) -> np.ndarray:
"""Filter one sample."""
if self.y is None:
self.y = x.astype(np.float64)
else:
self.y = alpha * x + (1.0 - alpha) * self.y
return self.y
def reset(self) -> None:
"""Clear filter state."""
self.y = None
class OneEuroFilter:
"""One-euro cursor filter."""
def __init__(
self,
freq: float = 30.0,
min_cutoff: float = 1.2,
beta: float = 0.05,
d_cutoff: float = 1.0,
) -> None:
self.freq = freq
self.min_cutoff = min_cutoff
self.beta = beta
self.d_cutoff = d_cutoff
self._x = _LowPass()
self._dx = _LowPass()
self._prev: np.ndarray | None = None
@staticmethod
def _alpha(cutoff: float, freq: float) -> float:
"""Smoothing factor from cutoff."""
tau = 1.0 / (2.0 * math.pi * cutoff)
te = 1.0 / freq
return 1.0 / (1.0 + tau / te)
def reset(self) -> None:
"""Clear filter state."""
self._x.reset()
self._dx.reset()
self._prev = None
def __call__(self, point, dt: float | None = None) -> np.ndarray:
"""Filter one point."""
x = np.asarray(point, dtype=np.float64)
if dt is not None and dt > 1e-6:
self.freq = 1.0 / dt
prev = self._prev if self._prev is not None else x
dx = (x - prev) * self.freq
self._prev = x
edx = self._dx(dx, self._alpha(self.d_cutoff, self.freq))
cutoff = self.min_cutoff + self.beta * float(np.linalg.norm(edx))
return self._x(x, self._alpha(cutoff, self.freq))
class LandmarkFilter:
"""One-euro filter for landmarks."""
def __init__(self, min_cutoff: float = 0.8, beta: float = 0.03) -> None:
self.min_cutoff = min_cutoff
self.beta = beta
self._x = _LowPass()
self._dx = _LowPass()
self._prev: np.ndarray | None = None
self.freq = 30.0
def reset(self) -> None:
"""Clear filter state."""
self._x.reset()
self._dx.reset()
self._prev = None
def __call__(self, points: np.ndarray, dt: float | None = None) -> np.ndarray:
"""Filter all keypoints."""
x = np.asarray(points, dtype=np.float64)
if dt is not None and dt > 1e-6:
self.freq = 1.0 / dt
if self._prev is None or self._prev.shape != x.shape:
self.reset()
self._prev = x
return self._x(x, 1.0)
dx = (x - self._prev) * self.freq
self._prev = x
edx = self._dx(dx, OneEuroFilter._alpha(self.d_cutoff, self.freq))
speed = float(np.linalg.norm(edx, axis=-1).mean())
cutoff = self.min_cutoff + self.beta * speed
return self._x(x, OneEuroFilter._alpha(cutoff, self.freq))
d_cutoff = 1.0
class ScalarEMA:
"""Scalar exponential moving average."""
def __init__(self, alpha: float = 0.35, value: float = 0.0) -> None:
self.alpha = alpha
self.value = value
def __call__(self, target: float) -> float:
"""Advance toward target."""
self.value += self.alpha * (target - self.value)
return self.value
def set(self, value: float) -> None:
"""Force current value."""
self.value = value
|