随笔-252  评论-2220  文章-0 

目标跟踪学习笔记_3(particle filter初探2)

  上次已经初步体验了下particle filter,直接用别人的代码,见我前面的博文http://www.cnblogs.com/tornadomeet/archive/2012/03/18/2404817.html

   一开始是内存出错,后面干脆自己学了下particle filter滤波的原理,把代码认真看了一遍,然后自己从头敲了遍代码,虽然运行时不再出现内存溢出等bug,但是没有跟踪效果。

  这次的代码和上次一样,有跟踪效果,不过不理想。依旧是参照博主:http://blog.csdn.net/yang_xian521/article/details/6928131 的代码,但是算法稍微改了3点。

  1.仔细阅读其代码发现其代码有粒子滤波原则性的错误,即物体的运动模型,作者本意是将运动模型近视为二阶自回归模型,但是程序中粒子的坐标计算过程中,前一次粒子坐标和当前粒子坐标是完全一样的。后面把这个bug改后。

  2.最后跟踪物体的矩形采用权值最大粒子的矩形,而不是才用所以粒子的期望,经过试验发现采用权值最大的粒子的效果要好些。

  3.计算粒子的权值改为采用的是巴斯距离。

 

  工程环境:opencv2.3.1+vs2010

  程序功能:驱动摄像头,待程序运行后用鼠标单击拖动选定需要跟踪的目标,然后程序会自动跟踪该目标。

  其程序代码如下:

  1 // particle_tracking.cpp : 定义控制台应用程序的入口点。
  2 //
  3 
  4 #include "stdafx.h"
  5 #include <opencv2/core/core.hpp>
  6 #include "opencv2/imgproc/imgproc.hpp"
  7 #include <opencv2/highgui/highgui.hpp>
  8 #include <stdio.h>
  9 #include <iostream>
 10 
 11 using namespace cv;
 12 using namespace std;
 13 
 14 Rect select;
 15 bool select_flag=false;
 16 bool tracking=false;//跟踪标志位
 17 bool select_show=false;
 18 Point origin;
 19 Mat frame,hsv;
 20 int after_select_frames=0;//选择矩形区域完后的帧计数
 21 
 22 /****rgb空间用到的变量****/
 23 //int hist_size[]={16,16,16};//rgb空间各维度的bin个数
 24 //float rrange[]={0,255.0};
 25 //float grange[]={0,255.0};
 26 //float brange[]={0,255.0};
 27 //const float *ranges[] ={rrange,grange,brange};//range相当于一个二维数组指针
 28 
 29 /****hsv空间用到的变量****/
 30 int hist_size[]={16,16,16};
 31 float hrange[]={0,179.0};
 32 float srange[]={0,255.0};
 33 float vrange[]={0,255.0};
 34 
 35 //int hist_size[]={32,32,32};
 36 //float hrange[]={0,359.0.0};
 37 //float srange[]={0,1.0};
 38 //float vrange[]={0,1.0};
 39 const float *ranges[]={hrange,srange,vrange};
 40 
 41 int channels[]={0,1,2};
 42 
 43 /****有关粒子窗口变化用到的相关变量****/
 44 int A1=2;
 45 int A2=-1;
 46 int B0=1;
 47 double sigmax=1.0;
 48 double sigmay=0.5;
 49 double sigmas=0.001;
 50 
 51 /****定义使用粒子数目宏****/
 52 #define PARTICLE_NUMBER 100 //如果这个数设定太大,经测试这个数字超过25就会报错,则在运行时将会出现错误
 53 
 54 /****定义粒子结构体****/
 55 typedef struct particle
 56 {
 57     int orix,oriy;//原始粒子坐标
 58     int x,y;//当前粒子的坐标
 59     double scale;//当前粒子窗口的尺寸
 60     int prex,prey;//上一帧粒子的坐标
 61     double prescale;//上一帧粒子窗口的尺寸
 62     Rect rect;//当前粒子矩形窗口
 63     Mat hist;//当前粒子窗口直方图特征
 64     double weight;//当前粒子权值
 65 }PARTICLE;
 66 
 67 PARTICLE particles[PARTICLE_NUMBER];
 68 
 69 /************************************************************************************************************************/
 70 /****                            如果采用这个onMouse()函数的话,则可以画出鼠标拖动矩形框的4种情形                        ****/
 71 /************************************************************************************************************************/
 72 void onMouse(int event,int x,int y,int,void*)
 73 {
 74     //Point origin;//不能在这个地方进行定义,因为这是基于消息响应的函数,执行完后origin就释放了,所以达不到效果。
 75     if(select_flag)
 76     {
 77         select.x=MIN(origin.x,x);//不一定要等鼠标弹起才计算矩形框,而应该在鼠标按下开始到弹起这段时间实时计算所选矩形框
 78         select.y=MIN(origin.y,y);
 79         select.width=abs(x-origin.x);//算矩形宽度和高度
 80         select.height=abs(y-origin.y);
 81         select&=Rect(0,0,frame.cols,frame.rows);//保证所选矩形框在视频显示区域之内
 82 
 83         //        rectangle(frame,select,Scalar(0,0,255),3,8,0);//显示手动选择的矩形框
 84     }
 85     if(event==CV_EVENT_LBUTTONDOWN)
 86     {
 87         select_flag=true;//鼠标按下的标志赋真值
 88         tracking=false;
 89         select_show=true;
 90         after_select_frames=0;//还没开始选择,或者重新开始选择,计数为0
 91         origin=Point(x,y);//保存下来单击是捕捉到的点
 92         select=Rect(x,y,0,0);//这里一定要初始化,因为在opencv中Rect矩形框类内的点是包含左上角那个点的,但是不含右下角那个点。
 93     }
 94     else if(event==CV_EVENT_LBUTTONUP)
 95     {
 96         select_flag=false;
 97         tracking=true;
 98         select_show=false;
 99         after_select_frames=1;//选择完后的那一帧当做第1帧
100     }
101 }
102 
103 /****粒子权值降序排列函数****/
104 int particle_decrease(const void *p1,const void *p2)
105 {
106     PARTICLE* _p1=(PARTICLE*)p1;
107     PARTICLE* _p2=(PARTICLE*)p2;
108     if(_p1->weight<_p2->weight)
109         return 1;
110     else if(_p1->weight>_p2->weight)
111         return -1;
112     return 0;//相等的情况下返回0
113 }
114 
115 int main(int argc, unsigned char* argv[])
116 {
117     char c;
118     Mat target_img,track_img;
119     Mat target_hist,track_hist;
120     PARTICLE *pParticle;
121 
122     /***打开摄像头****/
123     VideoCapture cam(0);
124     if (!cam.isOpened())
125         return -1;
126 
127     /****建立窗口****/
128     namedWindow("camera",1);//显示视频原图像的窗口
129 
130     /****捕捉鼠标****/
131     setMouseCallback("camera",onMouse,0);
132 
133     while(1)
134     {
135         /****读取一帧图像****/
136         cam>>frame;
137         if(frame.empty())
138             return -1;
139 
140         /****将rgb空间转换为hsv空间****/
141         cvtColor(frame,hsv,CV_BGR2HSV);
142 
143         if(tracking)
144         {
145 
146             if(1==after_select_frames)//选择完目标区域后
147             {
148                 /****计算目标模板的直方图特征****/
149                 target_img=Mat(hsv,select);//在此之前先定义好target_img,然后这样赋值也行,要学会Mat的这个操作
150                 calcHist(&target_img,1,channels,Mat(),target_hist,3,hist_size,ranges);
151                 normalize(target_hist,target_hist);
152 
153                 /****初始化目标粒子****/
154                 pParticle=particles;//指针初始化指向particles数组
155                 for(int x=0;x<PARTICLE_NUMBER;x++)
156                 {
157                     pParticle->x=cvRound(select.x+0.5*select.width);//选定目标矩形框中心为初始粒子窗口中心
158                     pParticle->y=cvRound(select.y+0.5*select.height);
159                     pParticle->orix=pParticle->x;//粒子的原始坐标为选定矩形框(即目标)的中心
160                     pParticle->oriy=pParticle->y;
161                     pParticle->prex=pParticle->x;//更新上一次的粒子位置
162                     pParticle->prey=pParticle->y;
163                     pParticle->rect=select;
164                     pParticle->prescale=1;
165                     pParticle->scale=1;
166                     pParticle->hist=target_hist;
167                     pParticle->weight=0;
168                     pParticle++;
169                 }
170             }
171             else if(2==after_select_frames)//从第二帧开始就可以开始跟踪了
172             {
173                 double sum=0.0;
174                 pParticle=particles;
175                 RNG rng;//随机数产生器
176 
177                 /****更新粒子结构体的大部分参数****/
178                 for(int i=0;i<PARTICLE_NUMBER;i++)
179                 {
180                     int x,y;
181                     int xpre,ypre;
182                     double s,pres;
183 
184                     xpre=pParticle->x;
185                     ypre=pParticle->y;
186                     pres=pParticle->scale;
187 
188                     /****更新粒子的矩形区域即粒子中心****/
189                     x=cvRound(A1*(pParticle->x-pParticle->orix)+A2*(pParticle->prex-pParticle->orix)+
190                         B0*rng.gaussian(sigmax)+pParticle->orix);
191                     pParticle->x=max(0,min(x,frame.cols-1));
192 
193                     y=cvRound(A1*(pParticle->y-pParticle->oriy)+A2*(pParticle->prey-pParticle->oriy)+
194                         B0*rng.gaussian(sigmay)+pParticle->oriy);
195                     pParticle->y=max(0,min(y,frame.rows-1));
196 
197                     s=A1*(pParticle->scale-1)+A2*(pParticle->prescale-1)+B0*(rng.gaussian(sigmas))+1.0;
198                     pParticle->scale=max(1.0,min(s,3.0));
199 
200                     pParticle->prex=xpre;
201                     pParticle->prey=ypre;
202                     pParticle->prescale=pres;
203             //        pParticle->orix=pParticle->orix;
204             //        pParticle->oriy=pParticle->oriy;
205 
206                     //注意在c语言中,x-1.0,如果x是int型,则这句语法有错误,但如果前面加了cvRound(x-0.5)则是正确的
207                     pParticle->rect.x=max(0,min(cvRound(pParticle->x-0.5*pParticle->scale*pParticle->rect.width),frame.cols));
208                     pParticle->rect.y=max(0,min(cvRound(pParticle->y-0.5*pParticle->scale*pParticle->rect.height),frame.rows));
209                     pParticle->rect.width=min(cvRound(pParticle->scale*pParticle->rect.width),frame.cols-pParticle->rect.x);
210                     pParticle->rect.height=min(cvRound(pParticle->scale*pParticle->rect.height),frame.rows-pParticle->rect.y);
211 
212                     /****计算粒子区域的新的直方图特征****/
213                     track_img=Mat(hsv,pParticle->rect);
214                     calcHist(&track_img,1,channels,Mat(),track_hist,3,hist_size,ranges);
215                     normalize(track_hist,track_hist);
216 
217                     /****更新粒子的权值****/
218                     //            pParticle->weight=compareHist(target_hist,track_hist,CV_COMP_INTERSECT);
219                     pParticle->weight=1.0-compareHist(target_hist,track_hist,CV_COMP_BHATTACHARYYA);//采用巴氏系数计算相似度
220 
221                     /****累加粒子权值****/
222                     sum+=pParticle->weight;
223                     pParticle++;
224                 }
225 
226                 /****归一化粒子权重****/
227                 pParticle=particles;
228                 for(int i=0;i<PARTICLE_NUMBER;i++)
229                 {
230                     pParticle->weight/=sum;
231                     pParticle++;
232                 }
233 
234                 /****根据粒子的权值降序排列****/
235                 pParticle=particles;
236                 qsort(pParticle,PARTICLE_NUMBER,sizeof(PARTICLE),&particle_decrease);
237 
238                 /****根据粒子权重重采样粒子****/
239                 PARTICLE newParticle[PARTICLE_NUMBER];
240                 int np=0,k=0;
241                 for(int i=0;i<PARTICLE_NUMBER;i++)
242                 {
243                     np=cvRound(pParticle->weight*PARTICLE_NUMBER);
244                     for(int j=0;j<np;j++)
245                     {
246                         newParticle[k++]=particles[i];
247                         if(k==PARTICLE_NUMBER)
248                             goto EXITOUT;
249                     }
250                 }
251                 while(k<PARTICLE_NUMBER)
252                     newParticle[k++]=particles[0];
253 EXITOUT:
254                 for(int i=0;i<PARTICLE_NUMBER;i++)
255                     particles[i]=newParticle[i];
256             }//end else
257 
258             qsort(pParticle,PARTICLE_NUMBER,sizeof(PARTICLE),&particle_decrease);
259 
260             /****计算粒子期望,采用所有粒子位置的期望值做为跟踪结果****/
261             /*Rect_<double> rectTrackingTemp(0.0,0.0,0.0,0.0);
262             pParticle=particles;
263             for(int i=0;i<PARTICLE_NUMBER;i++)
264             {
265                 rectTrackingTemp.x+=pParticle->rect.x*pParticle->weight;
266                 rectTrackingTemp.y+=pParticle->rect.y*pParticle->weight;
267                 rectTrackingTemp.width+=pParticle->rect.width*pParticle->weight;
268                 rectTrackingTemp.height+=pParticle->rect.height*pParticle->weight;
269                 pParticle++;
270             }*/
271 
272             /****计算最大权重目标的期望位置,作为跟踪结果****/
273             Rect rectTrackingTemp(0,0,0,0);
274             pParticle=particles;
275             rectTrackingTemp.x=pParticle->x-0.5*pParticle->rect.width;
276             rectTrackingTemp.y=pParticle->y-0.5*pParticle->rect.height;
277             rectTrackingTemp.width=pParticle->rect.width;
278             rectTrackingTemp.height=pParticle->rect.height;
279 
280             /****计算最大权重目标的期望位置,采用权值最大的1/4个粒子数作为跟踪结果****/
281             /*Rect rectTrackingTemp(0,0,0,0);
282             double weight_temp=0.0;
283             pParticle=particles;
284             for(int i=0;i<PARTICLE_NUMBER/4;i++)
285             {
286                 weight_temp+=pParticle->weight;
287                 pParticle++;
288             }
289             pParticle=particles;
290             for(int i=0;i<PARTICLE_NUMBER/4;i++)
291             {
292                 pParticle->weight/=weight_temp;
293                 pParticle++;
294             }
295             pParticle=particles;
296             for(int i=0;i<PARTICLE_NUMBER/4;i++)
297             {
298                 rectTrackingTemp.x+=pParticle->rect.x*pParticle->weight;
299                 rectTrackingTemp.y+=pParticle->rect.y*pParticle->weight;
300                 rectTrackingTemp.width+=pParticle->rect.width*pParticle->weight;
301                 rectTrackingTemp.height+=pParticle->rect.height*pParticle->weight;
302                 pParticle++;
303             }*/
304 
305 
306             //创建目标矩形区域
307             Rect tracking_rect(rectTrackingTemp);
308 
309             pParticle=particles;
310 
311             /****显示各粒子运动结果****/
312             for(int m=0;m<PARTICLE_NUMBER;m++)
313             {
314                 rectangle(frame,pParticle->rect,Scalar(255,0,0),1,8,0);
315                 pParticle++;
316             }
317 
318             /****显示跟踪结果****/
319             rectangle(frame,tracking_rect,Scalar(0,0,255),3,8,0);
320 
321             after_select_frames++;//总循环每循环一次,计数加1
322             if(after_select_frames>2)//防止跟踪太长,after_select_frames计数溢出
323                 after_select_frames=2;
324         }
325 
326         if(select_show)
327             rectangle(frame,select,Scalar(0,0,255),3,8,0);//显示手动选择的矩形框
328         //显示视频图片到窗口
329         imshow("camera",frame);
330 
331         //    select.zeros();
332         //键盘响应
333         c=(char)waitKey(20);
334         if(27==c)//ESC键
335             return -1;
336     }
337 
338     return 0;
339 }

 

  但是该程序跟踪效果不是很好,不是特别稳定,可能跟粒子数目有关。因为程序还有bug,粒子数目设置太大后,编译通过。当运行时,手动选择目标区域后就自动退出了,即鼠标一拖动完后就在终端显示如下:

  

   Debug程序后出现的错误提示:

  

  后面试了下,当粒子数目大于等于53时,运行程序,手选目标区域后程序没有自动退出,而是显示如下错误提示:

  

  因此,程序只有当粒子数目小于53才能正常运行,但是由于粒子数目太少所以效果不是特别的好。

  等以后有时间来调试下内存方面的bug。

 

 

 

 

posted on 2012-05-08 23:11 tornadomeet 阅读(...) 评论(...) 编辑 收藏

阿萨德发斯蒂芬