前面2篇博客已经提到当粒子数增加时会内存报错，后面又仔细查了下程序，是代码方面的问题。所以本次的代码与前几次改变比较小。当然这些code基本也是参考网上的。代码写得很不规范，时间不够，等以后有机会将其优化并整理成类的形式。)
Opencv实现粒子滤波算法
摘要
本文通过opencv实现了一种目标跟踪算法——粒子滤波算法，算法的思想来源于文献[1][2],且在其思想上稍 微做了些修改。其大概过程是：首先手动用鼠标框出一个目标区域，计算其直方图特征值作为模板，然后在该目标中心周围撒粒子，根据所撒粒子为中心的矩形框内 计算其直方图特征，并与目标相比较，最后根据比较出的结果重复上面过程，即重采样的方法撒粒子，粒子扩散，状态观察，目标预测。最后通过实验证明，取得了 较好的效果。
关键字：目标跟踪，粒子滤波，opencv
前言
目标跟踪过程分为2部分，即目标特征提取和目标跟踪算法。
其中目标特征提取又包括以下几种：1. 各种色彩空间直方图，利用色彩空间的直方图分布作为目标跟踪的特征，可以减少物体远近距离的影响，因为其颜色分布大致相同。2.轮廓特征，提取目标的轮廓 特征，可以加快算法的速度，且可以在目标有小部分影响的情况下同样有效果。3. 纹理特征，如果被跟踪目标是有纹理的，则根据其纹理特征来跟踪效果会有所改善。
目标跟踪算法目前大概分为以下4种：1. 基于meanshift算法，即利用meanshift算法可以快速找到领域目标最相似的地方，效果还不错，但是其只能找到局部最大值，且不能解决遮挡问 题以及不能自适应跟踪目标的形状，方向等。其后面有学者对其做了改进，比如说camshift，就可以自适应物体的大小，方向，具有较好的跟踪效果。2. Kalman滤波的思想，该思想是利用物体的运动模型来，即服从高斯模型，来对目标状态进行预测，然后与观察模型进行比较，根据2者之间的误差来寻找运动 目标的状态，但是该算法的精度不高，因为其高斯运动模型在现实生活中很多条件下并得不到满足，并且该算法对杂乱的背景也很敏感。3. 基于粒子滤波的思想，每次通过实验可以重采样粒子的分布，根据该分布对粒子进行扩散，然后通过扩散的结果来观察目标的状态，最后更新目标的状态。该算法最 大的特点是跟踪速度快，且能解决一部分遮挡问题，在实际应用过程中越来越多。4.基于目标建模的方法。该方法具有一定的针对性，需要提前知道所需跟踪的目 标是什么，比如说车辆，人脸，行人等。由于已经知道了跟踪目标，所以必须对目标进行建模，然后利用该模型来进行跟踪。该方法的局限性是必须提前知道所跟踪 的目标是什么，因而其推广性比较差。
本文通过学习文献[1],初步从理论上了解了粒子滤波的几个步骤，然后参考文献[2]中基于颜色直方图的跟踪算法，自己实现了一个粒子滤波目标跟踪器。本文实现的算法只能进行单目标跟踪，后面有学者将粒子滤波的思想扩展到多目标跟踪，比如说文献[3].
实现过程
1. 在摄像头采集到的视频序列中手动标注一个目标区域，用矩形框表示，并计算该目标区域内的直方图，作为匹配模板。
2. 在选中的目标区域中心处撒预定值为100的粒子数目，并对这100个粒子的结构体都初始化为同样的值，比如说粒子位置为目标区域中心，粒子矩形长宽为目标矩形长宽，粒子的初始尺寸为1.等等。
其粒子的结构体和注释如下：
/****定义粒子结构体****/
typedef struct particle
{
int orix,oriy;//原始粒子坐标
int x,y;//当前粒子的坐标
double scale;//当前粒子窗口的尺寸
int prex,prey;//上一帧粒子的坐标
double prescale;//上一帧粒子窗口的尺寸
Rect rect;//当前粒子矩形窗口
Mat hist;//当前粒子窗口直方图特征
double weight;//当前粒子权值
}PARTICLE;
3. 利用二阶动态模型对这100个粒子进行随机扩散，每个粒子根据二阶随机动态模型扩散到一个新的位置。
4. 计算以新位置处每个粒子为中心的矩形框内图像的直方图特征，然后每个特征都与目标模板直方图进行比较，计算其相似度。
5. 根据计算得到的相似度算出每个粒子的权值，即相似度伟大的权值越大，并且对权值进行归一化。
6. 取最大权值处的粒子中心为跟踪目标中心，并根据粒子结构体中的尺寸成员算出目标的矩形框。
7. 根据上一个状态的粒子分布情况，按照其权值乘以粒子总数100进行重采用，即权值大的粒子其采样得到的粒子数也大。这样可以重采用重新得到100个粒子。
8. 根据这100个粒子的情况，重新进入步骤3进行循环，经过对粒子权值分布的重采样，根据二阶随机运动模型进行粒子扩散，根据直方图的巴氏距离推断粒子权值，然后取权值最大的那个作为目标，如此循环。
程序流程图
实验代码
实验代码如下：
// particle_tracking.cpp : 定义控制台应用程序的入口点。 // #include "stdafx.h" #include <opencv2/core/core.hpp> #include "opencv2/imgproc/imgproc.hpp" #include <opencv2/highgui/highgui.hpp> #include <stdio.h> #include <iostream> 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<PARTICLE_NUMBER;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<PARTICLE_NUMBER;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<PARTICLE_NUMBER;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<PARTICLE_NUMBER;i++) { np=cvRound(pParticle->weight*PARTICLE_NUMBER); for(int j=0;j<np;j++) { newParticle[k++]=particles[i]; if(k==PARTICLE_NUMBER) goto EXITOUT; } } while(k<PARTICLE_NUMBER) newParticle[k++]=particles[0]; EXITOUT: for(int i=0;i<PARTICLE_NUMBER;i++) particles[i]=newParticle[i]; }//end else //????????这个排序很慢，粒子数一多就卡 // qsort(pParticle,PARTICLE_NUMBER,sizeof(PARTICLE),&particle_decrease); /****计算粒子期望，采用所有粒子位置的期望值做为跟踪结果****/ /*Rect_<double> rectTrackingTemp(0.0,0.0,0.0,0.0); pParticle=particles; for(int i=0;i<PARTICLE_NUMBER;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<PARTICLE_NUMBER/4;i++) { weight_temp+=pParticle->weight; pParticle++; } pParticle=particles; for(int i=0;i<PARTICLE_NUMBER/4;i++) { pParticle->weight/=weight_temp; pParticle++; } pParticle=particles; for(int i=0;i<PARTICLE_NUMBER/4;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<PARTICLE_NUMBER;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<PARTICLE_NUMBER;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; }
实验结果
实验结果如下图所示，其中红色框为跟踪到的目标，绿色框为跟踪过程中粒子所在位置的矩形框。分为4组实验对比，分别手动标注4个不同的部位进行跟踪，达到了较好的效果。（此处图略，图片是用摄像头跟踪本人多个部位的结果，不好贴图）
目标1的跟踪效果：(略)
目标2的跟踪效果：(略)
目标3的跟踪效果：(略)
目标4的跟踪效果：(略)
实验总结
通过本次实验可以更加深刻的理解了粒子滤波的算法思想，熟悉了粒子滤波算法的几个步骤，并且对opencv这个工具有了更深一步的认识，编程功底也得到了相应的提高，体会 到了科研的乐趣。
参考文献：
1. Isard, M. and A. Blake (1998). "Condensation—conditional density propagation for visual tracking." International journal of computer vision 29(1): 5-28.
2. Nummiaro, K., E. Koller-Meier, et al. (2003). "An adaptive color-based particle filter." Image and Vision Computing 21(1): 99-110.
3. Okuma, K., A. Taleghani, et al. (2004). "A boosted particle filter: Multitarget detection and tracking." Computer Vision-ECCV 2004: 28-39.