mirror/opencv
1
0
Fork 0
mirror of https://github.com/opencv/opencv.git synced 2025-06-07 17:44:04 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Merge pull request #20564 from AleksandrPanov:update_kalman_sample

Browse Source 
Update kalman sample

* updated view and comments, fixed dims

* updated view and comments, added statePost
This commit is contained in:
Alexander Panov 2021-08-20 13:57:05 +03:00 committed by GitHub
parent a9817e9127
commit d6306f8ccb
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 83 additions and 72 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
samples/cpp/kalman.cpp
View File

@ -1,6 +1,6 @@
#include "opencv2/video/tracking.hpp" #include "opencv2/video/tracking.hpp"
#include "opencv2/highgui.hpp" #include "opencv2/highgui.hpp"
#include "opencv2/core/cvdef.h"
#include <stdio.h> #include <stdio.h>
using namespace cv; using namespace cv;
@ -14,15 +14,19 @@ static void help()
{ {
printf( "\nExample of c calls to OpenCV's Kalman filter.\n" printf( "\nExample of c calls to OpenCV's Kalman filter.\n"
" Tracking of rotating point.\n" " Tracking of rotating point.\n"
" Rotation speed is constant.\n" " Point moves in a circle and is characterized by a 1D state.\n"
" state_k+1 = state_k + speed + process_noise N(0, 1e-5)\n"
" The speed is constant.\n"
" Both state and measurements vectors are 1D (a point angle),\n" " Both state and measurements vectors are 1D (a point angle),\n"
" Measurement is the real point angle + gaussian noise.\n" " Measurement is the real state + gaussian noise N(0, 1e-1).\n"
" The real and the estimated points are connected with yellow line segment,\n" " The real and the measured points are connected with red line segment,\n"
" the real and the measured points are connected with red line segment.\n" " the real and the estimated points are connected with yellow line segment,\n"
" the real and the corrected estimated points are connected with green line segment.\n"
" (if Kalman filter works correctly,\n" " (if Kalman filter works correctly,\n"
" the yellow segment should be shorter than the red one).\n" " the yellow segment should be shorter than the red one and\n"
" the green segment should be shorter than the yellow one)."
"\n" "\n"
" Pressing any key (except ESC) will reset the tracking with a different speed.\n" " Pressing any key (except ESC) will reset the tracking.\n"
" Pressing ESC will stop the program.\n" " Pressing ESC will stop the program.\n"
); );
} }
@ -39,7 +43,9 @@ int main(int, char**)
for(;;) for(;;)
{ {
randn( state, Scalar::all(0), Scalar::all(0.1) ); img = Scalar::all(0);
state.at<float>(0) = 0.0f;
state.at<float>(1) = 2.f * (float)CV_PI / 6;
KF.transitionMatrix = (Mat_<float>(2, 2) << 1, 1, 0, 1); KF.transitionMatrix = (Mat_<float>(2, 2) << 1, 1, 0, 1);
setIdentity(KF.measurementMatrix); setIdentity(KF.measurementMatrix);
@ -60,36 +66,40 @@ int main(int, char**)
double predictAngle = prediction.at<float>(0); double predictAngle = prediction.at<float>(0);
Point predictPt = calcPoint(center, R, predictAngle); Point predictPt = calcPoint(center, R, predictAngle);
randn( measurement, Scalar::all(0), Scalar::all(KF.measurementNoiseCov.at<float>(0)));
// generate measurement // generate measurement
randn( measurement, Scalar::all(0), Scalar::all(KF.measurementNoiseCov.at<float>(0)));
measurement += KF.measurementMatrix*state; measurement += KF.measurementMatrix*state;
double measAngle = measurement.at<float>(0); double measAngle = measurement.at<float>(0);
Point measPt = calcPoint(center, R, measAngle); Point measPt = calcPoint(center, R, measAngle);
// correct the state estimates based on measurements
// updates statePost & errorCovPost
KF.correct(measurement);
double improvedAngle = KF.statePost.at<float>(0);
Point improvedPt = calcPoint(center, R, improvedAngle);
// plot points // plot points
#define drawCross( center, color, d ) \ img = img * 0.2;
line( img, Point( center.x - d, center.y - d ), \ drawMarker(img, measPt, Scalar(0, 0, 255), cv::MARKER_SQUARE, 5, 2);
Point( center.x + d, center.y + d ), color, 1, LINE_AA, 0); \ drawMarker(img, predictPt, Scalar(0, 255, 255), cv::MARKER_SQUARE, 5, 2);
line( img, Point( center.x + d, center.y - d ), \ drawMarker(img, improvedPt, Scalar(0, 255, 0), cv::MARKER_SQUARE, 5, 2);
Point( center.x - d, center.y + d ), color, 1, LINE_AA, 0 ) drawMarker(img, statePt, Scalar(255, 255, 255), cv::MARKER_STAR, 10, 1);
// forecast one step
Mat test = Mat(KF.transitionMatrix*KF.statePost);
drawMarker(img, calcPoint(center, R, Mat(KF.transitionMatrix*KF.statePost).at<float>(0)),
Scalar(255, 255, 0), cv::MARKER_SQUARE, 12, 1);
img = Scalar::all(0); line( img, statePt, measPt, Scalar(0,0,255), 1, LINE_AA, 0 );
drawCross( statePt, Scalar(255,255,255), 3 ); line( img, statePt, predictPt, Scalar(0,255,255), 1, LINE_AA, 0 );
drawCross( measPt, Scalar(0,0,255), 3 ); line( img, statePt, improvedPt, Scalar(0,255,0), 1, LINE_AA, 0 );
drawCross( predictPt, Scalar(0,255,0), 3 );
line( img, statePt, measPt, Scalar(0,0,255), 3, LINE_AA, 0 );
line( img, statePt, predictPt, Scalar(0,255,255), 3, LINE_AA, 0 );
if(theRNG().uniform(0,4) != 0)
KF.correct(measurement);
randn( processNoise, Scalar(0), Scalar::all(sqrt(KF.processNoiseCov.at<float>(0, 0)))); randn( processNoise, Scalar(0), Scalar::all(sqrt(KF.processNoiseCov.at<float>(0, 0))));
state = KF.transitionMatrix*state + processNoise; state = KF.transitionMatrix*state + processNoise;
imshow( "Kalman", img ); imshow( "Kalman", img );
code = (char)waitKey(100); code = (char)waitKey(1000);
if( code > 0 ) if( code > 0 )
break; break;

97
samples/python/kalman.py
View File

@ -1,14 +1,18 @@
#!/usr/bin/env python #!/usr/bin/env python
""" """
Tracking of rotating point. Tracking of rotating point.
Rotation speed is constant. Point moves in a circle and is characterized by a 1D state.
state_k+1 = state_k + speed + process_noise N(0, 1e-5)
The speed is constant.
Both state and measurements vectors are 1D (a point angle), Both state and measurements vectors are 1D (a point angle),
Measurement is the real point angle + gaussian noise. Measurement is the real state + gaussian noise N(0, 1e-1).
The real and the estimated points are connected with yellow line segment, The real and the measured points are connected with red line segment,
the real and the measured points are connected with red line segment. the real and the estimated points are connected with yellow line segment,
the real and the corrected estimated points are connected with green line segment.
(if Kalman filter works correctly, (if Kalman filter works correctly,
the yellow segment should be shorter than the red one). the yellow segment should be shorter than the red one and
Pressing any key (except ESC) will reset the tracking with a different speed. the green segment should be shorter than the yellow one).
Pressing any key (except ESC) will reset the tracking.
Pressing ESC will stop the program. Pressing ESC will stop the program.
""" """
# Python 2/3 compatibility # Python 2/3 compatibility
@ -21,8 +25,7 @@ if PY3:
import numpy as np import numpy as np
import cv2 as cv import cv2 as cv
from math import cos, sin, sqrt from math import cos, sin, sqrt, pi
import numpy as np
def main(): def main():
img_height = 500 img_height = 500
@ -30,64 +33,62 @@ def main():
kalman = cv.KalmanFilter(2, 1, 0) kalman = cv.KalmanFilter(2, 1, 0)
code = long(-1) code = long(-1)
num_circle_steps = 12
cv.namedWindow("Kalman")
while True: while True:
state = 0.1 * np.random.randn(2, 1) img = np.zeros((img_height, img_width, 3), np.uint8)
state = np.array([[0.0],[(2 * pi) / num_circle_steps]]) # start state
kalman.transitionMatrix = np.array([[1., 1.], [0., 1.]]) kalman.transitionMatrix = np.array([[1., 1.], [0., 1.]]) # F. input
kalman.measurementMatrix = 1. * np.ones((1, 2)) kalman.measurementMatrix = 1. * np.eye(1, 2) # H. input
kalman.processNoiseCov = 1e-5 * np.eye(2) kalman.processNoiseCov = 1e-5 * np.eye(2) # Q. input
kalman.measurementNoiseCov = 1e-1 * np.ones((1, 1)) kalman.measurementNoiseCov = 1e-1 * np.ones((1, 1)) # R. input
kalman.errorCovPost = 1. * np.ones((2, 2)) kalman.errorCovPost = 1. * np.eye(2, 2) # P._k|k KF state var
kalman.statePost = 0.1 * np.random.randn(2, 1) kalman.statePost = 0.1 * np.random.randn(2, 1) # x^_k|k KF state var
while True: while True:
def calc_point(angle): def calc_point(angle):
return (np.around(img_width/2 + img_width/3*cos(angle), 0).astype(int), return (np.around(img_width / 2. + img_width / 3.0 * cos(angle), 0).astype(int),
np.around(img_height/2 - img_width/3*sin(angle), 1).astype(int)) np.around(img_height / 2. - img_width / 3.0 * sin(angle), 1).astype(int))
img = img * 1e-3
state_angle = state[0, 0] state_angle = state[0, 0]
state_pt = calc_point(state_angle) state_pt = calc_point(state_angle)
# advance Kalman filter to next timestep
# updates statePre, statePost, errorCovPre, errorCovPost
# k-> k+1, x'(k) = A*x(k)
# P'(k) = temp1*At + Q
prediction = kalman.predict() prediction = kalman.predict()
predict_angle = prediction[0, 0]
predict_pt = calc_point(predict_angle)
measurement = kalman.measurementNoiseCov * np.random.randn(1, 1)
predict_pt = calc_point(prediction[0, 0]) # equivalent to calc_point(kalman.statePre[0,0])
# generate measurement # generate measurement
measurement = kalman.measurementNoiseCov * np.random.randn(1, 1)
measurement = np.dot(kalman.measurementMatrix, state) + measurement measurement = np.dot(kalman.measurementMatrix, state) + measurement
measurement_angle = measurement[0, 0] measurement_angle = measurement[0, 0]
measurement_pt = calc_point(measurement_angle) measurement_pt = calc_point(measurement_angle)
# plot points # correct the state estimates based on measurements
def draw_cross(center, color, d): # updates statePost & errorCovPost
cv.line(img,
(center[0] - d, center[1] - d), (center[0] + d, center[1] + d),
color, 1, cv.LINE_AA, 0)
cv.line(img,
(center[0] + d, center[1] - d), (center[0] - d, center[1] + d),
color, 1, cv.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)
cv.line(img, state_pt, measurement_pt, (0, 0, 255), 3, cv.LINE_AA, 0)
cv.line(img, state_pt, predict_pt, (0, 255, 255), 3, cv.LINE_AA, 0)
kalman.correct(measurement) kalman.correct(measurement)
improved_pt = calc_point(kalman.statePost[0, 0])
process_noise = sqrt(kalman.processNoiseCov[0,0]) * np.random.randn(2, 1) # plot points
state = np.dot(kalman.transitionMatrix, state) + process_noise cv.drawMarker(img, measurement_pt, (0, 0, 255), cv.MARKER_SQUARE, 5, 2)
cv.drawMarker(img, predict_pt, (0, 255, 255), cv.MARKER_SQUARE, 5, 2)
cv.drawMarker(img, improved_pt, (0, 255, 0), cv.MARKER_SQUARE, 5, 2)
cv.drawMarker(img, state_pt, (255, 255, 255), cv.MARKER_STAR, 10, 1)
# forecast one step
cv.drawMarker(img, calc_point(np.dot(kalman.transitionMatrix, kalman.statePost)[0, 0]),
(255, 255, 0), cv.MARKER_SQUARE, 12, 1)
cv.line(img, state_pt, measurement_pt, (0, 0, 255), 1, cv.LINE_AA, 0) # red measurement error
cv.line(img, state_pt, predict_pt, (0, 255, 255), 1, cv.LINE_AA, 0) # yellow pre-meas error
cv.line(img, state_pt, improved_pt, (0, 255, 0), 1, cv.LINE_AA, 0) # green post-meas error
# update the real process
process_noise = sqrt(kalman.processNoiseCov[0, 0]) * np.random.randn(2, 1)
state = np.dot(kalman.transitionMatrix, state) + process_noise # x_k+1 = F x_k + w_k
cv.imshow("Kalman", img) cv.imshow("Kalman", img)
code = cv.waitKey(1000)
code = cv.waitKey(100)
if code != -1: if code != -1:
break break