Python 一欧元滤波器 OneEuroFilter PyQt可视化跟随鼠标

import sys import time from PyQt5.QtCore import * from PyQt5.QtGui import * from PyQt5.QtWidgets import * import numpy as np import cv2 WIN_W = 1000 WIN_H = 600 SIGMA = 10 x = 0 y = 0 class OneEuroFilter: def __init__(self, te, mincutoff=1.0, beta=0.007, dcutoff=1.0): self.x = None self.dx = 0 self.te = te self.mincutoff = mincutoff self.beta = beta self.dcutoff = dcutoff self.alpha = self._alpha(self.mincutoff) self.dalpha = self._alpha(self.dcutoff) def _alpha(self, cutoff): tau = 1.0 / (2 * np.pi * cutoff) return 1.0 / (1.0 + tau / self.te) def predict(self, x, te): result = x if self.x is not None: edx = (x - self.x) / te self.dx = self.dx + (self.dalpha * (edx - self.dx)) cutoff = self.mincutoff + self.beta * abs(self.dx) self.alpha = self._alpha(cutoff) result = self.x + self.alpha * (x - self.x) self.x = result return result class MainThread(QThread): def __init__(self, canvas): super(MainThread, self).__init__() self.canvas = canvas self.t1 = time.perf_counter() self.te = 0.04 # time elapsed per frame self.filter_x = OneEuroFilter(self.te) self.filter_y = OneEuroFilter(self.te) def update_fps(self): t2 = time.perf_counter() self.te = t2 - self.t1 self.t1 = t2 def run(self): while True: global x, y noise_x = x + np.random.normal(0, SIGMA) noise_y = y + np.random.normal(0, SIGMA) predict_x = self.filter_x.predict(noise_x, self.te) predict_y = self.filter_y.predict(noise_y, self.te) img = np.ones((WIN_H, WIN_W, 3), dtype=np.uint8) * 255 cv2.circle(img, (int(noise_x), int(noise_y)), 5, (0, 0, 0), -1) cv2.circle(img, (int(predict_x), int(predict_y)), 5, (0, 0, 255), -1) cv2.putText(img, "Te: {:.4f}s".format(self.te), (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 2) cv2.putText(img, "Black: Noise", (10, 80), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 0), 2) cv2.putText(img, "Red: Filtered", (10, 110), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2) img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) self.canvas.setPixmap(QPixmap.fromImage(QImage(img.data, img.shape[1], img.shape[0], img.shape[1] * 3, QImage.Format_RGB888))) time.sleep(0.03) self.update_fps() class Canvas(QLabel): def __init__(self, parent=None): super(Canvas, self).__init__(parent) self.setFixedSize(WIN_W, WIN_H) self.setMouseTracking(True) self.setStyleSheet("background-color: white;") self.main_thread = MainThread(self) self.main_thread.start() def mouseMoveEvent(self, event): global x, y x = event.x() y = event.y() class MainWidget(QWidget): def __init__(self, parent=None): super(MainWidget, self).__init__(parent) self.initUI() def initUI(self): self.setWindowTitle("One Euro Filter") self.resize(WIN_W, WIN_H) self.canvas = Canvas(self) self.show() if __name__ == "__main__": app = QApplication(sys.argv) mainWidget = MainWidget() sys.exit(app.exec_())