粒子滤波跟踪算法
#include
#include "opencv2/imgproc/imgproc.hpp"
#include
#include
#include
using namespace cv;
using namespace std;
Rect select;
bool select_flag = false;
bool tracking = false;//跟踪标志位
bool select_show = false;
Point origin;
Mat frame, hsv;
int after_select_frames = 0;//选择矩形区域完后的帧计数
/****rgb空间用到的变量****/
//int hist_size[]={16,16,16};//rgb空间各维度的bin个数
//float rrange[]={0,255.0};
//float grange[]={0,255.0};
//float brange[]={0,255.0};
//const float *ranges[] ={rrange,grange,brange};//range相当于一个二维数组指针
/****hsv空间用到的变量****/
int hist_size[] = { 16, 16, 16 };
float hrange[] = { 0, 180.0 };
float srange[] = { 0, 256.0 };
float vrange[] = { 0, 256.0 };
//int hist_size[]={32,32,32};
//float hrange[]={0,359.0.0};
//float srange[]={0,1.0};
//float vrange[]={0,1.0};
const float *ranges[] = { hrange, srange, vrange };
int channels[] = { 0, 1, 2 };
/****有关粒子窗口变化用到的相关变量****/
int A1 = 2;
int A2 = -1;
int B0 = 1;
double sigmax = 1.0;
double sigmay = 0.5;
double sigmas = 0.001;
/****定义使用粒子数目宏****/
#define PARTICLE_NUMBER 100 //如果这个数设定太大，经测试这个数字超过25就会报错，则在运行时将会出现错误
/****定义粒子结构体****/
typedef struct particle
{
int orix, oriy;//原始粒子坐标
int x, y;//当前粒子的坐标
double scale;//当前粒子窗口的尺寸
int prex, prey;//上一帧粒子的坐标
double prescale;//上一帧粒子窗口的尺寸
Rect rect;//当前粒子矩形窗口
Mat hist;//当前粒子窗口直方图特征
double weight;//当前粒子权值
}PARTICLE;
PARTICLE particles[PARTICLE_NUMBER];
/************************************************************************************************************************/
/**** 如果采用这个onMouse()函数的话，则可以画出鼠标拖动矩形框的4种情形 ****/
/************************************************************************************************************************/
void onMouse(int event, int x, int y, int, void*)
{
//Point origin;//不能在这个地方进行定义，因为这是基于消息响应的函数，执行完后origin就释放了，所以达不到效果。
if (select_flag)
{
select.x = MIN(origin.x, x);//不一定要等鼠标弹起才计算矩形框，而应该在鼠标按下开始到弹起这段时间实时计算所选矩形框
select.y = MIN(origin.y, y);
select.width = abs(x - origin.x);//算矩形宽度和高度
select.height = abs(y - origin.y);
select &= Rect(0, 0, frame.cols, frame.rows);//保证所选矩形框在视频显示区域之内
// rectangle(frame,select,Scalar(0,0,255),3,8,0);//显示手动选择的矩形框
}
if (event == CV_EVENT_LBUTTONDOWN)
{
select_flag = true;//鼠标按下的标志赋真值
tracking = false;
select_show = true;
after_select_frames = 0;//还没开始选择，或者重新开始选择，计数为0
origin = Point(x, y);//保存下来单击是捕捉到的点
select = Rect(x, y, 0, 0);//这里一定要初始化，因为在opencv中Rect矩形框类内的点是包含左上角那个点的，但是不含右下角那个点。
}
else if (event == CV_EVENT_LBUTTONUP)
{
select_flag = false;
tracking = true;
select_show = false;
after_select_frames = 1;//选择完后的那一帧当做第1帧
}
}
/****粒子权值降序排列函数****/
int particle_decrease(const void *p1, const void *p2)
{
PARTICLE* _p1 = (PARTICLE*)p1;
PARTICLE* _p2 = (PARTICLE*)p2;
if (_p1->weight<_p2->weight)
return 1;
else if (_p1->weight>_p2->weight)
return -1;
return 0;//相等的情况下返回0
}
int main(int argc, unsigned char* argv[])
{
char c;
Mat target_img, track_img;
Mat target_hist, track_hist;
PARTICLE *pParticle;
/***打开摄像头****/
VideoCapture cam(0);
if (!cam.isOpened())
return -1;
/****读取一帧图像****/
cam >> frame;
if (frame.empty())
return -1;
VideoWriter output_dst("demo.avi", CV_FOURCC('M', 'J', 'P', 'G'), 10, frame.size(), 1);
/****建立窗口****/
namedWindow("camera", 1);//显示视频原图像的窗口
/****捕捉鼠标****/
setMouseCallback("camera", onMouse, 0);
while (1)
{
/****读取一帧图像****/
cam >> frame;
if (frame.empty())
return -1;
/****将rgb空间转换为hsv空间****/
cvtColor(frame, hsv, CV_BGR2HSV);
if (tracking)
{
if (1 == after_select_frames)//选择完目标区域后
{
/****计算目标模板的直方图特征****/
target_img = Mat(hsv, select);//在此之前先定义好target_img,然后这样赋值也行，要学会Mat的这个操作
calcHist(&target_img, 1, channels, Mat(), target_hist, 3, hist_size, ranges);
normalize(target_hist, target_hist);
/****初始化目标粒子****/
pParticle = particles;//指针初始化指向particles数组
for (int x = 0; x
pParticle->x = cvRound(select.x + 0.5*select.width);//选定目标矩形框中心为初始粒子窗口中心
pParticle->y = cvRound(select.y + 0.5*select.height);
pParticle->orix = pParticle->x;//粒子的原始坐标为选定矩形框(即目标)的中心
pParticle->oriy = pParticle->y;
pParticle->prex = pParticle->x;//更新上一次的粒子位置
pParticle->prey = pParticle->y;
pParticle->rect = select;
pParticle->prescale = 1;
pParticle->scale = 1;
pParticle->hist = target_hist;
pParticle->weight = 0;
pParticle++;
}
}
else if (2 == after_select_frames)//从第二帧开始就可以开始跟踪了
{
double sum = 0.0;
pParticle = particles;
RNG rng;//随机数产生器
/****更新粒子结构体的大部分参数****/
for (int i = 0; i
int x, y;
int xpre, ypre;
double s, pres;
xpre = pParticle->x;
ypre = pParticle->y;
pres = pParticle->scale;
/****更新粒子的矩形区域即粒子中心****/
x = cvRound(A1*(pParticle->x - pParticle->orix) + A2*(pParticle->prex - pParticle->orix) +
B0*rng.gaussian(sigmax) + pParticle->orix);
pParticle->x = max(0, min(x, frame.cols - 1));
y = cvRound(A1*(pParticle->y - pParticle->oriy) + A2*(pParticle->prey - pParticle->oriy) +
B0*rng.gaussian(sigmay) + pParticle->oriy);
pParticle->y = max(0, min(y, frame.rows - 1));
s = A1*(pParticle->scale - 1) + A2*(pParticle->prescale - 1) + B0*(rng.gaussian(sigmas)) + 1.0;
pParticle->scale = max(1.0, min(s, 3.0));
pParticle->prex = xpre;
pParticle->prey = ypre;
pParticle->prescale = pres;
// pParticle->orix=pParticle->orix;
// pParticle->oriy=pParticle->oriy;
//注意在c语言中，x-1.0，如果x是int型，则这句语法有错误,但如果前面加了cvRound(x-0.5)则是正确的
pParticle->rect.x = max(0, min(cvRound(pParticle->x - 0.5*pParticle->scale*pParticle->rect.width), frame.cols));
pParticle->rect.y = max(0, min(cvRound(pParticle->y - 0.5*pParticle->scale*pParticle->rect.height), frame.rows));
pParticle->rect.width = min(cvRound(pParticle->rect.width), frame.cols - pParticle->rect.x);
pParticle->rect.height = min(cvRound(pParticle->rect.height), frame.rows - pParticle->rect.y);
// pParticle->rect.width=min(cvRound(pParticle->scale*pParticle->rect.width),frame.cols-pParticle->rect.x);
// pParticle->rect.height=min(cvRound(pParticle->scale*pParticle->rect.height),frame.rows-pParticle->rect.y);
/****计算粒子区域的新的直方图特征****/
track_img = Mat(hsv, pParticle->rect);
calcHist(&track_img, 1, channels, Mat(), track_hist, 3, hist_size, ranges);
normalize(track_hist, track_hist);
/****更新粒子的权值****/
// pParticle->weight=compareHist(target_hist,track_hist,CV_COMP_INTERSECT);
//采用巴氏系数计算相似度,永远与最开始的那一目标帧相比较
pParticle->weight = 1.0 - compareHist(target_hist, track_hist, CV_COMP_BHATTACHARYYA);
/****累加粒子权值****/
sum += pParticle->weight;
pParticle++;
}
/****归一化粒子权重****/
pParticle = particles;
for (int i = 0; i
pParticle->weight /= sum;
pParticle++;
}
/****根据粒子的权值降序排列****/
pParticle = particles;
qsort(pParticle, PARTICLE_NUMBER, sizeof(PARTICLE), &particle_decrease);
/****根据粒子权重重采样粒子****/
PARTICLE newParticle[PARTICLE_NUMBER];
int np = 0, k = 0;
for (int i = 0; i
np = cvRound(pParticle->weight*PARTICLE_NUMBER);
for (int j = 0; j
newParticle[k++] = particles[i];
if (k == PARTICLE_NUMBER)
goto EXITOUT;
}
}
while (k
EXITOUT:
for (int i = 0; i
}//end else
//????????这个排序很慢，粒子数一多就卡
// qsort(pParticle,PARTICLE_NUMBER,sizeof(PARTICLE),&particle_decrease);
/****计算粒子期望，采用所有粒子位置的期望值做为跟踪结果****/
/*Rect_
pParticle=particles;
for(int i=0;i
rectTrackingTemp.x+=pParticle->rect.x*pParticle->weight;
rectTrackingTemp.y+=pParticle->rect.y*pParticle->weight;
rectTrackingTemp.width+=pParticle->rect.width*pParticle->weight;
rectTrackingTemp.height+=pParticle->rect.height*pParticle->weight;
pParticle++;
}*/
/****计算最大权重目标的期望位置，作为跟踪结果****/
Rect rectTrackingTemp(0, 0, 0, 0);
pParticle = particles;
rectTrackingTemp.x = pParticle->x - 0.5*pParticle->rect.width;
rectTrackingTemp.y = pParticle->y - 0.5*pParticle->rect.height;
rectTrackingTemp.width = pParticle->rect.width;
rectTrackingTemp.height = pParticle->rect.height;
/****计算最大权重目标的期望位置，采用权值最大的1/4个粒子数作为跟踪结果****/
/*Rect rectTrackingTemp(0,0,0,0);
double weight_temp=0.0;
pParticle=particles;
for(int i=0;i
weight_temp+=pParticle->weight;
pParticle++;
}
pParticle=particles;
for(int i=0;i
pParticle->weight/=weight_temp;
pParticle++;
}
pParticle=particles;
for(int i=0;i
rectTrackingTemp.x+=pParticle->rect.x*pParticle->weight;
rectTrackingTemp.y+=pParticle->rect.y*pParticle->weight;
rectTrackingTemp.width+=pParticle->rect.width*pParticle->weight;
rectTrackingTemp.height+=pParticle->rect.height*pParticle->weight;
pParticle++;
}*/
/****计算最大权重目标的期望位置，采用所有粒子数作为跟踪结果****/
/*Rect rectTrackingTemp(0,0,0,0);
pParticle=particles;
for(int i=0;i
rectTrackingTemp.x+=cvRound(pParticle->rect.x*pParticle->weight);
rectTrackingTemp.y+=cvRound(pParticle->rect.y*pParticle->weight);
pParticle++;
}
pParticle=particles;
rectTrackingTemp.width = pParticle->rect.width;
rectTrackingTemp.height = pParticle->rect.height;*/
//创建目标矩形区域
Rect tracking_rect(rectTrackingTemp);
pParticle = particles;
/****显示各粒子运动结果****/
for (int m = 0; m
rectangle(frame, pParticle->rect, Scalar(255, 0, 0), 1, 8, 0);
pParticle++;
}
/****显示跟踪结果****/
rectangle(frame, tracking_rect, Scalar(0, 0, 255), 3, 8, 0);
after_select_frames++;//总循环每循环一次，计数加1
if (after_select_frames>2)//防止跟踪太长，after_select_frames计数溢出
after_select_frames = 2;
}
if (select_show)
rectangle(frame, select, Scalar(0, 0, 255), 3, 8, 0);//显示手动选择的矩形框
output_dst << frame;
//显示视频图片到窗口
imshow("camera", frame);
// select.zeros();
//键盘响应
c = (char)waitKey(20);
if (27 == c)//ESC键
return -1;
}
return 0;
}