上次已經初步體驗了下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。