cvoptflowbm.cpp.svn-base

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

    if( usePrev )
    {
        float *vel = velocityX;

        for( i = 0; i < NumberBlocksY; i++ )
        {
            for( j = 0; j < NumberBlocksX; j++ )
            {
                *((int *) (&vel[j])) = cvRound( vel[j] );
            }
            *((char **) &vel) += velStep;
        }

        vel = velocityY;
        for( i = 0; i < NumberBlocksY; i++ )
        {
            for( j = 0; j < NumberBlocksX; j++ )
            {
                *((int *) (&vel[j])) = cvRound( vel[j] );
            }
            *((char **) &vel) += velStep;
        }
    }
/****************************************************************************************\
* 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 )
            {
                icvCopy_8u_C1R( imgA + X1, imgStep, blockA,
                                CurSize.width, CurSize );
                icvCopy_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 );

                icvCopy_8u_C1R( imgA + X1, imgStep, blockA, blockSize.width, CurSize );
                icvCopy_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 )
                    {
                        icvCopy_8u_C1R( imgB + Y2 * imgStep + X2,
                                        imgStep, blockB, CurSize.width, CurSize );

                        *((int64 *) (blockB + patch_ofs)) &= patch_mask;
                    }
                    else
                    {
                        memset( blockB, 0, bufferSize );
                        icvCopy_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 */
        *((char **) &int_velocityX) += velStep;
        *((char **) &int_velocityY) += velStep;

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

/****************************************************************************************\
* Converting fixed point velocities to floating point                                    *
\****************************************************************************************/
    {
        int *vel = (int *) velocityX;

        for( i = 0; i < NumberBlocksY; i++ )
        {
            for( j = 0; j < NumberBlocksX; j++ )
            {
                float tmp = (float) vel[j] * back;

                *((float *) (&vel[j])) = tmp;
            }
            *((char **) &vel) += velStep;
        }

        vel = (int *) velocityY;
        for( i = 0; i < NumberBlocksY; i++ )
        {
            for( j = 0; j < NumberBlocksX; j++ )
            {
                float tmp = (float) vel[j] * back;

                *((float *) (&vel[j])) = tmp;
            }
            *((char **) &vel) += velStep;
        }
    }

    icvFree( &ss );
    icvFree( &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, icvGetMatSize( srcA ), blockSize,
                                            shiftSize, maxRange, usePrevious,
                                            velx->data.fl, vely->data.fl, velx->step ));
    __END__;
}


/* End of file. */