卡尔曼滤波器跟踪鼠标

卡尔曼滤波器跟踪鼠标

#include "stdafx.h"

//#include <cv.h>  
///#include <cxcore.h>  
//#include <highgui.h>  
#include "opencv2\opencv.hpp"  
#include <cmath>  
#include <vector>  
#include <iostream>  
using namespace std; 
using namespace cv;
  
const int winHeight=600;  
const int winWidth=800;  
  
  
CvPoint mousePosition=cvPoint(winWidth>>1,winHeight>>1);  
  
//mouse event callback  
void mouseEvent(int event, int x, int y, int flags, void *param )  
{  
    if (event==CV_EVENT_MOUSEMOVE) {  
        mousePosition=cvPoint(x,y);  
    }  
}  
  
int main (void)  
{  
    //1.kalman filter setup  
    const int stateNum=4;  
    const int measureNum=2;  
    CvKalman* kalman = cvCreateKalman( stateNum, measureNum, 0 );//state(x,y,detaX,detaY)  
    CvMat* process_noise = cvCreateMat( stateNum, 1, CV_32FC1 );  
    CvMat* measurement = cvCreateMat( measureNum, 1, CV_32FC1 );//measurement(x,y)  
    CvRNG rng = cvRNG(-1);  
    float A[stateNum][stateNum] ={//transition matrix  
        1,0,1,0,  
        0,1,0,1,  
        0,0,1,0,  
        0,0,0,1  
    };  
  
    memcpy( kalman->transition_matrix->data.fl,A,sizeof(A));  
    cvSetIdentity(kalman->measurement_matrix,cvRealScalar(1) );  
    cvSetIdentity(kalman->process_noise_cov,cvRealScalar(1e-5));  
    cvSetIdentity(kalman->measurement_noise_cov,cvRealScalar(1e-1));  
    cvSetIdentity(kalman->error_cov_post,cvRealScalar(1));  
    //initialize post state of kalman filter at random  
    cvRandArr(&rng,kalman->state_post,CV_RAND_UNI,cvRealScalar(0),cvRealScalar(winHeight>winWidth?winWidth:winHeight));  
  
    CvFont font;  
    cvInitFont(&font,CV_FONT_HERSHEY_SCRIPT_COMPLEX,1,1);  
  
    cvNamedWindow("kalman");  
    cvSetMouseCallback("kalman",mouseEvent);  
    IplImage* img=cvCreateImage(cvSize(winWidth,winHeight),8,3);  
    while (1){  
        //2.kalman prediction  
        const CvMat* prediction=cvKalmanPredict(kalman,0);  
        CvPoint predict_pt=cvPoint((int)prediction->data.fl[0],(int)prediction->data.fl[1]);  
  
        //3.update measurement  
        measurement->data.fl[0]=(float)mousePosition.x;  
        measurement->data.fl[1]=(float)mousePosition.y;  
  
        //4.update  
        cvKalmanCorrect( kalman, measurement );       
  
        //draw   
        cvSet(img,cvScalar(255,255,255,0));  
        cvCircle(img,predict_pt,5,CV_RGB(0,255,0),3);//predicted point with green  
        cvCircle(img,mousePosition,5,CV_RGB(255,0,0),3);//current position with red  
        char buf[256];  
        sprintf_s(buf,256,"predicted position:(%3d,%3d)",predict_pt.x,predict_pt.y);  
        cvPutText(img,buf,cvPoint(10,30),&font,CV_RGB(0,0,0));  
        sprintf_s(buf,256,"current position :(%3d,%3d)",mousePosition.x,mousePosition.y);  
        cvPutText(img,buf,cvPoint(10,60),&font,CV_RGB(0,0,0));  
          
        cvShowImage("kalman", img);  
        int key=cvWaitKey(3);  
        if (key==27){//esc     
            break;     
        }  
    }        
  
    cvReleaseImage(&img);  
    cvReleaseKalman(&kalman);  
    return 0;  

}  

读取鼠标的点，然后用卡尔曼滤波器预估鼠标的位置，并做轨迹。