OpenCV学习——物体跟踪的粒子滤波算法实现

从概念上讲，一个粒子滤波算法包含一个被监视系统的状态的概率分布。在本项目中，状态就是指被追踪物体的位置、大小等等。在许多情况下，非线性和非高斯型在物体的运动和相似性建模上会得到一个难以处理的滤波分布。粒子滤波采用将这个分布重新表示为一组加权值，或称为粒子的方法克服了这个困难。每个粒子表示一个可能的系统状态实例。换句话说，每个粒子描述了被追踪物体可能处于的一个方位。一个粒子集包含了被追踪物体最有可能处于的方位。因此，我们可以通过寻找在粒子滤波分布中最大的权重来确定物体最有可能处于的状态。算法流程1.命令行参数处理 －>
2.设置随机数生成器环境，创建随机数生成器、并且对其初始化。->
3.初始化视频句柄－>
4.取视频中的一帧进行处理－>
1）GRB－>HSV
2）保存当前帧在frames
3) 判断是否为第一帧，
若是则，
（1）忙等用户选定欲跟踪的区域
（2）计算相关区域直方图
（3）得到跟踪粒子
若不是则，
（1）对每个粒子作变换，并计算每个粒子的权重
（2）对粒子集合进行归一化
（3）重新采样粒子
4）画出粒子所代表的区域

5.释放图像void arg_parse( int argc,char** argv )
{
int i = 0;
/*extract program name from command line (remove path, if present) */
pname = remove_path( argv[0] );

/*parse commandline options */
while( TRUE )
{
char* arg_check;
int arg = getopt( argc, argv, OPTIONS );
if( arg == -1 )
break;

switch( arg )
{
/* user asked for help */
case 'h':
usage( pname );
exit(0);
break;

case 'a':
show_all = TRUE;
break;

/* user wants to output tracking sequence */
case 'o':
export = TRUE;
break;

/* user wants to set number of particles */
case 'p':
if( ! optarg )
fatal_error( "error parsingarguments at -%c/n" /
"Try '%s-h' for help.", arg, pname );
num_particles = strtol( optarg, &arg_check, 10 );
if( arg_check == optarg || *arg_check !='/0' )
fatal_error( "-%c option requires aninteger argument/n" /
"Try '%s-h' for help.", arg, pname );
break;

/* catch invalid arguments */
default:
fatal_error( "-%c: invalid option/nTry '%s -h'for help.",
optopt, pname );
}
}

/* make sure input and output files are specified */
if( argc - optind < 1 )
fatal_error( "no input image specified./nTry '%s -h'for help.", pname );
if( argc - optind > 2 )
fatal_error( "too many arguments./nTry '%s -h' forhelp.", pname );

/* record video file name */
vid_file = argv[optind];
}//作者使用Getopt这个系统函数对命令行进行解析，－h表示显示帮助，－a表示将所有粒子所代表的位置都显示出来，－o表示输出tracking的帧，－pnumber进行粒子数的设定，然后再最后指定要处理的视频文件。gsl_rng_env_setup();//setupthe 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部分所述进行随机数生成有三个步骤：
随机数生成器环境建立，随机数生成器的创建，随机数生成器的初始化。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空间。/*
Computes a reference histogram for each of the object regions defined by
the user

@param frame video frame in which to compute histograms; should havebeen
converted to hsv using bgr2hsv in observation.h
@param regions regions of /a frame over which histograms should becomputed
@param n number of regions in /a regions
@param export if TRUE, object region images are exported

@return Returns an /a n element array of normalized histogramscorresponding
to regions of /a frame specified in /a regions.
*/
histogram** compute_ref_histos( IplImage* frame, CvRect* regions, int n )
{
histogram** histos = malloc( n * sizeof( histogram* ) );
IplImage* tmp;
int i;

/* extract each region from frame and compute its histogram */
for( i = 0; i < n; i++ )
{
cvSetImageROI( frame, regions[i] );//set theregion 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 ahistogram so all bins sum to 1.0
cvReleaseImage( &tmp );
}

return histos;
}

程序中先设置了一个类型为histogram的指向指针的指针，是histogram指针数组的指针，这个数组是多个选定区域的直方图数据存放的位置。然后对于每一个用户指定的区域，在第一帧中都进行了ROI区域设置，通过对ROI区域的设置取出选定区域，交给函数calc_histogram计算出直方图，并使用normalize_histogram对直方图进行归一化。
计算直方图的函数详解如下：
/*
Calculates a cumulative histogram as defined above for a given array
of images

@param img an array of images over which to compute a cumulativehistogram;
each must have been converted to HSV colorspace usingbgr2hsv()
@param n the number of images in imgs

@return Returns an un-normalized HSV histogram for /a imgs
*/
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 */
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 eachpixel */
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/*
Creates an initial distribution of particles at specified locations

@param regions an array of regions describing player locations around
which particles are to be sampled
@param histos array of histograms describing regions in /a regions
@param n the number of regions in /a regions
@param p the total number of particles to be assigned

@return Returns an array of /a p particles sampled from around regions in
/a regions
*/
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 */
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;
}

//程序中的变量np是指若有多个区域n，则一个区域内的粒子数为p/n，这样粒子的总数为p。然后程序对每个区域（n个）中p/n个粒子进行初始化，三个位置坐标都为选定区域的中点，比例都为1，宽度和高度为选定区域的高度。然后又跑了个循环确定p个粒子被初始化。/*
Samples a transition model for a given particle

@param p a particle to be transitioned
@param w video frame width
@param h video frame height
@param rng a random number generator from which to sample

@return Returns a new particle sampled based on <EM>p</EM>'stransition
model
*/
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;
}

//程序使用动态二阶自回归模型作为基本变换思路，变换的对象有坐标x，坐标y，和比例s。变换的x和y要符合在width和height之内的条件。/*
Normalizes particle weights so they sum to 1

@param particles an array of particles whose weights are to benormalized
@param n the number of particles in /a particles
*/
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;
}/*
Re-samples a set of particles according to their weights to produce a
new set of unweighted particles

@param particles an old set of weighted particles whose weights havebeen
normalized with normalize_weights()
@param n the number of particles in /a particles

@return Returns a new set of unweighted particles sampled from /aparticles
*/
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;
}http://blog.csdn.net/ysyoyoys/article/details/7410195