cvoptflowbm.cpp.svn-base

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

/*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                        Intel License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2000, Intel Corporation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
//   * The name of Intel Corporation may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/

#include "_cv.h"

IPCVAPI( CvStatus , icvCalcOpticalFlowBM_8u32fR,( uchar* imgA, uchar* imgB,
                                                  int  imgStep, CvSize imgSize,
                                                  CvSize blockSize, CvSize shiftSize,
                                                  CvSize maxRange,
                                                  int usePrevious,
                                                  float*   velocityX,
                                                  float*   velocityY,
                                                  int      velStep ))


/*F///////////////////////////////////////////////////////////////////////////////////////
//    Name: icvCalcOpticalFlowBM_8u32fR
//    Purpose: calculate Optical flow for 2 images using block matching algorithm
//    Context:
//    Parameters:
//            imgA,         // pointer to first frame ROI
//            imgB,         // pointer to second frame ROI
//            imgStep,      // full width of input images in bytes
//            imgSize,      // size of the image
//            blockSize,    // size of basic blocks which are compared
//            shiftSize,    // coordinates increments.
//            maxRange,     // size of the scanned neighborhood.
//            usePrevious,  // flag of using previous velocity field
//            velocityX,    //  pointer to ROI of horizontal and
//            velocityY,    //  vertical components of optical flow
//            velStep);     //  full width of velocity frames in bytes
//    Returns: CV_OK or error code
//    Notes:
//F*/
#define SMALL_DIFF 2
#define BIG_DIFF 128

IPCVAPI_IMPL( CvStatus, icvCalcOpticalFlowBM_8u32fR, (uchar * imgA,
                                                      uchar * imgB,
                                                      int imgStep,
                                                      CvSize imgSize,
                                                      CvSize blockSize,
                                                      CvSize shiftSize,
                                                      CvSize maxRange,
                                                      int usePrev,
                                                      float *velocityX,
                                                      float *velocityY, int velStep) )
{
    const float back = 1.f / (float) (1 << 16);

    /* scanning scheme coordinates */

    CvPoint *ss = 0;
    int ss_count = 0;

    int stand_accept_level = blockSize.height * blockSize.width * SMALL_DIFF;
    int stand_escape_level = blockSize.height * blockSize.width * BIG_DIFF;

    int i, j;

    int *int_velocityX = (int *) velocityX;
    int *int_velocityY = (int *) velocityY;

    /* if image sizes can't be divided by block sizes then right blocks will  */
    /* have not full width  - BorderWidth                                     */
    /* and bottom blocks will                                                 */
    /* have not full height - BorderHeight                                    */
    int BorderWidth;
    int BorderHeight;

    int CurrentWidth;
    int CurrentHeight;

    int NumberBlocksX;
    int NumberBlocksY;

    int Y1 = 0;
    int X1 = 0;

    int DownStep = blockSize.height * imgStep;

    uchar *blockA = 0;
    uchar *blockB = 0;
    uchar *blockZ = 0;
    int blSize = blockSize.width * blockSize.height;
    int bufferSize = icvAlign(blSize + 9,16);
    int cmpSize = icvAlign(blSize,4);
    int patch_ofs = blSize & -8;
    int64 patch_mask = (((int64) 1) << (blSize - patch_ofs * 8)) - 1;

    if( patch_ofs == blSize )
        patch_mask = (int64) - 1;

/****************************************************************************************\
*   Checking bad arguments                                                               *
\****************************************************************************************/
    if( imgA == NULL )
        return CV_NULLPTR_ERR;
    if( imgB == NULL )
        return CV_NULLPTR_ERR;

/****************************************************************************************\
*   Allocate buffers                                                                     *
\****************************************************************************************/
    blockA = (uchar *) icvAlloc( bufferSize * 3 );
    if( !blockA )
        return CV_OUTOFMEM_ERR;

    blockB = blockA + bufferSize;
    blockZ = blockB + bufferSize;

    memset( blockZ, 0, bufferSize );

    ss = (CvPoint *) icvAlloc( (2 * maxRange.width + 1) * (2 * maxRange.height + 1) *
                               sizeof( CvPoint ));
    if( !ss )
    {
        icvFree( &blockA );
        return CV_OUTOFMEM_ERR;
    }

/****************************************************************************************\
*   Calculate scanning scheme                                                            *
\****************************************************************************************/
    {
        int X_shift_count = maxRange.width / shiftSize.width;
        int Y_shift_count = maxRange.height / shiftSize.height;
        int min_count = MIN( X_shift_count, Y_shift_count );

        /* cycle by neighborhood rings */
        /* scanning scheme is 

           . 9  10 11 12
           . 8  1  2  13
           . 7  *  3  14
           . 6  5  4  15      
           20 19 18 17 16
         */

        for( i = 0; i < min_count; i++ )
        {
            /* four cycles along sides */
            int y = -(i + 1) * shiftSize.height;
            int x = -(i + 1) * shiftSize.width;

            /* upper side */
            for( j = -i; j <= i + 1; j++, ss_count++ )
            {
                x += shiftSize.width;
                ss[ss_count].x = x;
                ss[ss_count].y = y;
            }

            /* right side */
            for( j = -i; j <= i + 1; j++, ss_count++ )
            {
                y += shiftSize.height;
                ss[ss_count].x = x;
                ss[ss_count].y = y;
            }

            /* bottom side */
            for( j = -i; j <= i + 1; j++, ss_count++ )
            {
                x -= shiftSize.width;
                ss[ss_count].x = x;
                ss[ss_count].y = y;
            }

            /* left side */
            for( j = -i; j <= i + 1; j++, ss_count++ )
            {
                y -= shiftSize.height;
                ss[ss_count].x = x;
                ss[ss_count].y = y;
            }
        }

        /* the rest part */
        if( X_shift_count < Y_shift_count )
        {
            int xleft = -min_count * shiftSize.width;

            /* cycle by neighbor rings */
            for( i = min_count; i < Y_shift_count; i++ )
            {
                /* two cycles by x */
                int y = -(i + 1) * shiftSize.height;
                int x = xleft;

                /* upper side */
                for( j = -X_shift_count; j <= X_shift_count; j++, ss_count++ )
                {
                    ss[ss_count].x = x;
                    ss[ss_count].y = y;
                    x += shiftSize.width;
                }

                x = xleft;
                y = -y;
                /* bottom side */
                for( j = -X_shift_count; j <= X_shift_count; j++, ss_count++ )
                {
                    ss[ss_count].x = x;
                    ss[ss_count].y = y;
                    x += shiftSize.width;
                }
            }
        }
        else if( X_shift_count > Y_shift_count )
        {
            int yupper = -min_count * shiftSize.height;

            /* cycle by neighbor rings */
            for( i = min_count; i < X_shift_count; i++ )
            {
                /* two cycles by y */
                int x = -(i + 1) * shiftSize.width;
                int y = yupper;

                /* left side */
                for( j = -Y_shift_count; j <= Y_shift_count; j++, ss_count++ )
                {
                    ss[ss_count].x = x;
                    ss[ss_count].y = y;
                    y += shiftSize.height;
                }

                y = yupper;
                x = -x;
                /* right side */
                for( j = -Y_shift_count; j <= Y_shift_count; j++, ss_count++ )
                {
                    ss[ss_count].x = x;
                    ss[ss_count].y = y;
                    y += shiftSize.height;
                }
            }
        }

    }

/****************************************************************************************\
*   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                          *
\****************************************************************************************/