算法详解
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维的值
与粒子滤波相关的几个函数:
- cvCreateConDensation:用于构造上述滤波器数据结构
- cvConDensInitSampleSet:初始化粒子集
- cvConDensUpdateByTime:更新粒子集
- 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;
}
结果:
粉色点为“粒子”,蓝线为“鼠标”移动轨迹,绿线为“跟踪轨迹”。