基于粒子滤波器的目标跟踪算法及实现

原文地址：http://blog.csdn.net/jinshengtao/article/details/30970733

                  http://blog.csdn.net/jinshengtao?viewmode=contents

推荐大家看论文《An adaptive color-based particle filter》

这次我直接截图我的硕士毕业论文的第二章的一部分，应该讲得比较详细了。最后给出我当时在pudn找到的最适合学习的实现代码

代码实现：

运行方式：按P停止，在前景窗口鼠标点击目标，会自动生成外接矩形，再次按P，对该选定目标进行跟踪。

[cpp]  view plain  copy

1. // TwoLevel.cpp : 定义控制台应用程序的入口点。  
2. //  
3.   
4. /************************************************************************/  
5. /*参考文献real-time Multiple Objects Tracking with Occlusion Handling in Dynamic Scenes  */  
6. /************************************************************************/  
7.   
8. #include "stdafx.h"  
9. #include <cv.h>  
10. #include <cxcore.h>  
11. #include <highgui.h>  
12. #include <math.h>  
13. # include <time.h>  
14. #include <iostream>  
15. using namespace std;  
16.   
17.   
18. #define B(image,x,y) ((uchar*)(image->imageData + image->widthStep*(y)))[(x)*3]       //B  
19. #define G(image,x,y) ((uchar*)(image->imageData + image->widthStep*(y)))[(x)*3+1] //G  
20. #define R(image,x,y) ((uchar*)(image->imageData + image->widthStep*(y)))[(x)*3+2] //R  
21. #define S(image,x,y) ((uchar*)(image->imageData + image->widthStep*(y)))[(x)]   
22. #define  Num 10  //帧差的间隔  
23. #define  T 40    //Tf  
24. #define Re 30     //  
25. #define ai 0.08   //学习率  
26.   
27. #define CONTOUR_MAX_AREA 10000  
28. #define CONTOUR_MIN_AREA 50  
29.   
30. # define R_BIN      8  /* 红色分量的直方图条数 */  
31. # define G_BIN      8  /* 绿色分量的直方图条数 */  
32. # define B_BIN      8  /* 兰色分量的直方图条数 */   
33.   
34. # define R_SHIFT    5  /* 与上述直方图条数对应 */  
35. # define G_SHIFT    5  /* 的R、G、B分量左移位数 */  
36. # define B_SHIFT    5  /* log2( 256/8 )为移动位数 */  
37.   
38. /* 
39. 采用Park and Miller方法产生[0,1]之间均匀分布的伪随机数 
40. 算法详细描述见： 
41. [1] NUMERICAL RECIPES IN C: THE ART OF SCIENTIFIC COMPUTING. 
42. Cambridge University Press. 1992. pp.278-279. 
43. [2] Park, S.K., and Miller, K.W. 1988, Communications of the ACM,  
44. vol. 31, pp. 1192–1201. 
45. */  
46.   
47. #define IA 16807  
48. #define IM 2147483647  
49. #define AM (1.0/IM)  
50. #define IQ 127773  
51. #define IR 2836  
52. #define MASK 123459876  
53.   
54.   
55. typedef struct __SpaceState {  /* 状态空间变量 */  
56.     int xt;               /* x坐标位置 */  
57.     int yt;               /* x坐标位置 */  
58.     float v_xt;           /* x方向运动速度 */  
59.     float v_yt;           /* y方向运动速度 */  
60.     int Hxt;              /* x方向半窗宽 */  
61.     int Hyt;              /* y方向半窗宽 */  
62.     float at_dot;         /* 尺度变换速度 */  
63. } SPACESTATE;  
64.   
65.   
66. bool pause=false;//是否暂停  
67. bool track = false;//是否跟踪  
68. IplImage *curframe=NULL;   
69. IplImage *pBackImg=NULL;  
70. IplImage *pFrontImg=NULL;  
71. IplImage *pTrackImg =NULL;  
72. unsigned char * img;//把iplimg改到char*  便于计算  
73. int xin,yin;//跟踪时输入的中心点  
74. int xout,yout;//跟踪时得到的输出中心点  
75. int Wid,Hei;//图像的大小  
76. int WidIn,HeiIn;//输入的半宽与半高  
77. int WidOut,HeiOut;//输出的半宽与半高  
78.   
79. long ran_seed = 802163120; /* 随机数种子，为全局变量，设置缺省值 */  
80.   
81. float DELTA_T = (float)0.05;    /* 帧频，可以为30，25，15，10等 */  
82. int POSITION_DISTURB = 15;      /* 位置扰动幅度   */  
83. float VELOCITY_DISTURB = 40.0;  /* 速度扰动幅值   */  
84. float SCALE_DISTURB = 0.0;      /* 窗宽高扰动幅度 */  
85. float SCALE_CHANGE_D = (float)0.001;   /* 尺度变换速度扰动幅度 */  
86.   
87. int NParticle = 75;       /* 粒子个数   */  
88. float * ModelHist = NULL; /* 模型直方图 */  
89. SPACESTATE * states = NULL;  /* 状态数组 */  
90. float * weights = NULL;   /* 每个粒子的权重 */  
91. int nbin;                 /* 直方图条数 */  
92. float Pi_Thres = (float)0.90; /* 权重阈值   */  
93. float Weight_Thres = (float)0.0001;  /* 最大权重阈值，用来判断是否目标丢失 */  
94.   
95.   
96. /* 
97. 设置种子数 
98. 一般利用系统时间来进行设置，也可以直接传入一个long型整数 
99. */  
100. long set_seed( long setvalue )  
101. {  
102.     if ( setvalue != 0 ) /* 如果传入的参数setvalue!=0，设置该数为种子 */  
103.         ran_seed = setvalue;  
104.     else                 /* 否则，利用系统时间为种子数 */  
105.     {  
106.         ran_seed = time(NULL);  
107.     }  
108.     return( ran_seed );  
109. }  
110.   
111. /* 
112. 计算一幅图像中某个区域的彩色直方图分布 
113. 输入参数： 
114. int x0, y0：           指定图像区域的中心点 
115. int Wx, Hy：           指定图像区域的半宽和半高 
116. unsigned char * image：图像数据，按从左至右，从上至下的顺序扫描， 
117. 颜色排列次序：RGB, RGB, ... 
118. (或者：YUV, YUV, ...) 
119. int W, H：             图像的宽和高 
120. 输出参数： 
121. float * ColorHist：    彩色直方图，颜色索引按： 
122. i = r * G_BIN * B_BIN + g * B_BIN + b排列 
123. int bins：             彩色直方图的条数R_BIN*G_BIN*B_BIN（这里取8x8x8=512） 
124. */  
125. void CalcuColorHistogram( int x0, int y0, int Wx, int Hy,   
126.                          unsigned char * image, int W, int H,  
127.                          float * ColorHist, int bins )  
128. {  
129.     int x_begin, y_begin;  /* 指定图像区域的左上角坐标 */  
130.     int y_end, x_end;  
131.     int x, y, i, index;  
132.     int r, g, b;  
133.     float k, r2, f;  
134.     int a2;  
135.   
136.     for ( i = 0; i < bins; i++ )     /* 直方图各个值赋0 */  
137.         ColorHist[i] = 0.0;  
138.     /* 考虑特殊情况：x0, y0在图像外面，或者，Wx<=0, Hy<=0 */  
139.     /* 此时强制令彩色直方图为0 */  
140.     if ( ( x0 < 0 ) || (x0 >= W) || ( y0 < 0 ) || ( y0 >= H )   
141.         || ( Wx <= 0 ) || ( Hy <= 0 ) ) return;  
142.   
143.     x_begin = x0 - Wx;               /* 计算实际高宽和区域起始点 */  
144.     y_begin = y0 - Hy;  
145.     if ( x_begin < 0 ) x_begin = 0;  
146.     if ( y_begin < 0 ) y_begin = 0;  
147.     x_end = x0 + Wx;  
148.     y_end = y0 + Hy;  
149.     if ( x_end >= W ) x_end = W-1;  
150.     if ( y_end >= H ) y_end = H-1;  
151.     a2 = Wx*Wx+Hy*Hy;                /* 计算核函数半径平方a^2 */  
152.     f = 0.0;                         /* 归一化系数 */  
153.     for ( y = y_begin; y <= y_end; y++ )  
154.         for ( x = x_begin; x <= x_end; x++ )  
155.         {  
156.             r = image[(y*W+x)*3] >> R_SHIFT;   /* 计算直方图 */  
157.             g = image[(y*W+x)*3+1] >> G_SHIFT; /*移位位数根据R、G、B条数 */  
158.             b = image[(y*W+x)*3+2] >> B_SHIFT;  
159.             index = r * G_BIN * B_BIN + g * B_BIN + b;  
160.             r2 = (float)(((y-y0)*(y-y0)+(x-x0)*(x-x0))*1.0/a2); /* 计算半径平方r^2 */  
161.             k = 1 - r2;   /* 核函数k(r) = 1-r^2, |r| < 1; 其他值 k(r) = 0 */  
162.             f = f + k;  
163.             ColorHist[index] = ColorHist[index] + k;  /* 计算核密度加权彩色直方图 */  
164.         }  
165.         for ( i = 0; i < bins; i++ )     /* 归一化直方图 */  
166.             ColorHist[i] = ColorHist[i]/f;  
167.   
168.         return;  
169. }  
170.   
171. /* 
172. 计算Bhattacharyya系数 
173. 输入参数： 
174. float * p, * q：      两个彩色直方图密度估计 
175. int bins：            直方图条数 
176. 返回值： 
177. Bhattacharyya系数 
178. */  
179. float CalcuBhattacharyya( float * p, float * q, int bins )  
180. {  
181.     int i;  
182.     float rho;  
183.   
184.     rho = 0.0;  
185.     for ( i = 0; i < bins; i++ )  
186.         rho = (float)(rho + sqrt( p[i]*q[i] ));  
187.   
188.     return( rho );  
189. }  
190.   
191.   
192. /*# define RECIP_SIGMA  3.98942280401  / * 1/(sqrt(2*pi)*sigma), 这里sigma = 0.1 * /*/  
193. # define SIGMA2       0.02           /* 2*sigma^2, 这里sigma = 0.1 */  
194.   
195. float CalcuWeightedPi( float rho )  
196. {  
197.     float pi_n, d2;  
198.   
199.     d2 = 1 - rho;  
200.     //pi_n = (float)(RECIP_SIGMA * exp( - d2/SIGMA2 ));  
201.     pi_n = (float)(exp( - d2/SIGMA2 ));  
202.   
203.     return( pi_n );  
204. }  
205.   
206. /* 
207. 采用Park and Miller方法产生[0,1]之间均匀分布的伪随机数 
208. 算法详细描述见： 
209. [1] NUMERICAL RECIPES IN C: THE ART OF SCIENTIFIC COMPUTING. 
210. Cambridge University Press. 1992. pp.278-279. 
211. [2] Park, S.K., and Miller, K.W. 1988, Communications of the ACM,  
212. vol. 31, pp. 1192–1201. 
213. */  
214.   
215. float ran0(long *idum)  
216. {  
217.     long k;  
218.     float ans;  
219.   
220.     /* *idum ^= MASK;*/      /* XORing with MASK allows use of zero and other */  
221.     k=(*idum)/IQ;            /* simple bit patterns for idum.                 */  
222.     *idum=IA*(*idum-k*IQ)-IR*k;  /* Compute idum=(IA*idum) % IM without over- */  
223.     if (*idum < 0) *idum += IM;  /* flows by Schrage’s method.               */  
224.     ans=AM*(*idum);          /* Convert idum to a floating result.            */  
225.     /* *idum ^= MASK;*/      /* Unmask before return.                         */  
226.     return ans;  
227. }  
228.   
229.   
230. /* 
231. 获得一个[0,1]之间均匀分布的随机数 
232. */  
233. float rand0_1()  
234. {  
235.     return( ran0( &ran_seed ) );  
236. }  
237.   
238.   
239.   
240. /* 
241. 获得一个x - N(u,sigma)Gaussian分布的随机数 
242. */  
243. float randGaussian( float u, float sigma )  
244. {  
245.     float x1, x2, v1, v2;  
246.     float s = 100.0;  
247.     float y;  
248.   
249.     /* 
250.     使用筛选法产生正态分布N(0,1)的随机数(Box-Mulles方法) 
251.     1. 产生[0,1]上均匀随机变量X1,X2 
252.     2. 计算V1=2*X1-1,V2=2*X2-1,s=V1^2+V2^2 
253.     3. 若s<=1,转向步骤4，否则转1 
254.     4. 计算A=(-2ln(s)/s)^(1/2),y1=V1*A, y2=V2*A 
255.     y1,y2为N(0,1)随机变量 
256.     */  
257.     while ( s > 1.0 )  
258.     {  
259.         x1 = rand0_1();  
260.         x2 = rand0_1();  
261.         v1 = 2 * x1 - 1;  
262.         v2 = 2 * x2 - 1;  
263.         s = v1*v1 + v2*v2;  
264.     }  
265.     y = (float)(sqrt( -2.0 * log(s)/s ) * v1);  
266.     /* 
267.     根据公式 
268.     z = sigma * y + u 
269.     将y变量转换成N(u,sigma)分布 
270.     */  
271.     return( sigma * y + u );      
272. }  
273.   
274.   
275.   
276. /* 
277. 初始化系统 
278. int x0, y0：        初始给定的图像目标区域坐标 
279. int Wx, Hy：        目标的半宽高 
280. unsigned char * img：图像数据，RGB形式 
281. int W, H：          图像宽高 
282. */  
283. int Initialize( int x0, int y0, int Wx, int Hy,  
284.                unsigned char * img, int W, int H )  
285. {  
286.     int i, j;  
287.     float rn[7];  
288.   
289.     set_seed( 0 ); /* 使用系统时钟作为种子，这个函数在 */  
290.     /* 系统初始化时候要调用一次,且仅调用1次 */  
291.     //NParticle = 75; /* 采样粒子个数 */  
292.     //Pi_Thres = (float)0.90; /* 设置权重阈值 */  
293.     states = new SPACESTATE [NParticle]; /* 申请状态数组的空间 */  
294.     if ( states == NULL ) return( -2 );  
295.     weights = new float [NParticle];     /* 申请粒子权重数组的空间 */  
296.     if ( weights == NULL ) return( -3 );      
297.     nbin = R_BIN * G_BIN * B_BIN; /* 确定直方图条数 */  
298.     ModelHist = new float [nbin]; /* 申请直方图内存 */  
299.     if ( ModelHist == NULL ) return( -1 );  
300.   
301.     /* 计算目标模板直方图 */  
302.     CalcuColorHistogram( x0, y0, Wx, Hy, img, W, H, ModelHist, nbin );  
303.   
304.     /* 初始化粒子状态(以(x0,y0,1,1,Wx,Hy,0.1)为中心呈N(0,0.4)正态分布) */  
305.     states[0].xt = x0;  
306.     states[0].yt = y0;  
307.     states[0].v_xt = (float)0.0; // 1.0  
308.     states[0].v_yt = (float)0.0; // 1.0  
309.     states[0].Hxt = Wx;  
310.     states[0].Hyt = Hy;  
311.     states[0].at_dot = (float)0.0; // 0.1  
312.     weights[0] = (float)(1.0/NParticle); /* 0.9; */  
313.     for ( i = 1; i < NParticle; i++ )  
314.     {  
315.         for ( j = 0; j < 7; j++ ) rn[j] = randGaussian( 0, (float)0.6 ); /* 产生7个随机高斯分布的数 */  
316.         states[i].xt = (int)( states[0].xt + rn[0] * Wx );  
317.         states[i].yt = (int)( states[0].yt + rn[1] * Hy );  
318.         states[i].v_xt = (float)( states[0].v_xt + rn[2] * VELOCITY_DISTURB );  
319.         states[i].v_yt = (float)( states[0].v_yt + rn[3] * VELOCITY_DISTURB );  
320.         states[i].Hxt = (int)( states[0].Hxt + rn[4] * SCALE_DISTURB );  
321.         states[i].Hyt = (int)( states[0].Hyt + rn[5] * SCALE_DISTURB );  
322.         states[i].at_dot = (float)( states[0].at_dot + rn[6] * SCALE_CHANGE_D );  
323.         /* 权重统一为1/N，让每个粒子有相等的机会 */  
324.         weights[i] = (float)(1.0/NParticle);  
325.     }  
326.   
327.     return( 1 );  
328. }  
329.   
330.   
331.   
332. /* 
333. 计算归一化累计概率c'_i 
334. 输入参数： 
335. float * weight：    为一个有N个权重（概率）的数组 
336. int N：             数组元素个数 
337. 输出参数： 
338. float * cumulateWeight： 为一个有N+1个累计权重的数组， 
339. cumulateWeight[0] = 0; 
340. */  
341. void NormalizeCumulatedWeight( float * weight, float * cumulateWeight, int N )  
342. {  
343.     int i;  
344.   
345.     for ( i = 0; i < N+1; i++ )   
346.         cumulateWeight[i] = 0;  
347.     for ( i = 0; i < N; i++ )  
348.         cumulateWeight[i+1] = cumulateWeight[i] + weight[i];  
349.     for ( i = 0; i < N+1; i++ )  
350.         cumulateWeight[i] = cumulateWeight[i]/ cumulateWeight[N];  
351.   
352.     return;  
353. }  
354.   
355. /* 
356. 折半查找，在数组NCumuWeight[N]中寻找一个最小的j，使得 
357. NCumuWeight[j] <=v 
358. float v：              一个给定的随机数 
359. float * NCumuWeight：  权重数组 
360. int N：                数组维数 
361. 返回值： 
362. 数组下标序号 
363. */  
364. int BinearySearch( float v, float * NCumuWeight, int N )  
365. {  
366.     int l, r, m;  
367.   
368.     l = 0;  r = N-1;   /* extreme left and extreme right components' indexes */  
369.     while ( r >= l)  
370.     {  
371.         m = (l+r)/2;  
372.         if ( v >= NCumuWeight[m] && v < NCumuWeight[m+1] ) return( m );  
373.         if ( v < NCumuWeight[m] ) r = m - 1;  
374.         else l = m + 1;  
375.     }  
376.     return( 0 );  
377. }  
378.   
379. /* 
380. 重新进行重要性采样 
381. 输入参数： 
382. float * c：          对应样本权重数组pi(n) 
383. int N：              权重数组、重采样索引数组元素个数 
384. 输出参数： 
385. int * ResampleIndex：重采样索引数组 
386. */  
387. void ImportanceSampling( float * c, int * ResampleIndex, int N )  
388. {  
389.     float rnum, * cumulateWeight;  
390.     int i, j;  
391.   
392.     cumulateWeight = new float [N+1]; /* 申请累计权重数组内存，大小为N+1 */  
393.     NormalizeCumulatedWeight( c, cumulateWeight, N ); /* 计算累计权重 */  
394.     for ( i = 0; i < N; i++ )  
395.     {  
396.         rnum = rand0_1();       /* 随机产生一个[0,1]间均匀分布的数 */   
397.         j = BinearySearch( rnum, cumulateWeight, N+1 ); /* 搜索<=rnum的最小索引j */  
398.         if ( j == N ) j--;  
399.         ResampleIndex[i] = j;   /* 放入重采样索引数组 */       
400.     }  
401.   
402.     delete cumulateWeight;  
403.   
404.     return;   
405. }  
406.   
407. /* 
408. 样本选择，从N个输入样本中根据权重重新挑选出N个 
409. 输入参数： 
410. SPACESTATE * state：     原始样本集合（共N个） 
411. float * weight：         N个原始样本对应的权重 
412. int N：                  样本个数 
413. 输出参数： 
414. SPACESTATE * state：     更新过的样本集 
415. */  
416. void ReSelect( SPACESTATE * state, float * weight, int N )  
417. {  
418.     SPACESTATE * tmpState;  
419.     int i, * rsIdx;  
420.   
421.     tmpState = new SPACESTATE[N];  
422.     rsIdx = new int[N];  
423.   
424.     ImportanceSampling( weight, rsIdx, N ); /* 根据权重重新采样 */  
425.     for ( i = 0; i < N; i++ )  
426.         tmpState[i] = state[rsIdx[i]];//temState为临时变量,其中state[i]用state[rsIdx[i]]来代替  
427.     for ( i = 0; i < N; i++ )  
428.         state[i] = tmpState[i];  
429.   
430.     delete[] tmpState;  
431.     delete[] rsIdx;  
432.   
433.     return;  
434. }  
435.   
436. /* 
437. 传播：根据系统状态方程求取状态预测量 
438. 状态方程为： S(t) = A S(t-1) + W(t-1) 
439. W(t-1)为高斯噪声 
440. 输入参数： 
441. SPACESTATE * state：      待求的状态量数组 
442. int N：                   待求状态个数 
443. 输出参数： 
444. SPACESTATE * state：      更新后的预测状态量数组 
445. */  
446. void Propagate( SPACESTATE * state, int N)  
447. {  
448.     int i;  
449.     int j;  
450.     float rn[7];  
451.   
452.     /* 对每一个状态向量state[i](共N个)进行更新 */  
453.     for ( i = 0; i < N; i++ )  /* 加入均值为0的随机高斯噪声 */  
454.     {  
455.         for ( j = 0; j < 7; j++ ) rn[j] = randGaussian( 0, (float)0.6 ); /* 产生7个随机高斯分布的数 */  
456.         state[i].xt = (int)(state[i].xt + state[i].v_xt * DELTA_T + rn[0] * state[i].Hxt + 0.5);  
457.         state[i].yt = (int)(state[i].yt + state[i].v_yt * DELTA_T + rn[1] * state[i].Hyt + 0.5);  
458.         state[i].v_xt = (float)(state[i].v_xt + rn[2] * VELOCITY_DISTURB);  
459.         state[i].v_yt = (float)(state[i].v_yt + rn[3] * VELOCITY_DISTURB);  
460.         state[i].Hxt = (int)(state[i].Hxt+state[i].Hxt*state[i].at_dot + rn[4] * SCALE_DISTURB + 0.5);  
461.         state[i].Hyt = (int)(state[i].Hyt+state[i].Hyt*state[i].at_dot + rn[5] * SCALE_DISTURB + 0.5);  
462.         state[i].at_dot = (float)(state[i].at_dot + rn[6] * SCALE_CHANGE_D);  
463.         cvCircle(pTrackImg,cvPoint(state[i].xt,state[i].yt),3, CV_RGB(0,255,0),-1);  
464.     }  
465.     return;  
466. }  
467.   
468. /* 
469. 观测，根据状态集合St中的每一个采样，观测直方图，然后 
470. 更新估计量，获得新的权重概率 
471. 输入参数： 
472. SPACESTATE * state：      状态量数组 
473. int N：                   状态量数组维数 
474. unsigned char * image：   图像数据，按从左至右，从上至下的顺序扫描， 
475. 颜色排列次序：RGB, RGB, ...                          
476. int W, H：                图像的宽和高 
477. float * ObjectHist：      目标直方图 
478. int hbins：               目标直方图条数 
479. 输出参数： 
480. float * weight：          更新后的权重 
481. */  
482. void Observe( SPACESTATE * state, float * weight, int N,  
483.              unsigned char * image, int W, int H,  
484.              float * ObjectHist, int hbins )  
485. {  
486.     int i;  
487.     float * ColorHist;  
488.     float rho;  
489.   
490.     ColorHist = new float[hbins];  
491.   
492.     for ( i = 0; i < N; i++ )  
493.     {  
494.         /* (1) 计算彩色直方图分布 */  
495.         CalcuColorHistogram( state[i].xt, state[i].yt,state[i].Hxt, state[i].Hyt,  
496.             image, W, H, ColorHist, hbins );  
497.         /* (2) Bhattacharyya系数 */  
498.         rho = CalcuBhattacharyya( ColorHist, ObjectHist, hbins );  
499.         /* (3) 根据计算得的Bhattacharyya系数计算各个权重值 */  
500.         weight[i] = CalcuWeightedPi( rho );       
501.     }  
502.   
503.     delete ColorHist;  
504.   
505.     return;   
506. }  
507.   
508. /* 
509. 估计，根据权重，估计一个状态量作为跟踪输出 
510. 输入参数： 
511. SPACESTATE * state：      状态量数组 
512. float * weight：          对应权重 
513. int N：                   状态量数组维数 
514. 输出参数： 
515. SPACESTATE * EstState：   估计出的状态量 
516. */  
517. void Estimation( SPACESTATE * state, float * weight, int N,   
518.                 SPACESTATE & EstState )  
519. {  
520.     int i;  
521.     float at_dot, Hxt, Hyt, v_xt, v_yt, xt, yt;  
522.     float weight_sum;  
523.   
524.     at_dot = 0;  
525.     Hxt = 0;    Hyt = 0;  
526.     v_xt = 0;   v_yt = 0;  
527.     xt = 0;     yt = 0;  
528.     weight_sum = 0;  
529.     for ( i = 0; i < N; i++ ) /* 求和 */  
530.     {  
531.         at_dot += state[i].at_dot * weight[i];  
532.         Hxt += state[i].Hxt * weight[i];  
533.         Hyt += state[i].Hyt * weight[i];  
534.         v_xt += state[i].v_xt * weight[i];  
535.         v_yt += state[i].v_yt * weight[i];  
536.         xt += state[i].xt * weight[i];  
537.         yt += state[i].yt * weight[i];  
538.         weight_sum += weight[i];  
539.     }  
540.     /* 求平均 */  
541.     if ( weight_sum <= 0 ) weight_sum = 1; /* 防止被0除，一般不会发生 */  
542.     EstState.at_dot = at_dot/weight_sum;  
543.     EstState.Hxt = (int)(Hxt/weight_sum + 0.5 );  
544.     EstState.Hyt = (int)(Hyt/weight_sum + 0.5 );  
545.     EstState.v_xt = v_xt/weight_sum;  
546.     EstState.v_yt = v_yt/weight_sum;  
547.     EstState.xt = (int)(xt/weight_sum + 0.5 );  
548.     EstState.yt = (int)(yt/weight_sum + 0.5 );  
549.   
550.     return;  
551. }  
552.   
553.   
554. /************************************************************ 
555. 模型更新 
556. 输入参数： 
557. SPACESTATE EstState：   状态量的估计值 
558. float * TargetHist：    目标直方图 
559. int bins：              直方图条数 
560. float PiT：             阈值（权重阈值） 
561. unsigned char * img：   图像数据，RGB形式 
562. int W, H：              图像宽高  
563. 输出： 
564. float * TargetHist：    更新的目标直方图 
565. ************************************************************/  
566. # define ALPHA_COEFFICIENT      0.2     /* 目标模型更新权重取0.1-0.3 */  
567.   
568. int ModelUpdate( SPACESTATE EstState, float * TargetHist, int bins, float PiT,  
569.                 unsigned char * img, int W, int H )  
570. {  
571.     float * EstHist, Bha, Pi_E;  
572.     int i, rvalue = -1;  
573.   
574.     EstHist = new float [bins];  
575.   
576.     /* (1)在估计值处计算目标直方图 */  
577.     CalcuColorHistogram( EstState.xt, EstState.yt, EstState.Hxt,   
578.         EstState.Hyt, img, W, H, EstHist, bins );  
579.     /* (2)计算Bhattacharyya系数 */  
580.     Bha  = CalcuBhattacharyya( EstHist, TargetHist, bins );  
581.     /* (3)计算概率权重 */  
582.     Pi_E = CalcuWeightedPi( Bha );  
583.   
584.     if ( Pi_E > PiT )   
585.     {  
586.         for ( i = 0; i < bins; i++ )  
587.         {  
588.             TargetHist[i] = (float)((1.0 - ALPHA_COEFFICIENT) * TargetHist[i]  
589.             + ALPHA_COEFFICIENT * EstHist[i]);  
590.         }  
591.         rvalue = 1;  
592.     }  
593.   
594.     delete EstHist;  
595.   
596.     return( rvalue );  
597. }  
598.   
599. /* 
600. 系统清除 
601. */  
602. void ClearAll()  
603. {  
604.     if ( ModelHist != NULL ) delete [] ModelHist;  
605.     if ( states != NULL ) delete [] states;  
606.     if ( weights != NULL ) delete [] weights;  
607.   
608.     return;  
609. }  
610.   
611. /********************************************************************** 
612. 基于彩色直方图的粒子滤波算法总流程 
613. 输入参数： 
614. unsigned char * img： 图像数据，RGB形式 
615. int W, H：            图像宽高 
616. 输出参数： 
617. int &xc, &yc：        找到的图像目标区域中心坐标 
618. int &Wx_h, &Hy_h：    找到的目标的半宽高  
619. float &max_weight：   最大权重值 
620. 返回值：               
621. 成功1，否则-1 
622.  
623. 基于彩色直方图的粒子滤波跟踪算法的完整使用方法为： 
624. （1）读取彩色视频中的1帧，并确定初始区域，以此获得该区域的中心点、 
625. 目标的半高、宽，和图像数组（RGB形式）、图像高宽参数。 
626. 采用初始化函数进行初始化 
627. int Initialize( int x0, int y0, int Wx, int Hy, 
628. unsigned char * img, int W, int H ) 
629. （2）循环调用下面函数，直到N帧图像结束 
630. int ColorParticleTracking( unsigned char * image, int W, int H,  
631. int & xc, int & yc, int & Wx_h, int & Hy_h ) 
632. 每次调用的输出为：目标中心坐标和目标的半高宽 
633. 如果函数返回值<0，则表明目标丢失。 
634. （3）清除系统各个变量，结束跟踪 
635. void ClearAll() 
636.  
637. **********************************************************************/  
638. int ColorParticleTracking( unsigned char * image, int W, int H,   
639.                           int & xc, int & yc, int & Wx_h, int & Hy_h,  
640.                           float & max_weight)  
641. {  
642.     SPACESTATE EState;  
643.     int i;  
644.     /* 选择：选择样本，并进行重采样 */  
645.     ReSelect( states, weights, NParticle );  
646.     /* 传播：采样状态方程，对状态变量进行预测 */  
647.     Propagate( states, NParticle);  
648.     /* 观测：对状态量进行更新 */  
649.     Observe( states, weights, NParticle, image, W, H,  
650.         ModelHist, nbin );  
651.     /* 估计：对状态量进行估计，提取位置量 */  
652.     Estimation( states, weights, NParticle, EState );  
653.     xc = EState.xt;  
654.     yc = EState.yt;  
655.     Wx_h = EState.Hxt;  
656.     Hy_h = EState.Hyt;  
657.     /* 模型更新 */  
658.     ModelUpdate( EState, ModelHist, nbin, Pi_Thres, image, W, H );  
659.   
660.     /* 计算最大权重值 */  
661.     max_weight = weights[0];  
662.     for ( i = 1; i < NParticle; i++ )  
663.         max_weight = max_weight < weights[i] ? weights[i] : max_weight;  
664.     /* 进行合法性检验，不合法返回-1 */  
665.     if ( xc < 0 || yc < 0 || xc >= W || yc >= H ||  
666.         Wx_h <= 0 || Hy_h <= 0 ) return( -1 );  
667.     else   
668.         return( 1 );          
669. }  
670.   
671.   
672.   
673. //把iplimage 转到img 数组中,BGR->RGB  
674. void IplToImge(IplImage* src, int w,int h)  
675. {  
676.     int i,j;  
677.     for ( j = 0; j < h; j++ ) // 转成正向图像  
678.         for ( i = 0; i < w; i++ )  
679.         {  
680.             img[ ( j*w+i )*3 ] = R(src,i,j);  
681.             img[ ( j*w+i )*3+1 ] = G(src,i,j);  
682.             img[ ( j*w+i )*3+2 ] = B(src,i,j);  
683.         }  
684. }  
685. void mouseHandler(int event, int x, int y, int flags, void* param)//在这里要注意到要再次调用cvShowImage，才能显示方框  
686. {  
687.     CvMemStorage* storage = cvCreateMemStorage(0);  
688.     CvSeq * contours;  
689.     IplImage* pFrontImg1 = 0;  
690.     int centerX,centerY;  
691.     int delt = 10;  
692.     pFrontImg1=cvCloneImage(pFrontImg);//这里也要注意到如果在 cvShowImage("foreground",pFrontImg1)中用pFrontImg产效果，得重新定义并复制  
693.     switch(event){  
694.       case CV_EVENT_LBUTTONDOWN:  
695.           //printf("laskjfkoasfl\n");  
696.           //寻找轮廓  
697.           if(pause)  
698.           {  
699.               cvFindContours(pFrontImg,storage,&contours,sizeof(CvContour),CV_RETR_EXTERNAL,  
700.                   CV_CHAIN_APPROX_SIMPLE);  
701.   
702.               //在原场景中绘制目标轮廓的外接矩形  
703.               for (;contours;contours = contours->h_next)     
704.               {  
705.                   CvRect r = ((CvContour*)contours)->rect;  
706.                   if(x>r.x&&x<(r.x+r.width)&&y>r.y&&r.y<(r.y+r.height))  
707.                   {  
708.                       if (r.height*r.width>CONTOUR_MIN_AREA && r.height*r.width<CONTOUR_MAX_AREA)  
709.                       {  
710.                           centerX = r.x+r.width/2;//得到目标中心点  
711.                           centerY = r.y+r.height/2;  
712.                           WidIn = r.width/2;//得到目标半宽与半高  
713.                           HeiIn = r.height/2;  
714.                           xin = centerX;  
715.                           yin = centerY;  
716.                           cvRectangle(pFrontImg1,cvPoint(r.x,r.y),cvPoint(r.x+r.width,r.y+r.height),cvScalar(255,255,255),2,8,0);     
717.                           //Initial_MeanShift_tracker(centerX,centerY,WidIn,HeiIn,img,Wid,Hei,1./delt);  //初始化跟踪变量  
718.                           /* 初始化跟踪器 */  
719.                           Initialize( centerX, centerY, WidIn, HeiIn, img, Wid, Hei );  
720.                           track = true;//进行跟踪  
721.                           cvShowImage("foreground",pFrontImg1);  
722.                           return;  
723.   
724.                       }  
725.                   }  
726.   
727.               }  
728.           }  
729.   
730.           break;  
731.   
732.           case CV_EVENT_LBUTTONUP:  
733.                   printf("Left button up\n");  
734.                   break;  
735.     }  
736. }  
737. //void on_mouse(int event, int x, int y, int flags, void *param)  
738. //{  
739. //  if(!image)  
740. //      return ;  
741. //  if(image->origin)  
742. //  {  
743. //      image->origin = 0;  
744. //      y = image->height - y;  
745. //  }  
746. //  if(selecting) //正在选择物体  
747. //  {  
748. //      selection.x = MIN(x,origin.x);  
749. //      selection.y = MIN(y,origin.y);  
750. //      selection.width = selection.x + CV_IABS(x - origin.x);  
751. //      selection.height = selection.y + CV_IABS(y - origin.y);  
752. //  
753. //      selection.x = MAX(selection.x ,0);  
754. //      selection.y = MAX(selection.y,0);  
755. //      selection.width = MIN(selection.width,image->width);  
756. //      selection.height = MIN(selection.height,image->height);  
757. //      selection.width -= selection.x;  
758. //      selection.height -= selection.y;  
759. //  }  
760. //  switch(event)  
761. //  {  
762. //  case CV_EVENT_LBUTTONDOWN:  
763. //      origin = cvPoint(x,y);  
764. //      selection = cvRect(x,y,0,0);  
765. //      selecting = 1;  
766. //      break;  
767. //  case CV_EVENT_LBUTTONUP:  
768. //      selecting = 0;  
769. //      if(selection.width >0 && selection.height >0)  
770. //          selected = 1;  
771. //      break;  
772. //  }  
773. //}  
774.   
775. void main()  
776. {  
777.     int FrameNum=0;  //帧号  
778.     int k=0;  
779.     CvCapture *capture = cvCreateFileCapture("test.avi");  
780.     char res1[20],res2[20];  
781.     //CvCapture *capture = cvCreateFileCapture("test1.avi");  
782.     //CvCapture *capture = cvCreateFileCapture("camera1_mov.avi");  
783.     IplImage* frame[Num]; //用来存放图像  
784.     int i,j;  
785.     uchar key = false;      //用来设置暂停  
786.     float rho_v;//表示相似度  
787.     float max_weight;  
788.   
789.     int sum=0;    //用来存放两图像帧差后的值  
790.     for (i=0;i<Num;i++)  
791.     {  
792.         frame[i]=NULL;  
793.     }  
794.   
795.     IplImage *curFrameGray=NULL;  
796.     IplImage *frameGray=NULL;  
797.   
798.     CvMat *Mat_D,*Mat_F;   //动态矩阵与帧差后矩阵  
799.     int row ,col;  
800.     cvNamedWindow("video",1);  
801.   
802.     cvNamedWindow("background",1);   
803.     cvNamedWindow("foreground",1);     
804.     cvNamedWindow("tracking",1);  
805.     cvSetMouseCallback("tracking",mouseHandler,0);//响应鼠标  
806.       
807.     while (capture)  
808.     {  
809.         curframe=cvQueryFrame(capture); //抓取一帧  
810.         if(FrameNum<Num)  
811.         {  
812.             if(FrameNum==0)//第一帧时初始化过程  
813.             {  
814.                 curFrameGray=cvCreateImage(cvGetSize(curframe),IPL_DEPTH_8U,1);  
815.                 frameGray=cvCreateImage(cvGetSize(curframe),IPL_DEPTH_8U,1);  
816.                 pBackImg=cvCreateImage(cvGetSize(curframe),IPL_DEPTH_8U,1);  
817.                 pFrontImg=cvCreateImage(cvGetSize(curframe),IPL_DEPTH_8U,1);  
818.                 pTrackImg = cvCreateImage(cvGetSize(curframe),IPL_DEPTH_8U,3);  
819.   
820.                 cvSetZero(pFrontImg);    
821.                 cvCvtColor(curframe,pBackImg,CV_RGB2GRAY);  
822.   
823.                 row=curframe->height;  
824.                 col=curframe->width;  
825.                 Mat_D=cvCreateMat(row,col,CV_32FC1);  
826.                 cvSetZero(Mat_D);    
827.                 Mat_F=cvCreateMat(row,col,CV_32FC1);  
828.                 cvSetZero(Mat_F);  
829.                 Wid = curframe->width;  
830.                 Hei = curframe->height;   
831.                 img = new unsigned char [Wid * Hei * 3];  
832.             }  
833.             frame[k]=cvCloneImage(curframe);  //把前num帧存入到图像数组  
834.             pTrackImg = cvCloneImage(curframe);  
835.         }  
836.         else  
837.         {  
838.             k=FrameNum%Num;  
839.             pTrackImg = cvCloneImage(curframe);  
840.             IplToImge(curframe,Wid,Hei);  
841.             cvCvtColor(curframe,curFrameGray,CV_RGB2GRAY);  
842.             cvCvtColor(frame[k],frameGray,CV_RGB2GRAY);   
843.             for(i=0;i<curframe->height;i++)  
844.                 for(j=0;j<curframe->width;j++)  
845.                 {  
846.                     sum=S(curFrameGray,j,i)-S(frameGray,j,i);  
847.                     sum=sum<0 ? -sum : sum;  
848.                     if(sum>T)   //文献中公式（1）  
849.                     {  
850.                         CV_MAT_ELEM(*Mat_F,float,i,j)=1;  
851.                     }  
852.                     else   
853.                     {  
854.                         CV_MAT_ELEM(*Mat_F,float,i,j)=0;  
855.                     }  
856.   
857.                     if(CV_MAT_ELEM(*Mat_F,float,i,j)!=0)//文献中公式（2）  
858.                         CV_MAT_ELEM(*Mat_D,float,i,j)=Re;  
859.                     else{  
860.                         if(CV_MAT_ELEM(*Mat_D,float,i,j)!=0)  
861.                             CV_MAT_ELEM(*Mat_D,float,i,j)=CV_MAT_ELEM(*Mat_D,float,i,j)-1;  
862.                     }  
863.                     if(CV_MAT_ELEM(*Mat_D,float,i,j)==0.0)  
864.                     {  
865.                         //文献中公式（3）  
866.                         S(pBackImg,j,i)=(uchar)((1-ai)*S(pBackImg,j,i)+ai*S(curFrameGray,j,i));  
867.                     }  
868.                     sum=S(curFrameGray,j,i)-S(pBackImg,j,i);//背景差分法  
869.                     sum=sum<0 ? -sum : sum;  
870.                     if(sum>40)  
871.                     {  
872.                         S(pFrontImg,j,i)=255;  
873.                     }  
874.                     else   
875.                         S(pFrontImg,j,i)=0;  
876.   
877.                 }  
878.                 frame[k]=cvCloneImage(curframe);   
879.         }  
880.         FrameNum++;   
881.         k++;  
882.         cout<<FrameNum<<endl;  
883.   
884.         //进行形态学滤波，去噪  
885.         cvDilate(pFrontImg, pFrontImg, 0, 2);  
886.         cvErode(pFrontImg, pFrontImg, 0, 3);  
887.         cvDilate(pFrontImg, pFrontImg, 0, 1);  
888.         if(track)  
889.         {  
890.             /* 跟踪一帧 */  
891.             rho_v = ColorParticleTracking( img, Wid, Hei, xout, yout, WidOut, HeiOut, max_weight);  
892.             /* 画框: 新位置为蓝框 */  
893.             if ( rho_v > 0 && max_weight > 0.0001 )  /* 判断是否目标丢失 */  
894.             {  
895.                     cvRectangle(pFrontImg,cvPoint(xout - WidOut,yout - HeiOut),cvPoint(xout+WidOut,yout+HeiOut),cvScalar(255,255,255),2,8,0);  
896.                     cvRectangle(pTrackImg,cvPoint(xout - WidOut,yout - HeiOut),cvPoint(xout+WidOut,yout+HeiOut),cvScalar(255,255,255),2,8,0);  
897.                     xin = xout; yin = yout;  
898.                     WidIn = WidOut; HeiIn = HeiOut;  
899.                 /*draw_rectangle( pBuffer, Width, Height, xo, Height-yo-1, wo, ho, 0x00ff0000, 2 ); 
900.                 xb = xo; yb = yo; 
901.                 wb = wo; hb = ho;*/  
902.             }  
903.         }  
904.   
905.         cvShowImage("video",curframe);  
906.         cvShowImage("foreground",pFrontImg);  
907.         cvShowImage("background",pBackImg);  
908.         cvShowImage("tracking",pTrackImg);  
909.         /*sprintf(res1,"fore%d.jpg",FrameNum); 
910.         cvSaveImage(res1,pFrontImg); 
911.         sprintf(res2,"ground%d.jpg",FrameNum); 
912.         cvSaveImage(res2,pBackImg);*/  
913.         cvSetMouseCallback("foreground",mouseHandler,0);//响应鼠标  
914.         key = cvWaitKey(1);  
915.         if(key == 'p') pause = true;  
916.         while(pause)  
917.             if(cvWaitKey(0)=='p')  
918.                 pause = false;        
919.   
920.     }  
921.     cvReleaseImage(&curFrameGray);  
922.     cvReleaseImage(&frameGray);  
923.     cvReleaseImage(&pBackImg);  
924.     cvReleaseImage(&pFrontImg);  
925.     cvDestroyAllWindows();  
926. //  Clear_MeanShift_tracker();  
927.     ClearAll();  
928.     }  

实验结果：

自此，毕业论文涉及的经典算法已经全部给出，我自己提出的破算法就不献丑了。

马上去华为上班咯，可能搞通信去了，破企业网部门，唉

如果周末有空的话，我还是会继续搞图像处理的，这次下了不少人脸美化、超分辨率修正的论文，得好好读读。

另外打个广告，我毕业前自己弄得android app《色盲相机》，下载地址：

木蚂蚁：http://www.mumayi.com/android-631836.html 
360：     http://zhushou.360.cn/detail/index/soft_id/1780912 
网易：    http://m.163.com/android/software/32jkam.html

核心思想来自斯坦福大学的课程设计及一个日本老头公开的matlab代码

有空大家给我点点广告哈~~