http://blog.csdn.net/hujingshuang/article/details/45535423

前言：

粒子滤波广泛的应用于目标跟踪，粒子滤波器是一种序列蒙特卡罗滤波方法，其实质是利用一系列随机抽取的样本（即粒子）来替代状态的后验概率分布。在此不打算介绍和推理繁杂的概率公式，我们来分析Rob Hess源码从而深入理解粒子滤波算法。

试验平台：

VS2010 + opencv2.4.10 + gsl1.8库 + RobHess粒子滤波源码

源码分析：

下面用粒子滤波算法来实现单目标的跟踪，在初始时需要手动选取待跟踪目标区域。

步骤一：粒子初始化

1、目标的选取：

在起始帧画面中标记待跟踪的区域（目标物体），在以后的连续帧视频中跟踪目标物体。

2、粒子的定义：

粒子即样本，Rob Hess源码中默认粒子数目PARTICLES=100，下面是粒子的属性（可以看出一个粒子代表的就是一个矩形区域）：

[cpp] view plaincopy  
 typedef struct particle {  
   float x;          /**< 当前x坐标 */  
   float y;          /**< 当前y坐标 */  
   float s;          /**< scale */  
   float xp;         /**< 之前x坐标 */  
   float yp;         /**< 之前y坐标 */  
   float sp;         /**< previous scale */  
   float x0;         /**< x0 */  
   float y0;         /**< y0 */  
   int width;        /**< 粒子宽度 */  
   int height;       /**< 粒子高度 */  
   histogram* histo; /**< 跟踪区域的参考直方图 */  
   float w;          /**< 权重 */  
 } particle;  

3、特征提取：

目标跟踪最重要的就是特征，应该选取好的特征（拥有各种不变性的特征当然是最好的）；另外考虑算法的效率，目标跟踪一般是实时跟踪，所以对算法实时性有一定的要求。Rob Hess源码提取的是目标的颜色特征（颜色特征对图像本身的尺寸、方向、视角的依赖性较小，从而具有较高的鲁棒性），粒子与目标的直方图越相似，则说明越有可能是目标。

捕捉到一帧图像，标记待跟踪的区域，将其从BGR转化到HSV空间，提取感兴趣部分（目标）的HSV，进行直方图统计并归一化直方图。

[cpp] view plaincopy  
 /* increment appropriate histogram bin for each pixel */  
 //计算直方图  
 for( r = 0; r < img->height; r++ )  
     for( c = 0; c < img->width; c++ )  
     {  
         bin = histo_bin( /*pixval32f( h, r, c )*/((float*)(h->imageData + h->widthStep*r) )[c],  
                     ((float*)(s->imageData + s->widthStep*r) )[c],  
                     ((float*)(v->imageData + v->widthStep*r) )[c] );  
         hist[bin] += 1;  
     }  

[cpp] view plaincopy  
 int histo_bin( float h, float s, float v )  
 {  
   int hd, sd, vd;  
   
   /* if S or V is less than its threshold, return a "colorless" bin */  
   vd = MIN( (int)(v * NV / V_MAX), NV-1 );  
   if( s < S_THRESH  ||  v < V_THRESH )  
     return NH * NS + vd;  
     
   /* otherwise determine "colorful" bin */  
   hd = MIN( (int)(h * NH / H_MAX), NH-1 );  
   sd = MIN( (int)(s * NS / S_MAX), NS-1 );  
   return sd * NH + hd;  
 }  

img是目标矩形区域，h、s、v是个分量，hist就是直方图统计；NV、V_MAX....等是宏定义的固定值。

[cpp] view plaincopy  
 void normalize_histogram( histogram* histo )//直方图归一化  
 {  
   float* hist;  
   float sum = 0, inv_sum;  
   int i, n;  
   
   hist = histo->histo;  
   n = histo->n;  
   
   /* compute sum of all bins and multiply each bin by the sum's inverse */  
   for( i = 0; i < n; i++ )  
     sum += hist[i];  
   inv_sum = 1.0 / sum;  
   for( i = 0; i < n; i++ )  
     hist[i] *= inv_sum;  
 }  

4、粒子初始化

根据选定的目标区域来初始化粒子，初始时所有粒子都为等权重，具有同样的属性。

[cpp] view plaincopy  
 /* create particles at the centers of each of n regions */  
 for( i = 0; i < n; i++ )  
 {  
     width = regions[i].width;  
     height = regions[i].height;  
     x = regions[i].x + width / 2;<span style="white-space:pre">   </span>//粒子中心  
     y = regions[i].y + height / 2;  
     for( j = 0; j < np; j++ )  
     {  
         particles[k].x0 = particles[k].xp = particles[k].x = x;  
         particles[k].y0 = particles[k].yp = particles[k].y = y;  
         particles[k].sp = particles[k].s = 1.0;  
         particles[k].width = width;  
         particles[k].height = height;  
         particles[k].histo = histos[i];  
         particles[k++].w = 0;  
     }  
 }  

步骤二、粒子相似度搜索、计算

5、粒子搜索

在步骤一中，初始化了100个粒子，由于初始帧中指定了目标区域，而该目标会在下一帧中发生偏移，但是相邻帧目标移动得不是太远，所以在目标区域附近随机撒出100个粒子。（此处使用的是二阶动态回归来估计偏移后的粒子位置）

[cpp] view plaincopy  
 particle transition( particle p, int w, int h, gsl_rng* rng )//随机撒出一个粒子  
 {  
   float x, y, s;  
   particle pn;  
   //回归模型的参数即A1、A2、B0等Rob Hess在代码中已设定（我不知道是怎么来的？）  
   /* sample new state using second-order autoregressive dynamics （使用二阶动态回归来自动更新粒子状态）*/  
   x = A1 * ( p.x - p.x0 ) + A2 * ( p.xp - p.x0 ) +  
     B0 * gsl_ran_gaussian( rng, TRANS_X_STD ) + p.x0;  
   pn.x = MAX( 0.0, MIN( (float)w - 1.0, x ) );  
   y = A1 * ( p.y - p.y0 ) + A2 * ( p.yp - p.y0 ) +  
     B0 * gsl_ran_gaussian( rng, TRANS_Y_STD ) + p.y0;  
   pn.y = MAX( 0.0, MIN( (float)h - 1.0, y ) );  
   s = A1 * ( p.s - 1.0 ) + A2 * ( p.sp - 1.0 ) +  
     B0 * gsl_ran_gaussian( rng, TRANS_S_STD ) + 1.0;  
   pn.s = MAX( 0.1, s );  
   pn.xp = p.x;  
   pn.yp = p.y;  
   pn.sp = p.s;  
   pn.x0 = p.x0;  
   pn.y0 = p.y0;  
   pn.width = p.width;  
   pn.height = p.height;  
   pn.histo = p.histo;  
   pn.w = 0;  
   
   return pn;  
 }  

6、相似度计算

然后计算这100个粒子hsv空间直方图与目标hsv空间直方图相似成度，马氏距离（Battacharyya）来度量两个粒子的相似度系数。

[cpp] view plaincopy  
 //计算粒子与跟踪区域直方图相似程度，越相似则权值越大  
 particles[j].w = likelihood( hsv_frame, cvRound(particles[j].y),  
             cvRound( particles[j].x ),  
             cvRound( particles[j].width * s ),  
             cvRound( particles[j].height * s ),  
             particles[j].histo );  

[cpp] view plaincopy  
 float histo_dist_sq( histogram* h1, histogram* h2 )//两个粒子的  
 {  
   float* hist1, * hist2;  
   float sum = 0;  
   int i, n;  
   
   n = h1->n;  
   hist1 = h1->histo;  
   hist2 = h2->histo;  
   
   /* 
     According the the Battacharyya similarity coefficient, 
      
     D = \sqrt{ 1 - \sum_1^n{ \sqrt{ h_1(i) * h_2(i) } } }//马氏距离公式 
   */  
   for( i = 0; i < n; i++ )  
     sum += sqrt( hist1[i]*hist2[i] );  
   return 1.0 - sum;  
 }  

步骤三、重采样

7、重采样

由步骤二知，经过一次搜索后，粒子的权重会发生改变，离目标距离远的粒子权重小，距离近的权重大。那么经过多帧的跟踪后，有的粒子权重会变得相当的小，也就是与目标不相似了，即粒子退化现象，这种粒子我们要抛弃，那么什么时候该抛弃它呢？就得设一个权重阈值，凡是权重低于阈值的粒子就抛弃。OK，原来有100个粒子，然后总会有被抛弃的粒子，似的粒子总数不满100个，此时就要找一些新的粒子来补充，那么就用最大权值来填充。（比如现在抛弃了20个粒子，我们就复制20个权值最大的粒子，放到里面填满100个。）——这就是重采样的过程。

[cpp] view plaincopy  
 particle* resample( particle* particles, int n )  
 {  
   particle* new_particles;  
   int i, j, np, k = 0;  
   
   qsort( particles, n, sizeof( particle ), &particle_cmp );//根据权重进行排序  
   new_particles = malloc( n * sizeof( particle ) );  
     for( i = 0; i < n; i++ )  
     {  
         np = cvRound( particles[i].w * n );//淘汰弱权值样本，保留阈值以上样本  
         for( j = 0; j < np; j++ )  
         {  
             new_particles[k++] = particles[i];  
             if( k == n )  
             goto exit;  
         }  
     }  
   while( k < n )  
     new_particles[k++] = particles[0];//复制大权值样本以填充满样本空间  
   
  exit:  
   return new_particles;  
 }  