cvlkpyramid.cpp.svn-base

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

    if( count <= 0 )
        return CV_BADRANGE_ERR;

    result = icvInitPyramidalAlgorithm( imgA, imgB, imgStep, imgSize,
                                        pyrA, pyrB, level, &criteria, MAX_ITERS, flags,
                                        &imgI, &imgJ, &step, &size, &scale, &pyr_buffer );

    if( result < 0 )
        goto func_exit;

    /* buffer_size = <size for patches> + <size for pyramids> */
    bufferBytes = (srcPatchLen + patchLen * 3) * sizeof( patchI[0] );

    buffer = (uchar *) icvAlloc( bufferBytes );
    if( !buffer )
    {
        result = CV_OUTOFMEM_ERR;
        goto func_exit;
    }

    patchI = (float *) buffer;
    patchJ = patchI + srcPatchLen;
    Ix = patchJ + patchLen;
    Iy = Ix + patchLen;

    memset( status, 1, count );

    if( !(flags & CV_LKFLOW_INITIAL_GUESSES) )
    {
        memcpy( featuresB, featuresA, count * sizeof( featuresA[0] ));
    }

    /* find flow for each given point */
    for( i = 0; i < count; i++ )
    {
        CvPoint2D32f v;
        CvPoint minI, maxI, minJ, maxJ;
        int l, pt_status = 1;

        minI = maxI = minJ = maxJ = cvPoint( 0, 0 );

        v.x = (float) (featuresB[i].x * scale[level] * 0.5);
        v.y = (float) (featuresB[i].y * scale[level] * 0.5);

        /* do processing from top pyramid level (smallest image)
           to the bottom (original image) */
        for( l = level; l >= 0; l-- )
        {
            CvPoint2D32f u;
            CvSize levelSize = size[l];
            double Gxx0 = 0, Gxy0 = 0, Gyy0 = 0;
            int Gready = 0;

            v.x += v.x;
            v.y += v.y;

            u.x = (float) (featuresA[i].x * scale[l]);
            u.y = (float) (featuresA[i].y * scale[l]);

            if( icvGetRectSubPix_8u32f_C1R( imgI[l], step[l], levelSize,
                                            patchI, srcPatchStep, srcPatchSize, u ) < 0 )
            {
                /* point is outside the image. take the next */
                pt_status = 0;
                break;
            }

            /* calc Ix */
            icvSepConvSmall3_32f( patchI, srcPatchStep, Ix, patchStep,
                                  srcPatchSize, kerX, kerY, patchJ );

            /* calc Iy */
            icvSepConvSmall3_32f( patchI, srcPatchStep, Iy, patchStep,
                                  srcPatchSize, kerY, kerX, patchJ );

            /* repack patchI (remove borders) */
            for( k = 0; k < patchSize.height; k++ )
                memcpy( patchI + k * patchSize.width,
                        patchI + (k + 1) * srcPatchSize.width + 1, patchStep );

            intersect( u, winSize, levelSize, &minI, &maxI );

            for( j = 0; j < criteria.maxIter; j++ )
            {
                double bx = 0, by = 0;
                float mx, my;
                double D;
                double Gxx, Gxy, Gyy;

                if( icvGetRectSubPix_8u32f_C1R( imgJ[l], step[l], levelSize,
                                                patchJ, patchStep, patchSize, v ) < 0 )
                {
                    /* point is outside image. take the next */
                    pt_status = 0;
                    break;
                }

                intersect( v, winSize, levelSize, &minJ, &maxJ );

                minJ.x = MAX( minJ.x, minI.x );
                minJ.y = MAX( minJ.y, minI.y );

                maxJ.x = MIN( maxJ.x, maxI.x );
                maxJ.y = MIN( maxJ.y, maxI.y );

                if( Gready &&
                    maxJ.x - minJ.x == patchSize.width &&
                    maxJ.y - minJ.y == patchSize.height )
                {
                    for( y = minJ.y; y < maxJ.y; y++ )
                    {
                        for( x = minJ.x; x < maxJ.x; x++ )
                        {
                            int idx = y * (winSize.width * 2 + 1) + x;
                            double t = patchI[idx] - patchJ[idx];

                            bx += (double) (t * Ix[idx]);
                            by += (double) (t * Iy[idx]);
                        }
                    }
                    Gxx = Gxx0; Gyy = Gyy0; Gxy = Gxy0;
                }
                else
                {
                    Gxx = Gyy = Gxy = 0;
                    
                    for( y = minJ.y; y < maxJ.y; y++ )
                    {
                        for( x = minJ.x; x < maxJ.x; x++ )
                        {
                            int idx = y * (winSize.width * 2 + 1) + x;
                            double t = patchI[idx] - patchJ[idx];

                            bx += (double) (t * Ix[idx]);
                            by += (double) (t * Iy[idx]);
                            Gxx += Ix[idx] * Ix[idx];
                            Gxy += Ix[idx] * Iy[idx];
                            Gyy += Iy[idx] * Iy[idx];
                        }
                    }

                    if( !Gready &&
                        maxJ.x - minJ.x == patchSize.width &&
                        maxJ.y - minJ.y == patchSize.height )
                    {
                        Gxx0 = Gxx; Gyy0 = Gyy; Gxy0 = Gxy;
                        Gready = 1;
                    }
                }

                D = Gxx * Gyy - Gxy * Gxy;
                if( D < DBL_EPSILON )
                {
                    pt_status = 0;
                    break;
                }

                D = 1. / D;

                mx = (float) ((Gyy * bx - Gxy * by) * D);
                my = (float) ((Gxx * by - Gxy * bx) * D);

                v.x += mx;
                v.y += my;

                if( mx * mx + my * my < criteria.epsilon )
                    break;
            }

            if( pt_status == 0 )
                break;
        }

        if( pt_status )
        {
            featuresB[i] = v;

            if( error )
            {
                /* calc error */
                double err = 0;

                for( y = minJ.y; y < maxJ.y; y++ )
                {
                    for( x = minJ.x; x < maxJ.x; x++ )
                    {
                        int idx = y * (winSize.width * 2 + 1) + x;
                        double t = patchI[idx] - patchJ[idx];

                        err += t * t;
                    }
                }
                error[i] = (float) sqrt( err );
            }
        }

        if( status )
            status[i] = (char) pt_status;
    }

  func_exit:

    icvFree( &pyr_buffer );
    icvFree( &buffer );

    return result;
#undef MAX_LEVEL
}


/* Affine tracking algorithm */
static  CvStatus  icvCalcAffineFlowPyrLK_8uC1R( uchar * imgA, uchar * imgB,
                                                int imgStep, CvSize imgSize,
                                                uchar * pyrA, uchar * pyrB,
                                                CvPoint2D32f * featuresA,
                                                CvPoint2D32f * featuresB,
                                                float *matrices, int count,
                                                CvSize winSize, int level,
                                                char *status, float *error,
                                                CvTermCriteria criteria, int flags )
{
#define MAX_LEVEL 10
#define MAX_ITERS 100

    static const float kerX[] = { -1, 0, 1 }, kerY[] =
    {
    0.09375, 0.3125, 0.09375};  /* 3/32, 10/32, 3/32 */

    uchar *buffer = 0;
    uchar *pyr_buffer = 0;
    int bufferBytes = 0;

    uchar **imgI = 0;
    uchar **imgJ = 0;
    int *step = 0;
    double *scale = 0;
    CvSize* size = 0;

    float *patchI;
    float *patchJ;
    float *Ix;
    float *Iy;

    int i, j, k;
    int x, y;

    CvSize patchSize = cvSize( winSize.width * 2 + 1, winSize.height * 2 + 1 );
    int patchLen = patchSize.width * patchSize.height;
    int patchStep = patchSize.width * sizeof( patchI[0] );

    CvSize srcPatchSize = cvSize( patchSize.width + 2, patchSize.height + 2 );
    int srcPatchLen = srcPatchSize.width * srcPatchSize.height;
    int srcPatchStep = srcPatchSize.width * sizeof( patchI[0] );

    CvStatus result = CV_OK;

    /* check input arguments */
    if( !featuresA || !featuresB || !matrices )
        return CV_NULLPTR_ERR;
    if( winSize.width <= 1 || winSize.height <= 1 )
        return CV_BADSIZE_ERR;

    if( (flags & ~7) != 0 )
        return CV_BADFLAG_ERR;
    if( count <= 0 )
        return CV_BADRANGE_ERR;

    result = icvInitPyramidalAlgorithm( imgA, imgB, imgStep, imgSize,
                                        pyrA, pyrB, level, &criteria, MAX_ITERS, flags,
                                        &imgI, &imgJ, &step, &size, &scale, &pyr_buffer );

    if( result < 0 )
        goto func_exit;

    /* buffer_size = <size for patches> + <size for pyramids> */
    bufferBytes = (srcPatchLen + patchLen * 3) * sizeof( patchI[0] ) +

        (36 * 2 + 6) * sizeof( double );

    buffer = (uchar *) icvAlloc( bufferBytes );
    if( !buffer )
    {
        result = CV_OUTOFMEM_ERR;
        goto func_exit;
    }

    patchI = (float *) buffer;
    patchJ = patchI + srcPatchLen;
    Ix = patchJ + patchLen;
    Iy = Ix + patchLen;

    if( status )
        memset( status, 1, count );

    if( !(flags & CV_LKFLOW_INITIAL_GUESSES) )
    {
        memcpy( featuresB, featuresA, count * sizeof( featuresA[0] ));
        for( i = 0; i < count * 4; i += 4 )
        {
            matrices[i] = matrices[i + 2] = 1.f;
            matrices[i + 1] = matrices[i + 3] = 0.f;
        }
    }

    /* find flow for each given point */
    for( i = 0; i < count; i++ )
    {
        CvPoint2D32f v;
        float A[4];
        double G[36];
        int l;
        int pt_status = 1;

        memcpy( A, matrices + i * 4, sizeof( A ));

        v.x = (float) (featuresB[i].x * scale[level] * 0.5);
        v.y = (float) (featuresB[i].y * scale[level] * 0.5);

        /* do processing from top pyramid level (smallest image)
           to the bottom (original image) */
        for( l = level; l >= 0; l-- )
        {
            CvPoint2D32f u;
            CvSize levelSize = size[l];
            int x, y;

            v.x += v.x;
            v.y += v.y;

            u.x = (float) (featuresA[i].x * scale[l]);
            u.y = (float) (featuresA[i].y * scale[l]);

            if( icvGetRectSubPix_8u32f_C1R( imgI[l], step[l], levelSize,
                                             patchI, srcPatchStep, srcPatchSize, u ) < 0 )
            {
                /* point is outside the image. take the next */
                pt_status = 0;
                break;
            }

            /* calc Ix */
            icvSepConvSmall3_32f( patchI, srcPatchStep, Ix, patchStep,
                                  srcPatchSize, kerX, kerY, patchJ );

            /* calc Iy */
            icvSepConvSmall3_32f( patchI, srcPatchStep, Iy, patchStep,
                                  srcPatchSize, kerY, kerX, patchJ );

            /* repack patchI (remove borders) */
            for( k = 0; k < patchSize.height; k++ )
                memcpy( patchI + k * patchSize.width,
                        patchI + (k + 1) * srcPatchSize.width + 1, patchStep );