1 #include "opencv2/video/tracking.hpp"
2 #include "opencv2/highgui/highgui.hpp"
8 static inline Point calcPoint(Point2f center, double R, double angle)
10 return center + Point2f((float)cos(angle), (float)-sin(angle))*(float)R;
15 printf( "\nExample of c calls to OpenCV's Kalman filter.\n"
16 " Tracking of rotating point.\n"
17 " Rotation speed is constant.\n"
18 " Both state and measurements vectors are 1D (a point angle),\n"
19 " Measurement is the real point angle + gaussian noise.\n"
20 " The real and the estimated points are connected with yellow line segment,\n"
21 " the real and the measured points are connected with red line segment.\n"
22 " (if Kalman filter works correctly,\n"
23 " the yellow segment should be shorter than the red one).\n"
25 " Pressing any key (except ESC) will reset the tracking with a different speed.\n"
26 " Pressing ESC will stop the program.\n"
33 Mat img(500, 500, CV_8UC3);
34 KalmanFilter KF(2, 1, 0);
35 Mat state(2, 1, CV_32F); /* (phi, delta_phi) */
36 Mat processNoise(2, 1, CV_32F);
37 Mat measurement = Mat::zeros(1, 1, CV_32F);
42 randn( state, Scalar::all(0), Scalar::all(0.1) );
43 KF.transitionMatrix = (Mat_<float>(2, 2) << 1, 1, 0, 1);
45 setIdentity(KF.measurementMatrix);
46 setIdentity(KF.processNoiseCov, Scalar::all(1e-5));
47 setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));
48 setIdentity(KF.errorCovPost, Scalar::all(1));
50 randn(KF.statePost, Scalar::all(0), Scalar::all(0.1));
54 Point2f center(img.cols*0.5f, img.rows*0.5f);
55 float R = img.cols/3.f;
56 double stateAngle = state.at<float>(0);
57 Point statePt = calcPoint(center, R, stateAngle);
59 Mat prediction = KF.predict();
60 double predictAngle = prediction.at<float>(0);
61 Point predictPt = calcPoint(center, R, predictAngle);
63 randn( measurement, Scalar::all(0), Scalar::all(KF.measurementNoiseCov.at<float>(0)));
65 // generate measurement
66 measurement += KF.measurementMatrix*state;
68 double measAngle = measurement.at<float>(0);
69 Point measPt = calcPoint(center, R, measAngle);
72 #define drawCross( center, color, d ) \
73 line( img, Point( center.x - d, center.y - d ), \
74 Point( center.x + d, center.y + d ), color, 1, LINE_AA, 0); \
75 line( img, Point( center.x + d, center.y - d ), \
76 Point( center.x - d, center.y + d ), color, 1, LINE_AA, 0 )
79 drawCross( statePt, Scalar(255,255,255), 3 );
80 drawCross( measPt, Scalar(0,0,255), 3 );
81 drawCross( predictPt, Scalar(0,255,0), 3 );
82 line( img, statePt, measPt, Scalar(0,0,255), 3, LINE_AA, 0 );
83 line( img, statePt, predictPt, Scalar(0,255,255), 3, LINE_AA, 0 );
85 if(theRNG().uniform(0,4) != 0)
86 KF.correct(measurement);
88 randn( processNoise, Scalar(0), Scalar::all(sqrt(KF.processNoiseCov.at<float>(0, 0))));
89 state = KF.transitionMatrix*state + processNoise;
91 imshow( "Kalman", img );
92 code = (char)waitKey(100);
97 if( code == 27 || code == 'q' || code == 'Q' )