Camshift算法（2）

     这里主要介绍下MeanShift算法的迭代过程，毕竟Camshift算法是以它为核心的。MeanShift算法是一种寻找局部极值的方法。做为一种直观上的理解是它一步一步爬向最高点即爬山算法.而怎么个爬法，用计算出的重心做为下一步窗口的中心，直到窗口的位置不再变化。在理解MeanShift算法的时候，可以先不加入核函数（是计算距离对统计分布的影响）和权重函数（如人为主观的影响）。    

在Camshift算法中MeanShift是通过1阶矩除以0阶矩来计算重心的。其算法的代码如下：

代码

CV_IMPL  int
cvMeanShift(  const   void *  imgProb, CvRect windowIn,
             CvTermCriteria criteria, CvConnectedComp *  comp )
{
    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; // 初始化联通区域

    moments.m00  =  moments.m10  =  moments.m01  =   0 ;  // 初始化0、1阶矩

    __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  ||        // x，y是指角点坐标而不是中心坐标
        windowIn.y  <   0   ||  windowIn.y  +  windowIn.height  >  mat -> rows )
        CV_ERROR( CV_StsBadArg,  " Initial window is not inside the image ROI "  );

    CV_CALL( criteria  =  cvCheckTermCriteria( criteria,  1 .,  100  )); // 迭代的结束条件，

    eps  =  cvRound( criteria.epsilon  *  criteria.epsilon );

     for ( i  =   0 ; i  <  criteria.max_iter; i ++  )
    {
         int  dx, dy, nx, ny;
         double  inv_m00;

        CV_CALL( cvGetSubRect( mat,  & cur_win, cur_rect ));  // cur_win指向窗口内的数据
        CV_CALL( cvMoments(  & cur_win,  & moments ));          // 计算窗口内的各种矩

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

        inv_m00  =  moments.inv_sqrt_m00 * moments.inv_sqrt_m00;
        dx  =  cvRound( moments.m10  *  inv_m00  -  windowIn.width * 0.5  ); // 中心点的坐标-宽的一半
        dy  =  cvRound( moments.m01  *  inv_m00  -  windowIn.height * 0.5  ); // 中心点的坐标-高的一半

        nx  =  cur_rect.x  +  dx; // 新的x坐标
        ny  =  cur_rect.y  +  dy; // 新的y坐标

         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__;

     if ( comp ) // 返回矩形和0阶矩
    {
        comp -> rect  =  cur_rect;
        comp -> area  =  ( float )moments.m00;
    }

     return  i;   // 返回迭代次数
}

 

Camshift算法代码：

 

代码

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; // 初始化comp

    __BEGIN__;

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

    CV_CALL( itersUsed  =  cvMeanShift( mat, windowIn, criteria,  & comp )); // 调用meanshift计算质心
    windowIn  =  comp.rect; // 获得新的窗口的位置

     // 为了容错，窗口的四边都增大了TOLERANCE
    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;
    m10  =  moments.m10;
    m01  =  moments.m01;
    mu11  =  moments.mu11;
    mu20  =  moments.mu20;
    mu02  =  moments.mu02;

     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 tetta 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;
}