生活随笔
收集整理的這篇文章主要介紹了
粒子滤波 演示与opencv代码
小編覺得挺不錯的,現(xiàn)在分享給大家,幫大家做個參考.
粒子濾波的理論實在是太美妙了，用一組不同權(quán)重的隨機狀態(tài)來逼近復(fù)雜的概率密度函數(shù)。其再非線性、非高斯系統(tǒng)中具有優(yōu)良的特性。opencv給出了一個實現(xiàn)，但是沒有給出范例，學(xué)習(xí)過程中發(fā)現(xiàn)網(wǎng)絡(luò)上也找不到。learning opencv一書中有介紹，但距離直接使用還是有些距離。在經(jīng)過一番坎坷后，終于可以用了，希望對你有幫助。
?
?本文中給出的例子跟 我的另一篇博文是同一個應(yīng)用例子，都是對二維坐標(biāo)進行平滑、預(yù)測
使用方法：
1.創(chuàng)建并初始化
const int stateNum=4;//狀態(tài)數(shù)
?const int measureNum=2;//測量變量數(shù)
?const int sampleNum=2000;//粒子數(shù)
?CvConDensation* condens = cvCreateConDensation(stateNum,measureNum,sampleNum);
在不影響性能的情況下，粒子數(shù)量越大，系統(tǒng)表現(xiàn)的越穩(wěn)定
其他初始化內(nèi)容請參考learning opencv
2.預(yù)測
3.更新例子可信度，也就是權(quán)重。本例中更新方法與learning opencv中有所不同，想看代碼?
4.更新CvConDensation
?
代碼：
[cpp]?view plain
?copy #include?<cv.h>?? #include?<cxcore.h>?? #include?<highgui.h>?? #include?<cvaux.h>?? ?? #include?<cmath>?? #include?<vector>?? #include?<iostream>?? using?namespace?std;?? ?? const?int?winHeight=600;?? const?int?winWidth=800;?? ?? ?? CvPoint?mousePosition=cvPoint(winWidth>>1,winHeight>>1);?? ?? ?? void?mouseEvent(int?event,int?x,int?y,int?flags,void?*param?)?? {?? ????if?(event==CV_EVENT_MOUSEMOVE)?{?? ????????mousePosition=cvPoint(x,y);?? ????}?? }?? ?? int?main?(void)?? {?? ?????? ????const?int?stateNum=4;?? ????const?int?measureNum=2;?? ????const?int?sampleNum=2000;?? ?? ????CvConDensation*?condens?=?cvCreateConDensation(stateNum,measureNum,sampleNum);?? ????CvMat*?lowerBound;?? ????CvMat*?upperBound;?? ????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(lowerBound,3,0,0.0?);??? ????cvmSet(upperBound,3,0,0.0?);?? ????float?A[stateNum][stateNum]?={?? ????????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?<?sampleNum;?i++){?? ????????condens->flSamples[i][0]?=?float(cvRandInt(?&rng_state?)?%?winWidth);??? ????????condens->flSamples[i][1]?=?float(cvRandInt(?&rng_state?)?%?winHeight);?? ????}?? ?? ????CvFont?font;?? ????cvInitFont(&font,CV_FONT_HERSHEY_SCRIPT_COMPLEX,1,1);?? ?? ????char*?winName="condensation";?? ????cvNamedWindow(winName);?? ????cvSetMouseCallback(winName,mouseEvent);?? ????IplImage*?img=cvCreateImage(cvSize(winWidth,winHeight),8,3);?? ????bool?isPredictOnly=false;?? ????while?(1){?? ?????????? ????????CvPoint?predict_pt=cvPoint((int)condens->State[0],(int)condens->State[1]);?? ?? ????????float?variance[measureNum]={0};??????? ?????????? ????????for?(int?i=0;i<measureNum;i++)?{?? ?????????????? ????????????float?sumState=0;?? ????????????for?(int?j=0;j<condens->SamplesNum;j++)?{?? ????????????????sumState+=condens->flSamples[i][j];?? ????????????}?? ?????????????? ????????????sumState/=sampleNum;?? ?????????????? ????????????for?(int?j=0;j<condens->SamplesNum;j++)?{?? ????????????????variance[i]+=(condens->flSamples[i][j]-sumState)*?? ????????????????????(condens->flSamples[i][j]-sumState);?? ????????????}?? ????????????variance[i]/=sampleNum-1;?? ????????}?? ?????????? ????????CvPoint?pt;?? ????????if?(isPredictOnly)?{?? ????????????pt=predict_pt;?? ????????}else{?? ????????????pt=mousePosition;?? ????????}?? ????????for?(int?i=0;i<condens->SamplesNum;i++)?{?? ????????????float?probX=(float)exp(-1*(pt.x-condens->flSamples[i][0])?? ????????????????*(pt.x-condens->flSamples[i][0])/(2*variance[0]));?? ????????????float?probY=(float)exp(-1*(pt.y-condens->flSamples[i][1])?? ????????????????*(pt.y-condens->flSamples[i][1])/(2*variance[1]));?? ????????????condens->flConfidence[i]=probX*probY;?? ????????}?? ?????????? ????????cvConDensUpdateByTime(condens);?? ?????????? ?????????? ????????cvSet(img,cvScalar(255,255,255,0));?? ????????cvCircle(img,predict_pt,5,CV_RGB(0,255,0),3);?? ????????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));?? ????????if?(!isPredictOnly)?{?? ????????????cvCircle(img,mousePosition,5,CV_RGB(255,0,0),3);?? ????????????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){?? ????????????break;?? ????????}else?if?(key=='?')?{?? ?????????????? ????????????if?(isPredictOnly)?{?? ????????????????isPredictOnly=false;?? ????????????}else{?? ????????????????isPredictOnly=true;?? ????????????}?? ????????}?? ????}???????? ?? ????cvReleaseImage(&img);?? ????cvReleaseConDensation(&condens);?? ????return?0;?? }??
?
kalman filter 視頻演示：
演示中粒子數(shù)分別為100,200,2000
請仔細(xì)觀測效果
http://v.youku.com/v_show/id_XMjU4MzE0ODgw.html
demo snapshot：
總結(jié)
以上是生活随笔為你收集整理的粒子滤波 演示与opencv代码的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。
如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。
