谁有粒子滤波C++实现的代码

程序流程：

1.命令行参数处理 －>

2.设置随机数生成器环境，创建随机数生成器、并且对其初始化。->

3.初始化视频句柄 －>

4.取视频中的一帧进行处理 －>

1）GRB－>HSV

2）保存当前帧在frames

3) 判断是否为第一帧，

若是则，

（1）忙等用户选定欲跟踪的区域

（2）计算相关区域直方图

（3）得到跟踪粒子

若不是则，

（1）对每个粒子作变换，并计算每个粒子的权重

（2）对粒子集合进行归一化

（3）重新采样粒子

4）画出粒子所代表的区域

5.释放图像

OpenCV学习——物体跟踪的粒子滤波算法实现之命令行参数处理

void arg_parse( int argc, char** argv )

{

int i = 0

pname = remove_path( argv[0] )

while( TRUE )

{

char* arg_check

int arg = getopt( argc, argv, OPTIONS )

if( arg == -1 )

break

switch( arg )

{

case 'h':

usage( pname )

exit(0)

break

case 'a':

show_all = TRUE

break

case 'o':

export = TRUE

break

case 'p':

if( ! optarg )

fatal_error( "error parsing arguments 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 an integer argument\n"\

"Try '%s -h' for help.", arg, pname )

break

default:

fatal_error( "-%c: invalid option\nTry '%s -h' for help.",

optopt, pname )

}

}

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' for help.", pname )

vid_file = argv[optind]

}

作者使用Getopt这个系统函数对命令行进行解析，－h表示显示帮助，－a表示将所有粒子所代表的位置都显示出来，－o表示输出tracking的帧，－p number进行粒子数的设定，然后再最后指定要处理的视频文件。

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()//setup 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 = 0i <ni++ )

{

cvSetImageROI( frame, regions[i] )//set 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 = 0i <ni++ )

{

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 = 0r <img->heightr++ )

for( c = 0c <img->widthc++ )

{

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 3NHNS＊NHNS＊NH＋1NS*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 = 0i <ni++ )

{

width = regions[i].width

height = regions[i].height

x = regions[i].x + width / 2

y = regions[i].y + height / 2

for( j = 0j <npj++ )

{

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 = 0i <ni++ )

{

np = cvRound( particles[i].w * n )

for( j = 0j <npj++ )

{

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 = 0i <ni++ )

sum += particles[i].w

for( i = 0i <ni++ )

particles[i].w /= sum

}

粒子滤波算法受到许多领域的研究人员的重视,该算法的主要思想是使用一个带有权值的粒子集合来表示系统的后验概率密度.在扩展卡尔曼滤波和Unscented卡尔曼滤波算法的基础上,该文提出一种新型粒子滤波算法.首先用Unscented卡尔曼滤波器产生系统的状态估计,然后用扩展卡尔曼滤波器重复这一过程并产生系统在k时刻的最终状态估计.在实验中,针对非线性程度不同的两种系统,分别采用5种粒子滤波算法进行实验

---