mirror of https://github.com/opencv/opencv.git
Open Source Computer Vision Library
https://opencv.org/
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
96 lines
3.2 KiB
96 lines
3.2 KiB
#!/usr/bin/python |
|
""" |
|
Tracking of rotating point. |
|
Rotation speed is constant. |
|
Both state and measurements vectors are 1D (a point angle), |
|
Measurement is the real point angle + gaussian noise. |
|
The real and the estimated points are connected with yellow line segment, |
|
the real and the measured points are connected with red line segment. |
|
(if Kalman filter works correctly, |
|
the yellow segment should be shorter than the red one). |
|
Pressing any key (except ESC) will reset the tracking with a different speed. |
|
Pressing ESC will stop the program. |
|
""" |
|
# Python 2/3 compatibility |
|
import sys |
|
PY3 = sys.version_info[0] == 3 |
|
|
|
if PY3: |
|
long = int |
|
|
|
import cv2 |
|
from math import cos, sin |
|
import numpy as np |
|
|
|
if __name__ == "__main__": |
|
|
|
img_height = 500 |
|
img_width = 500 |
|
kalman = cv2.KalmanFilter(2, 1, 0) |
|
|
|
code = long(-1) |
|
|
|
cv2.namedWindow("Kalman") |
|
|
|
while True: |
|
state = 0.1 * np.random.randn(2, 1) |
|
|
|
kalman.transitionMatrix = np.array([[1., 1.], [0., 1.]]) |
|
kalman.measurementMatrix = 1. * np.ones((1, 2)) |
|
kalman.processNoiseCov = 1e-5 * np.eye(2) |
|
kalman.measurementNoiseCov = 1e-1 * np.ones((1, 1)) |
|
kalman.errorCovPost = 1. * np.ones((2, 2)) |
|
kalman.statePost = 0.1 * np.random.randn(2, 1) |
|
|
|
while True: |
|
def calc_point(angle): |
|
return (np.around(img_width/2 + img_width/3*cos(angle), 0).astype(int), |
|
np.around(img_height/2 - img_width/3*sin(angle), 1).astype(int)) |
|
|
|
state_angle = state[0, 0] |
|
state_pt = calc_point(state_angle) |
|
|
|
prediction = kalman.predict() |
|
predict_angle = prediction[0, 0] |
|
predict_pt = calc_point(predict_angle) |
|
|
|
measurement = kalman.measurementNoiseCov * np.random.randn(1, 1) |
|
|
|
# generate measurement |
|
measurement = np.dot(kalman.measurementMatrix, state) + measurement |
|
|
|
measurement_angle = measurement[0, 0] |
|
measurement_pt = calc_point(measurement_angle) |
|
|
|
# plot points |
|
def draw_cross(center, color, d): |
|
cv2.line(img, |
|
(center[0] - d, center[1] - d), (center[0] + d, center[1] + d), |
|
color, 1, cv2.LINE_AA, 0) |
|
cv2.line(img, |
|
(center[0] + d, center[1] - d), (center[0] - d, center[1] + d), |
|
color, 1, cv2.LINE_AA, 0) |
|
|
|
img = np.zeros((img_height, img_width, 3), np.uint8) |
|
draw_cross(np.int32(state_pt), (255, 255, 255), 3) |
|
draw_cross(np.int32(measurement_pt), (0, 0, 255), 3) |
|
draw_cross(np.int32(predict_pt), (0, 255, 0), 3) |
|
|
|
cv2.line(img, state_pt, measurement_pt, (0, 0, 255), 3, cv2.LINE_AA, 0) |
|
cv2.line(img, state_pt, predict_pt, (0, 255, 255), 3, cv2.LINE_AA, 0) |
|
|
|
kalman.correct(measurement) |
|
|
|
process_noise = kalman.processNoiseCov * np.random.randn(2, 1) |
|
state = np.dot(kalman.transitionMatrix, state) + process_noise |
|
|
|
cv2.imshow("Kalman", img) |
|
|
|
code = cv2.waitKey(100) % 0x100 |
|
if code != -1: |
|
break |
|
|
|
if code in [27, ord('q'), ord('Q')]: |
|
break |
|
|
|
cv2.destroyWindow("Kalman")
|
|
|