uiplutils.cpp

来自「微软的基于HMM的人脸识别原代码, 非常经典的说」· C++ 代码 · 共 1,799 行 · 第 1/4 页

                {
                    p = ((char*)data)[x*ch];
                }
                else
                {
                    p = ((int*)data)[x*ch]*2 != 0;
                }
                xp = x*p;
                tm0 += p;
                tm1 += xp;
                tm2 += ((int64)x2)*p;
                tm3 += ((int64)x2)*xp;
            }

            m00 += tm0;
            m10 += tm1;
            m01 += tm0*y;
            m20 += tm2;
            m11 += ((int64)tm1)*y;
            m02 += ((int64)tm0)*y2;
            m30 += tm3;
            m21 += tm2*y;
            m12 += ((int64)tm1)*y2;
            m03 += (((int64)tm0)*y2)*y;
        }

        if( m00 != 0 )
        {
            cx = ((double)m10)/m00;
            cy = ((double)m01)/m00;
        }

        s.m00 = (double)m00;
        s.m10 = (double)m10;
        s.m01 = (double)m01;
        s.m20 = (double)m20;
        s.m11 = (double)m11;
        s.m02 = (double)m02;
        s.m30 = (double)m30;
        s.m21 = (double)m21;
        s.m12 = (double)m12;
        s.m03 = (double)m03;
    }
    else /* floating-point spatial moments */
    {
        int x2, y2; /* x^2 & y^2 */
        float* data = (float*)img_data;

        assert( (img_step&3) == 0 );
        img_step /= 4;

        /* calc spatial moments */
        for( y = 0, y2 = 0; y < sz.height; y2 += 2*y + 1, y++, data += img_step )
        {
            double tm0 = 0, tm1 = 0, tm2 = 0, tm3 = 0;

            for( x = 0, x2 = 0; x < sz.width; x2 += 2*x + 1, x++ )
            {
                double p = data[x*ch], xp = x*p;
                tm0 += p;
                tm1 += xp;
                tm2 += x2*p;
                tm3 += x2*xp;
            }

            s.m00 += tm0;
            s.m10 += tm1;
            s.m01 += tm0*y;
            s.m20 += tm2;
            s.m11 += tm1*y;
            s.m02 += tm0*y2;
            s.m30 += tm3;
            s.m21 += tm2*y;
            s.m12 += tm1*y2;
            s.m03 += (tm0*y2)*y;
        }

        if( s.m00 != 0 )
        {
            cx = s.m10/s.m00;
            cy = s.m01/s.m00;
        }
        img_step *= 4;
    }

    /* calc central moments */
    for( y = 0; y < sz.height; y++, img_data += img_step )
    {
        double  tm0 = 0;
        double  tm1 = 0, tm2 = 0, tm3 = 0;
        double  yc = y - cy;
        double  yc2 = yc*yc;

        for( x = 0; x < sz.width; x++ )
        {
            double p;
            double xc = x - cx;
            double xc2 = xc*xc;
            double xcp;

            if( depth == IPL_DEPTH_8U )
            {
                p = binary ? img_data[x*ch] != 0 : img_data[x*ch];
            }
            else if( depth == IPL_DEPTH_8S )
            {
                p = ((char*)img_data)[x*ch];
            }
            else
            {
                p = binary ? ((int*)img_data)[x*ch]*2 != 0 : ((float*)img_data)[x*ch];
            }

            xcp = xc*p;
            tm0 += p;
            tm1 += xcp;
            tm2 += xc2*p;
            tm3 += xc2*xcp;
        }

        s.mu20 += tm2;
        s.mu11 += tm1*yc;
        s.mu02 += tm0*yc2;
        s.mu30 += tm3;
        s.mu21 += tm2*yc;
        s.mu12 += tm1*yc2;
        s.mu03 += tm0*yc2*yc;
    }

    /* calc normalized moments */
    {
        double inv_m00 = s.m00 == 0 ? 0 : 1./s.m00;
        double s2 = inv_m00*inv_m00; /* 1./(m00 ^ (2/2 + 1)) */
        double s3 = s2*sqrt(inv_m00); /* 1./(m00 ^ (3/2 + 1)) */

        s.nu20 = s.mu20 * s2;
        s.nu11 = s.mu11 * s2;
        s.nu02 = s.mu02 * s2;

        s.nu30 = s.mu30 * s3;
        s.nu21 = s.mu21 * s3;
        s.nu12 = s.mu12 * s3;
        s.nu03 = s.mu03 * s3;
    }

    *state = s;
}


/* The function draws line in 8uC1/C3 image */
void  atsDrawLine( IplImage* img, float x1, float y1, float x2, float y2, int color )
{
    float    dx = x2 - x1;
    float    dy = y2 - y1;
    float    adx = (float)fabs(dx);
    float    ady = (float)fabs(dy);
    float    steps = 0;
    uchar*   img_data;
    int      img_step;
    CvSize  sz;
    int      depth = 0, ch = 0, bt_pix = 0;

    uchar    b = (uchar)(color & 0xff);
    uchar    g = (uchar)((color >> 8) & 0xff);
    uchar    r = (uchar)((color >> 16) & 0xff);

    atsGetImageInfo( img, (void**)&img_data, &img_step, &sz, &depth, &ch, &bt_pix );

    assert( depth == IPL_DEPTH_8U );
    assert( ch == 1 || ch == 3);

    if( adx > ady )
    {
        dy /= adx;
        dx = dx > 0 ? 1.f : -1.f;
        steps = adx;
    }
    else if( ady != 0 )
    {
        dx /= ady;
        dy = dy > 0 ? 1.f : -1.f;
        steps = ady;
    }
    else
    {
        dx = dy = 0;
    }

    do
    {
        int x = atsRound(x1);
        int y = atsRound(y1);

        if( (unsigned)x < (unsigned)sz.width &&
            (unsigned)y < (unsigned)sz.height )
        {
            uchar* data = img_data + y*img_step + x*bt_pix;
            data[0] = b;
            if( ch == 3 )
            {
                data[1] = g;
                data[2] = r;
            }
        }

        x1 += dx;
        y1 += dy;
        steps--;
    }
    while( steps >= 0 );
}


/* The function draws ellipse arc in 8uC1/C3 image */
void  atsDrawEllipse( IplImage* img, float xc, float yc, float a, float b,
                      float angle, float arc0, float arc1, int color )
{
    assert( a >= b );
    if( a == 0 ) a = 0.1f;
    if( b == 0 ) b = 0.1f;
    {
    double  ba = b/a;
    double  e = sqrt( 1. - ba*ba);
    double  mag = b*ba;
    double  alpha, beta;
    double  c = a*e;
    double  x, y;
    double  a0 = arc0, a1 = arc1;
    double  ang;
    int     is_pt = arc0 == arc1;

    ang  = IPLsDegToRad(angle);
    alpha = cos(ang);
    beta  = sin(ang);

    if( a0 > a1 )
    {
        double temp = a0;
        a0 = a1;
        a1 = temp;
    }

    if( a1 - a0 >= 360 )
    {
        a0 = a1 = 0;
    }

    a0 = IPL_DegToRad(a0);
    a1 = IPL_DegToRad(a1);

    x = c + a*cos(a0);
    y = b*sin(a0);
    a0 = atan2( y, x )*180/IPL_PI;
    if( a0 < 0 ) a0 += 360.;

    if( is_pt )
        a0 = a1;
    else
    {
        x = c + a*cos(a1);
        y = b*sin(a1);
        a1 = atan2( y, x )*180/IPL_PI;
        if( a1 < 0 ) a1 += 360.f;
        if( a0 > a1 - 0.1 ) a0 -= 360;
    }

    xc = (float)( xc - c*alpha);
    yc = (float)( yc + c*beta);

    atsDrawConic( img, xc, yc, (float)mag, (float)e, angle, (float)a0, (float)a1, color );
    }
}


/* The function draws conic arc in 8uC1/C3 image */
void  atsDrawConic( IplImage* img, float xc, float yc, float mag, float e,
                    float angle, float arc0, float arc1, int color )
{
    int delta = 1;
    double  dang = IPL_PI*delta/180;
    double  alpha, beta;
    double  da = cos(dang), db = sin(dang);
    double  a, b;
    float   x1 = 0.f, y1 = 0.f;
    int     i, n, fl = 0;

    assert( mag > 0 && e >= 0 );

    angle = IPLsDegToRad(angle);
    alpha = cos(angle);
    beta  = sin(angle);

    if( arc0 > arc1 )
    {
        float temp = arc0;
        arc0 = arc1;
        arc1 = temp;
    }

    n = atsRound( arc1 - arc0 );
    if( n > 360 ) n = 360;

    arc0 = IPLsDegToRad(arc0);

    a = cos( arc0 );
    b = sin( arc0 );

    for( i = 0; i < n + delta; i += delta )
    {
        double d;

        if( i > n )
        {
            arc1 = IPLsDegToRad(arc1);
            a = cos(arc1);
            b = sin(arc1);
        }

        d = 1 - e*a;
        if( d != 0 )
        {
            double r = mag/d;
            double x = r*a;
            double y = r*b;
            float  x2 = (float)(xc + x*alpha - y*beta);
            float  y2 = (float)(yc - x*beta - y*alpha);
            if( fl ) atsDrawLine( img, x1, y1, x2, y2, color );
            x1 = x2;
            y1 = y2;
            fl = 1;
        }
        else
        {
            fl = 0;
        }
        d = a*da - b*db;
        b = a*db + b*da;
        a = d;
    }
}


static void _atsCalcConicPoint( double xc, double yc,
                                double mag, double e, double alpha,
                                double beta, double ang, CvPoint* pt )
{
    double a = cos( ang );
    double b = sin( ang );

    double d = 1 - e*a;
    if( d == 0 )
    {
        pt->x = pt->y = -10000;
    }
    else
    {
        d = mag/d;
        a *= d;
        b *= d;
        pt->x = atsRound( xc + alpha*a - beta*b );
        pt->y = atsRound( yc - beta*a - alpha*b );
    }
}


int  atsCalcQuadricCoeffs( double xc, double yc, double mag, double e,
                           double angle, double arc0, double arc1,
                           double* _A, double* _B, double* _C, double* _D, double* _E,
                           CvPoint* pt1, CvPoint* pt2 )
{
    double ang = angle*IPL_PI/180;
    double alpha = cos( ang );
    double beta = sin( ang );
    double alal = alpha*alpha;
    double bebe = beta*beta;
    double albe = alpha*beta;
    double a = 0, c = 0;
    double A = 0, B = 0, C = 0, D = 0, E = 0;
    double dx;
    int    code = 0;
    double cf_max = 0;
    /*double arcm; */
    /*int    oct1, oct2; */
    /*CvPoint ptm; */

    assert( mag > 0 && e >= 0 );

    if( arc0 > arc1 )
    {
        double temp = arc0;
        arc0 = arc1;
        arc1 = temp;
    }

    if( arc1 - arc0 > 360 )
    {
        arc0 = 0;
        arc1 = 360;
    }

    if( e != 1 ) /* non-parabolic case */
    {
        a = mag/fabs(e*e - 1);
        c = sqrt( mag*a );
        dx = a*e;
        a = 1./(a*a);
        c = 1./(c*c);
        D = -2*dx*a;
        if( e > 1 )
        {
            c = -c;
            D = -D;
        }
    }
    else
    {
        c = 1;
        dx = mag*0.5;
        D = -mag*2;
    }

    E = -beta*D;
    D *= alpha;

    /* rotate matrix */
    A = alal*a + bebe*c;
    B = albe*(c - a)*2;
    C = bebe*a + alal*c;

    /* move to (xc, yc) */
    D -= (2*A*xc + B*yc);
    E -= (B*xc + 2*C*yc);

    arc0 *= IPL_PI/180;
    arc1 *= IPL_PI/180;

    _atsCalcConicPoint( xc, yc, mag, e, alpha, beta, arc0, pt1 );
    if( pt1->x == -10000 ) code--;

    _atsCalcConicPoint( xc, yc, mag, e, alpha, beta, arc1, pt2 );
    if( pt2->x == -10000 ) code--;

    if( pt1->x == pt2->x && pt1->y == pt2->y )
    {
        if( arc1 - arc0 < 1.f ) code--;
    }

    if( fabs(A) > cf_max ) cf_max = fabs(A);
    if( fabs(B) > cf_max ) cf_max = fabs(B);
    if( fabs(C) > cf_max ) cf_max = fabs(C);