From 5c3261250921d88e0afcf0a01914b69f97bab3ac Mon Sep 17 00:00:00 2001 From: Prohurtz <48768484+RedHawk989@users.noreply.github.com> Date: Thu, 14 Jul 2022 15:21:23 -0700 Subject: [PATCH] fix filter via summers changes --- EyeTrackApp-SingleCam/one_euro_filter.py | 33 +++-- EyeTrackApp-SingleCam/osc.py | 8 +- EyeTrackApp-SingleCam/ransac.py | 61 +++------ EyeTrackApp/eye_processor.py | 162 ++++++----------------- EyeTrackApp/one_euro_filter.py | 35 ++--- EyeTrackApp/osc.py | 18 ++- 6 files changed, 110 insertions(+), 207 deletions(-) diff --git a/EyeTrackApp-SingleCam/one_euro_filter.py b/EyeTrackApp-SingleCam/one_euro_filter.py index 8347296..c672ab9 100644 --- a/EyeTrackApp-SingleCam/one_euro_filter.py +++ b/EyeTrackApp-SingleCam/one_euro_filter.py @@ -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 \ No newline at end of file diff --git a/EyeTrackApp-SingleCam/osc.py b/EyeTrackApp-SingleCam/osc.py index d6baa5e..41fbb12 100644 --- a/EyeTrackApp-SingleCam/osc.py +++ b/EyeTrackApp-SingleCam/osc.py @@ -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 diff --git a/EyeTrackApp-SingleCam/ransac.py b/EyeTrackApp-SingleCam/ransac.py index 9022b68..2e644fe 100644 --- a/EyeTrackApp-SingleCam/ransac.py +++ b/EyeTrackApp-SingleCam/ransac.py @@ -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) diff --git a/EyeTrackApp/eye_processor.py b/EyeTrackApp/eye_processor.py index 801fe69..c8a77b5 100644 --- a/EyeTrackApp/eye_processor.py +++ b/EyeTrackApp/eye_processor.py @@ -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) diff --git a/EyeTrackApp/one_euro_filter.py b/EyeTrackApp/one_euro_filter.py index 9ea15ab..c672ab9 100644 --- a/EyeTrackApp/one_euro_filter.py +++ b/EyeTrackApp/one_euro_filter.py @@ -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 \ No newline at end of file diff --git a/EyeTrackApp/osc.py b/EyeTrackApp/osc.py index 5d609f7..828b899 100644 --- a/EyeTrackApp/osc.py +++ b/EyeTrackApp/osc.py @@ -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