OpenCV3学习（12.4） 粒子滤波Condensation算法

算法详解

      opencv2.3以后，condensation算法放在legacy中了，也就是说要引入下面文件，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维的值

与粒子滤波相关的几个函数：

1. cvCreateConDensation：用于构造上述滤波器数据结构
2. cvConDensInitSampleSet：初始化粒子集
3. cvConDensUpdateByTime：更新粒子集
4. cvReleaseConDensation：释放滤波器

cvCreateConDensation( int DP, int MP, int SamplesNum )

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

CVAPI(CvConDensation*)  cvCreateConDensation( int dynam_params,
                                             int measure_params,
                                             int sample_count );

cvConDensInitSampleSet

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

CVAPI(void) cvConDensInitSampleSet(CvConDensation* condens,
 CvMat* lower_bound,  //粒子初始化时，取值下界
CvMat* 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);

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

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

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

cvConDensUpdateByTime(ConDens);

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

CVAPI(void)  cvConDensUpdateByTime( CvConDensation* condens);

cvReleaseConDensation( CvConDensation** condens );

CVAPI(void)  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实现了在窗口上对鼠标进行检测与跟踪

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

结果：

粉色点为“粒子”，蓝线为“鼠标”移动轨迹，绿线为“跟踪轨迹”。 

from：https://blog.csdn.net/gdfsg/article/details/50794528