cvoptflowbm.cpp

opencv库在TI DM6437上的移植,目前包括两个库cv.lib和cxcore.lib的工程

/****************************************************************************************\
*   Calculate some neeeded variables                                                     *
\****************************************************************************************/
    /* Calculate number of full blocks */
    NumberBlocksX = (int) imgSize.width / blockSize.width;
    NumberBlocksY = (int) imgSize.height / blockSize.height;

    /* add 1 if not full border blocks exist */
    BorderWidth = imgSize.width % blockSize.width;
    if( BorderWidth )
        NumberBlocksX++;
    else
        BorderWidth = blockSize.width;

    BorderHeight = imgSize.height % blockSize.height;
    if( BorderHeight )
        NumberBlocksY++;
    else
        BorderHeight = blockSize.height;

/****************************************************************************************\
* Round input velocities integer searching area center position                          *
\****************************************************************************************/
    if( usePrev )
    {
        float *velxf = velocityX, *velyf = velocityY;
        int* velx = (int*)velocityX, *vely = (int*)velocityY;

        for( i = 0; i < NumberBlocksY; i++, velxf += velStep, velyf += velStep,
                                            velx += velStep, vely += velStep )
        {
            for( j = 0; j < NumberBlocksX; j++ )
            {
                int vx = cvRound( velxf[j] ), vy = cvRound( velyf[j] );
                velx[j] = vx; vely[j] = vy;
            }
        }
    }
/****************************************************************************************\
* Main loop                                                                              *
\****************************************************************************************/
    Y1 = 0;
    for( i = 0; i < NumberBlocksY; i++ )
    {
        /* calculate height of current block */
        CurrentHeight = (i == NumberBlocksY - 1) ? BorderHeight : blockSize.height;
        X1 = 0;

        for( j = 0; j < NumberBlocksX; j++ )
        {
            int accept_level;
            int escape_level;

            int blDist;

            int VelocityX = 0;
            int VelocityY = 0;

            int offX = 0, offY = 0;

            int CountDirection = 1;

            int main_flag = i < NumberBlocksY - 1 && j < NumberBlocksX - 1;
            CvSize CurSize;

            /* calculate width of current block */
            CurrentWidth = (j == NumberBlocksX - 1) ? BorderWidth : blockSize.width;

            /* compute initial offset */
            if( usePrev )
            {
                offX = int_velocityX[j];
                offY = int_velocityY[j];
            }

            CurSize.width = CurrentWidth;
            CurSize.height = CurrentHeight;

            if( main_flag )
            {
                icvCopyBM_8u_C1R( imgA + X1, imgStep, blockA,
                                  CurSize.width, CurSize );
                icvCopyBM_8u_C1R( imgB + (Y1 + offY)*imgStep + (X1 + offX),
                                  imgStep, blockB, CurSize.width, CurSize );

                *((int64 *) (blockA + patch_ofs)) &= patch_mask;
                *((int64 *) (blockB + patch_ofs)) &= patch_mask;
            }
            else
            {
                memset( blockA, 0, bufferSize );
                memset( blockB, 0, bufferSize );

                icvCopyBM_8u_C1R( imgA + X1, imgStep, blockA, blockSize.width, CurSize );
                icvCopyBM_8u_C1R( imgB + (Y1 + offY) * imgStep + (X1 + offX), imgStep,
                                  blockB, blockSize.width, CurSize );
            }

            if( !main_flag )
            {
                int tmp = CurSize.width * CurSize.height;

                accept_level = tmp * SMALL_DIFF;
                escape_level = tmp * BIG_DIFF;
            }
            else
            {
                accept_level = stand_accept_level;
                escape_level = stand_escape_level;
            }

            blDist = icvCmpBlocksL1_8u_C1( blockA, blockB, cmpSize );

            if( blDist > accept_level )
            {
                int k;
                int VelX = 0;
                int VelY = 0;

                /* walk around basic block */

                /* cycle for neighborhood */
                for( k = 0; k < ss_count; k++ )
                {
                    int tmpDist;

                    int Y2 = Y1 + offY + ss[k].y;
                    int X2 = X1 + offX + ss[k].x;

                    /* if we break upper border */
                    if( Y2 < 0 )
                    {
                        continue;
                    }
                    /* if we break bottom border */
                    if( Y2 + CurrentHeight >= imgSize.height )
                    {
                        continue;
                    }
                    /* if we break left border */
                    if( X2 < 0 )
                    {
                        continue;
                    }
                    /* if we break right border */
                    if( X2 + CurrentWidth >= imgSize.width )
                    {
                        continue;
                    }

                    if( main_flag )
                    {
                        icvCopyBM_8u_C1R( imgB + Y2 * imgStep + X2,
                                          imgStep, blockB, CurSize.width, CurSize );

                        *((int64 *) (blockB + patch_ofs)) &= patch_mask;
                    }
                    else
                    {
                        memset( blockB, 0, bufferSize );
                        icvCopyBM_8u_C1R( imgB + Y1 * imgStep + X1, imgStep,
                                          blockB, blockSize.width, CurSize );
                    }

                    tmpDist = icvCmpBlocksL1_8u_C1( blockA, blockB, cmpSize );

                    if( tmpDist < accept_level )
                    {
                        VelX = ss[k].x;
                        VelY = ss[k].y;
                        break;  /*for */
                    }
                    else if( tmpDist < blDist )
                    {
                        blDist = tmpDist;
                        VelX = ss[k].x;
                        VelY = ss[k].y;
                        CountDirection = 1;
                    }
                    else if( tmpDist == blDist )
                    {
                        VelX += ss[k].x;
                        VelY += ss[k].y;
                        CountDirection++;
                    }
                }
                if( blDist > escape_level )
                {
                    VelX = VelY = 0;
                    CountDirection = 1;
                }
                if( CountDirection > 1 )
                {
                    int temp = CountDirection == 2 ? 1 << 15 : ((1 << 16) / CountDirection);

                    VelocityX = VelX * temp;
                    VelocityY = VelY * temp;
                }
                else
                {
                    VelocityX = VelX << 16;
                    VelocityY = VelY << 16;
                }
            }                   /*if */

            int_velocityX[j] = VelocityX + (offX << 16);
            int_velocityY[j] = VelocityY + (offY << 16);

            X1 += blockSize.width;

        }                       /*for */
        int_velocityX += velStep;
        int_velocityY += velStep;

        imgA += DownStep;
        Y1 += blockSize.height;
    }                           /*for */

/****************************************************************************************\
* Converting fixed point velocities to floating point                                    *
\****************************************************************************************/
    {
        float *velxf = velocityX, *velyf = velocityY;
        int* velx = (int*)velocityX, *vely = (int*)velocityY;

        for( i = 0; i < NumberBlocksY; i++, velxf += velStep, velyf += velStep,
                                            velx += velStep, vely += velStep )
        {
            for( j = 0; j < NumberBlocksX; j++ )
            {
                float vx = (float)velx[j]*back, vy = (float)vely[j]*back;
                velxf[j] = vx; velyf[j] = vy;
            }
        }
    }

    cvFree( &ss );
    cvFree( &blockA );
    
    return CV_OK;
}                               /*cvCalcOpticalFlowBM_8u */


/*F///////////////////////////////////////////////////////////////////////////////////////
//    Name:    cvCalcOpticalFlowBM
//    Purpose: Optical flow implementation
//    Context: 
//    Parameters:
//             srcA, srcB - source image
//             velx, vely - destination image
//    Returns:
//
//    Notes:
//F*/
CV_IMPL void
cvCalcOpticalFlowBM( const void* srcarrA, const void* srcarrB,
                     CvSize blockSize, CvSize shiftSize,
                     CvSize maxRange, int usePrevious,
                     void* velarrx, void* velarry )
{
    CV_FUNCNAME( "cvCalcOpticalFlowBM" );

    __BEGIN__;

    CvMat stubA, *srcA = (CvMat*)srcarrA;
    CvMat stubB, *srcB = (CvMat*)srcarrB;
    CvMat stubx, *velx = (CvMat*)velarrx;
    CvMat stuby, *vely = (CvMat*)velarry;

    CV_CALL( srcA = cvGetMat( srcA, &stubA ));
    CV_CALL( srcB = cvGetMat( srcB, &stubB ));

    CV_CALL( velx = cvGetMat( velx, &stubx ));
    CV_CALL( vely = cvGetMat( vely, &stuby ));

    if( !CV_ARE_TYPES_EQ( srcA, srcB ))
        CV_ERROR( CV_StsUnmatchedFormats, "Source images have different formats" );

    if( !CV_ARE_TYPES_EQ( velx, vely ))
        CV_ERROR( CV_StsUnmatchedFormats, "Destination images have different formats" );

    if( !CV_ARE_SIZES_EQ( srcA, srcB ) ||
        !CV_ARE_SIZES_EQ( velx, vely ) ||
        (unsigned)(velx->width*blockSize.width - srcA->width) >= (unsigned)blockSize.width ||
        (unsigned)(velx->height*blockSize.height - srcA->height) >= (unsigned)blockSize.height )
        CV_ERROR( CV_StsUnmatchedSizes, "" );

    if( CV_MAT_TYPE( srcA->type ) != CV_8UC1 ||
        CV_MAT_TYPE( velx->type ) != CV_32FC1 )
        CV_ERROR( CV_StsUnsupportedFormat, "Source images must have 8uC1 type and "
                                           "destination images must have 32fC1 type" );

    if( srcA->step != srcB->step || velx->step != vely->step )
        CV_ERROR( CV_BadStep, "two source or two destination images have different steps" );

    IPPI_CALL( icvCalcOpticalFlowBM_8u32fR( (uchar*)srcA->data.ptr, (uchar*)srcB->data.ptr,
                                            srcA->step, cvGetMatSize( srcA ), blockSize,
                                            shiftSize, maxRange, usePrevious,
                                            velx->data.fl, vely->data.fl, velx->step ));
    __END__;
}


/* End of file. */