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. OK, I Understand
 
Top