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

/** @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);
}