首先提供幾篇關於粒子濾波算法的博客：
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 }