cvoptflowbm.cpp

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

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

/* 
   Finds L1 norm between two blocks.
*/
static int
icvCmpBlocksL1_8u_C1( const uchar * vec1, const uchar * vec2, int len )
{
    int i, sum = 0;

    for( i = 0; i <= len - 4; i += 4 )
    {
        int t0 = abs(vec1[i] - vec2[i]);
        int t1 = abs(vec1[i + 1] - vec2[i + 1]);
        int t2 = abs(vec1[i + 2] - vec2[i + 2]);
        int t3 = abs(vec1[i + 3] - vec2[i + 3]);

        sum += t0 + t1 + t2 + t3;
    }

    for( ; i < len; i++ )
    {
        int t0 = abs(vec1[i] - vec2[i]);
        sum += t0;
    }

    return sum;
}


static void
icvCopyBM_8u_C1R( const uchar* src, int src_step,
                  uchar* dst, int dst_step, CvSize size )
{
    for( ; size.height--; src += src_step, dst += dst_step )
        memcpy( dst, src, size.width );
}


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

static CvStatus CV_STDCALL
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 = cvAlign(blSize + 9,16);
    int cmpSize = cvAlign(blSize,4);
    int patch_ofs = blSize & -8;
    int64 patch_mask = (((int64) 1) << (blSize - patch_ofs * 8)) - 1;

    velStep /= sizeof(velocityX[0]);

    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 *) cvAlloc( bufferSize * 3 );
    if( !blockA )
        return CV_OUTOFMEM_ERR;

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

    memset( blockZ, 0, bufferSize );

    ss = (CvPoint *) cvAlloc( (2 * maxRange.width + 1) * (2 * maxRange.height + 1) *
                               sizeof( CvPoint ));
    if( !ss )
    {
        cvFree( &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;
                }
            }
        }

    }