cvshapedescr.cpp.svn-base

来自「非结构化路识别」· SVN-BASE 代码 · 共 1,147 行 · 第 1/3 页

    /* shift all points to zero */
    for( i = 0; i < n; i++ )
    {
        if( !is_float )
        {
            offx += (float)((CvPoint*)reader.ptr)->x;
            offy += (float)((CvPoint*)reader.ptr)->y;
        }
        else
        {
            offx += ((CvPoint2D32f*)reader.ptr)->x;
            offy += ((CvPoint2D32f*)reader.ptr)->y;
        }
        CV_NEXT_SEQ_ELEM( points->elem_size, reader );
    }

    c = 1.f / n;
    offx *= c;
    offy *= c;

    /* fill matrix rows as (x*x, x*y, y*y, x, y, 1 ) */
    matr = D;
    for( i = 0; i < n; i++ )
    {
        float x, y;
        
        if( !is_float )
        {
            x = (float)((CvPoint*)reader.ptr)->x - offx;
            y = (float)((CvPoint*)reader.ptr)->y - offy;
        }
        else
        {
            x = ((CvPoint2D32f*)reader.ptr)->x - offx;
            y = ((CvPoint2D32f*)reader.ptr)->y - offy;
        }
        CV_NEXT_SEQ_ELEM( points->elem_size, reader );
        
        matr[0] = x * x;
        matr[1] = x * y;
        matr[2] = y * y;
        matr[3] = x;
        matr[4] = y;
        matr[5] = 1.f;
        matr += 6;
    }

    /* compute S */
    status1 = icvMulTransMatrixR_32f( D, 6, n, S );
    assert( status1 == CV_OK );

    /* fill matrix C */
    icvSetZero_32f( C, 6, 6 );
    C[2] = 2.f;  //icvSetElement_32f( C, 6, 6, 0, 2, 2.f );
    C[7] = -1.f; //icvSetElement_32f( C, 6, 6, 1, 1, -1.f );
    C[12] = 2.f; //icvSetElement_32f( C, 6, 6, 2, 0, 2.f );
    
    /* find eigenvalues */
    status1 = icvJacobiEigens_32f( S, INVEIGV, eigenvalues, 6, 0.f );
    assert( status1 == CV_OK );

    //avoid troubles with small negative values
    for( i = 0; i < 6; i++ )
       eigenvalues[i] = (float)fabs( eigenvalues[i] ); 

    /* compute sqrt of every eigenvalue; they all must be positive */
    status1 = icvbSqrt_32f( eigenvalues, eigenvalues, 6 );
    assert( status1 == CV_OK );

    /* compute inverse of Q */
    status1 = icvbInvSqrt_32f( eigenvalues, eigenvalues, 6 );
    assert( status1 == CV_OK );

    for( i = 0; i < 6; i++ )
    {
        icvScaleVector_32f( &INVEIGV[i * 6], &INVEIGV[i * 6], 6, eigenvalues[i] );
    }

    // INVQ = transp(INVEIGV) * INVEIGV
    status1 = icvMulTransMatrixR_32f( INVEIGV, 6, 6, INVQ );
    assert( status1 == CV_OK );
    
    /* create matrix INVQ*C*INVQ */
    icvMulMatrix_32f( INVQ, 6, 6, C, 6, 6, TMP1 );
    icvMulMatrix_32f( TMP1, 6, 6, INVQ, 6, 6, TMP2 );

    /* find its eigenvalues and vectors */
    status1 = icvJacobiEigens_32f( TMP2, INVEIGV, eigenvalues, 6, 0.f );
    assert( status1 == CV_OK );
    /* search for positive eigenvalue */

    for( i = 0; i < 3; i++ )
    {
        if( eigenvalues[i] > 0 )
        {
            index = i;
            break;
        }
    }

    /* only 3 eigenvalues must be not zero 
       and only one of them must be positive 
       if it is not true - return zero result
    */
    if( index == -1 )
    {
        box->center.x = box->center.y = 
        box->size.width = box->size.height = 
        box->angle = 0.f;
        goto error;
    }

    /* now find truthful eigenvector */
    icvTransformVector_32f( INVQ, &INVEIGV[index * 6], u, 6, 6 );
    assert( status1 == CV_OK );
    /* extract vector components */
    a = u[0];
    b = u[1];
    c = u[2];
    d = u[3];
    e = u[4];
    f = u[5];
    {
        /* extract ellipse axes from above values */

        /* 
           1) find center of ellipse 
           it satisfy equation  
           | a     b/2 | *  | x0 | +  | d/2 | = |0 |
           | b/2    c  |    | y0 |    | e/2 |   |0 |

         */
        float x0, y0;
        float idet = 1.f / (a * c - b * b * 0.25f);

        /* we must normalize (a b c d e f ) to fit (4ac-b^2=1) */
        float scale = cvSqrt( 0.25f * idet );

        if (!scale) 
        {
            box->center.x = box->center.y = 
            box->size.width = box->size.height = 
            box->angle = 0.f;
            goto error;
        }
           
        a *= scale;
        b *= scale;
        c *= scale;
        d *= scale;
        e *= scale;
        f *= scale;

        //x0 = box->center.x = (-d * c * 0.5f + e * b * 0.25f) * 4.f;
        //y0 = box->center.y = (-a * e * 0.5f + d * b * 0.25f) * 4.f;
        x0 = box->center.x = (-d * c + e * b * 0.5f) * 2.f;
        y0 = box->center.y = (-a * e + d * b * 0.5f) * 2.f;

        /* offset ellipse to (x0,y0) */
        /* new f == F(x0,y0) */
        f += a * x0 * x0 + b * x0 * y0 + c * y0 * y0 + d * x0 + e * y0;

        if (!f) 
        {
            box->center.x = box->center.y = 
            box->size.width = box->size.height = 
            box->angle = 0.f;
            goto error;
        }

        scale = -1.f / f;
        /* normalize to f = 1 */
        a *= scale;
        b *= scale;
        c *= scale;
    }
    /* recover center */
    box->center.x += offx;
    box->center.y += offy;

    /* extract axis of ellipse */
    /* one more eigenvalue operation */
    TMP1[0] = a;
    TMP1[1] = TMP1[2] = b * 0.5f;
    TMP1[3] = c;

    status1 = icvJacobiEigens_32f( TMP1, INVEIGV, eigenvalues, 2, 0.f );
    assert( status1 == CV_OK );

    /* exteract axis length from eigenvectors */
    box->size.height = 2 * cvInvSqrt( eigenvalues[0] );
    box->size.width = 2 * cvInvSqrt( eigenvalues[1] );

    if ( !(box->size.height && box->size.width) )
    {
        assert(0);
    }

    /* calc angle */
    box->angle = icvFastArctan32f( INVEIGV[3], INVEIGV[2] );

error:

    if( D )
        icvDeleteMatrix( D );

    return status1 < 0 ? status1 : status;
}


CV_IMPL CvBox2D
cvFitEllipse2( const CvArr* array )
{
    CvBox2D box;
    
    CV_FUNCNAME( "cvFitEllipse2" );

    memset( &box, 0, sizeof(box));

    __BEGIN__;

    CvContour contour_header;
    CvSeq* ptseq = 0;
    CvSeqBlock block;

    if( CV_IS_SEQ( array ))
    {
        ptseq = (CvSeq*)array;
        if( !CV_IS_SEQ_POINT_SET( ptseq ))
            CV_ERROR( CV_StsBadArg, "Unsupported sequence type" );
    }
    else
    {
        CV_CALL( ptseq = icvPointSeqFromMat(
            CV_SEQ_KIND_GENERIC, array, &contour_header, &block ));
    }

    if( ptseq->total < 6 )
        CV_ERROR( CV_StsBadSize, "Number of points should be >= 6" );

    IPPI_CALL( icvFitEllipse_32f( ptseq, &box ));

    __END__;

    return box;
}


/* Calculates bounding rectagnle of a point set or retrieves already calculated */
CV_IMPL  CvRect
cvBoundingRect( const void* array, int update )
{
    CvSeqReader reader;
    CvRect  rect = { 0, 0, 0, 0 };
    CvContour contour_header;
    CvSeq* ptseq = 0;
    CvSeqBlock block;

    CV_FUNCNAME( "cvBoundingRect" );

    __BEGIN__;

    int calculate = update;

    if( CV_IS_SEQ( array ))
    {
        ptseq = (CvSeq*)array;
        if( !CV_IS_SEQ_POINT_SET( ptseq ))
            CV_ERROR( CV_StsBadArg, "Unsupported sequence type" );

        if( ptseq->header_size < (int)sizeof(CvContour))
        {
            if( update == 1 )
                CV_ERROR( CV_StsBadArg, "The header is too small to fit the rectangle, "
                                        "so it could not be updated" );
            calculate = 1;
        }
    }
    else
    {
        CV_CALL( ptseq = icvPointSeqFromMat(
            CV_SEQ_KIND_GENERIC, array, &contour_header, &block ));
        calculate = 1;
    }

    if( calculate )
    {
        if( ptseq->total )
        {   
            int  is_float = CV_SEQ_ELTYPE(ptseq) == CV_32FC2;
            int  xmin, ymin, xmax, ymax, i;
            cvStartReadSeq( ptseq, &reader, 0 );

            if( !is_float )
            {
                CvPoint pt;
                /* init values */
                CV_READ_SEQ_ELEM( pt, reader );
                xmin = xmax = pt.x;
                ymin = ymax = pt.y;
    
                for( i = 1; i < ptseq->total; i++ )
                {            
                    CV_READ_SEQ_ELEM( pt, reader );
            
                    if( xmin > pt.x )
                        xmin = pt.x;
            
                    if( xmax < pt.x )
                        xmax = pt.x;

                    if( ymin > pt.y )
                        ymin = pt.y;

                    if( ymax < pt.y )
                        ymax = pt.y;
                }
            }
            else
            {
                CvPoint pt;
                /* init values */
                CV_READ_SEQ_ELEM( pt, reader );
                xmin = xmax = CV_TOGGLE_FLT(pt.x);
                ymin = ymax = CV_TOGGLE_FLT(pt.y);
    
                for( i = 1; i < ptseq->total; i++ )
                {            
                    CV_READ_SEQ_ELEM( pt, reader );
                    pt.x = CV_TOGGLE_FLT(pt.x);
                    pt.y = CV_TOGGLE_FLT(pt.y);
            
                    if( xmin > pt.x )
                        xmin = pt.x;
            
                    if( xmax < pt.x )
                        xmax = pt.x;

                    if( ymin > pt.y )
                        ymin = pt.y;

                    if( ymax < pt.y )
                        ymax = pt.y;
                }

                xmin = CV_TOGGLE_FLT(xmin);
                ymin = CV_TOGGLE_FLT(ymin);
                xmax = CV_TOGGLE_FLT(xmax);
                ymax = CV_TOGGLE_FLT(ymax);

                xmin = cvFloor( (float&)xmin );
                ymin = cvFloor( (float&)ymin );
                /* because right and bottom sides of
                   the bounding rectangle are not inclusive,
                   cvFloor is used here (instead of cvCeil) */
                xmax = cvFloor( (float&)xmax );
                ymax = cvFloor( (float&)ymax );
            }

            rect.x = xmin;
            rect.y = ymin;
            rect.width = xmax - xmin + 1;
            rect.height = ymax - ymin + 1;
        }

        if( update )
            ((CvContour*)ptseq)->rect = rect;
    }
    else
    {
        rect = ((CvContour*)ptseq)->rect;
    }

    __END__;

    return rect;
}


/* End of file. */