粒子滤波 演示与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：

posted @ 2012-06-09 09:13  lawdong  Views(229)  Comments(0)  Edit  收藏  举报
http://home.cnblogs.com/ing/