49 lines
1.8 KiB
Python
49 lines
1.8 KiB
Python
import math
|
|
|
|
from edge.models import Keypoint
|
|
|
|
|
|
class OneEuroFilter:
|
|
"""1€ filter (Casiez et al.). 저지연 지터 감쇠."""
|
|
|
|
def __init__(self, min_cutoff: float = 1.0, beta: float = 0.0, d_cutoff: float = 1.0):
|
|
self.min_cutoff = min_cutoff
|
|
self.beta = beta
|
|
self.d_cutoff = d_cutoff
|
|
self._x_prev: float | None = None
|
|
self._dx_prev: float = 0.0
|
|
self._t_prev: float | None = None
|
|
|
|
@staticmethod
|
|
def _alpha(cutoff: float, dt: float) -> float:
|
|
tau = 1.0 / (2 * math.pi * cutoff)
|
|
return 1.0 / (1.0 + tau / dt)
|
|
|
|
def filter(self, value: float, t: float) -> float:
|
|
if self._x_prev is None or self._t_prev is None or t <= self._t_prev:
|
|
self._x_prev, self._t_prev, self._dx_prev = value, t, 0.0
|
|
return value
|
|
dt = t - self._t_prev
|
|
dx = (value - self._x_prev) / dt
|
|
a_d = self._alpha(self.d_cutoff, dt)
|
|
dx_hat = a_d * dx + (1 - a_d) * self._dx_prev
|
|
cutoff = self.min_cutoff + self.beta * abs(dx_hat)
|
|
a = self._alpha(cutoff, dt)
|
|
x_hat = a * value + (1 - a) * self._x_prev
|
|
self._x_prev, self._dx_prev, self._t_prev = x_hat, dx_hat, t
|
|
return x_hat
|
|
|
|
|
|
class KeypointSmoother:
|
|
"""관절 17개 각각 x,y에 독립 1€ 필터."""
|
|
|
|
def __init__(self, min_cutoff: float = 1.0, beta: float = 0.007):
|
|
self._fx = [OneEuroFilter(min_cutoff, beta) for _ in range(17)]
|
|
self._fy = [OneEuroFilter(min_cutoff, beta) for _ in range(17)]
|
|
|
|
def smooth(self, keypoints: list[Keypoint], t: float) -> list[Keypoint]:
|
|
out = []
|
|
for i, k in enumerate(keypoints):
|
|
out.append(Keypoint(self._fx[i].filter(k.x, t),
|
|
self._fy[i].filter(k.y, t), k.conf))
|
|
return out
|