Opencv MeanShif 代码分析

#include "_cv.h"

/*F///
//    Name:    cvMeanShift
//    Purpose: MeanShift algorithm
//    Context:
//    Parameters:
//      imgProb     - 2D object probability distribution
//      windowIn    - CvRect of CAMSHIFT Window intial size
//      numIters    - If CAMSHIFT iterates this many times, stop
//      windowOut   - Location, height and width of converged CAMSHIFT window
//      len         - If != NULL, return equivalent len
//      width       - If != NULL, return equivalent width
//      itersUsed   - Returns number of iterations CAMSHIFT took to converge
//    Returns:
//      The function itself returns the area found
//    Notes:
//F*/
CV_IMPL int
cvMeanShift( const void* imgProb, CvRect windowIn,
             CvTermCriteria criteria, CvConnectedComp* comp )
{
    //CvMoments用来计算矩形的重心，面积等形状特征
    CvMoments moments;
    int    i = 0, eps;
    CvMat  stub, *mat = (CvMat*)imgProb;
    CvMat  cur_win;
    CvRect cur_rect = windowIn;


    CV_FUNCNAME( "cvMeanShift" );


    //初始化跟踪窗口
    if( comp )
        comp->rect = windowIn;


    //把0阶矩和1阶矩先初始化置零
    moments.m00 = moments.m10 = moments.m01 = 0;


    __BEGIN__;


    CV_CALL( mat = cvGetMat( mat, &stub ));


    //各种输入变量不符合要求时显示错误信息
    if( CV_MAT_CN( mat->type ) > 1 )
        CV_ERROR( CV_BadNumChannels, cvUnsupportedFormat );
    if( windowIn.height <= 0 || windowIn.width <= 0 )
        CV_ERROR( CV_StsBadArg, "Input window has non-positive sizes" );
    if( windowIn.x < 0 || windowIn.x + windowIn.width > mat->cols ||
        windowIn.y < 0 || windowIn.y + windowIn.height > mat->rows )
        CV_ERROR( CV_StsBadArg, "Initial window is not inside the image ROI" );


    //迭代的标准，精度=1.0，迭代次数=100
    CV_CALL( criteria = cvCheckTermCriteria( criteria, 1., 100 ));


    //精度eps=1
    eps = cvRound( criteria.epsilon * criteria.epsilon );


    //最大循环次数=最大迭代次数criteria.max_iter=100
    for( i = 0; i < criteria.max_iter; i++ )
    {
        int dx, dy, nx, ny;
        double inv_m00;


        //选取搜索区域，对该矩形区域计算它的0,1阶矩
        CV_CALL( cvGetSubRect( mat, &cur_win, cur_rect )); 
        CV_CALL( cvMoments( &cur_win, &moments ));


        /* Calculating center of mass */
        if( fabs(moments.m00) < DBL_EPSILON )
            break;


        //搜索区域的质量m00
        inv_m00 = moments.inv_sqrt_m00*moments.inv_sqrt_m00;
        //搜索区域的水平重心偏移dx
        dx = cvRound( moments.m10 * inv_m00 - windowIn.width*0.5 );
        //搜索区域的垂直重心偏移dy
        dy = cvRound( moments.m01 * inv_m00 - windowIn.height*0.5 );


        //搜索区域的重心坐标(nx,ny)
        nx = cur_rect.x + dx;
        ny = cur_rect.y + dy;


        //跟踪目标处于图像边缘时进行一些相应的处理
        if( nx < 0 )
            nx = 0;
        else if( nx + cur_rect.width > mat->cols )
            nx = mat->cols - cur_rect.width;


        if( ny < 0 )
            ny = 0;
        else if( ny + cur_rect.height > mat->rows )
            ny = mat->rows - cur_rect.height;


        dx = nx - cur_rect.x;
        dy = ny - cur_rect.y;
        cur_rect.x = nx;
        cur_rect.y = ny;


        /* Check for coverage centers mass & window */
        //精度达到要求时即可退出循环
        if( dx*dx + dy*dy < eps )
            break;
    }


    __END__;


    //对meanshift函数的返回值赋值
    //comp->rect是收敛后窗口位置，comp->area是最终窗口中所有像素点的和
    if( comp )
    {
        comp->rect = cur_rect;
        comp->area = (float)moments.m00;
    }


    return i;
}




/*F///
//    Name:    cvCamShift
//    Purpose: CAMSHIFT algorithm
//    Context:
//    Parameters:
//      imgProb     - 2D object probability distribution
//      windowIn    - CvRect of CAMSHIFT Window intial size
//      criteria    - criteria of stop finding window
//      windowOut   - Location, height and width of converged CAMSHIFT window
//      orientation - If != NULL, return distribution orientation
//      len         - If != NULL, return equivalent len
//      width       - If != NULL, return equivalent width
//      area        - sum of all elements in result window
//      itersUsed   - Returns number of iterations CAMSHIFT took to converge
//    Returns:
//      The function itself returns the area found
//    Notes:
//F*/
CV_IMPL int
cvCamShift( const void* imgProb, CvRect windowIn,
            CvTermCriteria criteria,
            CvConnectedComp* _comp,
            CvBox2D* box )
{
    const int TOLERANCE = 10;
    CvMoments moments;
    double m00 = 0, m10, m01, mu20, mu11, mu02, inv_m00;
    double a, b, c, xc, yc;
    double rotate_a, rotate_c;
    double theta = 0, square;
    double cs, sn;
    double length = 0, width = 0;
    int itersUsed = 0;
    CvConnectedComp comp;
    CvMat  cur_win, stub, *mat = (CvMat*)imgProb;


    CV_FUNCNAME( "cvCamShift" );


    comp.rect = windowIn;


    __BEGIN__;


    CV_CALL( mat = cvGetMat( mat, &stub ));


    //调用cvMeanShift函数
    CV_CALL( itersUsed = cvMeanShift( mat, windowIn, criteria, &comp ));
    windowIn = comp.rect;


    //对边界情况进行处理
    windowIn.x -= TOLERANCE;
    if( windowIn.x < 0 )
        windowIn.x = 0;


    windowIn.y -= TOLERANCE;
    if( windowIn.y < 0 )
        windowIn.y = 0;


    windowIn.width += 2 * TOLERANCE;
    if( windowIn.x + windowIn.width > mat->width )
        windowIn.width = mat->width - windowIn.x;


    windowIn.height += 2 * TOLERANCE;
    if( windowIn.y + windowIn.height > mat->height )
        windowIn.height = mat->height - windowIn.y;


    CV_CALL( cvGetSubRect( mat, &cur_win, windowIn ));


    /* Calculating moments in new center mass */
    //计算新中心处的颜色统计矩
    CV_CALL( cvMoments( &cur_win, &moments ));


    m00 = moments.m00; //0阶
    m10 = moments.m10; //水平1阶
    m01 = moments.m01; //垂直1阶
    mu11 = moments.mu11; //水平垂直2阶
    mu20 = moments.mu20; //水平2阶
    mu02 = moments.mu02; //垂直2阶


    //目标矩形的质量太小了就退出
    if( fabs(m00) < DBL_EPSILON )
        EXIT;


    //质量的倒数，只是为了下面计算方便，可以把除法表示成乘法
    inv_m00 = 1. / m00;
    xc = cvRound( m10 * inv_m00 + windowIn.x );
    yc = cvRound( m01 * inv_m00 + windowIn.y );
    a = mu20 * inv_m00;
    b = mu11 * inv_m00;
    c = mu02 * inv_m00;


    /* Calculating width & height */
    square = sqrt( 4 * b * b + (a - c) * (a - c) );


    /* Calculating orientation */
    //计算目标主轴方向角度
    theta = atan2( 2 * b, a - c + square );


    /* Calculating width & length of figure */
    cs = cos( theta );
    sn = sin( theta );


    rotate_a = cs * cs * mu20 + 2 * cs * sn * mu11 + sn * sn * mu02;
    rotate_c = sn * sn * mu20 - 2 * cs * sn * mu11 + cs * cs * mu02;
    //下次搜索窗口的长宽
    length = sqrt( rotate_a * inv_m00 ) * 4;
    width = sqrt( rotate_c * inv_m00 ) * 4;


    /* In case, when theta is 0 or 1.57... the Length & Width may be exchanged */
    if( length < width )
    {
        double t;
        
        CV_SWAP( length, width, t );
        CV_SWAP( cs, sn, t );
        theta = CV_PI*0.5 - theta;
    }


    /* Saving results */
    if( _comp || box )
    {
        int t0, t1;
        int _xc = cvRound( xc );
        int _yc = cvRound( yc );


        t0 = cvRound( fabs( length * cs ));
        t1 = cvRound( fabs( width * sn ));


        t0 = MAX( t0, t1 ) + 2;
        comp.rect.width = MIN( t0, (mat->width - _xc) * 2 );


        t0 = cvRound( fabs( length * sn ));
        t1 = cvRound( fabs( width * cs ));


        t0 = MAX( t0, t1 ) + 2;
        comp.rect.height = MIN( t0, (mat->height - _yc) * 2 );


        comp.rect.x = MAX( 0, _xc - comp.rect.width / 2 );
        comp.rect.y = MAX( 0, _yc - comp.rect.height / 2 );


        comp.rect.width = MIN( mat->width - comp.rect.x, comp.rect.width );
        comp.rect.height = MIN( mat->height - comp.rect.y, comp.rect.height );
        comp.area = (float) m00;
    }


    __END__;


    if( _comp )
        *_comp = comp;
    
    if( box )
    {
        box->size.height = (float)length;
        box->size.width = (float)width;
        box->angle = (float)(theta*180./CV_PI);
        box->center = cvPoint2D32f( comp.rect.x + comp.rect.width*0.5f,
                                    comp.rect.y + comp.rect.height*0.5f);
    }


    return itersUsed;
}


/* End of file. */