class KalmanFilterTracker(QWidget):
def __init__(self, title='', image=''):
super().__init__()
self.title = title
self.desktop = QDesktopWidget()
self.screen = self.desktop.availableGeometry()
self.logger = logging.getLogger(self.__class__.__name__)
# drawing setting
self._drawing = False
self._image = image
self._w, self._h, self._c = (1280, 720, 3)
self._pen_measure = QPen(Qt.black, 3, cap=Qt.RoundCap)
self._pen_measure_line = QPen(Qt.black, 1, style=Qt.SolidLine)
self._pen_predict = QPen(Qt.blue, 3, cap=Qt.RoundCap)
self._pen_predict_line = QPen(Qt.blue, 1, style=Qt.SolidLine)
self._pen_correct = QPen(Qt.red, 5, cap=Qt.RoundCap)
self._pen_correct_line = QPen(Qt.red, 1, style=Qt.SolidLine)
self._measure_points = QPolygon()
self._predict_points = QPolygon()
self._correct_points = QPolygon()
self._measure_text = f"Mouse measurement - ()"
self._predict_text = f"Kalman Filter predict - ()"
self._correct_text = f"Kalman Filter correct - ()"
# init
self.init_ui()
def _ndarray_to_qimage(self, arr):
w, h = arr.shape[:2]
return QImage(arr.data, w, h, w * 3, QImage.Format_RGB888)
def _qimage_to_qpixmap(self, qimg):
return QPixmap.fromImage(qimg)
def _reset_qpixmap(self):
self._pixmap = self._qimage_to_qpixmap(self._image)
def _reset_polygon(self):
self._measure_points = QPolygon()
self._correct_points = QPolygon()
self._predict_points = QPolygon()
def init_ui(self):
# set windows
self.setWindowTitle(self.title)
self.resize(self._w, self._h)
frame_geo = self.frameGeometry()
frame_geo.moveCenter(self.screen.center())
self.move(frame_geo.topLeft())
# set layout
self.root = QGridLayout()
self._hbox_body = QHBoxLayout()
self.setLayout(self.root)
self.show()
# set image
if self._image:
self._w, self.h = self._image.shape[:2]
else:
self._image = np.zeros((self._w, self._h, self._c),
dtype='float32')
self._image = self._ndarray_to_qimage(self._image)
self._pixmap = self._qimage_to_qpixmap(self._image)
def init_kalman_filter(self, x: float, y: float):
dim_x = 4 # Number of state variables (x, y, x_velocity, y_velocity)
dim_z = 2 # Number of of measurement inputs (x, y)
self.kf = KalmanFilter(dim_x=dim_x, dim_z=dim_z)
self.kf.x = np.array([x, y, 0., 0.
]) # init state (position, velocity) (dim_x, 1)
self.kf.F = np.array([
[1., 0., 1., 0.], # state transition matrix (dim_x, dim_x)
[0., 1., 0., 1.],
[0., 0., 1., 0.],
[0., 0., 0., 1.]
])
self.kf.H = np.array([
[1., 0., 0., 0.], # measurement function, ((dim_z, dim_x))
[0., 1., 0., 0.]
])
self.kf.P = np.eye(dim_x) * 1000. # covariance matrix (dim_x, dim_x)
self.kf.R = np.eye(
dim_z) * 10. # measurement noise covariance (dim_z, dim_z)
self.kf.Q = np.eye(dim_x) * 1e-3 # process uncertainty (dim_x, dim_x)
self._predict_text = f"Kalman Filter predict - ()"
self._correct_text = f"Kalman Filter correct - ()"
def paintEvent(self, e):
painter = QPainter(self)
painter.setRenderHint(QPainter.Antialiasing)
assert self._measure_points.count() == \
self._predict_points.count() == \
self._correct_points.count()
for i in range(self._measure_points.count()):
painter.setPen(self._pen_measure)
painter.drawPoint(self._measure_points.point(i))
painter.setPen(self._pen_predict)
painter.drawPoint(self._predict_points.point(i))
painter.setPen(self._pen_correct)
painter.drawPoint(self._correct_points.point(i))
if i:
painter.setPen(self._pen_measure_line)
painter.drawLine(self._measure_points.point(i - 1),
self._measure_points.point(i))
painter.setPen(self._pen_predict_line)
painter.drawLine(self._predict_points.point(i - 1),
self._predict_points.point(i))
painter.setPen(self._pen_correct_line)
painter.drawLine(self._correct_points.point(i - 1),
self._correct_points.point(i))
painter.setPen(self._pen_measure_line)
painter.drawText(50, 60, self._measure_text)
painter.setPen(self._pen_predict_line)
painter.drawText(50, 80, self._predict_text)
painter.setPen(self._pen_correct_line)
painter.drawText(50, 100, self._correct_text)
def mousePressEvent(self, e):
if e.button() == Qt.LeftButton:
self._drawing = True
self.logger.info(f"========== Reset QPolygon ==========")
x, y = float(e.x()), float(e.y())
self._reset_polygon()
self.init_kalman_filter(x, y)
self._measure_text = f"Mouse measurement - ({e.x()}, {e.y()})"
def mouseMoveEvent(self, e):
if e.buttons() and Qt.LeftButton and self._drawing:
self.logger.debug(f"Record {e.pos()}")
# measurement
self._measure_points << e.pos()
self._measure_text = f"Mouse measurement - ({e.x()}, {e.y()})"
# predict
self.kf.predict()
predict_pts = self.kf.x[:2]
predict_x, predict_y = predict_pts
predict_x, predict_y = int(predict_x), int(predict_y)
self._predict_points << QPoint(predict_x, predict_y)
self._predict_text = f"Kalman Filter predict - ({predict_pts[0]:.4f}, {predict_pts[1]:4f})"
# correct
self.kf.update(
np.array([e.x(), e.y()], dtype='float32').reshape((2, 1)))
correct_pts = self.kf.x[:2]
correct_x, correct_y = correct_pts
correct_x, correct_y = int(correct_x), int(correct_y)
self._correct_points << QPoint(correct_x, correct_y)
self._correct_text = f"Kalman Filter correct - ({correct_pts[0]:4f}, {correct_pts[1]:4f})"
# draw
self.logger.info(
f"mouse ({e.x()}, {e.y()}); KF predict {predict_pts}; KF correct {correct_pts}"
)
self.update()
def mouseReleaseEvent(self, e):
if e.button() == Qt.LeftButton:
self._drawing = False
class LabelDots(QLabel):
def __init__(self, parent):
super().__init__(parent=parent)
self.center = None
self.pixmap_laser = None
self.centerCoords = None
self.pt_edges = None
def getCoord(self, x, y):
Parent = self.parent().parent().parent().parent()
Width, Height = self.width(), self.height()
width, height = Parent.laserScreenshotWidth, Parent.laserScreenshotHeight
widthDiff, heightDiff = Width - width, Height - height
xmin, xmax, ymin, ymax = Parent.xyminmax
xcoord, ycoord = (x - widthDiff / 2) / width * (xmax - xmin) + xmin, (
Height - y - heightDiff / 2) / height * (ymax - ymin) + ymin
return xcoord, ycoord
def getPos(self, xcoord, ycoord):
Parent = self.parent().parent().parent().parent()
Width, Height = self.width(), self.height()
width, height = Parent.laserScreenshotWidth, Parent.laserScreenshotHeight
widthDiff, heightDiff = Width - width, Height - height
xmin, xmax, ymin, ymax = Parent.xyminmax
x = int((xcoord - xmin) / (xmax - xmin) * width + widthDiff / 2)
y = int(-((ycoord - ymin) /
(ymax - ymin) * height + heightDiff / 2 - Height))
return x, y
def mousePressEvent(self, event):
super().mousePressEvent(event)
# self.label_laserScreenshot.clear()
# self.clear()
pos = event.pos()
x = pos.x()
y = pos.y()
xcoord, ycoord = self.getCoord(x, y)
# print(xcoord,ycoord)
self.centerCoords = (xcoord, ycoord)
self.center.clear()
self.center << pos
# print(pos,9999999,self.center)
self.pos = pos
Parent = self.parent().parent().parent().parent()
Parent.label_center.setText("Center Coordinate:\nx = " + str(xcoord) +
"\ny = " + str(ycoord))
# print(x, y, 111)
# print(self.center.point(0), 222)
self.update()
def paintEvent(self, event):
super().paintEvent(event)
# print('tttt')
if self.pixmap_laser != None:
# self.abel_laserScreenshotl.clear()
qp = QPainter(self)
qp.begin(self)
qp.setRenderHints(QPainter.Antialiasing, True)
pen = QPen(Qt.red, 5)
brush = QBrush(Qt.red)
qp.setPen(pen)
qp.setBrush(brush)
# qp.drawPixmap(event.rect(), self.pixmap_laser)
# self.setPixmap(self.pixmap_laser)
# print(self.center.point(0))
if self.center == None:
Parent = self.parent().parent().parent().parent()
width, height = Parent.laserScreenshotWidth, Parent.laserScreenshotHeight
self.center = QPolygon()
self.center << QPoint(*self.getPos(0, 0))
qp.drawEllipse(self.center.point(0), 1, 1)
if self.pt_edges != None:
pen = QPen(Qt.gray, 1)
brush = QBrush(Qt.gray)
qp.setPen(pen)
qp.setBrush(brush)
# print(self.pt_edges)
# qp.drawLine(0,0,200,200);
for pt in self.pt_edges:
# print(*pt,123)
pt1 = self.getPos(*pt[0])
pt2 = self.getPos(*pt[1])
qp.drawLine(*pt1, *pt2)
qp.end()
# print(self.label_laserScreenshot.mapToParent(self.center.point(0)))
# print(event.rect())
# def findDotPos(self ,event):
# self.label_laserScreenshot.clear()
# pos = event.pos()
# x = pos.x()
# y = pos.y()
# # print(x,y)
# self.center.clear()
# # self.label_laserScreenshot.clear()
# self.center << pos
# self.pos = pos
# self.label_center.setText(str(x ) +' , ' +str(y))
# # self.update()
# # self.label_image.update()
# print(x ,y ,111)
# # self.label_laserScreenshot.clear()
# # painter = QPainter(self)
# # pen = QPen()
# # brush = QBrush(Qt.red)
# # painter.setPen(pen)
# # painter.setBrush(brush)
# # painter.setRenderHint(QPainter.Antialiasing,True)
# # painter.drawPixmap(self.label_laserScreenshot.rect(),self.pixmap_laser)
# # painter.drawEllipse(self.label_image.mapToParent(self.center.point(0)), 100, 100)
# # painter.drawPoint(x,y)
# # self.label_laserScreenshot.setPixmap(self.pixmap_laser)
# print(self.center.point(0) ,222)
# def paintEvent2(self, event):
# super().paintEvent(event)
# print('tttt')
# if True:#self.pixmap_laser != None:
# # self.label_laserScreenshot.clear()
# qp = QPainter(self)
# qp.begin(self)
# qp.setRenderHints(QPainter.Antialiasing,True)
# pen = QPen(Qt.red,5)
# brush = QBrush(Qt.red)
# qp.setPen(pen)
# qp.setBrush(brush)
# qp.drawPixmap(event.rect(), self.pixmap_laser)
# qp.drawEllipse(self.center.point(0),5,5)
# qp.end()
# print(self.label_laserScreenshot.mapToParent(self.center.point(0)))
# print(event.rect())
def containspoly(poly1: QPolygon, poly2: QPolygon):
for point_index in range(poly2.size()-1):
if not poly1.containsPoint(poly2.point(point_index), Qt.OddEvenFill):
return False
return True
