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.
98 lines
3.9 KiB
98 lines
3.9 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. |
|
""" |
|
import urllib2 |
|
import cv2.cv as cv |
|
from math import cos, sin, sqrt |
|
import sys |
|
|
|
if __name__ == "__main__": |
|
A = [ [1, 1], [0, 1] ] |
|
|
|
img = cv.CreateImage((500, 500), 8, 3) |
|
kalman = cv.CreateKalman(2, 1, 0) |
|
state = cv.CreateMat(2, 1, cv.CV_32FC1) # (phi, delta_phi) |
|
process_noise = cv.CreateMat(2, 1, cv.CV_32FC1) |
|
measurement = cv.CreateMat(1, 1, cv.CV_32FC1) |
|
rng = cv.RNG(-1) |
|
code = -1L |
|
|
|
cv.Zero(measurement) |
|
cv.NamedWindow("Kalman", 1) |
|
|
|
while True: |
|
cv.RandArr(rng, state, cv.CV_RAND_NORMAL, cv.RealScalar(0), cv.RealScalar(0.1)) |
|
|
|
kalman.transition_matrix[0,0] = 1 |
|
kalman.transition_matrix[0,1] = 1 |
|
kalman.transition_matrix[1,0] = 0 |
|
kalman.transition_matrix[1,1] = 1 |
|
|
|
cv.SetIdentity(kalman.measurement_matrix, cv.RealScalar(1)) |
|
cv.SetIdentity(kalman.process_noise_cov, cv.RealScalar(1e-5)) |
|
cv.SetIdentity(kalman.measurement_noise_cov, cv.RealScalar(1e-1)) |
|
cv.SetIdentity(kalman.error_cov_post, cv.RealScalar(1)) |
|
cv.RandArr(rng, kalman.state_post, cv.CV_RAND_NORMAL, cv.RealScalar(0), cv.RealScalar(0.1)) |
|
|
|
|
|
while True: |
|
def calc_point(angle): |
|
return (cv.Round(img.width/2 + img.width/3*cos(angle)), |
|
cv.Round(img.height/2 - img.width/3*sin(angle))) |
|
|
|
state_angle = state[0,0] |
|
state_pt = calc_point(state_angle) |
|
|
|
prediction = cv.KalmanPredict(kalman) |
|
predict_angle = prediction[0, 0] |
|
predict_pt = calc_point(predict_angle) |
|
|
|
cv.RandArr(rng, measurement, cv.CV_RAND_NORMAL, cv.RealScalar(0), |
|
cv.RealScalar(sqrt(kalman.measurement_noise_cov[0, 0]))) |
|
|
|
# generate measurement |
|
cv.MatMulAdd(kalman.measurement_matrix, state, measurement, measurement) |
|
|
|
measurement_angle = measurement[0, 0] |
|
measurement_pt = calc_point(measurement_angle) |
|
|
|
# plot points |
|
def draw_cross(center, color, d): |
|
cv.Line(img, (center[0] - d, center[1] - d), |
|
(center[0] + d, center[1] + d), color, 1, cv.CV_AA, 0) |
|
cv.Line(img, (center[0] + d, center[1] - d), |
|
(center[0] - d, center[1] + d), color, 1, cv.CV_AA, 0) |
|
|
|
cv.Zero(img) |
|
draw_cross(state_pt, cv.CV_RGB(255, 255, 255), 3) |
|
draw_cross(measurement_pt, cv.CV_RGB(255, 0,0), 3) |
|
draw_cross(predict_pt, cv.CV_RGB(0, 255, 0), 3) |
|
cv.Line(img, state_pt, measurement_pt, cv.CV_RGB(255, 0,0), 3, cv. CV_AA, 0) |
|
cv.Line(img, state_pt, predict_pt, cv.CV_RGB(255, 255, 0), 3, cv. CV_AA, 0) |
|
|
|
cv.KalmanCorrect(kalman, measurement) |
|
|
|
cv.RandArr(rng, process_noise, cv.CV_RAND_NORMAL, cv.RealScalar(0), |
|
cv.RealScalar(sqrt(kalman.process_noise_cov[0, 0]))) |
|
cv.MatMulAdd(kalman.transition_matrix, state, process_noise, state) |
|
|
|
cv.ShowImage("Kalman", img) |
|
|
|
code = cv.WaitKey(100) % 0x100 |
|
if code != -1: |
|
break |
|
|
|
if code in [27, ord('q'), ord('Q')]: |
|
break |
|
|
|
cv.DestroyWindow("Kalman")
|
|
|