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