粒子滤波的鼠标跟踪

程序使用http://blog.csdn.net/onezeros/article/details/6319180作者的成果，程序很简洁，实现了两种状态：1，运行后，粒子不断跟踪鼠标点的位置；2，按下空格键，鼠标位置不再标识，粒子跟踪程序中定义的一个任意位置。在源程序的基础上，对每一步加入了我自己的理解作为注释部分，关于粒子权重计算和优劣粒子的取舍我还有许多不明白的地方，先列出自己的一点心得。

#include  
#include  
#include  
#include  
#include  
#include  
#include  
using namespace std;
//显示窗口的尺寸；
const int winHeight = 600;
const int winWidth = 800;
//定义包括两个整数元素winWidth,winHeight的结构体,存放鼠标位置；
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()
{
//1.初始化状态数、测量变量数、粒子数 
const int stateNum = 4;
const int measureNum = 2;
const int sampleNum = 2000;
//构造粒子滤波的数据结构；
CvConDensation* condens = cvCreateConDensation(stateNum, measureNum, sampleNum);
CvMat* lowerBound = cvCreateMat(stateNum, 1, CV_32F);
CvMat* 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_SIMPLEX, 1, 1);
//响应窗口中的鼠标运行；
char* winName = "condensation";
cvNamedWindow(winName);
cvSetMouseCallback(winName, mouseEvent);
IplImage* img = cvCreateImage(cvSize(winWidth, winHeight), 8, 3);
bool isPredictOnly = false;//按下空格键，目标粒子随机产生；
//粒子滤波过程开始；
while (1){
//定义预测粒子的存储矩阵；
CvPoint predict_pt = cvPoint((int)condens->State[0], (int)condens->State[1]);
float variance[measureNum] = { 0 };
//这个地方不是很明白；
for (int i = 0; i//sum  
float sumState = 0;
for (int j = 0; jSamplesNum; j++) {
sumState += condens->flSamples[i][j];
}
sumState /= sampleNum; 
for (int j = 0; jSamplesNum; j++) {
variance[i] += (condens->flSamples[i][j] - sumState)*
(condens->flSamples[i][j] - sumState);
}
variance[i] /= sampleNum - 1;
}
//选择目标粒子，并计算权重； 
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;
}
//更新粒子集  
cvConDensUpdateByTime(condens);
//绘制预测粒子的位置，绿色；
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;
}