fix filter via summers changes

This commit is contained in:
Prohurtz 2022-07-14 15:21:23 -07:00
parent 31569c3b41
commit 5c32612509
6 changed files with 110 additions and 207 deletions

View File

@ -1,8 +1,8 @@
import math
import numpy as np
from time import time
def smoothing_factor(t_e, cutoff):
r = 2 * math.pi * cutoff * t_e
r = 2 * np.pi * cutoff * t_e
return r / (r + 1)
@ -11,29 +11,34 @@ def exponential_smoothing(a, x, x_prev):
class OneEuroFilter:
def __init__(self, t0, x0, dx0=0.0, min_cutoff=1.0, beta=0.0,
def __init__(self, x0, dx0=0.0, min_cutoff=1.0, beta=0.0,
d_cutoff=1.0):
"""Initialize the one euro filter."""
# The parameters.
self.min_cutoff = float(min_cutoff)
self.beta = float(beta)
self.d_cutoff = float(d_cutoff)
self.data_shape = x0.shape
self.min_cutoff = np.full(x0.shape, min_cutoff)
self.beta = np.full(x0.shape, beta)
self.d_cutoff = np.full(x0.shape, d_cutoff)
# Previous values.
self.x_prev = float(x0)
self.dx_prev = float(dx0)
self.t_prev = float(t0)
self.x_prev = x0.astype(np.float)
self.dx_prev = np.full(x0.shape, dx0)
self.t_prev = time()
def __call__(self, t, x):
def __call__(self, x):
"""Compute the filtered signal."""
assert x.shape == self.data_shape
t = time()
t_e = t - self.t_prev
#print(x, self.x_prev, t, self.t_prev, t_e)
t_e = np.full(x.shape, t_e)
# The filtered derivative of the signal.
a_d = smoothing_factor(t_e, self.d_cutoff)
dx = (x - self.x_prev) / t_e
dx_hat = exponential_smoothing(a_d, dx, self.dx_prev)
# The filtered signal.
cutoff = self.min_cutoff + self.beta * abs(dx_hat)
cutoff = self.min_cutoff + self.beta * np.abs(dx_hat)
a = smoothing_factor(t_e, cutoff)
x_hat = exponential_smoothing(a, x, self.x_prev)
@ -42,4 +47,4 @@ class OneEuroFilter:
self.dx_prev = dx_hat
self.t_prev = t
return x_hat
return x_hat

View File

@ -33,10 +33,10 @@ class VRChatOSC:
self.client.send_message("/avatar/parameters/LeftEyeX", eye_info.x)
self.client.send_message("/avatar/parameters/EyesY", eye_info.y)
if was_blinking:
self.client.send_message("/avatar/parameters/LeftEyeLid", float(0))
self.client.send_message("/avatar/parameters/RightEyeLid", float(0))
self.client.send_message("/avatar/parameters/LeftEyeLid", float(1))
self.client.send_message("/avatar/parameters/RightEyeLid", float(1))
was_blinking = False
else:
self.client.send_message("/avatar/parameters/LeftEyeLid", float(1))
self.client.send_message("/avatar/parameters/RightEyeLid", float(1))
self.client.send_message("/avatar/parameters/LeftEyeLid", float(0))
self.client.send_message("/avatar/parameters/RightEyeLid", float(0))
was_blinking = True

View File

@ -8,6 +8,7 @@ import threading
import numpy as np
import cv2
from enum import Enum
from one_euro_filter import OneEuroFilter
class InformationOrigin(Enum):
RANSAC = 1
@ -235,9 +236,9 @@ class Ransac:
out_x = max(0.0, min(1.0, xr))
if xl > 0:
out_x = -abs(max(0.0, min(1.0, xl)))
if yd < 0:
if yd > 0:
out_y = -abs(max(0.0, min(1.0, yd)))
if yu > 0:
if yu < 0:
out_y = max(0.0, min(1.0, yu))
self.output_images_and_update(larger_threshold, EyeInformation(InformationOrigin.BLOB, out_x, out_y, False))
@ -250,8 +251,15 @@ class Ransac:
detector_3d = Detector3D(camera=camera_model, long_term_mode=DetectorMode.blocking)
yf = []
xf = []
min_cutoff = 0.0004
beta = 0.7
noisy_point = np.array([1, 1])
one_euro_filter = OneEuroFilter(
noisy_point,
min_cutoff=min_cutoff,
beta=beta
)
while True:
# Check to make sure we haven't been requested to close
if self.cancellation_event.is_set():
@ -372,12 +380,15 @@ class Ransac:
self.calibration_frame_counter -= 1
eye_position_scalar = self.config.vrc_eye_position_scalar
noisy_point = np.array([cx, cy]) #fliter our values with a One Euro Filter
point_hat = one_euro_filter(noisy_point)
cx = point_hat[0]
cy = point_hat[1]
xl = float(((cx - self.xoff) * eye_position_scalar) / (self.xmax - self.xoff))
xr = float(((cx - self.xoff) * eye_position_scalar) / (self.xmin - self.xoff))
yu = float(((cy - self.yoff) * eye_position_scalar) / (self.ymax - self.yoff))
yd = float(((cy - self.yoff) * eye_position_scalar) / (self.ymin - self.yoff))
# print(f"{exm} {eym} {xl} {xr} {yu} {yd}")
out_x = 0
out_y = 0
@ -390,45 +401,7 @@ class Ransac:
if yu > 0:
out_y = max(0.0, min(1.0, yu))
if len(xf) != 4:
xf.append(out_x)
if len(yf) != 4:
yf.append(out_y)
if len(xf) == 4:
xf.insert(0, out_x)
xf.pop()
vx = abs(xf[0] - xf[1])
if vx <= 0.4 and vx > 0.3:
out_x = (xf[0] + xf[1]) / 2
elif vx <= 0.3 and vx > 0.2:
out_x = (xf[0] + xf[1] + xf[2]) / 3
elif vx <= 0.2 and vx > 0.1:
out_x = (xf[0] + xf[1] + xf[2] + xf[3]) / 4
elif vx <= 0.1 and vx >= 0.0:
out_x = (xf[0] + xf[1] + xf[2] + xf[3]) / 4
else:
out_x = out_x
if len(yf) == 4:
yf.insert(0, out_y)
yf.pop()
vy = abs(yf[0] - yf[1])
if vy <= 0.4 and vy > 0.3:
out_y = (yf[0] + yf[1]) / 2
elif vy <= 0.3 and vy > 0.2:
out_y = (yf[0] + yf[1] + yf[2]) / 3
elif vy <= 0.2 and vy > 0.15:
out_y = (yf[0] + yf[1] + yf[2] + yf[3]) / 4
elif vy <= 0.15 and vy >= 0.0:
out_y = (yf[0] + yf[1] + yf[2] + yf[3]) / 4
else:
out_y = out_y
output_info = EyeInformation(InformationOrigin.RANSAC, out_x, out_y, False)

View File

@ -9,9 +9,10 @@ import threading
import numpy as np
import cv2
from enum import Enum
from one_euro_filter import OneEuroFilter
import time
import statistics
from one_euro_filter import OneEuroFilter
from sympy import symbols, Eq, solve
class InformationOrigin(Enum):
RANSAC = 1
BLOB = 2
@ -23,6 +24,7 @@ class EyeInformation:
info_type: InformationOrigin
x: float
y: float
pupil_dialation: int
blink: bool
@ -35,72 +37,6 @@ def run_once(f):
return wrapper
# if filterinit == 1:
# print('raw', cx, cy)
# cy = one_euro_filterx(st, cy) #1st is time of inputs, 2nd is data
# cx = one_euro_filtery(st, cx)
# print('filtered', cx, cy)
# if filterinit == 0: ## initialize filter and define function
# one_euro_filterx = OneEuroFilter(
# 0, cx,
# min_cutoff=0.004,
# beta=0.7)
#one_euro_filtery = OneEuroFilter(
# 0, cy,
# min_cutoff=0.004,
# beta=0.7)
#filterinit = 1
@run_once
def initfilter(cx, cy):
print('######################## ONECE')
initfilter.one_euro_filterx = OneEuroFilter(
0, cx,
min_cutoff=0.0000004,
beta=0.1)
#initfilter.one_euro_filtery = OneEuroFilter(
# 0, cy,
# min_cutoff=0.0000004,
# beta=0.1)
def filter_smooth(cx, cy, st):
print('raw', cx, cy)
OneEuroFilter(
st, cy,
min_cutoff=0.0000004,
beta=0.1)
print(st)
#cy = initfilter.one_euro_filterx(st, cy) #1st is time of inputs, 2nd is data
# cx = initfilter.one_euro_filtery(st, cx)
print('filtered', cx, cy)
return cx, cy
def fit_rotated_ellipse_ransac(
@ -235,8 +171,6 @@ class EyeProcessor:
self.ymin = -69420
self.previous_rotation = self.config.rotation_angle
self.recenter_eye = False
self.filterinit = 0
self.calibration_frame_counter
@ -329,7 +263,7 @@ class EyeProcessor:
# order to have a projected sphere.
if self.lkg_projected_sphere == None:
self.output_images_and_update(
larger_threshold, EyeInformation(InformationOrigin.FAILURE, 0, 0, False)
larger_threshold, EyeInformation(InformationOrigin.FAILURE, 0, 0, 0, False)
)
return
@ -416,13 +350,16 @@ class EyeProcessor:
out_y = max(0.0, min(1.0, yu))
self.output_images_and_update(
larger_threshold,
EyeInformation(InformationOrigin.BLOB, out_x, out_y, False),
EyeInformation(InformationOrigin.BLOB, out_x, out_y, w, False),
)
return
self.output_images_and_update(
larger_threshold, EyeInformation(InformationOrigin.BLOB, 0, 0, True)
larger_threshold, EyeInformation(InformationOrigin.BLOB, 0, 0, 0, True)
)
print("[INFO] BLINK Detected.")
@ -431,8 +368,20 @@ class EyeProcessor:
detector_3d = None
xf = []
yf = []
pd = []
out_pupil_dialation = 1
min_cutoff = 0.0004
beta = 0.7
noisy_point = np.array([1, 1])
one_euro_filter = OneEuroFilter(
noisy_point,
min_cutoff=min_cutoff,
beta=beta
)
while True:
st = time.time()
# oef = init_filter()
# Check to make sure we haven't been requested to close
if self.cancellation_event.is_set():
print("Exiting RANSAC thread")
@ -567,8 +516,8 @@ class EyeProcessor:
#d = ellipse_3d['diameter']
#print(w)
d = result_3d["diameter_3d"]
if self.calibration_frame_counter == 0 or self.recenter_eye:
@ -588,15 +537,13 @@ class EyeProcessor:
self.calibration_frame_counter -= 1
eye_position_scalar = self.config.vrc_eye_position_scalar
end = time.time()
pt = end - st
#fd = filter_smooth(cx, cy, pt)
#cx = fd[0]
#cy = fd[1]
# print('tracked', cx, cy)
noisy_point = np.array([cx, cy]) #fliter our values with a One Euro Filter
point_hat = one_euro_filter(noisy_point)
cx = point_hat[0]
cy = point_hat[1]
xl = float(
@ -606,15 +553,13 @@ class EyeProcessor:
((cx - self.xoff) * eye_position_scalar) / (self.xmin - self.xoff)
)
yu = float(
((cy - self.yoff) * eye_position_scalar) / (self.ymin - self.yoff)
)
yd = float(
((cy - self.yoff) * eye_position_scalar) / (self.ymax - self.yoff)
)
# print(f"{exm} {eym} {xl} {xr} {yu} {yd}")
yd = float(
((cy - self.yoff) * eye_position_scalar) / (self.ymin - self.yoff)
)
out_x = 0
@ -629,44 +574,11 @@ class EyeProcessor:
out_y = max(0.0, min(1.0, yu))
if len(xf) != 4:
xf.append(out_x)
if len(yf) != 4:
yf.append(out_y)
if len(xf) == 4:
xf.insert(0, out_x)
xf.pop()
vx = abs(xf[0] - xf[1])
if vx <= 0.4 and vx > 0.3:
out_x = (xf[0] + xf[1]) / 2
elif vx <= 0.3 and vx > 0.2:
out_x = (xf[0] + xf[1] + xf[2]) / 3
elif vx <= 0.2 and vx >= 0.0:
out_x = (xf[0] + xf[1] + xf[2] + xf[3]) / 4
else:
out_x = out_x
if len(yf) == 4:
yf.insert(0, out_y)
yf.pop()
vy = abs(yf[0] - yf[1])
if vy <= 0.4 and vy > 0.3:
out_y = (yf[0] + yf[1]) / 2
elif vy <= 0.3 and vy > 0.2:
out_y = (yf[0] + yf[1] + yf[2]) / 3
elif vy <= 0.2 and vy >= 0.0:
out_y = (yf[0] + yf[1] + yf[2] + yf[3]) / 4
else:
out_y = out_y
output_info = EyeInformation(InformationOrigin.RANSAC, out_x, out_y, False)
output_info = EyeInformation(InformationOrigin.RANSAC, out_x, out_y, out_pupil_dialation, False)
# Draw our image and stack it for visual output
cv2.drawContours(self.current_image_gray, contours, -1, (255, 0, 0), 1)

View File

@ -1,8 +1,8 @@
import math
import numpy as np
from time import time
def smoothing_factor(t_e, cutoff):
r = 2 * math.pi * cutoff * t_e
r = 2 * np.pi * cutoff * t_e
return r / (r + 1)
@ -11,29 +11,34 @@ def exponential_smoothing(a, x, x_prev):
class OneEuroFilter:
def __init__(self, t0, x0, dx0=0.0, min_cutoff=1.0, beta=0.0,
def __init__(self, x0, dx0=0.0, min_cutoff=1.0, beta=0.0,
d_cutoff=1.0):
"""Initialize the one euro filter."""
# The parameters.
self.min_cutoff = float(min_cutoff)
self.beta = float(beta)
self.d_cutoff = float(d_cutoff)
self.data_shape = x0.shape
self.min_cutoff = np.full(x0.shape, min_cutoff)
self.beta = np.full(x0.shape, beta)
self.d_cutoff = np.full(x0.shape, d_cutoff)
# Previous values.
self.x_prev = float(x0)
self.dx_prev = float(dx0)
self.t_prev = float(t0)
self.x_prev = x0.astype(np.float)
self.dx_prev = np.full(x0.shape, dx0)
self.t_prev = time()
def __call__(self, t, x):
def __call__(self, x):
"""Compute the filtered signal."""
t_e = t #- self.t_prev
#print(x, self.x_prev, t, self.t_prev, t_e)
assert x.shape == self.data_shape
t = time()
t_e = t - self.t_prev
t_e = np.full(x.shape, t_e)
# The filtered derivative of the signal.
a_d = smoothing_factor(t_e, self.d_cutoff)
dx = (x - self.x_prev) / t_e
dx_hat = exponential_smoothing(a_d, dx, self.dx_prev)
# The filtered signal.
cutoff = self.min_cutoff + self.beta * abs(dx_hat)
cutoff = self.min_cutoff + self.beta * np.abs(dx_hat)
a = smoothing_factor(t_e, cutoff)
x_hat = exponential_smoothing(a, x, self.x_prev)
@ -42,4 +47,4 @@ class OneEuroFilter:
self.dx_prev = dx_hat
self.t_prev = t
return x_hat
return x_hat

View File

@ -20,7 +20,7 @@ class VRChatOSC:
def __init__(
self,
cancellation_event: "threading.Event",
msg_queue: "queue.Queue[tuple[bool, int, int]]",
msg_queue: "queue.Queue[tuple[bool, int, int, int]]",
):
self.client = udp_client.SimpleUDPClient(VRChatOSC.OSC_IP, VRChatOSC.OSC_PORT)
self.cancellation_event = cancellation_event
@ -44,28 +44,36 @@ class VRChatOSC:
self.client.send_message(
"/avatar/parameters/RightEye", eye_info.x
)
self.client.send_message(
"/avatar/parameters/EyesDilation", eye_info.pupil_dialation
)
if eye_id in [EyeId.LEFT, EyeId.BOTH]:
self.client.send_message(
"/avatar/parameters/LeftEye", eye_info.x
)
self.client.send_message(
"/avatar/parameters/EyesDilation", eye_info.pupil_dialation
)
self.client.send_message("/avatar/parameters/EyesY", eye_info.y)
if was_blinking:
if eye_id in [EyeId.LEFT, EyeId.BOTH]:
self.client.send_message(
"/avatar/parameters/LeftEyeLid", float(0)
"/avatar/parameters/LeftEyeLid", float(1)
)
if eye_id in [EyeId.RIGHT, EyeId.BOTH]:
self.client.send_message(
"/avatar/parameters/RightEyeLid", float(0)
"/avatar/parameters/RightEyeLid", float(1)
)
was_blinking = False
else:
if eye_id in [EyeId.LEFT, EyeId.BOTH]:
self.client.send_message(
"/avatar/parameters/LeftEyeLid", float(1)
"/avatar/parameters/LeftEyeLid", float(0)
)
if eye_id in [EyeId.RIGHT, EyeId.BOTH]:
self.client.send_message(
"/avatar/parameters/RightEyeLid", float(1)
"/avatar/parameters/RightEyeLid", float(0)
)
was_blinking = True