/** @file
Definitions related to tracking with particle filtering
@author Rob Hess
@version 1.0.0-20060307
jsxyhelu 2016年11月修改 更多代码，请浏览jsxyhelu.cnblogs.com
jsxyhelu 2016年12月代码梳理,重构
*/
#include "stdafx.h"
#include "GOPF.h"
using namespace std;
using namespace cv;
//非常值得注意的一点是，本例中的指针的运用极大地提高了系统速度
//使用方法
/*//变量
int num_particles = 100;//狗的数目
int iFrame = 0;//当前为第几帧
Rect rectStart;//初始化矩形
Mat matFrame; //原始帧
Mat matClone; //原始帧复制
Mat matHSV; //原始帧的HSV形式
particle* particles, * new_particles;
//gsl初始化
gsl_rng* rng;
gsl_rng_env_setup();
rng = gsl_rng_alloc( gsl_rng_mt19937 );
gsl_rng_set( rng, time(NULL) );
//gsl初始化结束,rng为输出
//打开视频
VideoCapture videocapture;
videocapture.open("e:/sandbox/1.avi");
if (!videocapture.isOpened())
{
printf("视频打开失败！");
return -1;
}
//TODO
rectStart = Rect(449,86,21,13);
//主要循环
for (;;)
{
videocapture >> matFrame;
if (matFrame.empty())
break;
matClone = matFrame.clone();
imshow("原始图像",matFrame);
//step 0 当前帧转换为hsv模式，注意是在float下转换
matFrame.convertTo(matHSV,CV_32F,1.0/255);
cvtColor(matHSV,matHSV,COLOR_BGR2HSV);
//step 1 如果为第一帧，初始化相关数据
if (iFrame == 0)
{
//计算初始区域的粒子
histogram* ref_histos = compute_ref_histos( matHSV, rectStart);
particles = init_distribution( rectStart, ref_histos,1, num_particles );
iFrame = 1;
}
else
{
//step 2 放出100狗
for( int j = 0; j < num_particles; j++ )
{
//生成随机狗
particles[j] = transition( particles[j], matFrame.cols,matFrame.rows, rng );
float s = particles[j].s;
//计算相似性
particles[j].w = likelihood( matHSV, cvRound(particles[j].y),
cvRound( particles[j].x ),
cvRound( particles[j].width * s ),
cvRound( particles[j].height * s ),
particles[j].histo );
}
//step 3 重新规划
normalize_weights( particles, num_particles );
new_particles = resample( particles, num_particles );
free( particles );
particles = new_particles;
}
display_particle( &matClone, *particles );
imshow("结果图像",matClone);
waitKey(15);
iFrame = iFrame +1;
}*/
//根据hsv的bin的返回结果
int histo_bin( float h, float s, float v )
{
#define H_MAX 360.0
#define S_MAX 1.0
#define V_MAX 1.0
#define S_THRESH 0.1
#define V_THRESH 0.2
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;
}
//计算直方图结果，因为所有的值都是计算成直方图，所以这个是核心算法(意味运行次数最多，优化最有效）
histogram* calc_histogram( IplImage* imgs, int n )
{
histogram* histo;
IplImage* h, * s, * v;
float* hist;
int i, r, c, bin;
histo = (histogram* )malloc( sizeof(histogram) );
histo->n = NH*NS + NV;
hist = histo->histo;
memset( hist, 0, histo->n * sizeof(float) );
h = cvCreateImage( cvGetSize(imgs), IPL_DEPTH_32F, 1 );
s = cvCreateImage( cvGetSize(imgs), IPL_DEPTH_32F, 1 );
v = cvCreateImage( cvGetSize(imgs), IPL_DEPTH_32F, 1 );
cvSplit( imgs, h, s, v, NULL );
/* increment appropriate histogram bin for each pixel */
for( r = 0; r < imgs->height; r++ )
for( c = 0; c < imgs->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;
}
//直方图正定化
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;
}
//计算n个histogram的数据结构
histogram* compute_ref_histos(Mat src , Rect rect )
{
IplImage matsrc(src);
IplImage* frame = &matsrc;
CvRect regions = (CvRect)rect;
histogram* histos = (histogram*) malloc( sizeof( histogram* ) );
IplImage* tmp;
cvSetImageROI( frame, regions );
tmp = cvCreateImage( cvGetSize( frame ), IPL_DEPTH_32F, 3 );
cvCopy( frame, tmp, NULL );
cvResetImageROI( frame );
histos = calc_histogram( tmp, 1 );
normalize_histogram( histos );
cvReleaseImage( &tmp );
return histos;
}
//初始化distribution
particle* init_distribution( Rect rect, histogram* histos, int n, int p)
{
CvRect regions = (CvRect)rect;
particle* particles;
int np;
float x, y;
int i, j, width, height, k = 0;
particles =(particle*) malloc( p * sizeof( particle ) );
np = p / n;
width = regions.width;
height = regions.height;
x = regions.x + width / 2;
y = regions.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;
particles[k++].w = 0;
}
/* make sure to create exactly p particles */
i = 0;
while( k < p )
{
width = regions.width;
height = regions.height;
x = regions.x + width / 2;
y = regions.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;
particles[k++].w = 0;
i = ( i + 1 ) % n;
}
return particles;
}
//预测狗的位置
particle transition( particle p, int w, int h, gsl_rng* rng )
{
#define TRANS_X_STD 1.0
#define TRANS_Y_STD 0.5
#define TRANS_S_STD 0.001
/* autoregressive dynamics parameters for transition model */
#define A1 2.0
#define A2 -1.0
#define B0 1.0000
float x, y, s;
particle pn;
//采用 second-order 进行狗的估算
/* 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;
}
//histogram比较,马氏距离
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;
for( i = 0; i < n; i++ )
sum += sqrt( hist1[i]*hist2[i] );
return 1.0 - sum;
}
//粒子比较
int particle_cmp( const void* p1,const 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;
}
//相似性计算
float likelihood( Mat matSrc, int r, int c,int w, int h, histogram* ref_histo )
{
#define LAMBDA 20
IplImage matimg(matSrc);
IplImage* img = &matimg;
IplImage* tmp;
histogram* histo;
float d_sq;
/* extract region around (r,c) and compute and normalize its histogram */
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 );
}
//权值归一化
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 )
{
particle* new_particles;
int i, j, np, k = 0;
//quick sort
qsort( particles, n, sizeof( particle ), &particle_cmp );
new_particles =(particle* ) 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;
}
//显示PF的结果
void display_particle( Mat* img, particle p )
{
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 );
cv::rectangle(*img,Point(x0,y0),Point(x1,y1),Scalar(0,0,255));
}
