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

原文地址: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代码

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

评论 19
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值