RobHess的粒子滤波（轨迹跟踪）代码学习

Rob Hess的粒子跟踪代码学习

样例演示

基本操作指令

NAME	TYPES
OPTIONS	p:oah
PARTICLES	100
EXPORT_BASE	./frames/frame_
EXPORT_EXTN	.png
MAX_FRAMES	2048
MAX_OBJECTs	1

定义参数的结构体

typedef struct params {
  CvPoint loc1[MAX_OBJECTS];
  CvPoint loc2[MAX_OBJECTS];
  IplImage* objects[MAX_OBJECTS];
  char* win_name;
  IplImage* orig_img;
  IplImage* cur_img;
  int n;
} params;

粒子的结构体

typedef struct particle {
  float x;          /**<当前 x 坐标 */
  float y;          /**< 当前 y 坐标 */
  float s;          /**< 尺度 */
  float xp;         /**< 之前 X 坐标 */
  float yp;         /**< 之前 y 坐标 */
  float sp;         /**< 之前的尺度 */
  float x0;         /**<  原始x的坐标 */
  float y0;         /**< 原始y的坐标 */
  int width;        /**< 粒子描述区域的原始宽度 */
  int height;       /**< 粒子描述区域的原始宽度 */
  histogram* histo; /**< 描述被跟踪区域的参考直方图 */
  float w;          /**< 权重 */
} particle;

全局变量

NAME	type	value
pname	char*
vid_file	char*
num_particles	int
show_all	int
export	int

代码库中常见的类型定义

typename	explain	types
gsl_rng	gsl_rng_type保存关于每种生成器类型的静态信息，gsl_rng描述从给定gsl_rng_type创建的生成器的实例。
IplImage	是OpenCV中CxCore部分基础的数据结构，用来表示图像。OpenCV2.1版本之前使用IplImage*数据结构来表示图像，2.1之后的版本使用图像容器Mat来存储。
histogram	由NH*NS+NV箱表示的HSV直方图	自定义的类
CvCapture	CvCapture是一个结构体，用来保存图像捕获的信息，在OpenCv中，它最大的作用就是处理视频时（程序是按一帧一帧读取），让程序读下一帧的位置，CvCapture结构中，每获取一帧后，这些信息都将被更新，获取下一帧回复
particle	粒子的结构体	自定义的类
CvScalar	CvScalar看做是一个普通的结构体时 ，其内部只不过是存储了四个double型的值，分别为val[0],val[1],val[2],val[3],我们通常用的是前三个，val[0],val[1],val[2]的含义分别是彩色照片的三个通道，R,G,B通道。R是红色分量，G是绿色分量，B是蓝色分量，a是alpha。
CvRect	通过矩形左上角坐标和矩形的宽和高来确定一个矩形区域。包含4个数据成员，x,y,width,height,通过定义矩形左上角坐标和矩形的宽和高来确定一个矩形。
CvPoint	表示一个坐标为整数的二维点，是一个包含integer类型成员x和y的简单结构体。

函数执行流程

1. 定义函数的初始值
2. 生成随机数
3. 获取视频流中的图像
4. 对视频流的图像进行处理
   1. 获取视频流图像的对应帧数（从第一帧开始）
   2. 由于获取图像的图像类型需要将其转换成HSV图像类型 IplImage* bgr2hsv( IplImage* bgr ）
   3. 克隆当前的图像帧给frames[i] ----cvClone( frame )
   4. 当读取到第一帧的时候，允许用户在第一帧中选择需要被跟踪的对象

      1. 获取当前图像帧的长，宽 width ,height

      2. 在命令行窗口弹出显示字符

      3. 当所选的跟踪对象为空的时候，进入循环

         1. get_regions( frame, ®ions ) region–矩形坐标框CvRect frame–当前的图像帧 获取用户对第一帧进行鼠标点击事件时获得的矩形框区域
         2. 计算由用户定义的目标区域的关联直方图 compute_ref_histos( IplImage* frame, CvRect* regions, int n )
         3. 对粒子进行初始化，初始化粒子的分布particle* init_distribution( CvRect* regions, histogram** histos, int n, int p)

      4. 当当前帧不是第一帧时

         1. 对每一个粒子进行预测和计算

         2. 首先对给定粒子的转换模型进行采样transition( particles[j], w, h, rng )

         3. 计算图像中选中区域的目标可能出现的位置float likelihood( IplImage* img, int r, int c, int w, int h, histogram* ref_histo )

         4. 对粒子进行归一化，并对一系列未加权的粒子进行重采样操作void normalize_weights( particle* particles, int n )

         5. 对归一化的粒子进行重采样操作，得到新粒子

            new_particles = resample( particles, num_particles );

      5. 根据需要，选择是否要将所有的粒子显示出来

         1. 首先根据粒子的权重的重要性进行排序

         2. 按照从小到大的原则将粒子显示出来

            void display_particle( IplImage* img, particle p, CvScalar color )

      6. 显示最可能的粒子

         1. 将粒子的排序后的第0个粒子显示出来，因为其是最有可能的边界

相关函数的解析

IplImage* bgr2hsv( IplImage* bgr )

将BGR类型的图像转换成HSV颜色空间的图像，输入参数为要转换的图像，返回值 将bgr图像转换成三通道，32bit，其中
H-色调值在[0,360],S-饱和度和V-明亮度在[0%-100%]之间的HSV图像

cvCreateImage：创建图像标头并分配图像数据
参数：size–图像的尺寸（长，宽） depth–图像元素的位深 channel–每个像素的通道数
cvConvertScale： 使用可选的线性变换将一个数组转换为另一个数组。
参数：src:源数组 dst：目标数组 scale–比例因子 shift–添加到缩放原数组元素的值
cvCvtColor：将输入图像从一个颜色空间转换到另一个颜色空间，同时需要指明是BGR还是RGB
参数：src:源图像 dst:目标图像，但需要和src具有相同的大小和深度
code–定义图像转换的方式，本例为BGR2HSV dst–目标图像的通道数
cvReleaseImage：释放IPL图像的头和数据

IplImage* bgr2hsv( IplImage* bgr )
{
  IplImage* bgr32f, * hsv;

  bgr32f = cvCreateImage( cvGetSize(bgr), IPL_DEPTH_32F, 3 );
  hsv = cvCreateImage( cvGetSize(bgr), IPL_DEPTH_32F, 3 );
  cvConvertScale( bgr, bgr32f, 1.0 / 255.0, 0 );
  cvCvtColor( bgr32f, hsv, CV_BGR2HSV );
  cvReleaseImage( &bgr32f );
  return hsv;
}

get_regions( frame, ®ions )

get_regions( IplImage* frame, CvRect** regions )
允许用户使用交互的方式来选择对象区域
参数 frame 要选择跟踪对象的当前帧
参数 region 指向要用矩形填充的数组的指针，用以定义对象区域
返回值 返回的是用户选择的对象数目

步骤1：使用鼠标回调允许用户定义对象的区域
cvNamedWindow：创建显示窗口
Para: winname–窗口标题中可用做窗口标识符窗口的名称
flag–窗口的标志

​ cvShowImage：在指定窗口中显示图像
​ Para: const string& winname, InputArray image

​ cvSetMouseCallback:为指定窗口设置鼠标处理程序(相当于鼠标相应函数)
Para: const char* name, CvMouseCallback onMouse, void* param=NULL

步骤二：鼠标对第一帧的图像进行绘制边框，以及相应的鼠标的边框和目标框定

步骤三：销毁选择的第0帧图像，并将标记的图像也进行删除

步骤四：根据之前鼠标划分的目标矩形框，为目标矩形分配内存空间

步骤五：根据矩形空间得到的坐标值，赋值矩形框的大小并传递给指针regions,同时需要注意矩形框的高度和宽度都要为奇数

int get_regions( IplImage* frame, CvRect** regions )
{
  char* win_name = "First frame";
  params p;
  CvRect* r;
  int i, x1, y1, x2, y2, w, h;
  
  /* use mouse callback to allow user to define object regions */

  //使用鼠标回调允许用户定义对象的区域
  p.win_name = win_name;
  p.orig_img = cvClone( frame );
  p.cur_img = NULL;
  p.n = 0;
  cvNamedWindow( win_name, 1 );
  cvShowImage( win_name, frame );
  cvSetMouseCallback( win_name, &mouse, &p );
  cvWaitKey( 0 );
  cvDestroyWindow( win_name );
  cvReleaseImage( &(p.orig_img) );
  //将标记的图像销毁从第0帧开始
  if( p.cur_img )
    cvReleaseImage( &(p.cur_img) );

  /* extract regions defined by user; store as an array of rectangles */
  //提取用户定义的区域；存储为矩形数组
  //待续目标区域为0的时候，则分配空间不存在
  if( p.n == 0 )
    {
      *regions = NULL;
      return 0;
    }
  //为矩形分配空间
  r = malloc( p.n * sizeof( CvRect ) );
  for( i = 0; i < p.n; i++ )
    {
      //获取两点之间的坐标方便计算后续的长和宽
      x1 = MIN( p.loc1[i].x, p.loc2[i].x );
      x2 = MAX( p.loc1[i].x, p.loc2[i].x );
      y1 = MIN( p.loc1[i].y, p.loc2[i].y );
      y2 = MAX( p.loc1[i].y, p.loc2[i].y );
      w = x2 - x1;
      h = y2 - y1;

      /* ensure odd width and height */
      //确保奇数的长和宽
      w = ( w % 2 )? w : w+1;
      h = ( h % 2 )? h : h+1;
      r[i] = cvRect( x1, y1, w, h );
    }
  *regions = r;
  return p.n;
}

鼠标相应函数–void mouse( int event, int x, int y, int flags, void* param )

执行三个操作当鼠标左键被按下，左键释放，左键按下且移动的时候，矩形的矩形绘制

cvRectangle：绘制简单、粗或填充的右矩形
Para: Mat& img, Point pt1, Point pt2, const Scalar& color, int thickness=1, int lineType=8, int shift=0

void mouse( int event, int x, int y, int flags, void* param )
{
  params* p = (params*)param;
  CvPoint* loc;
  int n;
  IplImage* tmp;
  static int pressed = FALSE;
  
  /* on left button press, remember first corner of rectangle around object */
  //当鼠标左键被按下的时候,记下对象周围矩形的第一个角
  if( event == CV_EVENT_LBUTTONDOWN )
    {
      n = p->n;
      if( n == MAX_OBJECTS )
	        return;
      loc = p->loc1;
      loc[n].x = x;
      loc[n].y = y;
      pressed = TRUE;
    }

  /* on left button up, finalize the rectangle and draw it in black */
  //当鼠标左键释放的时候，完成矩形的绘制并将绘制的矩形的颜色用黑色进行标记
  else if( event == CV_EVENT_LBUTTONUP )
    {
      n = p->n;
      if( n == MAX_OBJECTS )
	    return;
      loc = p->loc2;
      loc[n].x = x;
      loc[n].y = y;
      cvReleaseImage( &(p->cur_img) );
      p->cur_img = NULL;
      cvRectangle( p->orig_img, p->loc1[n], loc[n], CV_RGB(0,0,0), 2, 8, 0 );
      cvShowImage( p->win_name, p->orig_img );
      pressed = FALSE;
      p->n++;
    }

  /* on mouse move with left button down, draw rectangle as defined in white */
  //当鼠标被按下进行移动的时候，使用白色的矩形标记定义的目标
  else if( event == CV_EVENT_MOUSEMOVE  &&  flags & CV_EVENT_FLAG_LBUTTON )
    {
      n = p->n;
      if( n == MAX_OBJECTS )
	    return;
      tmp = cvClone( p->orig_img );
      loc = p->loc1;
      cvRectangle( tmp, loc[n], cvPoint(x, y), CV_RGB(255,255,255), 2, 8, 0 );
      cvShowImage( p->win_name, tmp );
      if( p->cur_img )
	    cvReleaseImage( &(p->cur_img) );
      p->cur_img = tmp;
    }
}

compute_ref_histos( IplImage* frame, CvRect* regions, int n )

计算由用户定义的目标区域的关联直方图
Para:
IplImage* frame:计算直方图的视频帧的帧，必须转换将原始视频帧通过bgr2hsv函数转换成hsv格式
CvRect* regions：视频帧中需要计算直方图的区域
int n:用户选择区域的个数

参数 export 如果是True:代表需要导出对象区域图像
返回值 返回与区域中指定的帧区域相对应的规范化直方图的元素数组

前提条件

typedef struct histogram {
  float histo[NH*NS + NV];   /**< histogram array 直方图矩阵 */
  int n;                     /**< length of histogram array 直方图矩阵的数目 */
} histogram;

需要定义直方图的结构体：

一个HSV直方图由NH*NS+NV表示
NH * NS 表示像素大于S_THRESH 和 V_THRESH 的饱和度的像素
NV 值由无色的像素填充

主要步骤

步骤一：分配一个用户选择的区域的直方图大小malloc( n * sizeof( histogram* ) )

步骤二：在帧中选取区域并计算其中的直方图大小

1. 根据传入的矩阵大小，裁取获得roi区域
2. 将RoI区域赋值给局部变量tmp
3. 对tmp计算其累积的直方图calc_histogram( IplImage** imgs, int n )
4. 对得到的直方图进行归一化操作normalize_histogram( histogram* histo )

步骤三:返回与选中区域相对应的归一化直方图元素数组

calc_histogram( IplImage** imgs, int n )

histogram* calc_histogram( IplImage** imgs, int n )
为给定图像数组计算上面定义的累积直方图
Para:
IplImage** imgs:计算累计直方图的图像数组。必须先将该图片转换成HSV颜色空间
int n:需要计算的直方图数目
返回值：
返回目标区域的非归一化直方图

void *memset(void *s, int ch, size_t n)
将s中当前位置后面的n个字节 （typedef unsigned int size_t ）用 ch 替换并返回 s

cvCvtPixToPlane ==cvSplit
void cvSplit(const CvArr* src, CvArr* dst0, CvArr* dst1, CvArr* dst2, CvArr* dst3)
将多通道阵列划分为多个单通道阵列。

histogram* calc_histogram( IplImage** imgs, int n )
{
  IplImage* img;
  histogram* histo;
  IplImage* h, * s, * v;
  float* hist;
  int i, r, c, bin;

  histo = malloc( sizeof(histogram) );
  histo->n = NH*NS + NV;
  hist = histo->histo;

  memset( hist, 0, histo->n * sizeof(float) );

  for( i = 0; i < n; i++ )
    {
      /* extract individual HSV planes from image */
      //从图像中提取单个的HSV平面
      img = imgs[i];
      h = cvCreateImage( cvGetSize(img), IPL_DEPTH_32F, 1 );
      s = cvCreateImage( cvGetSize(img), IPL_DEPTH_32F, 1 );
      v = cvCreateImage( cvGetSize(img), IPL_DEPTH_32F, 1 );

      cvCvtPixToPlane( img, h, s, v, NULL );
      
      /* 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;
	        }
    cvReleaseImage( &h );
    cvReleaseImage( &s );
    cvReleaseImage( &v );
  }
  return histo;
}

函数步骤

步骤一：首先分配直方图大小的内存块，并进行初始化

步骤二：从所选取的直方图中提取单个的HSV平面

步骤三，为每一个像素增加适当的直方图单元。histo_bin( float h, float s, float v )，利用H、S、V三个不同平面的值进行计算。

步骤四，释放提取的单个HSV平面，返回目标区域的非归一化直方图

histo_bin( float h, float s, float v )

histo_bin( float h, float s, float v )
计算HSV条目所在的直方图区域
Para:
h:色调
s:饱和度
v:亮度
Return:
返回由h,s，v所定义的HSV色彩所关联的区域索引

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 */
  //如果S或V小于阈值，返回“clolorless”容器
  vd = MIN( (int)(v * NV / V_MAX), NV-1 );
  if( s < S_THRESH  ||  v < V_THRESH )
    return NH * NS + vd;
  
  /* otherwise determine "colorful" bin */
  //否则决定返回一个“colorful”的容器
  hd = MIN( (int)(h * NH / H_MAX), NH-1 );
  sd = MIN( (int)(s * NS / S_MAX), NS-1 );
  return sd * NH + hd;
}

normalize_histogram( histogram* histo )

normalize_histogram( histogram* histo )
归一化直方图，使所有元素的总和为1.0
Para:
histo:带归一化的直方图

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;
}

particle* init_distribution( CvRect* regions, histogram** histos, int n, int p)

particle* init_distribution( CvRect* regions, histogram** histos, int n, int p)
在特定区域中创建粒子初始化分布
Para:
regions:一系列区域将被用来粒子对目标的采样
histos:区域的直方图
n:区域的个数
p:将要被分配的粒子总数
Return:
返回一系列围绕着该区域的区域采样粒子

具体步骤

步骤一：首先对给定的粒子数目分配相应的内存空间

步骤二：在区域的中心位置放置粒子

步骤三：确保所有粒子都被初始化创建

particle* init_distribution( CvRect* regions, histogram** histos, int n, int p)
{
  particle* particles;
  int np;
  float x, y;
  int i, j, width, height, k = 0;
  
  particles = malloc( p * sizeof( particle ) );
  np = p / n;

  /* create particles at the centers of each of n regions */
  //在n个区域的中心位置放置粒子
  for( i = 0; i < n; i++ )
    {
      width = regions[i].width;
      height = regions[i].height;
      x = regions[i].x + width / 2;
      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;
	}
    }

  /* make sure to create exactly p particles */
  //确保所有粒子都被初始化创建
  i = 0;
  while( k < p )
    {
      width = regions[i].width;
      height = regions[i].height;
      x = regions[i].x + width / 2;
      y = regions[i].y + height / 2;
      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;
      i = ( i + 1 ) % n;
    }

  return particles;
}

particle transition( particle p, int w, int h, gsl_rng* rng )

particle transition( particle p, int w, int h, gsl_rng* rng )
对给定粒子的转换模型进行采样
Para:
p:需要被转换的粒子
w:视频帧的宽度
h:视频帧的高度
rng:需要被采样的随机生成数
Return:
返回一个基于之前粒子P转换模型的一个新粒子

gsl_ran_gaussian (const gsl_rng * r, const double sigma)
产生高斯随机数
函数返回期望（均值）为0,标准差为sigma的正态分布随机数

particle transition( particle p, int w, int h, gsl_rng* rng )
{
  float x, y, s;
  particle pn;
  
  /* 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;
}

float likelihood( IplImage* img, int r, int c, int w, int h, histogram* ref_histo )

float likelihood( IplImage* img, int r, int c, int w, int h, histogram* ref_histo )
计算图像中选定区域的目标可能出现的位置
Para:
IplImage* img:已经被转换为HSV格式的图片
int r：计算似然的窗口中心的行坐标
int c: 计算似然的窗口中心的列坐标
int w: 计算似然的区域宽度
int h: 计算似然的区域高度
histogram* ref_histo： 目标的直方图，必须使用normalize_histogram()归一化
Return：
返回图像中玩家可能存在的位置

void cvSetImageROI(IplImage* image, CvRect rect)：
为给定矩形设置图像感兴趣区域（ROI）

void cvResetImageROI(IplImage* image)
重置图像ROI以包含整个图像并释放ROI结构

函数执行步骤

步骤一：根据cvSetImageROI函数，再原始图像上设置感兴趣的ROI区域

步骤二，将img复制给临时变量tmp，再释放图像中的ROI区域

步骤三，重新计算图像的累计直方图，即histogram* calc_histogram( IplImage** imgs, int n )函数

步骤四，对得到的直方图重新进行初归一化normalize_histogram( histogram* histo )

步骤五：利用巴氏系数计算新得到的直方图和原来直方图之间的差异histo_dist_sq( histo, ref_histo );

步骤六：返回目标可能存在的区域

float likelihood( IplImage* img, int r, int c,
		  int w, int h, histogram* ref_histo )
{
  IplImage* tmp;
  histogram* histo;
  float d_sq;

  /* extract region around (r,c) and compute and normalize its histogram */
  //扩展（r,c）周围的区域并计算和归一化其直方图
  cvSetImageROI( img, cvRect( c - w / 2, r - h / 2, w, h ) );
  tmp = cvCreateImage( cvGetSize(img), IPL_DEPTH_32F, 3 );
  cvCopy( img, tmp, NULL );
  cvResetImageROI( img );
  histo = calc_histogram( &tmp, 1 );
  cvReleaseImage( &tmp );
  normalize_histogram( histo );

  /* compute likelihood as e^{\lambda D^2(h, h^*)} */
  //计算二者之间的相似性
  d_sq = histo_dist_sq( histo, ref_histo );
  free( histo );
  return exp( -LAMBDA * d_sq );
}

histo_dist_sq(histogram* h1, histogram* h2)

float histo_dist_sq(histogram* h1, histogram* h2)
基于直方图之间的Battacharyya相似系数计算平方距离度量
Para:
histogram* h1,第一张直方图
histogram* h2,第二张直方图
Return
根据h1和h2的Battacharyya相似系数返回平方距离

//评价粒子的相似性：
//使用Battacharyya距离（即巴氏距离）

巴氏距离：测量的是两个离散或连续概率分布的相似性

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;
}

void normalize_weights( particle* particles, int n )

void normalize_weights( particle* particles, int n )
归一化粒子的权重使得总和为1
Para:
particle* particles:需要被进行归一化的粒子
int n:粒子的数目

void normalize_weights( particle* particles, int n )
{
  float sum = 0;
  int i;

  for( i = 0; i < n; i++ )
    sum += particles[i].w;
  for( i = 0; i < n; i++ )
    particles[i].w /= sum;
}

particle* resample( particle* particles, int n )

重采样函数执行步骤

Step1:首先对输入的粒子依据一定的规则进行排序（int particle_cmp( void* p1, void* p2 )）

Step2:分配同样大小的粒子空间

Step3:逐个对每个粒子进行处理，按照原先的权重往新粒子中赋值，相当于重采样

Step4:根据粒子的数目是否大于和小于原先粒子的数目进行处理，小于则，以粒子的第一个值进行赋值，大于则直接返回即可

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++ )
    {
      //cvRound 将浮点数舍入为最接近的整数 赋值给np
      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;
}

int particle_cmp( void* p1, void* p2 )

int particle_cmp( void* p1, void* p2 )
基于两个粒子的权重返回
Return
如果p1 如果p1>p2,返回1
如果p1=p2,返回0

int particle_cmp( void* p1, void* p2 )
{
  particle* _p1 = (particle*)p1;
  particle* _p2 = (particle*)p2;

  if( _p1->w > _p2->w )
    return -1;
  if( _p1->w < _p2->w )
    return 1;
  return 0;
}

void display_particle( IplImage* img, particle p, CvScalar color )

void display_particle( IplImage* img, particle p, CvScalar color )
将图像上的粒子显示为粒子指定区域周围的矩形
Para:
IplImage* img:需要显示粒子的图像
particle p：将要被显示的粒子
CvScalar color：粒子的显示的颜色
void cvRectangle(CvArr* img, CvPoint pt1, CvPoint pt2, CvScalar color, int thickness=1, int lineType=8, int shift=0 )
绘制一个简单的，粗的
Para:
CvArr* img：图像
CvPoint pt1：矩形的顶点坐标
CvPoint pt2：相对于矩形顶点pt1的矩形坐标
CvScalar color：矩形框的颜色或亮度
int thickness=1：绘制矩形框的线条粗细
int lineType=8：线条的类型
int shift=0:点坐标中的分数位数

void display_particle( IplImage* img, particle p, CvScalar color )
{
  int x0, y0, x1, y1;

  x0 = cvRound( p.x - 0.5 * p.s * p.width );
  y0 = cvRound( p.y - 0.5 * p.s * p.height );
  x1 = x0 + cvRound( p.s * p.width );
  y1 = y0 + cvRound( p.s * p.height );
  
  cvRectangle( img, cvPoint( x0, y0 ), cvPoint( x1, y1 ), color, 1, 8, 0 );
}

区域周围的矩形
Para:
IplImage* img:需要显示粒子的图像
particle p：将要被显示的粒子
CvScalar color：粒子的显示的颜色
void cvRectangle(CvArr* img, CvPoint pt1, CvPoint pt2, CvScalar color, int thickness=1, int lineType=8, int shift=0 )
绘制一个简单的，粗的
Para:
CvArr* img：图像
CvPoint pt1：矩形的顶点坐标
CvPoint pt2：相对于矩形顶点pt1的矩形坐标
CvScalar color：矩形框的颜色或亮度
int thickness=1：绘制矩形框的线条粗细
int lineType=8：线条的类型
int shift=0:点坐标中的分数位数

void display_particle( IplImage* img, particle p, CvScalar color )
{
  int x0, y0, x1, y1;

  x0 = cvRound( p.x - 0.5 * p.s * p.width );
  y0 = cvRound( p.y - 0.5 * p.s * p.height );
  x1 = x0 + cvRound( p.s * p.width );
  y1 = y0 + cvRound( p.s * p.height );
  
  cvRectangle( img, cvPoint( x0, y0 ), cvPoint( x1, y1 ), color, 1, 8, 0 );
}