粒子滤波C++程序.docx
- 文档编号:2495867
- 上传时间:2022-10-30
- 格式:DOCX
- 页数:3
- 大小:20.40KB
粒子滤波C++程序.docx
《粒子滤波C++程序.docx》由会员分享,可在线阅读,更多相关《粒子滤波C++程序.docx(3页珍藏版)》请在冰豆网上搜索。
voidCFastTrackingDlg:
:
OnBnClickedButton4()
{
//TODO:
Addyourcontrolnotificationhandlercodehere
//1.condensationsetup
constintstateNum=4;
constintmeasureNum=2;
constintsampleNum=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);
floatA[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);
CvRNGrng_state=cvRNG(0xffffffff);
for(inti=0;i condens->flSamples[i][0]=float(cvRandInt(&rng_state)%winWidth);//width condens->flSamples[i][1]=float(cvRandInt(&rng_state)%winHeight);//height } CvFontfont; 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); boolisPredictOnly=false;//triggerforpredictiononly,pressSPACEBAR while (1){ //2.condensationprediction CvPointpredict_pt=cvPoint((int)condens->State[0],(int)condens->State[1]); floatvariance[measureNum]={0}; //getvariance/standarddeviationofeachstate for(inti=0;i { //sum floatsumState=0; for(intj=0;j { sumState+=condens->flSamples[j][i]; } //average sumState/=sampleNum; //variance for(intj=0;j { variance[i]+=(condens->flSamples[j][i]-sumState)* (condens->flSamples[j][i]-sumState); } variance[i]/=sampleNum-1; } //3.updateparticalsconfidence CvPointpt; if(isPredictOnly) { pt=predict_pt; } else { pt=mousePosition; } for(inti=0;i { floatprobX=(float)exp(-1*(pt.x-condens->flSamples[i][0]) *(pt.x-condens->flSamples[i][0])/(2*variance[0])); floatprobY=(float)exp(-1*(pt.y-condens->flSamples[i][1]) *(pt.y-condens->flSamples[i][1])/(2*variance[1])); condens->flConfidence[i]=probX*probY; } //4.updatecondensation cvConDensUpdateByTime(condens); //draw cvSet(img,cvScalar(255,255,255,0)); cvCircle(img,predict_pt,5,CV_RGB(0,255,0),3);//predictedpointwithgreen charbuf[256]; sprintf_s(buf,256,"predictedposition: (%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);//currentpositionwithred sprintf_s(buf,256,"realposition: (%3d,%3d)",mousePosition.x,mousePosition.y); cvPutText(img,buf,cvPoint(10,60),&font,CV_RGB(0,0,0)); } cvShowImage(winName,img); intkey=cvWaitKey(30); if(key==27){//esc break; } elseif(key=='') {//triggerforprediction //isPredict=! isPredict; if(isPredictOnly) { isPredictOnly=false; } else { isPredictOnly=true; } } } cvReleaseImage(&img); cvReleaseConDensation(&condens); }
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- 粒子 滤波 C+ 程序
![提示](https://static.bdocx.com/images/bang_tan.gif)