粒子滤波
粒子滤波算法：源于Montecarlo的思想，即以某事件出现的频率来指代该事件的概率。因此在滤波过程中，需要用到概率如P(x)的地方，一概对变量x采样，以大量采样的分布近似来表示P(x)。因此，采用此思想，在滤波过程中粒子滤波可以处理任意形式的概率，而不像Kalman滤波只能处理高斯分布的概率问题。粒子滤波的一大优势也在于此。
下来看看对任意如下的状态方程：
x(t)=f(x(t-1),u(t),w(t))
y(t)=h(x(t),e(t))
其中的x(t)为t时刻状态，u(t)为控制量，w(t) 和e(t)分别为模型噪声和，观测噪声。前一个当然是状态转移方程，后一个是观测方程。对于这么一个问题粒子滤波怎么来从观测y(t)，和x(t-1),u(t) 滤出真实状态x(t)呢？
滤波的预估阶段：粒子滤波首先根据x(t-1) 和他的概率分布生成大量的采样，这些采样就称之为粒子。那么这些采样在状态空间中的分布实际上就是x(t-1) 的概率分布了。好，接下来依据状态转移方程加上控制量可以对每一粒子得到一个预测粒子。
校正阶段：有了预测粒子，当然不是所有的预测粒子都能得到我们的观测值y，越是接近真实状态的粒子，越有可能获得观测值y。于是我们对所有的粒子需要有个评价，这个评价就是一个条件概率P(y|xi ),直白的说，这个条件概率代表了假设真实状态x(t)取第i个粒子xi时获得观测y的概率。令这个条件概率为第i个粒子的权重。如此这般下来，对所有粒子都进行这么一个评价，那么越有可能获得观测y的粒子，当然获得的权重越高。
重采样算法：去除低权值的粒子，复制高权值的粒子。所得到的当然就是我们说需要的真实状态x(t)了，而这些重采样后的粒子，就代表了真实状态的概率分布了。下一轮滤波，再将重采样过后的粒子集输入到状态转移方程中，直接就能够获得预测粒子了。。
初始状态的问题： 由于开始对x(0)一无所知，所有我们可以认为x(0)在全状态空间内平均分布。于是初始的采样就平均分布在整个状态空间中。然后将所有采样输入状态转移方程，得到预测粒子。然后再评价下所有预测粒子的权重，当然我们在整个状态空间中只有部分粒子能够获的高权值。最后进行重采样，去除低权值的，将下一轮滤波的考虑重点缩小到了高权值粒子附近。
具体过程：
1）初始化阶段-提取跟踪目标特征
该阶段要人工指定跟踪目标，程序计算跟踪目标的特征，比如可以采用目标的颜色特征。具体到Rob Hess的代码，开始时需要人工用鼠标拖动出一个跟踪区域，然后程序自动计算该区域色调(Hue)空间的直方图，即为目标的特征。直方图可以用一个向量来表示，所以目标特征就是一个N*1的向量V。
2）搜索阶段-放狗
好，我们已经掌握了目标的特征，下面放出很多条狗，去搜索目标对象，这里的狗就是粒子particle。狗有很多种放法。比如，a)均匀的放：即在整个图像平面均匀的撒粒子(uniform distribution)；b)在上一帧得到的目标附近按照高斯分布来放，可以理解成，靠近目标的地方多放，远离目标的地方少放。Rob Hess的代码用的是后一种方法。狗放出去后，每条狗怎么搜索目标呢？就是按照初始化阶段得到的目标特征(色调直方图，向量V)。每条狗计算它所处的位置处图像的颜色特征，得到一个色调直方图，向量Vi，计算该直方图与目标直方图的相似性。相似性有多种度量，最简单的一种是计算sum(abs(Vi-V)).每条狗算出相似度后再做一次归一化，使得所有的狗得到的相似度加起来等于1.
3）决策阶段
我们放出去的一条条聪明的狗向我们发回报告，“一号狗处图像与目标的相似度是0.3”,“二号狗处图像与目标的相似度是0.02”,“三号狗处图像与目标的相似度是0.0003”,“N号狗处图像与目标的相似度是0.013”...那么目标究竟最可能在哪里呢？我们做次加权平均吧。设N号狗的图像像素坐标是(Xn,Yn),它报告的相似度是Wn,于是目标最可能的像素坐标X = sum(Xn*Wn),Y = sum(Yn*Wn).
4）重采样阶段Resampling
既然我们是在做目标跟踪，一般说来，目标是跑来跑去乱动的。在新的一帧图像里，目标可能在哪里呢？还是让我们放狗搜索吧。但现在应该怎样放狗呢？让我们重温下狗狗们的报告吧。“一号狗处图像与目标的相似度是0.3”,“二号狗处图像与目标的相似度是0.02”,“三号狗处图像与目标的相似度是0.0003”,“N号狗处图像与目标的相似度是0.013”...综合所有狗的报告，一号狗处的相似度最高，三号狗处的相似度最低，于是我们要重新分布警力，正所谓好钢用在刀刃上，我们在相似度最高的狗那里放更多条狗，在相似度最低的狗那里少放狗，甚至把原来那条狗也撤回来。这就是Sampling Importance Resampling，根据重要性重采样(更具重要性重新放狗)。
(2)->(3)->(4)->(2)如是反复循环，即完成了目标的动态跟踪。
代码解析：
OpenCV学习——物体跟踪的粒子滤波算法实现之RGB->HSV
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;
}
程序现将图像的像素值归一化，然后使用OpenCV中的cvCvtcolor函数将图像从RGB空间转换到HSV空间。
OpenCV学习——物体跟踪的粒子滤波算法实现之设定随机数
gsl_rng_env_setup();/tup the enviorment of random number generator
rng = gsl_rng_alloc( gsl_rng_mt19937 );//create a random number generator
gsl_rng_set( rng, time(NULL) );//initializes the random number generator.
作者使用GSL库进行随机数的产生，GSL是GNU的科学计算库，其中手册中random部分所述进行随机数生成有三个步骤：
随机数生成器环境建立，随机数生成器的创建，随机数生成器的初始化。
OpenCV学习——物体跟踪的粒子滤波算法实现之计算选定区域直方图
histogram** compute_ref_histos( IplImage* frame, CvRect* regions, int n )
{
histogram** histos = malloc( n * sizeof( histogram* ) );
IplImage* tmp;
int i;
for( i = 0; i < n; i++ )
{
cvSetImageROI( frame, regions[i] );/t the region of interest
tmp = cvCreateImage( cvGetSize( frame ), IPL_DEPTH_32F, 3 );
cvCopy( frame, tmp, NULL );
cvResetImageROI( frame );//free the ROI
histos[i] = calc_histogram( &tmp, 1 );//calculate the hisrogram
normalize_histogram( histos[i] );//Normalizes a histogram so all bins sum to 1.0
cvReleaseImage( &tmp );
}
return histos;
}
程序中先设置了一个类型为histogram的指向指针的指针，是histogram指针数组的指针，这个数组是多个选定区域的直方图数据存放的位置。然后对于每一个用户指定的区域，在第一帧中都进行了ROI区域设置，通过对ROI区域的设置取出选定区域，交给函数calc_histogram计算出直方图，并使用normalize_histogram对直方图进行归一化。
计算直方图的函数详解如下：
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++ )
{
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 );
for( r = 0; r < img->height; r++ )
for( c = 0; c < img->width; c++ )
{
bin = histo_bin( pixval32f( h, r, c ),
pixval32f( s, r, c ),
pixval32f( v, r, c ) );
hist[bin] += 1;
}
cvReleaseImage( &h );
cvReleaseImage( &s );
cvReleaseImage( &v );
}
return histo;
}
这个函数将h、s、 v分别取出，然后以从上到下，从左到右的方式遍历以函数histo_bin的评判规则放入相应的bin中（很形象的）。函数histo_bin的评判规则详见下图：
｜－－－－｜－－－－｜－－－－｜。。。。｜－－－－｜－－－－－－｜－－－－－－｜。。。。｜－－－－－－－｜
1NH 2NH 3NH NS＊NH NS＊NH＋1 NS*NH+2 NS＊NH＋NV
OpenCV学习——物体跟踪的粒子滤波算法实现之初始化粒子集
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;
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;
}
}
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;
}
程序中的变量np是指若有多个区域n，则一个区域内的粒子数为p/n，这样粒子的总数为p。然后程序对每个区域（n个）中p/n个粒子进行初始化，三个位置坐标都为选定区域的中点，比例都为1，宽度和高度为选定区域的高度。然后又跑了个循环确定p个粒子被初始化。
OpenCV学习——物体跟踪的粒子滤波算法实现之粒子集合变换
particle transition( particle p, int w, int h, gsl_rng* rng )
{
float x, y, s;
particle pn;
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;
}
程序使用动态二阶自回归模型作为基本变换思路，变换的对象有坐标x，坐标y，和比例s。变换的x和y要符合在width和height之内的条件。
OpenCV学习——物体跟踪的粒子滤波算法实现之粒子集重新采样
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;
}
程序先使用C标准库中的qsort排序函数，按照权重，由大到小将原粒子集排序。然后将权重大的在新的粒子集中分配的多一点。
OpenCV学习——物体跟踪的粒子滤波算法实现之权重归一化
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;
}
其他关于粒子滤波的可参考：http://blog.csdn.net/yang_xian521/article/details/6928131