PariticalFilter在MFC上的运行，源代码公开

       由于项目需要，进行过一段时间的 PariticalFilter 研究。主要的工作就是将网络上的Console代码和Mfc融合在一起，并且添加了Mfc端的控制功能。

      程序还有不完善的地方，现将相关的函数发布出来，大家相互研究。 程序运行界面

程序的核心为两个部分，一个是核心PF函数，一个是界面操作
核心函数
/** @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)); 
}
界面处理相关
void CGOMfcTemplate2Dlg : :OnMouseMove(UINT nFlags, CPoint point) 
{ 
    CRect rect_ctr;   
    ( this - >GetDlgItem(IDC_CAM)) - >GetWindowRect( &rect_ctr); //获取Picture控件相对屏幕左上角的坐标，   
    ScreenToClient(rect_ctr); //获取Picture控件相对对话框客户区左上角的坐标   
    instant_position.x  = point.x; 
    instant_position.y  = point.y; 
    picture_ordinate_x  = point.x  - rect_ctr.left; 
    picture_ordinate_y  = point.y  - rect_ctr.top; //point获取的是鼠标相对对话框客户区左上角的坐标，减去rect_ctr.left和rect_ctr.top后，即为鼠标相对Picture控件左上角的坐标   
     if ((nFlags  == MK_LBUTTON)  && Is_LeftButton_Down) 
    { 
         : :SetCursor(cross); 
        CClientDC dc( this); 
        CBrush  *OldBrush; 
        OldBrush =(CBrush *)dc.SelectStockObject(NULL_BRUSH); 
        dc.SetROP2(R2_NOT); 
        dc.Rectangle(CRect(chosen_position,instant_position)); 
        dc.Rectangle(CRect(chosen_position,point)); 
        dc.SelectObject(OldBrush); 
        instant_position =point; 
    } 
     else 
         : :SetCursor(arrow); 
    CDialogEx : :OnMouseMove(nFlags, point); 
} 

void CGOMfcTemplate2Dlg : :OnLButtonDown(UINT nFlags, CPoint point) 
{ 
    Is_LeftButton_Down  =  true; 
     : :SetCursor(cross); 
    chosen_position  = instant_position  = point; 
    CDialogEx : :OnLButtonDown(nFlags, point); 
} 
void CGOMfcTemplate2Dlg : :OnLButtonUp(UINT nFlags, CPoint point) 
{ 
     if (Is_LeftButton_Down) 
    { 
        Is_LeftButton_Down  =  false; 
         : :SetCursor(arrow); 
        CClientDC dc( this); 
        CPen * pOldPen =(CPen *)dc.SelectObject( &pen); 
        dc.MoveTo(chosen_position); 
        dc.LineTo(chosen_position.x,instant_position.y); 
        dc.LineTo(instant_position); 
        dc.LineTo(instant_position.x,chosen_position.y); 
        dc.LineTo(chosen_position); 
         //写到rect_res中区 
         int rect_x  = min(chosen_position.x,instant_position.x); 
         int rect_y  = min(chosen_position.y,instant_position.y); 
         int rect_w  = abs(chosen_position.x  - instant_position.x); 
         int rect_h  = abs(chosen_position.y  - instant_position.y); 
        rectStart  = Rect(rect_x,rect_y,rect_w,rect_h); 
    } 
    CDialogEx : :OnLButtonUp(nFlags, point); 
}