• API
• FAQ
• Tools
• Archive
daily pastebin goal
63%
SHARE
TWEET

# KalmanFilterExampleSimple

orgicus Jul 31st, 2013 80 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
1. #include "stdafx.h"
2. #include <opencv2/highgui/highgui.hpp>
3. #include <opencv2/video/tracking.hpp>
4.
5. using namespace cv;
6. using namespace std;
7.
8. // plot points
9. #define drawCross( center, color, d )                                 \
10. line( img, Point( center.x - d, center.y - d ),                \
11. Point( center.x + d, center.y + d ), color, 2, CV_AA, 0); \
12. line( img, Point( center.x + d, center.y - d ),                \
13. Point( center.x - d, center.y + d ), color, 2, CV_AA, 0 )
14.
15. struct mouse_info_struct { int x,y; };
16. struct mouse_info_struct mouse_info = {-1,-1};
17.
18. vector<Point> mousev,kalmanv;
19.
20. KalmanFilter KF;
21.
22. void on_mouse(int event, int x, int y, int flags, void* param) {
23.         {
24.                 mouse_info.x = x;
25.                 mouse_info.y = y;
26.         }
27. }
28.
29. void reset(){
30.
31.         /*
32.         KF.statePre.at<float>(0) = mouse_info.x;
33.         KF.statePre.at<float>(1) = mouse_info.y;
34.         KF.statePre.at<float>(2) = 0;
35.         KF.statePre.at<float>(3) = 0;
36.         KF.transitionMatrix = *(Mat_<float>(4, 4) << 1,0,0,0,   0,1,0,0,  0,0,1,0,  0,0,0,1);
37.         //*/
38.     setIdentity(KF.measurementMatrix);
39.     setIdentity(KF.processNoiseCov, Scalar::all(1e-4));
40.     setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));
41.     setIdentity(KF.errorCovPost, Scalar::all(.1));
42.
43.         mousev.clear();
44.         kalmanv.clear();
45. }
46.
47. int _tmain(int argc, _TCHAR* argv[])
48. {
49.         Mat img(500, 500, CV_8UC3);
50.         KF = *(new KalmanFilter(4, 2, 0));
51.     //Mat_<float> state(4, 1); //(x, y, Vx, Vy)
52.     //Mat processNoise(4, 1, CV_32F);
53.     Mat_<float> measurement(2,1); measurement.setTo(Scalar(0));
54.     char code = (char)-1;
55.
56.         namedWindow("mk");
57.         setMouseCallback("mk", on_mouse, 0);
58.         reset();
59.         for(;;)
60.     {
61.                 KF.predict();
62.
63.         measurement(0) = mouse_info.x;
64.                 measurement(1) = mouse_info.y;
65.
66.                 Point measPt(measurement(0),measurement(1));
67.                 mousev.push_back(measPt);
68.
69.                 Mat estimated = KF.correct(measurement);
70.                 Point statePt(estimated.at<float>(0),estimated.at<float>(1));
71.                 kalmanv.push_back(statePt);
72.
73.         img = Scalar::all(0);
74.         drawCross( statePt, Scalar(255,255,255), 5 );
75.         drawCross( measPt, Scalar(0,0,255), 5 );
76.
77.                 for (int i = 0; i < mousev.size()-1; i++) {
78.                         line(img, mousev[i], mousev[i+1], Scalar(255,255,0), 1);
79.                 }
80.                 for (int i = 0; i < kalmanv.size()-1; i++) {
81.                         line(img, kalmanv[i], kalmanv[i+1], Scalar(0,255,0), 1);
82.                 }
83.
84.         imshow( "mk", img );
85.         code = (char)waitKey(100);
86.
87.                 if( code > 0) reset();
88.         if( code == 27) break;
89.     }
90.         return 0;
91. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.

Top