粒子滤波C++程序.docx

1、void CFastTrackingDlg:OnBnClickedButton4()/ TODO: Add your control notification handler code here/1.condensation setup const int stateNum=4; const int measureNum=2; const int sampleNum=2000; CvConDensation* condens = cvCreateConDensation(stateNum,measureNum,sampleNum); CvMat* lowerBound; CvMat* uppe

2、rBound; lowerBound = cvCreateMat(stateNum, 1, CV_32F); upperBound = cvCreateMat(stateNum, 1, CV_32F); cvmSet(lowerBound,0,0,0.0 ); cvmSet(upperBound,0,0,winWidth ); cvmSet(lowerBound,1,0,0.0 ); cvmSet(upperBound,1,0,winHeight ); cvmSet(lowerBound,2,0,0.0 ); cvmSet(upperBound,2,0,0.0 ); cvmSet(lowerB

3、ound,3,0,0.0 ); cvmSet(upperBound,3,0,0.0 ); float AstateNumstateNum = 1,0,1,0, 0,1,0,1, 0,0,1,0, 0,0,0,1 ; memcpy(condens-DynamMatr,A,sizeof(A); cvConDensInitSampleSet(condens, lowerBound, upperBound); CvRNG rng_state = cvRNG(0xffffffff); for(int i=0; i flSamplesi0 = float(cvRandInt( &rng_state ) %

4、 winWidth); /width condens-flSamplesi1 = float(cvRandInt( &rng_state ) % winHeight);/height CvFont font; cvInitFont(&font,CV_FONT_HERSHEY_SCRIPT_COMPLEX,1,1); char* winName=condensation; cvNamedWindow(winName); cvSetMouseCallback(winName,mouseEvent); IplImage* img=cvCreateImage(cvSize(winWidth,winHe

5、ight),8,3); bool isPredictOnly=false;/trigger for prediction only,press SPACEBAR while (1) /2.condensation prediction CvPoint predict_pt=cvPoint(int)condens-State0,(int)condens-State1); float variancemeasureNum=0; /get variance/standard deviation of each state for (int i=0;imeasureNum;i+) /sum float

6、 sumState=0; for (int j=0;jSamplesNum;j+) sumState+=condens-flSamplesji; /average sumState/=sampleNum; /variance for (int j=0;jSamplesNum;j+) variancei+=(condens-flSamplesji-sumState)* (condens-flSamplesji-sumState); variancei/=sampleNum-1; /3.update particals confidence CvPoint pt; if (isPredictOnl

7、y) pt=predict_pt; else pt=mousePosition; for (int i=0;iSamplesNum;i+) float probX=(float)exp(-1*(pt.x-condens-flSamplesi0) *(pt.x-condens-flSamplesi0)/(2*variance0); float probY=(float)exp(-1*(pt.y-condens-flSamplesi1) *(pt.y-condens-flSamplesi1)/(2*variance1); condens-flConfidencei=probX*probY; /4.

8、update condensation cvConDensUpdateByTime(condens); /draw cvSet(img,cvScalar(255,255,255,0); cvCircle(img,predict_pt,5,CV_RGB(0,255,0),3);/predicted point with green char buf256; sprintf_s(buf,256,predicted position:(%3d,%3d),predict_pt.x,predict_pt.y); cvPutText(img,buf,cvPoint(10,30),&font,CV_RGB(

9、0,0,0); if (!isPredictOnly) cvCircle(img,mousePosition,5,CV_RGB(255,0,0),3);/current position with red sprintf_s(buf,256,real position :(%3d,%3d),mousePosition.x,mousePosition.y); cvPutText(img,buf,cvPoint(10,60),&font,CV_RGB(0,0,0); cvShowImage(winName, img); int key=cvWaitKey(30); if (key=27)/esc break; else if (key= ) /trigger for prediction /isPredict=!isPredict; if (isPredictOnly) isPredictOnly=false; else isPredictOnly=true; cvReleaseImage(&img); cvReleaseConDensation(&condens);