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

 

代码：

#include
#include
#include
#include

#include
#include
#include
using namespace std;

const int winHeight=600;
const int winWidth=800;


CvPoint mousePosition=cvPoint(winWidth>>1,winHeight>>1);

//mouse event callback
void 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 setup
    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); //width
        condens->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 SPACEBAR
    while (1){
        //2.condensation prediction
        CvPoint predict_pt=cvPoint((int)condens->State[0],(int)condens->State[1]);

        float variance[measureNum]={0};
        //get variance/standard deviation of each state
        for (int i=0;i
            //sum
            float sumState=0;
            for (int j=0;jSamplesNum;j++) {
                sumState+=condens->flSamples[i][j];
            }
            //average
            sumState/=sampleNum;
            //variance
            for (int j=0;jSamplesNum;j++) {
                variance[i]+=(condens->flSamples[i][j]-sumState)*
                    (condens->flSamples[i][j]-sumState);
            }
            variance[i]/=sampleNum-1;
        }
        //3.update particals confidence
        CvPoint pt;
        if (isPredictOnly) {
            pt=predict_pt;
        }else{
            pt=mousePosition;
        }
        for (int i=0;iSamplesNum;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 condensation
        cvConDensUpdateByTime(condens);

        //draw
        cvSet(img,cvScalar(255,255,255,0));
        cvCircle(img,predict_pt,5,CV_RGB(0,255,0),3);//predicted point with green
        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);//current position with red
            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){//esc
            break;
        }else if (key==' ') {//trigger for prediction
            //isPredict=!isPredict;
            if (isPredictOnly) {
                isPredictOnly=false;
            }else{
                isPredictOnly=true;
            }
        }
    }

    cvReleaseImage(&img);
    cvReleaseConDensation(&condens);
    return 0;
}

#include #include #include #include #include #include #include using namespace std; const int winHeight=600; const int winWidth=800; CvPoint mousePosition=cvPoint(winWidth>>1,winHeight>>1); //mouse event callback void 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 setup 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); //width condens->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 SPACEBAR while (1){ //2.condensation prediction CvPoint predict_pt=cvPoint((int)condens->State[0],(int)condens->State[1]); float variance[measureNum]={0}; //get variance/standard deviation of each state for (int i=0;iSamplesNum;j++) { sumState+=condens->flSamples[i][j]; } //average sumState/=sampleNum; //variance for (int j=0;jSamplesNum;j++) { variance[i]+=(condens->flSamples[i][j]-sumState)* (condens->flSamples[i][j]-sumState); } variance[i]/=sampleNum-1; } //3.update particals confidence CvPoint pt; if (isPredictOnly) { pt=predict_pt; }else{ pt=mousePosition; } for (int i=0;iSamplesNum;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 condensation cvConDensUpdateByTime(condens); //draw cvSet(img,cvScalar(255,255,255,0)); cvCircle(img,predict_pt,5,CV_RGB(0,255,0),3);//predicted point with green 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);//current position with red 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){//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：