aoptflowlk.cpp

读源程序系列

LK光流算法

#include "cvtest.h"

#if 0

/* Testing parameters */
static char FuncName[] = "cvCalcOpticalFlowLK";
static char TestName[] = "Optical flow (Lucas & Kanade)";
static char TestClass[] = "Algorithm";

static long lImageWidth;
static long lImageHeight;
static long lWinWidth;
static long lWinHeight;

#define EPSILON 0.00001f

static int fmaCalcOpticalFlowLK( void )
{
    /* Some Variables */
    int* WH = NULL;
    int* WV = NULL;

    int W3[3] =  { 1,  2, 1 };
    int W5[5] =  { 1,  4,  6,  4, 1 };
    int W7[7] =  { 1,  6,  15, 20, 15, 6, 1 };
    int W9[9] =  { 1,  8,  28, 56, 70, 56, 28, 8, 1 };
    int W11[11] = {1, 10,  45, 120, 210, 252, 210, 120, 45, 10, 1 };

    int            i,j,m,k;
    uchar*         roiA;
    uchar*         roiB;

    float*         VelocityX;
    float*         VelocityY;

    float*         DerivativeX;
    float*         DerivativeY;
    float*         DerivativeT;

    long           lErrors = 0;
    CvSize        winSize;

    int HRad;
    int VRad;

    float A1, A2, B1, B2, C1, C2;

    static int  read_param = 0;

    /* Initialization global parameters */
    if( !read_param )
    {
        read_param = 1;
        /* Reading test-parameters */
        trslRead( &lImageHeight, "563", "Image height" );
        trslRead( &lImageWidth, "345", "Image width" );
        trslRead( &lWinHeight, "7", "win height 3/5/7/9/11 " );
        trslRead( &lWinWidth, "9", "win width 3/5/7/9/11 " );
    }

    /* Checking all sizes of source histogram in ranges */
    IplImage* imgA = cvCreateImage( cvSize(lImageWidth,lImageHeight), IPL_DEPTH_8U, 1 );
    IplImage* imgB = cvCreateImage( cvSize(lImageWidth,lImageHeight), IPL_DEPTH_8U, 1 );

    IplImage* testVelocityX = cvCreateImage( cvSize(lImageWidth,lImageHeight), IPL_DEPTH_32F, 1 );
    IplImage* testVelocityY = cvCreateImage( cvSize(lImageWidth,lImageHeight), IPL_DEPTH_32F, 1 );

    VelocityX = (float*)cvAlloc(  lImageWidth * lImageHeight * sizeof(float) );
    VelocityY = (float*)cvAlloc(  lImageWidth * lImageHeight * sizeof(float) );

    DerivativeX = (float*)cvAlloc( lImageWidth * lImageHeight * sizeof(float) );
    DerivativeY = (float*)cvAlloc( lImageWidth * lImageHeight * sizeof(float) );
    DerivativeT = (float*)cvAlloc( lImageWidth * lImageHeight * sizeof(float) );

    winSize.height = lWinHeight;
    winSize.width =  lWinWidth;

    switch (lWinHeight)
    {
    case 3:
        WV = W3; break;
    case 5:
        WV = W5; break;
    case 7:
        WV = W7; break;
    case 9:
        WV = W9; break;
    case 11:
        WV = W11; break;
    }
    switch (lWinWidth)
    {
    case 3:
        WH = W3; break;
    case 5:
        WH = W5; break;
    case 7:
        WH = W7; break;
    case 9:
        WH = W9; break;
    case 11:
        WH = W11; break;

    }

    HRad = (winSize.width - 1)/2;
    VRad = (winSize.height - 1)/2;

    /* Filling images */
    ats1bInitRandom( 0, 255, (uchar*)imgA->imageData, lImageHeight * imgA->widthStep );
    ats1bInitRandom( 0, 255, (uchar*)imgB->imageData, imgA->widthStep * lImageHeight );

    /* Run CVL function */
    cvCalcOpticalFlowLK( imgA , imgB, winSize,
                         testVelocityX, testVelocityY );


    /* Calc by other way */
    roiA = (uchar*)imgA->imageData;
    roiB = (uchar*)imgB->imageData;  

    /* Calculate derivatives */
    for (i=0; i<imgA->height; i++)
    {
        for(j=0; j<imgA->width; j++)
        {
            int jr,jl,it,ib;

            if ( j==imgA->width-1 )
                jr = imgA->width-1;
            else jr = j + 1;

            if ( j==0 )
                jl = 0;
            else jl = j - 1;

            if ( i==(imgA->height - 1) )
                ib = imgA->height - 1;
            else ib = i + 1;

            if ( i==0 )
                it = 0;
            else it = i - 1;

            DerivativeX[ i*lImageWidth + j ] = (float)
                (roiA[ (it)*imgA->widthStep + jr ]
                - roiA[ (it)*imgA->widthStep + jl ]
                + 2*roiA[ (i)*imgA->widthStep + jr ]
                - 2*roiA[ (i)*imgA->widthStep + jl ]
                + roiA[ (ib)*imgA->widthStep + jr ]
                - roiA[ (ib)*imgA->widthStep + jl ]) ;

            DerivativeY[ i*lImageWidth + j ] = (float)
                ( roiA[ (ib)*imgA->widthStep + jl ]
                + 2*roiA[ (ib)*imgA->widthStep + j  ]
                + roiA[ (ib)*imgA->widthStep + jr ]
                - roiA[ (it)*imgA->widthStep + jl ]
                - 2*roiA[ (it)*imgA->widthStep + j  ]
                - roiA[ (it)*imgA->widthStep + jr ])  ;

            DerivativeT[ i*lImageWidth + j ] = (float)
                (roiB[i*imgB->widthStep + j] - roiA[i*imgA->widthStep + j])*8;
        }
    }

    for( i = 0; i < lImageHeight; i++)
    {
        for(j = 0; j< lImageWidth; j++)
        {
            A1 =0;
            A2 =0;
            B1 =0;
            B2 =0;
            C1= 0;
            C2= 0;

            for( k = -VRad ; k <= VRad ; k++ )
            {
                for( m = - HRad; m <= HRad ; m++ )
                {
                    int coord = (i+k)*lImageWidth + (j+m);
                    if ( (j+m<0)              || 
                         (j+m >lImageWidth-1) || 
                         ( (k+i)<0 )          || 
                         ( (k+i)>lImageHeight-1) )
                    {continue;}

                    A1+=WV[k+VRad]*WH[m+HRad]* DerivativeX[coord]*DerivativeY[coord];
                    A2+=WV[k+VRad]*WH[m+HRad]* DerivativeX[coord]*DerivativeX[coord];
                    B1+=WV[k+VRad]*WH[m+HRad]* DerivativeY[coord]*DerivativeY[coord];
                    B2+=WV[k+VRad]*WH[m+HRad]* DerivativeX[coord]*DerivativeY[coord];
                    C1+=WV[k+VRad]*WH[m+HRad]* DerivativeY[coord]*DerivativeT[coord];
                    C2+=WV[k+VRad]*WH[m+HRad]* DerivativeX[coord]*DerivativeT[coord];
                }
            }
            if (A1*B2 - A2*B1)
            {
                VelocityX[i*lImageWidth + j] = - (C1*B2 - C2*B1)/(A1*B2 - A2*B1);
                VelocityY[i*lImageWidth + j] = - (A1*C2 - A2*C1)/(A1*B2 - A2*B1);
            }
            else if ( (A1+A2)*(A1+A2) + (B1+B2)*(B1+B2) )
            {   /* Calculate Normal flow */
                VelocityX[i*lImageWidth + j] = -(A1+A2)*(C1+C2)/((A1+A2)*(A1+A2)+(B1+B2)*(B1+B2));
                VelocityY[i*lImageWidth + j] = -(B1+B2)*(C1+C2)/((A1+A2)*(A1+A2)+(B1+B2)*(B1+B2));
            }
            else
            {
                VelocityX[i*lImageWidth + j] = 0;
                VelocityY[i*lImageWidth + j] = 0;
            }
        }
    }

    for( i = 0; i < lImageHeight; i++)
    {
        for(j = 0; j< lImageWidth; j++)
        {
            float tvx = ((float*)(testVelocityX->imageData + i*testVelocityX->widthStep))[j];
            float tvy = ((float*)(testVelocityY->imageData + i*testVelocityY->widthStep))[j];



            if (( fabs(tvx - VelocityX[i*lImageWidth + j])>EPSILON )||
                ( fabs(tvy - VelocityY[i*lImageWidth + j])>EPSILON ) )
            {
                //trsWrite( ATS_LST | ATS_CON, " ValueX %f \n", tvx );
                //trsWrite( ATS_LST | ATS_CON, " mustX  %f \n", VelocityX[i*lImageWidth + j] );

                //trsWrite( ATS_LST | ATS_CON, " ValueY %f \n", tvy );
                //trsWrite( ATS_LST | ATS_CON, " mustY  %f \n", VelocityY[i*lImageWidth + j] );

                //trsWrite( ATS_LST | ATS_CON, " Coordinates %d %d\n", i, j );

                lErrors++;
            }
        }
    }
    cvFree( &VelocityX );
    cvFree( &VelocityY );
    
    cvFree( &DerivativeX );
    cvFree( &DerivativeY );
    cvFree( &DerivativeT );

    cvReleaseImage( &imgA );
    cvReleaseImage( &imgB );
    cvReleaseImage( &testVelocityX );
    cvReleaseImage( &testVelocityY );


    if( lErrors == 0 ) return trsResult( TRS_OK, "No errors fixed for this text" );
    else return trsResult( TRS_FAIL, "Total fixed %d errors", lErrors );
} /*fmaCalcOpticalFlowLK*/

void InitACalcOpticalFlowLK( void )
{
    /* Registering test function */
    trsReg( FuncName, TestName, TestClass, fmaCalcOpticalFlowLK );
} /* InitAACalcOpticalFlowLK */

#endif

/* End of file. */