计算机视觉
图像处理

目标跟踪学习笔记_2(particle filter1)

首先提供几篇关于粒子滤波算法的博客:
http://www.cnblogs.com/yangyangcv/archive/2010/05/23/1742263.html 这篇博客比较通俗易懂,简单的介绍了粒子滤波的基本工作思想和步骤。
http://www.cnblogs.com/lwbaptx/archive/2011/10/20/2218419.html这篇博客用的是opencv1.0,实现的功能是用粒子滤波跟踪鼠标轨迹,有视频演示,效果还不错。
http://blog.csdn.net/yang_xian521/article/details/6928131 这篇博客是用粒子滤波来做视频目标跟踪的,里面也有opencv2.0的代码,有注释,结构比较清晰。我这里就把他的代码和分析贴出来,里面加了我添加了一些注释,这样看起来更加清晰。
代码和解释如下:

  1 // particle_demo.cpp : 定义控制台应用程序的入口点。
  2 //
  3 
  4 #include "stdafx.h"
  5 /************************************************************************/
  6 /* 
  7 Description:    基本的粒子滤波目标跟踪
  8 Author:            Yang Xian
  9 Email:            yang_xian521@163.com
 10 Version:        2011-11-2
 11 History:
 12 */
 13 /************************************************************************/
 14 #include <iostream>    // for standard I/O
 15 #include <string>   // for strings
 16 #include <iomanip>  // for controlling float print precision
 17 #include <sstream>  // string to number conversion
 18 
 19 #include <opencv2/imgproc/imgproc.hpp>  
 20 #include <opencv2/core/core.hpp>        // Basic OpenCV structures (cv::Mat, Scalar)
 21 #include <opencv2/highgui/highgui.hpp>  // OpenCV window I/O
 22 
 23 using namespace cv;
 24 using namespace std;
 25 
 26 // 以下这些参数对结果影响很大,而且也会根据视频内容,会对结果有很大的影响
 27 const int PARTICLE_NUM = 25;    // 粒子个数
 28 // 粒子放入的相关区域
 29 const double A1 = 2.0;
 30 const double A2 = -1.0;
 31 const double B0 = 1.0;
 32 // 高斯随机数sigma参数
 33 const double SIGMA_X = 1.0;
 34 const double SIGMA_Y = 0.5;
 35 const double SIGMA_SCALE = 0.001;
 36 
 37 // 粒子结构体
 38 typedef struct particle {
 39     double x;            // 当前x坐标
 40     double y;            // 当前y坐标
 41     double scale;        // 窗口比例系数
 42     double xPre;            // x坐标预测位置
 43     double yPre;            // y坐标预测位置
 44     double scalePre;        // 窗口预测比例系数
 45     double xOri;            // 原始x坐标
 46     double yOri;            // 原始y坐标
 47     Rect rect;            // 原始区域大小
 48     MatND hist;            // 粒子区域的特征直方图
 49     double weight;        // 该粒子的权重
 50 } PARTICLE;
 51 
 52 Mat hsv;    // hsv色彩空间的输入图像
 53 Mat roiImage;    // 目标区域
 54 MatND roiHist;    // 目标区域直方图
 55 Mat img;    // 输出的目标图像
 56 PARTICLE particles[PARTICLE_NUM];    // 粒子
 57 
 58 int nFrameNum = 0;
 59 
 60 bool bSelectObject = false;    // 区域选择标志
 61 bool bTracking = false;        // 开始跟踪标志
 62 Point origin;    // 鼠标按下时的点位置
 63 Rect selection;// 感兴趣的区域大小
 64 
 65 // 直方图相关参数,特征的选取也会对结果影响巨大
 66 // Quantize the hue to 30 levels
 67 // and the saturation to 32 levels
 68 // value to 10 levels
 69 int hbins = 180, sbins = 256, vbin = 10;
 70 int histSize[] = {hbins, sbins, vbin};
 71 // hue varies from 0 to 179, see cvtColor
 72 float hranges[] = { 0, 180 };
 73 // saturation varies from 0 (black-gray-white) to 255 (pure spectrum color)
 74 float sranges[] = { 0, 256 };
 75 // value varies from 0 (black-gray-white) to 255 (pure spectrum color)
 76 float vranges[] = { 0, 256 };
 77 const float* ranges[] = {hranges, sranges, vranges};
 78 // we compute the histogram from the 0-th and 1-st channels
 79 int channels[] = {0, 1, 2};
 80 
 81 // 鼠标响应函数,得到选择的区域,保存在selection
 82 void onMouse(int event, int x, int y, int, void*)
 83 {
 84     if( bSelectObject )
 85     {
 86         selection.x = MIN(x, origin.x);
 87         selection.y = MIN(y, origin.y);
 88         selection.width = std::abs(x - origin.x);
 89         selection.height = std::abs(y - origin.y);
 90 
 91         selection &= Rect(0, 0, img.cols, img.rows);
 92     }
 93 
 94     switch (event)
 95     {
 96     case CV_EVENT_LBUTTONDOWN:
 97         origin = Point(x,y);
 98         selection = Rect(x,y,0,0);
 99         bSelectObject = true;
100         bTracking = false;
101         break;
102     case CV_EVENT_LBUTTONUP:
103         bSelectObject = false;
104     //    if( selection.width > 0 && selection.height > 0 )
105         bTracking = true;
106         nFrameNum = 0;
107         break;
108     }
109 }
110 
111 // 快速排序算法排序函数
112 int particle_cmp(const void* p1,const void* p2)
113 {
114     PARTICLE* _p1 = (PARTICLE*)p1;
115     PARTICLE* _p2 = (PARTICLE*)p2;
116 
117     if(_p1->weight < _p2->weight)
118         return 1;    //按照权重降序排序
119     if(_p1->weight > _p2->weight)
120         return -1;
121     return 0;
122 }
123 
124 const char* keys = 
125 {
126     "{1|  | 0 | camera number}"
127 };
128 
129 int main(int argc, const char **argv)//这里char **argv前必须用const,why?
130 {
131     int delay = 30;    // 控制播放速度
132     char c;    // 键值
133 
134     /*读取avi文件*/
135     VideoCapture captRefrnc("IndoorGTTest1.avi");    // 视频文件
136 
137     /*打开摄像头*/
138     //VideoCapture captRefrnc;
139 //CommandLineParser parser(argc, argv, keys);//命令解析器函数
140 //int camNum = parser.get<int>("1");   
141 //captRefrnc.open(camNum);
142 
143     if ( !captRefrnc.isOpened())
144     {
145         return -1;
146     }
147 
148     // Windows
149     const char* WIN_RESULT = "Result";
150     namedWindow(WIN_RESULT, CV_WINDOW_AUTOSIZE);
151     //namedWindow(WIN_RESULT, 0);
152 //      namedWindow("Result",0);
153 // 鼠标响应函数
154 //setMouseCallback(WIN_RESULT, onMouse, 0);
155     setMouseCallback("Result", onMouse, 0);
156 
157     Mat frame;    //视频的每一帧图像
158 
159     bool paused = false;
160     PARTICLE * pParticles = particles;//particles为可装PARTICLE_NUM个PARTICLE结构体的数组,所以pParticles为指向其数组的指针
161 
162     while(true) //Show the image captured in the window and repeat
163     {
164         if(!paused)
165         {
166             captRefrnc >> frame;
167             if(frame.empty())
168                 break;
169         }
170 
171         frame.copyTo(img);    // 接下来的操作都是对src的
172 
173 
174 // 选择目标后进行跟踪
175         if (bTracking == true)//鼠标操作选完后
176         {
177             if(!paused)
178             {                
179                 nFrameNum++;//帧数计数器
180                 cvtColor(img, hsv, CV_BGR2HSV);
181                 Mat roiImage(hsv, selection);    // 目标区域,这个构造函数第二个参数表示截取selection部分
182 
183                 if (nFrameNum == 1)    //选择目标后的第一帧需要初始化
184                 {
185                     // step 1: 提取目标区域特征,难道其目标特征就是其色调的直方图分布?
186                     calcHist(&roiImage, 1, channels, Mat(), roiHist, 3, histSize, ranges);
187                     normalize(roiHist, roiHist);    // 归一化L2
188 
189 // step 2: 初始化particle
190                     pParticles = particles;
191                     for (int i=0; i<PARTICLE_NUM; i++)
192                     {
193                         pParticles->x = selection.x + 0.5 * selection.width;
194                         pParticles->y = selection.y + 0.5 * selection.height;
195                         pParticles->xPre = pParticles->x;
196                         pParticles->yPre = pParticles->y;
197                         pParticles->xOri = pParticles->x;
198                         pParticles->yOri = pParticles->y;
199                         pParticles->rect = selection;
200                         pParticles->scale = 1.0;
201                         pParticles->scalePre = 1.0;
202                         pParticles->hist = roiHist;
203                         pParticles->weight = 0;
204                         pParticles++;
205                     }
206                 }
207                 else //不是第一帧
208                 {                    
209                     pParticles = particles;    
210                     RNG rng;//随机数序列产生器
211                     for (int i=0; i<PARTICLE_NUM; i++)
212                     {
213                         // step 3: 求particle的transition,粒子结构中的参数全部更新过
214                         double x, y, s;
215 
216                         pParticles->xPre = pParticles->x;
217                         pParticles->yPre = pParticles->y;
218                         pParticles->scalePre = pParticles->scale;
219 
220                         x = A1 * (pParticles->x - pParticles->xOri) + A2 * (pParticles->xPre - pParticles->xOri) +
221                             B0 * rng.gaussian(SIGMA_X) + pParticles->xOri;//以当前点为中心产生高斯分布的粒子
222                         pParticles->x = std::max(0.0, std::min(x, img.cols-1.0));//其实就是x,只是考虑了边界在内而已
223 
224 
225                         y = A1 * (pParticles->y - pParticles->yOri) + A2 * (pParticles->yPre - pParticles->yOri) +
226                             B0 * rng.gaussian(SIGMA_Y) + pParticles->yOri;
227                         pParticles->y = std::max(0.0, std::min(y, img.rows-1.0));
228 
229                         s = A1 * (pParticles->scale - 1.0) + A2 * (pParticles->scalePre - 1.0) +
230                             B0 * rng.gaussian(SIGMA_SCALE) + 1.0;
231                         pParticles->scale = std::max(0.1, std::min(s, 3.0));
232                         // rect参数有待考证
233                         pParticles->rect.x = std::max(0, std::min(cvRound(pParticles->x - 0.5 * pParticles->rect.width * pParticles->scale), img.cols-1));        // 0 <= x <= img.rows-1
234                         pParticles->rect.y = std::max(0, std::min(cvRound(pParticles->y - 0.5 * pParticles->rect.height * pParticles->scale), img.rows-1));    // 0 <= y <= img.cols-1
235                         pParticles->rect.width = std::min(cvRound(pParticles->rect.width * pParticles->scale), img.cols - pParticles->rect.x);
236                         pParticles->rect.height = std::min(cvRound(pParticles->rect.height * pParticles->scale), img.rows - pParticles->rect.y);
237                         // Ori参数不改变
238 
239 // step 4: 求particle区域的特征直方图
240                         Mat imgParticle(img, pParticles->rect);
241                         calcHist(&imgParticle, 1, channels, Mat(), pParticles->hist, 3, histSize, ranges);
242                         normalize(pParticles->hist, pParticles->hist);    // 归一化L2
243 
244 // step 5: 特征的比对,更新particle权重
245 //compareHist()函数返回2个直方图之间的相似度,因为参数为CV_COMP_INTERSECT,所以返回的是最小直方图值之和
246                         pParticles->weight = compareHist(roiHist, pParticles->hist, CV_COMP_INTERSECT);//其差值直接作为其权值
247 
248                         pParticles++;    
249                     }
250 
251                     // step 6: 归一化粒子权重
252                     double sum = 0.0;
253                     int i;
254 
255                     pParticles = particles;
256                     for(i=0; i<PARTICLE_NUM; i++)
257                     {
258                         sum += pParticles->weight;
259                         pParticles++;
260                     }
261                     pParticles = particles;
262                     for(i=0; i<PARTICLE_NUM; i++)
263                     {
264                         pParticles->weight /= sum;
265                         pParticles++;
266                     }
267 
268                     // step 7: resample根据粒子的权重的后验概率分布重新采样
269                     pParticles = particles;
270                     //                     PARTICLE* newParticles = new PARTICLE[sizeof(PARTICLE) * PARTICLE_NUM];
271                     PARTICLE newParticles[PARTICLE_NUM];
272                     int np, k = 0;
273                     
274                     //qsort()函数为对数组进行快速排序,其中第4个参数表示的是排序是升序还是降序
275                     qsort(pParticles, PARTICLE_NUM, sizeof(PARTICLE), &particle_cmp);//这里采用的是降序排列
276                     for(int i=0; i<PARTICLE_NUM; i++)
277                     {
278                         np = cvRound(particles[i].weight * PARTICLE_NUM);//权值高的优先重采样
279                         for(int j=0; j<np; j++)
280                         {
281                             newParticles[k++] = particles[i];
282                             if(k == PARTICLE_NUM)//重采样后达到了个数要求则直接跳出
283                                 goto EXITOUT;
284                         }
285                     }
286                     while(k < PARTICLE_NUM)
287                     {
288                         newParticles[k++] = particles[0];//个数不够时,将权值最高的粒子重复给
289                     }
290 
291 EXITOUT:
292                     for (int i=0; i<PARTICLE_NUM; i++)
293                     {
294                         particles[i] = newParticles[i];
295                     }
296 
297                 }// end else
298 
299                 qsort(pParticles, PARTICLE_NUM, sizeof(PARTICLE), &particle_cmp);
300 
301                 // step 8: 计算粒子的期望,作为跟踪结果
302                 Rect_<double> rectTrackingTemp(0.0, 0.0, 0.0, 0.0);
303                 pParticles = particles;
304                 for (int i=0; i<PARTICLE_NUM; i++)
305                 {
306                     rectTrackingTemp.x += pParticles->rect.x * pParticles->weight;//坐标加上权重的偏移值
307                     rectTrackingTemp.y += pParticles->rect.y * pParticles->weight;
308                     rectTrackingTemp.width += pParticles->rect.width * pParticles->weight;//宽度也要加上权值的偏移值
309                     rectTrackingTemp.height += pParticles->rect.height * pParticles->weight;
310                     pParticles++;
311                 }
312                 Rect rectTracking(rectTrackingTemp);    // 跟踪结果
313 
314 // 显示各粒子的运动
315                 for (int i=0; i<PARTICLE_NUM; i++)
316                 {
317                     rectangle(img, particles[i].rect, Scalar(255,0,0));
318                 }
319                 // 显示跟踪结果
320                 rectangle(img, rectTracking, Scalar(0,0,255), 3);
321 
322             }
323         }// end Tracking
324 
325 //         imshow(WIN_SRC, frame);
326         imshow(WIN_RESULT, img);
327 
328         c = (char)waitKey(delay);
329         if( c == 27 )
330             break;
331         switch(c)
332         {
333         case 'p'://暂停键
334             paused = !paused;
335             break;
336         default:
337             ;
338         }
339     }// end while
340 }

但是用他的代码在进行鼠标选定后就出现如下错误。

我的工程环境:opencv2.2+vs2010

经过单步调试跟踪后发现,错误的那一行为:

     pParticles->weight = compareHist(roiHist, pParticles->hist, CV_COMP_INTERSECT);

     去掉该行程序可以正常运行,但是完成不了跟踪功能,其目标跟踪框不会移动,只会慢慢收敛到一个点,因为粒子的权重此时没有更新。

查找资料了很久,函数compareHist()的用法并没有错,也不知道是哪里错了。先工作暂时弄到这里,只要对粒子滤波有个感性认识即可。等过段时间再来真正学粒子滤波算法时完成该演示。

 

修改时间:2012.05.08:

    过了这么久,重新学习粒子滤波时,想解决上面那个遗留下来的问题。事实证明C/C++内存搞死人,上面那个问题debug了2天也不懂哪里出错了。自己又用了不少时间理解程序后敲了一遍代码。那个问题暂时解决了,但是跟踪起来根本无效过。修改后的代码如下:

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

转载注明来源:CV视觉网 » 目标跟踪学习笔记_2(particle filter1)

分享到:更多 ()
扫描二维码,给作者 打赏
pay_weixinpay_weixin

请选择你看完该文章的感受:

0不错 0超赞 0无聊 0扯淡 0不解 0路过

评论 3

评论前必须登录!