学习Opencv2——粒子滤波Condensation算法

                                                                                                              

                                                                                                                    ----------------------------------------------------------------  by Markala 

算法详解

opencv2.3以后，condensation算法放在legacy中了，也就是说要引入下面文件，

#include <opencv2/legacy/legacy.hpp>，

condensation算法的函数原型在” …\OpenCV\sources\modules\legacy\src”中，可以直接搜索condens.cpp。

 

整个Condensation算法只涉及5个变量和函数：CvConDensation、cvCreateConDensation、cvConDensInitSampleSet 、cvConDensUpdateByTime、cvReleaseConDensation。下面一一介绍

CvConDensation结构体

typedef struct CvConDensation
{
    int MP;                /* 测量向量维数  */
    int DP;                /* 状态向量维数  */
    float* DynamMatr;       /* 转移矩阵  */
    float* State;           /* Vector of State                       */
    int SamplesNum;         /* 粒子数                 */
    float** flSamples;      /* 表示粒子的向量             */
    float** flNewSamples;   /* temporary array of the Sample Vectors */
    float* flConfidence;    /* 每个粒子的权重            */
    float* flCumulative;    /* Cumulative confidence                 */
    float* Temp;            /* Temporary vector                      */
    float* RandomSample;    /* RandomVector to update sample set     */
    struct CvRandState* RandS; /* Array of structures to generate random vectors */
} CvConDensation;

这个是粒子结构体，最重要的参数是MP、DP、SamplesNum、flConfidence、flSamples、DynamMatr

ConDens->flConfidence[i] 表示第i个粒子的权重

ConDens->flSamples[i][k] 表示第i个粒子的状态向量的第k维的值

cvCreateConDensation( int DP, int MP, int SamplesNum );

这个函数是创建粒子结构体，只需要定义DP、MP和SameplesNum。

 

cvConDensInitSampleSet(CvConDensation* condens,

                                                           CvMat* lower_bound,  //粒子初始化时，取值下界

                                                           CvMat* upper_bound ); //粒子初始化时，取值上界

      用这个函数对粒子结构体中的其他参数进行初始化，根据粒子滤波的相关知识知道，初始化时将产生一个(lower_bound,upper_bound)范围内均匀分布的点集。

      假设MP=DP=2，也就是说用2×1的向量来表示粒子的状态，那么我们可以这么初始化

float minRange[] = { xmin, ymin };
float maxRange[] = { xmax, ymax };
CvMat LB, UB;
cvInitMatHeader(&LB, 2, 1, CV_32FC1, minRange);
cvInitMatHeader(&UB, 2, 1, CV_32FC1, maxRange);
cvConDensInitSampleSet(condens, &LB, &UB);

或者这样

<span style="font-size:14px;">CvMat* lowerBound; // 下界
CvMat* upperBound; // 上界
lowerBound //创建2行1列的矩阵lowerBound和upperBound
upperBound = cvCreateMat(2, 1, CV_32F);
cvmSet( lowerBound, 0, 0, xmin ); cvmSet( upperBound, 0, 0, xmax );
cvmSet( lowerBound, 1, 0, ymin ); cvmSet( upperBound, 1, 0, ymax );
cvConDensInitSampleSet(ConDens, lowerBound, upperBound);</span>

上面只是初始化了粒子结构体的相关参数，我们还需要对转移矩阵进行初始化，一般这么做

<span style="font-size:14px;">condens->DynamMatr[0] = 1.0; 	condens->DynamMatr[1] = 0.0;
condens->DynamMatr[2] = 0.0; 	condens->DynamMatr[3] = 1.0;</span>

这是DP=MP=2的情形，一般会设置成一个对角阵，可根据实际情况调整。

cvConDensUpdateByTime(ConDens);

    本函数用于更新粒子的状态，主要是权值。在进行更新前，需要自己定义权值的计算方式，也就是给ConDens->flConfidence[i]赋值。

 

cvReleaseConDensation( CvConDensation** condens );

  本函数用于释放内存。

对condens.cpp的分析，参见

http://blog.csdn.net/pp5576155/article/details/6972824

编程步骤

1）定义DP、MP和SameplesNum，创建CvConDensation结构体

      cvCreateConDensation( int DP, int MP, int SamplesNum );

2）初始化，主要是定义LB、UB和DynamMatr[] 

   float minRange[] = { xmin, ymin };
   float maxRange[] = { xmax, ymax };
   CvMat LB, UB;
   cvInitMatHeader(&LB, 2, 1, CV_32FC1, minRange);
   cvInitMatHeader(&UB, 2, 1, CV_32FC1, maxRange);
   cvConDensInitSampleSet(condens, &LB, &UB);
   condens->DynamMatr[0] = 1.0;         condens->DynamMatr[1] = 0.0;
   condens->DynamMatr[2] = 0.0; 	condens->DynamMatr[3] = 1.0;

3）定义权值计算方式，进行更新

cvConDensUpdateByTime(ConDens);

实例代码

例1：

例1实现了在窗口上对鼠标进行检测与跟踪

http://answers.opencv.org/question/6985/syntax-for-particle-filter-in-opencv-243/

// Example of how to use the OpenCV Particle Filter.
// 
// Stolen largely from morethantechnical.com's nice mouse_kalman project.
//

#include <iostream>
#include <vector>

#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/video/tracking.hpp>
#include <opencv2/legacy/legacy.hpp>

using namespace cv;
using namespace std;

#define drawCross( center, color, d )                  \
	line( img,  Point( center.x - d, center.y - d ),           \
	Point( center.x + d, center.y + d ), color, 2, CV_AA, 0);   \
	line( img,  Point( center.x + d, center.y - d ),           \
	 Point( center.x - d, center.y + d ), color, 2, CV_AA, 0 )

struct mouse_info_struct { int x,y; };
struct mouse_info_struct mouse_info = {-1,-1}, last_mouse;

vector< Point> mouseV, particleV;
int counter = -1;

// Define this to proceed one click at a time.
//#define CLICK 1
#define PLOT_PARTICLES 1

void on_mouse(int event, int x, int y, int flags, void* param) {
#ifdef CLICK
	if (event == CV_EVENT_LBUTTONUP) 
#endif
	{
		last_mouse = mouse_info;
		mouse_info.x = x;
		mouse_info.y = y;
		counter = 0;
	}
}

int main (int argc, char * const argv[]) {
	 Mat img(650, 650, CV_8UC3);
	char code = (char)-1;

	 namedWindow("mouse particle");
	 setMouseCallback("mouse particle", on_mouse, 0);

	 Mat_<float> measurement(2,1); 
	measurement.setTo( Scalar(0));

	int dim = 2;
	int nParticles = 25;
	float xRange = 650.0;
	float yRange = 650.0;

	float minRange[] = { 0, 0 };
	float maxRange[] = { xRange, yRange };
	CvMat LB, UB;
	cvInitMatHeader(&LB, 2, 1, CV_32FC1, minRange);
	cvInitMatHeader(&UB, 2, 1, CV_32FC1, maxRange);

	CvConDensation* condens = cvCreateConDensation(dim, dim, nParticles);

	cvConDensInitSampleSet(condens, &LB, &UB);

	// The OpenCV documentation doesn't tell you to initialize this
	// transition matrix, but you have to do it.  For this 2D example, 
	// we're just using a 2x2 identity matrix.  I'm sure there's a slicker 
	// way to do this, left as an exercise for the reader.
	condens->DynamMatr[0] = 1.0;
	condens->DynamMatr[1] = 0.0;
	condens->DynamMatr[2] = 0.0;
	condens->DynamMatr[3] = 1.0;

	for(;;) 
	{
		if (mouse_info.x < 0 || mouse_info.y < 0) 
		{
			imshow("mouse particle", img);
			 waitKey(30);
			continue;
		}

		mouseV.clear();
		particleV.clear();

		for(;;) 
		{
			code = (char) waitKey(100);

			if( code > 0 )
				break;

#ifdef CLICK
			if (counter++ > 0) {
				continue;
			} 
#endif

			measurement(0) = mouse_info.x;
			measurement(1) = mouse_info.y;

			 Point measPt(measurement(0),measurement(1));
			mouseV.push_back(measPt);

			// Clear screen
			img =  Scalar::all(100);

			for (int i = 0; i < condens->SamplesNum; i++) {

				float diffX = (measurement(0) - condens->flSamples[i][0])/xRange;
				float diffY = (measurement(1) - condens->flSamples[i][1])/yRange;

				condens->flConfidence[i] = 1.0 / (sqrt(diffX * diffX + diffY * diffY));

				// plot particles
#ifdef PLOT_PARTICLES
				 Point partPt(condens->flSamples[i][0], condens->flSamples[i][1]);
				drawCross(partPt ,  Scalar(255,0,255), 2);
#endif

			}

			cvConDensUpdateByTime(condens);

			 Point statePt(condens->State[0], condens->State[1]);
			particleV.push_back(statePt);

			// plot points
			drawCross( statePt,  Scalar(255,255,255), 5 );
			drawCross( measPt,  Scalar(0,0,255), 5 );

			for (int i = 0; i < mouseV.size() - 1; i++) {
				line(img, mouseV[i], mouseV[i+1],  Scalar(255,255,0), 1);
			}
			for (int i = 0; i < particleV.size() - 1; i++) {
				line(img, particleV[i], particleV[i+1],  Scalar(0,255,0), 1);
			}

			imshow( "mouse particle", img );
		}

		if( code == 27 || code == 'q' || code == 'Q' )
			break;
	}

	return 0;
}

效果图

例2：

http://blog.chinaunix.net/uid-25906157-id-3246199.html

// Condensation_demo.cpp : 定义控制台应用程序的入口点。

#ifdef _CH_
#pragma package <opencv>
#endif
#ifndef _EiC
#include "cv.h"
#include "cvAux.h"
#include "highgui.h"
#include "cxcore.h"
#include <stdio.h>
#include <ctype.h>
#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

例3

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

#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 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);    //set the font_face height and weight

	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,0};		
		//get variance/standard deviation of each state
		for (int i=0;i<measureNum;i++) {
			//sum
			float sumState=0;
			for (int j=0;j<condens->SamplesNum;j++) {
				sumState+=condens->flSamples[i][j];
			}
			//average
			sumState/=sampleNum;
			//variance
			for (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 confidence
		CvPoint 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 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;
}

效果图