1#include "opencv2/video/tracking.hpp"
2#include "opencv2/highgui/highgui.hpp"
3
4#include <stdio.h>
5
6using namespace cv;
7
8static inline Point calcPoint(Point2f center, double R, double angle)
9{
10    return center + Point2f((float)cos(angle), (float)-sin(angle))*(float)R;
11}
12
13static void help()
14{
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"
24            "\n"
25"   Pressing any key (except ESC) will reset the tracking with a different speed.\n"
26"   Pressing ESC will stop the program.\n"
27            );
28}
29
30int main(int, char**)
31{
32    help();
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);
38    char code = (char)-1;
39
40    for(;;)
41    {
42        randn( state, Scalar::all(0), Scalar::all(0.1) );
43        KF.transitionMatrix = (Mat_<float>(2, 2) << 1, 1, 0, 1);
44
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));
49
50        randn(KF.statePost, Scalar::all(0), Scalar::all(0.1));
51
52        for(;;)
53        {
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);
58
59            Mat prediction = KF.predict();
60            double predictAngle = prediction.at<float>(0);
61            Point predictPt = calcPoint(center, R, predictAngle);
62
63            randn( measurement, Scalar::all(0), Scalar::all(KF.measurementNoiseCov.at<float>(0)));
64
65            // generate measurement
66            measurement += KF.measurementMatrix*state;
67
68            double measAngle = measurement.at<float>(0);
69            Point measPt = calcPoint(center, R, measAngle);
70
71            // plot points
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 )
77
78            img = Scalar::all(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 );
84
85            if(theRNG().uniform(0,4) != 0)
86                KF.correct(measurement);
87
88            randn( processNoise, Scalar(0), Scalar::all(sqrt(KF.processNoiseCov.at<float>(0, 0))));
89            state = KF.transitionMatrix*state + processNoise;
90
91            imshow( "Kalman", img );
92            code = (char)waitKey(100);
93
94            if( code > 0 )
95                break;
96        }
97        if( code == 27 || code == 'q' || code == 'Q' )
98            break;
99    }
100
101    return 0;
102}
103