粒子滤波 演示与opencv代码

[转载]http://blog.csdn.net/onezeros/article/details/6319180

粒子滤波通俗介绍：对于（不限于）非线性非高斯问题，目标是知道x(t-1)的概率确定x(t)的概率。根据目标x(t-1)的概率产生若干粒子，将每个粒子xi放入状态转移方程，产生预测值yi，利用观测值和评价方程计算若x(t)取yi，获得观测值Y的概率，以此条件概率为yi的权重，求yi的权重和即为x(t)的概率的预测值。重采样，去掉权重低的粒子，复制权重高的粒子。反复。

 

对于目标跟踪：

1、选定目标，计算目标的特征，如颜色直方图；

2、洒出粒子，方式有很多，比如目标前方多撒一些，后方少撒一点。

3、计算每个粒子区域的特征，用和1一样的特征，如颜色直方图。

4、比较每个粒子的颜色直方图与目标颜色直方图的相似性，作为权重。

5、粒子坐标的权重和，作为目标的预测位置。

6、重采样，去掉小权重的粒子，复制权重大的粒子，保持粒子数量。即在权重大的地方撒出更多粒子。

7、转入第2步，完成跟踪。

从以上可以看出，粒子越多预测将约准确。

以上为本人的理解，不正确的地方，请高手批评指正。 粒子滤波貌似没有卡尔曼滤波的用观测值的纠正过程。不过计算权重那一步应该起到类似的作用。

 

粒子滤波的理论实在是太美妙了，用一组不同权重的随机状态来逼近复杂的概率密度函数。其再非线性、非高斯系统中具有优良的特性。 

 本文中给出的例子跟 我的另一篇博文是同一个应用例子，都是对二维坐标进行平滑、预测

使用方法：

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

 

代码：

[cpp] view plaincopyprint?

1. #include <cv.h>  
2. #include <cxcore.h>  
3. #include <highgui.h>  
4. #include <cvaux.h>  
5.   
6. #include <cmath>  
7. #include <vector>  
8. #include <iostream>  
9. using namespace std;  
10.   
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_s(buf,256,"predicted position:(%3d,%3d)",predict_pt.x,predict_pt.y);  
111.         cvPutText(img,buf,cvPoint(10,30),&font,CV_RGB(0,0,0));  
112.         if (!isPredictOnly) {  
113.             cvCircle(img,mousePosition,5,CV_RGB(255,0,0),3);//current position with red  
114.             sprintf_s(buf,256,"real position :(%3d,%3d)",mousePosition.x,mousePosition.y);  
115.             cvPutText(img,buf,cvPoint(10,60),&font,CV_RGB(0,0,0));  
116.         }  
117.           
118.         cvShowImage(winName, img);  
119.         int key=cvWaitKey(30);  
120.         if (key==27){//esc     
121.             break;  
122.         }else if (key==' ') {//trigger for prediction  
123.             //isPredict=!isPredict;  
124.             if (isPredictOnly) {  
125.                 isPredictOnly=false;  
126.             }else{  
127.                 isPredictOnly=true;  
128.             }  
129.         }  
130.     }        
131.   
132.     cvReleaseImage(&img);  
133.     cvReleaseConDensation(&condens);  
134.     return 0;  
135. }  

#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);//mouse event callbackvoid mouseEvent(int event,int x,int y,int flags,void *param ){if (event==CV_EVENT_MOUSEMOVE) {mousePosition=cvPoint(x,y);}}int main (void){//1.condensation setupconst 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); //widthcondens->flSamples[i][1] = 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,winHeight),8,3);bool isPredictOnly=false;//trigger for prediction only,press SPACEBARwhile (1){//2.condensation predictionCvPoint predict_pt=cvPoint((int)condens->State[0],(int)condens->State[1]);float variance[measureNum]={0};//get variance/standard deviation of each statefor (int i=0;i<measureNum;i++) {//sumfloat sumState=0;for (int j=0;j<condens->SamplesNum;j++) {sumState+=condens->flSamples[i][j];}//averagesumState/=sampleNum;//variancefor (int j=0;j<condens->SamplesNum;j++) {variance[i]+=(condens->flSamples[i][j]-sumState)*(condens->flSamples[i][j]-sumState);}variance[i]/=sampleNum-1;}//3.update particals confidenceCvPoint 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;}//4.update condensationcvConDensUpdateByTime(condens);//draw cvSet(img,cvScalar(255,255,255,0));cvCircle(img,predict_pt,5,CV_RGB(0,255,0),3);//predicted point with greenchar 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);//current position with redsprintf_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);return 0;}

 

粒子滤波 视频演示：

演示中粒子数分别为100,200,2000

请仔细观测效果

http://v.youku.com/v_show/id_XMjU4MzE0ODgw.html

demo snapshot：