#include "cv.h"
#include "highgui.h"
#include "cvAux.h"
#include "cxcore.h"
#include <fstream>
#include <iostream>
#include <stdlib.h>
#include <time.h>
#define Phi 3.1416
using namespace std;
unsigned int seed;
CvPoint CurrPoint = cvPoint(0,0);
typedef struct t //定义粒子结构
{
float x;
float y;
float vx;
float vy;
} PARTICLE;
PARTICLE particle[200]; //200个粒子
float weight[200]; //权重
CvMat noise = cvMat(1,1,CV_MAT32F,NULL);
CvMat B = cvMat(4,1,CV_MAT32F,NULL);
IplImage *image = 0, *hsv = 0, *hue = 0; //计算图像image 及转换到hsv空间的图像hue用于分离h分量
CvHistogram *hist = 0; //hist 第一帧所选区域的图像
int selecting = 0 ,selected = 0;
CvPoint origin;
CvRect selection;//需要跟踪的区域
int hdims = 48;
float hranges_arr[] = {0,180};
float *hranges = hranges_arr; //用于初始化CvHistogram
void on_mouse(int event, int x, int y, int flags, void *param)
{
if(!image)
return ;
if(image->origin) //image->origin = 0：顶-左结构；1：底-左结构
{
image->origin = 0;
y = image->height - y; //图像上的坐标均设为“底-左”结构
}
if(selecting) //正在选择物体
{
selection.x = MIN(x,origin.x);
selection.y = MIN(y,origin.y); //目标选择窗口
selection.width = selection.x + CV_IABS(x - origin.x);//CV_IABS取绝对值
selection.height = selection.y + CV_IABS(y - origin.y);
selection.x = MAX(selection.x ,0);
selection.y = MAX(selection.y,0);
selection.width = MIN(selection.width,image->width);
selection.height = MIN(selection.height,image->height);
selection.width -= selection.x;
selection.height -= selection.y;
}
switch(event)
{
case CV_EVENT_LBUTTONDOWN:
origin = cvPoint(x,y);
selection = cvRect(x,y,0,0);
selecting = 1;
break;
case CV_EVENT_LBUTTONUP:
selecting = 0;
if(selection.width >0 && selection.height >0)
selected = 1;
break;
}
}
void particleInitial()
{
/*CvMat *temp = cvCreateMat(4,1,CV_MAT32F);*/
memset(particle,0,sizeof(particle));
for(int i = 0;i<200;i++)
{
particle[i].x = selection.x;
particle[i].y = selection.y;
particle[i].vx = 0;
particle[i].vy = 0;
weight[i] = 0.005f;
}
CvRandState rng;//随机数产生器
cvRandInit(&rng,0,1,cvGetTickCount(),CV_RAND_NORMAL);
float no;
for(int i = 0;i<200;i++)
{
no = noise.data.fl[0];
particle[i].x = particle[i].x + B.data.fl[0]*no;
cvRand(&rng,&noise); //cvRanArr() noise为随机数组
no = noise.data.fl[0];
particle[i].y = particle[i].y + B.data.fl[0]*no;//对每个粒子加正态分布的随机噪声干扰
}
}
void particleUpdateWeight()
{
CvHistogram *hist_new = 0;
CvRect selection_new ;
CvMat mat;
hist_new = cvCreateHist(1,&hdims,CV_HIST_ARRAY,&hranges,1);
double bha = 0.f;
double nbha = 0.f;
float sum = 0.f;
int l = 0;
for(int i = 0;i < 200;i++)
{
selection_new.x = cvFloor(particle[i].x);//不大于particle[i].x最大整数
selection_new.y = cvFloor(particle[i].y);
selection_new.width = MIN((hue->width - selection_new.x),selection.width);
selection_new.height = MIN((hue->height-selection_new.y),selection.height);
selection_new.width = MAX(selection_new.width,0);
selection_new.height = MAX(selection_new.height,0);
if( selection_new.x < hue->width && selection_new.y < hue->height && selection_new.x >0 && selection_new.y>0)
{
cvSetImageROI(hue,selection_new);
cvCalcHist(&hue,hist_new,0,0);
cvNormalizeHist(hist_new,1.);
bha = cvCompareHist(hist,hist_new,CV_COMP_BHATTACHARYYA); //两个直方图的差（Bhattachayya距离）
weight[i] = (float) (weight[i]*(exp(-2*bha)));
// nbha = (1-bha)/(2*0.2*0.2);
// weight[i] = (float) ((1/sqrt(2*0.2*Phi))*(exp(-1*nbha)));
sum+=weight[i];
cvResetImageROI(hue);
}
else
{
weight[i] = 0;
}
}
for(int j = 0;j<200;j++) //归一化权重
{
weight[j] /= sum;
}
cvReleaseHist(&hist_new);
}
void particleUpdateByTime()
{
srand(time(0));
float h = ((float)(rand()%10000+1))/10000.0f;
float g = ((float)(rand()%10000+1))/10000.0f;
float sum = 0.f ;
int i = 0 , j = 0 ,in = 0;
while( sum < h ) //随机产生两个权值和的阈值h
{
sum += weight[i];
i++;
} //前i个粒子的权值之和小于h
sum = 0;
while(sum < g)
{
sum += weight[j];
j++;
} //前j个粒子的权值之和小于g
in = (i-2)>-1?i-2:i-1;
if( weight[in] < weight[(j-2)>-1?j-2:j-1])
in = (j-2)>-1?j-2:j-1; //比较weight[i] 与weight[j]的大小，取较大的那个权值
for(int index = 0; index < 200; index ++)
{
if(weight[index] < 0.001)
{
weight[index] = weight[in];
particle[index].x = particle[in].x;
particle[index].y = particle[in].y;
particle[index].vx = particle[in].vx;
particle[index].vy = particle[in].vy;
}
sum+=weight[index];
}
for(int index = 0;index < 200;index ++)
{
weight[index] /= sum ;
}
/*CvMat *temp = cvCreateMat(4,1,CV_MAT32F);*/
CvRandState rng;//随机数产生器
cvRandInit(&rng,0,1,cvGetTickCount(),CV_RAND_NORMAL);
float no;
for(int i = 0;i<200;i++)
{
cvRand(&rng,&noise);
no = noise.data.fl[0];
particle[i].x = particle[i].x + B.data.fl[0]*no;
cvRand(&rng,&noise);
no = noise.data.fl[0];
particle[i].y = particle[i].y + B.data.fl[0]*no;
}
}
void particleGetPosition(CvPoint &p)
{
float px = 0.f,py=0.f,sum = 0.f;
for(int i=0;i<200;i++)
{
px += (particle[i].x*weight[i]);
py += (particle[i].y*weight[i]);
/* cvRectangle(image,cvPoint(particle[i].x,particle[i].y),cvPoint(particle[i].x+selection.width,particle[i].y+selection.height),CV_RGB(0,255,0),1,8,0);*/
}
p = cvPoint(px,py);
}
//
/*char img_file[] = "..\\blackcar.avi";*/
/*char img_file[]="..\\video.long.raw.avi";*/
char img_file[]="..\\target_0001.wmv";
int main()
{
cvmAlloc(&noise);
cvmAlloc(&B);
ofstream out_stream;
ofstream out_streamy;
out_streamy.open("..\\current.txt"/*,ios::app*/);
out_streamy<<"Begin ......."<<endl;
out_stream.open("..\\data.txt",ios::app);
out_stream<<"每帧图像的处理时间："<<endl;
CvCapture *capture = 0;
IplImage *frame = 0;
capture = cvCaptureFromAVI(img_file);
if(!capture)
{
cout<<"载入视频出错"<<endl;
return 0;
}
cvNamedWindow("ParticleFilter");
cvSetMouseCallback("ParticleFilter",on_mouse,0);
int index=0;
for(;;)
{
frame = cvQueryFrame(capture);
if(!frame)
break;
if(!image) //image 还未初始化
{
image = cvCreateImage(cvGetSize(frame),8,3);
hsv = cvCreateImage(cvGetSize(frame),8,3);
hue = cvCreateImage(cvGetSize(frame),8,1);
hist = cvCreateHist(1,&hdims,CV_HIST_ARRAY,&hranges,1);
}
cvCopy(frame,image,0);
cvCvtColor(image,hsv,CV_BGR2HSV);
index ++;
if(selected)
{
double tt = (double)cvGetTickCount();
cvSplit(hsv,hue,0,0,0);
if(selected > 0) //LBUTTONUP
{
cvSetImageROI(hue,selection);
cvCalcHist(&hue,hist,0,0);
cvNormalizeHist(hist,1.);
cvResetImageROI(hue);
selected = -1;
particleInitial();
B.data.fl[0] = ((float)selection.width + (float)selection.height)/8;
B.data.fl[1] = B.data.fl[0];
B.data.fl[2] = B.data.fl[1]/10;
B.data.fl[3] = B.data.fl[2];
}
particleUpdateWeight();
CvPoint p;
particleGetPosition(p);
CurrPoint.x = p.x + selection.width/2;
CurrPoint.y = p.y + selection.height/2;
out_streamy<<"第"<<index<<"帧（"<<CurrPoint.x<<"，"<<CurrPoint.y<<"）"<<endl;
cvRectangle(image,p,cvPoint(p.x+selection.width,p.y+selection.height),CV_RGB(255,0,0),1,8,0);
particleUpdateByTime();
tt = (double)cvGetTickCount() - tt;
out_stream<<"第"<<index<<"帧的时间为"<<tt/(cvGetTickFrequency()*1000.)<<"ms"<<endl;
/*cout<<"计算时间："<<tt/(cvGetTickFrequency()*1000.)<<"ms"<<endl;*/
}
if( selecting && selection.width > 0 && selection.height > 0 )
//如果正处于物体选择，画出选择框
