粒子滤波 演示与opencv代码
转自http://blog.csdn.net/onezeros/article/details/6319180
粒子滤波的理论实在是太美妙了，用一组不同权重的随机状态来逼近复杂的概率密度函数。其再非线性、非高斯系统中具有优良的特性。opencv给出了一个实现，但是没有给出范例，学习过程中发现网络上也找不到。learning opencv一书中有介绍，但距离直接使用还是有些距离。在经过一番坎坷后，终于可以用了，希望对你有帮助。
本文中给出的例子跟 我的另一篇博文是同一个应用例子，都是对二维坐标进行平滑、预测
使用方法：
1.创建并初始化
const int stateNum=4;//状态数
const int measureNum=2;//测量变量数
const int sampleNum=2000;//粒子数
CvConDensation* condens = cvCreateConDensation(stateNum,measureNum,sampleNum);
在不影响性能的情况下，粒子数量越大，系统表现的越稳定
其他初始化内容请参考learning opencv
2.预测
3.更新例子可信度，也就是权重。本例中更新方法与learning opencv中有所不同，想看代码
4.更新CvConDensation
代码：
1 #include <cv.h> 2 #include <cxcore.h> 3 #include <highgui.h> 4 #include <cvaux.h> 5 #include<string.h> 6 #include <cmath> 7 #include <vector> 8 #include <iostream> 9 #include <stdlib.h> 10 using namespace std; 11 const int winHeight=600; 12 const int winWidth=800; 13 14 15 CvPoint mousePosition=cvPoint(winWidth>>1,winHeight>>1); 16 17 //mouse event callback 18 void mouseEvent(int event,int x,int y,int flags,void *param ) 19 { 20 if (event==CV_EVENT_MOUSEMOVE) { 21 mousePosition=cvPoint(x,y); 22 } 23 } 24 25 int main (void) 26 { 27 //1.condensation setup 28 const int stateNum=4; 29 const int measureNum=2; 30 const int sampleNum=2000; 31 32 CvConDensation* condens = cvCreateConDensation(stateNum,measureNum,sampleNum); 33 CvMat* lowerBound; 34 CvMat* upperBound; 35 lowerBound = cvCreateMat(stateNum, 1, CV_32F); 36 upperBound = cvCreateMat(stateNum, 1, CV_32F); 37 cvmSet(lowerBound,0,0,0.0 ); 38 cvmSet(upperBound,0,0,winWidth ); 39 cvmSet(lowerBound,1,0,0.0 ); 40 cvmSet(upperBound,1,0,winHeight ); 41 cvmSet(lowerBound,2,0,0.0 ); 42 cvmSet(upperBound,2,0,0.0 ); 43 cvmSet(lowerBound,3,0,0.0 ); 44 cvmSet(upperBound,3,0,0.0 ); 45 float A[stateNum][stateNum] ={ 46 1,0,1,0, 47 0,1,0,1, 48 0,0,1,0, 49 0,0,0,1 50 }; 51 memcpy(condens->DynamMatr,A,sizeof(A)); 52 cvConDensInitSampleSet(condens, lowerBound, upperBound); 53 54 CvRNG rng_state = cvRNG(0xffffffff); 55 for(int i=0; i < sampleNum; i++){ 56 condens->flSamples[i][0] = float(cvRandInt( &rng_state ) % winWidth); //width 57 condens->flSamples[i][1] = float(cvRandInt( &rng_state ) % winHeight);//height 58 } 59 60 CvFont font; 61 cvInitFont(&font,CV_FONT_HERSHEY_SCRIPT_COMPLEX,1,1); 62 63 char* winName="condensation"; 64 cvNamedWindow(winName); 65 cvSetMouseCallback(winName,mouseEvent); 66 IplImage* img=cvCreateImage(cvSize(winWidth,winHeight),8,3); 67 bool isPredictOnly=false;//trigger for prediction only,press SPACEBAR 68 while (1){ 69 //2.condensation prediction 70 CvPoint predict_pt=cvPoint((int)condens->State[0],(int)condens->State[1]); 71 72 float variance[measureNum]={0}; 73 //get variance/standard deviation of each state 74 for (int i=0;i<measureNum;i++) { 75 //sum 76 float sumState=0; 77 for (int j=0;j<condens->SamplesNum;j++) { 78 sumState+=condens->flSamples[i][j]; 79 } 80 //average 81 sumState/=sampleNum; 82 //variance 83 for (int j=0;j<condens->SamplesNum;j++) { 84 variance[i]+=(condens->flSamples[i][j]-sumState)* 85 (condens->flSamples[i][j]-sumState); 86 } 87 variance[i]/=sampleNum-1; 88 } 89 //3.update particals confidence 90 CvPoint pt; 91 if (isPredictOnly) { 92 pt=predict_pt; 93 }else{ 94 pt=mousePosition; 95 } 96 for (int i=0;i<condens->SamplesNum;i++) { 97 float probX=(float)exp(-1*(pt.x-condens->flSamples[i][0]) 98 *(pt.x-condens->flSamples[i][0])/(2*variance[0])); 99 float probY=(float)exp(-1*(pt.y-condens->flSamples[i][1]) 100 *(pt.y-condens->flSamples[i][1])/(2*variance[1])); 101 condens->flConfidence[i]=probX*probY; 102 } 103 //4.update condensation 104 cvConDensUpdateByTime(condens); 105 106 //draw 107 cvSet(img,cvScalar(255,255,255,0)); 108 cvCircle(img,predict_pt,5,CV_RGB(0,255,0),3);//predicted point with green 109 char buf[256]; 110 sprintf(buf,"predicted position:(%3d,%3d)",predict_pt.x,predict_pt.y); 111 //sprintf(buf,) 112 cvPutText(img,buf,cvPoint(10,30),&font,CV_RGB(0,0,0)); 113 if (!isPredictOnly) { 114 cvCircle(img,mousePosition,5,CV_RGB(255,0,0),3);//current position with red 115 sprintf(buf,"real position :(%3d,%3d)",mousePosition.x,mousePosition.y); 116 cvPutText(img,buf,cvPoint(10,60),&font,CV_RGB(0,0,0)); 117 } 118 119 cvShowImage(winName, img); 120 int key=cvWaitKey(30); 121 if (key==27){//esc 122 break; 123 }else if (key==' ') {//trigger for prediction 124 //isPredict=!isPredict; 125 if (isPredictOnly) { 126 isPredictOnly=false; 127 }else{ 128 isPredictOnly=true; 129 } 130 } 131 } 132 133 cvReleaseImage(&img); 134 cvReleaseConDensation(&condens); 135 return 0; 136 }
kalman filter 视频演示：
演示中粒子数分别为100,200,2000
请仔细观测效果
http://v.youku.com/v_show/id_XMjU4MzE0ODgw.html
demo snapshot：
