OpenCV--粒子滤波算法使用

OpenCV中实现了粒子滤波的代码，位置在opencv\cv\src\cvcondens.cpp文件

粒子滤波跟踪器的数据结构：

typedef struct CvConDensation
{
int MP; // 测量向量的维数： Dimension of measurement vector
int DP; // 状态向量的维数： Dimension of state vector
float* DynamMatr; // 线性动态系统矩阵：Matrix of the linear Dynamics system
float* State; // 状态向量： Vector of State
int SamplesNum; // 粒子数： Number of the Samples
float** flSamples; // 粒子向量数组： array of the Sample Vectors
float** flNewSamples; // 粒子向量临时数组： temporary array of the Sample Vectors
float* flConfidence; // 每个粒子的置信度(译者注：也就是粒子的权值)：Confidence for each Sample
float* flCumulative; // 权值的累计： Cumulative confidence
float* Temp; // 临时向量：Temporary vector
float* RandomSample; // 用来更新粒子集的随机向量： RandomVector to update sample set
CvRandState* RandS; // 产生随机向量的结构数组： Array of structures to generate random vectors
} CvConDensation;

 

与粒子滤波相关的几个函数：
cvCreateConDensation：用于构造上述滤波器数据结构
cvReleaseConDensation：释放滤波器
cvConDensInitSampleSet：初始化粒子集
cvConDensUpdateByTime：更新粒子集

 

粒子滤波分类：

1.经典粒子滤波（Sampling Importance Resampling，SIR)

         补充：序贯重要性采样（sequential importance sampling，SIS)是重要性采样的扩展。

2.正则粒子滤波(Regularized Particle Filter，RPF)

        是为了解决由重采样引入的新问题而提出的一种改进的粒子滤波。当通过序贯重要性采样后引起粒子退化问题时，前面提到可以用重采样的方法来减小退化的影响，但是引入重采样策略同时也引入了新的问题，即粒子匮乏问题，经过若干次迭代之后，所有粒子都趋向于同一个粒子，导致粒子的多样性丧失。这是因为在重采样过程中，粒子是从离散分布中采样取得的，而不是从连续分布中采样得到的。

正则粒子滤波正是为了解决上述问题而提出的。它与SIR粒子滤波的区别在于：在重采样过程中，SIR从离散近似的分布中重采样，而正则粒子滤波则从连续近似的分布中重采样。

根据正则化在选择步骤之前还是之后，RPF分为Post-RPF和Pre-RPF。

3.辅助粒子滤波(Auxiliary Particle Filter, APF)

        Pitt和Shephard在标准SIR滤波算法的基础上提出了辅助粒子滤波。与标准序列重要性重采样(SIR)算法相比，APF也是以序列重要性采样(SIS)算法为基础，只是选择了不同的重要性密度函数。

        与SIR滤波算法相比，辅助粒子滤波算法的优势在于它在k-1时刻的样本集合上随机抽取了一些点，抽取时以当前的观测数据为条件，这样可以更加接近真实的状态。

4.高斯粒子滤波(Gaussian Particle Filter，GPF)

        Jayesh和Petar提出的，将高斯滤波和粒子滤波结合，称为高斯粒子滤波。该方法的前提是用高斯分布来近似后验分布，它比其它的高斯滤波方法适用性更强，能处理更多非线性动态系统问题；而与一般的粒子滤波相比，因为GPF用高斯分布近似后验分布，所以只要所用的高斯分布是正确的，就不会产生粒子退化问题，就不需要对粒子进行重采样，从而使算法的计算量降低，复杂度也降低。

        高斯粒子滤波比其它高斯滤波有更好的性能，而与一般的粒子滤波相比计算量大大减小，复杂度降低。但是高斯滤波在后验分布不能用高斯分布近似的非线性动态空间模型或者非线性系统非加性高斯噪声模型时，滤波性不能令人满意。

5.边缘化粒子滤波（Marginalized Particle Filter，MPF）

 

 

简单示例代码

#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;
}

http://blog.csdn.net/onezeros/article/details/6319180

http://blog.csdn.net/wuxiaoyao12/article/details/7281281

 

另外一个很好的例子：

#include "stdafx.h"
#ifdef _CH_
#pragma package 
#endif
#include "stdafx.h"
#ifndef _EiC
#include "cv.h"
#include "cvAux.h"
#include "highgui.h"
#include "cxcore.h"
#include 
#include 
#endif


// 从图片的x、y坐标处返回相应的色调、饱和度和亮度

int getpixel(IplImage *image, int x, int y, int *h, int *s, int *v){
    *h =(uchar) image->imageData[y *image->widthStep+x * image->nChannels];
    *s =(uchar) image->imageData[y *image->widthStep+ x * image->nChannels + 1];
    *v =(uchar) image->imageData[y *image->widthStep+ x * image->nChannels + 2];
    return 0; 
}
//--------------------------------------------------------------------------------



int main( int argc, char** argv ){
    CvCapture* capture = 0;
    IplImage* image = 0;
    IplImage* HSV = 0;

    if( argc == 1 || (argc == 2 && strlen(argv[1]) == 1 && isdigit(argv[1][0]))){
        capture = cvCaptureFromCAM( argc == 2 ? argv[1][0] - '0' : 0 );
    }
    else if( argc == 2 ){
        capture = cvCaptureFromAVI( argv[1] );
    }

    if( !capture ){
        fprintf(stderr,"Could not initialize capturing...\n");
        return -1;
    }

    printf( "Hot keys: \n"
        "\tESC - quit the program\n");

    //创建Normal窗口

    cvNamedWindow("Normal", CV_WINDOW_AUTOSIZE );


    //Condensation结构体初始化-------------------------------------------------

    int DP=2; // 状态向量的维数

    int MP=2; // 观测向量的维数

    int SamplesNum=300; // 样本粒子的数量


    CvConDensation* ConDens=cvCreateConDensation( DP, MP, SamplesNum );
    //-----------------------------------------------------------------------


    //Condensation结构体中一些参数的初始化-----------------------------------

    CvMat* lowerBound; // 下界

    CvMat* upperBound; // 上界

    lowerBound = cvCreateMat(2, 1, CV_32F);
    upperBound = cvCreateMat(2, 1, CV_32F);
    //设置粒子坐标的上下界为窗口大小640*480

    cvmSet( lowerBound, 0, 0, 0.0 ); cvmSet( upperBound, 0, 0, 640.0 );
    cvmSet( lowerBound, 1, 0, 0.0 ); cvmSet( upperBound, 1, 0, 480.0 );

    cvConDensInitSampleSet(ConDens, lowerBound, upperBound);
    //-----------------------------------------------------------------------


    //设置窗口的中心为追踪的初始点------------------------------

    for(int i=0; i < SamplesNum; i++){
        ConDens->flSamples[i][0]+=320.0;
        ConDens->flSamples[i][1]+=240.0;
    }
    //-----------------------------------------------------------------------


    //迁移矩阵的初始化----------------------------

    ConDens->DynamMatr[0]=1.0;ConDens->DynamMatr[1]=0.0;
    ConDens->DynamMatr[2]=0.0;ConDens->DynamMatr[3]=1.0;
    //-----------------------------------------------------------------------


    for(;;){
        IplImage* frame = 0;
        int c;
        int X,Y,XX,YY;
        int H,S,V;

        frame = cvQueryFrame( capture );
        if( !frame ){
            break;
        }

        if( !image ){
            image = cvCreateImage( cvGetSize(frame), 8, 3 );
            image->origin = frame->origin;
            HSV = cvCreateImage( cvGetSize(frame), 8, 3 );
            HSV->origin = frame->origin;
        }

        cvCopy( frame, image, 0 );
        cvCvtColor(image ,HSV , CV_BGR2HSV);

        //粒子的置信度计算，置信度需要自己建模---------------------------------------------------

        for(int i=0; i < SamplesNum; i++){
            X=(int)ConDens->flSamples[i][0];
            Y=(int)ConDens->flSamples[i][1];

            if(X>=0 && X<=640 && Y>=0 && Y<=480){ //粒子的坐标在窗口范围之内

                getpixel(HSV, X, Y, &H, &S, &V);
                if(H<=19 && S>=48){ // 肤色的判定 //H<=19 S>=48

                    cvCircle(image, cvPoint(X,Y), 4, CV_RGB(255,0,0), 1);
                    ConDens->flConfidence[i]=1.0;
                }
                else{
                    ConDens->flConfidence[i]=0.0;
                }
            }
            else{
                ConDens->flConfidence[i]=0.0;
            }
        }
        //--------------------------------------------------------------------------


        //更新滤波器状态

        cvConDensUpdateByTime(ConDens);

        cvShowImage( "Normal", image );
        c = cvWaitKey(20);

        if( c == 27 ){
            break;
        }
    }

    //释放内存------------------------------------

    cvReleaseImage(&image);
    cvReleaseImage(&HSV);
    cvReleaseConDensation(&ConDens);
    cvReleaseMat( &lowerBound );
    cvReleaseMat( &upperBound );
    cvReleaseCapture( &capture );
    cvDestroyWindow("Normal");
    //---------------------------------------------


    return 0;
}

#ifdef _EiC
main(1,"condensation.cpp");
#endif

http://blog.chinaunix.net/uid-22674777-id-333535.html