cvlkpyramid.cpp.svn-base

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

/*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"
#include <float.h>

static void
intersect( CvPoint2D32f pt, CvSize win_size, CvSize img_size,
           CvPoint * min_pt, CvPoint * max_pt )
{
    CvPoint ipt;

    ipt.x = cvFloor( pt.x );
    ipt.y = cvFloor( pt.y );

    ipt.x -= win_size.width;
    ipt.y -= win_size.height;

    win_size.width = win_size.width * 2 + 1;
    win_size.height = win_size.height * 2 + 1;

    min_pt->x = MAX( 0, -ipt.x );
    min_pt->y = MAX( 0, -ipt.y );
    max_pt->x = MIN( win_size.width, img_size.width - ipt.x );
    max_pt->y = MIN( win_size.height, img_size.height - ipt.y );
}


static CvStatus
icvInitPyramidalAlgorithm( uchar * imgA, uchar * imgB,
                           int imgStep, CvSize imgSize,
                           uchar * pyrA, uchar * pyrB,
                           int level,
                           CvTermCriteria * criteria,
                           int max_iters, int flags,
                           uchar *** imgI, uchar *** imgJ,
                           int **step, CvSize** size,
                           double **scale, uchar ** buffer )
{
    uchar *pyr_down_temp_buffer = 0;
    CvStatus result = CV_OK;
    int pyrBytes, bufferBytes = 0;
    int level1 = level + 1;

    int i;
    CvSize levelSize;

    *buffer = 0;
    *imgI = *imgJ = 0;
    *step = 0;
    *scale = 0;
    *size = 0;

    /* check input arguments */
    if( !imgA || !imgB )
        return CV_NULLPTR_ERR;

    if( (flags & CV_LKFLOW_PYR_A_READY) != 0 && !pyrA ||
        (flags & CV_LKFLOW_PYR_B_READY) != 0 && !pyrB )
        return CV_BADFLAG_ERR;

    if( level < 0 )
        return CV_BADRANGE_ERR;

    switch (criteria->type)
    {
    case CV_TERMCRIT_ITER:
        criteria->epsilon = 0.f;
        break;
    case CV_TERMCRIT_EPS:
        criteria->maxIter = max_iters;
        break;
    case CV_TERMCRIT_ITER | CV_TERMCRIT_EPS:
        break;
    default:
        assert( 0 );
        return CV_BADFLAG_ERR;
    }

    /* compare squared values */
    criteria->epsilon *= criteria->epsilon;

    /* set pointers and step for every level */
    pyrBytes = 0;

#define ALIGN 8

    levelSize = imgSize;

    for( i = 1; i < level1; i++ )
    {
        levelSize.width = (levelSize.width + 1) >> 1;
        levelSize.height = (levelSize.height + 1) >> 1;

        int tstep = icvAlign(levelSize.width,ALIGN) * sizeof( imgA[0] );
        pyrBytes += tstep * levelSize.height;
    }

    assert( pyrBytes <= imgSize.width * imgSize.height * (int) sizeof( imgA[0] ) * 4 / 3 );

    /* buffer_size = <size for patches> + <size for pyramids> */
    bufferBytes = (level1 >= 0) * ((pyrA == 0) + (pyrB == 0)) * pyrBytes +
        (sizeof( imgI[0][0] ) * 2 + sizeof( step[0][0] ) +
         sizeof(size[0][0]) + sizeof( scale[0][0] )) * level1;

    *buffer = (uchar *) icvAlloc( bufferBytes );
    if( !buffer[0] )
        return CV_OUTOFMEM_ERR;

    *imgI = (uchar **) buffer[0];
    *imgJ = *imgI + level1;
    *step = (int *) (*imgJ + level1);
    *scale = (double *) (*step + level1);
    *size = (CvSize *)(*scale + level1);

    imgI[0][0] = imgA;
    imgJ[0][0] = imgB;
    step[0][0] = imgStep;
    scale[0][0] = 1;
    size[0][0] = imgSize;

    if( level > 0 )
    {
        uchar *bufPtr = (uchar *) (*size + level1);
        uchar *ptrA = pyrA;
        uchar *ptrB = pyrB;
        int pyr_down_buffer_size = 0;

        if( !ptrA )
        {
            ptrA = bufPtr;
            bufPtr += pyrBytes;
        }

        if( !ptrB )
            ptrB = bufPtr;

        icvPyrDownGetBufSize_Gauss5x5( imgSize.width, cv8u, 1, &pyr_down_buffer_size );
        pyr_down_temp_buffer = (uchar *) icvAlloc( pyr_down_buffer_size );
        
        levelSize = imgSize;

        /* build pyramids for both frames */
        for( i = 1; i <= level; i++ )
        {
            int levelBytes;
            CvSize srcSize = levelSize;

            levelSize.width = (levelSize.width + 1) >> 1;
            levelSize.height = (levelSize.height + 1) >> 1;

            size[0][i] = levelSize;
            step[0][i] = icvAlign( levelSize.width, ALIGN ) * sizeof( imgA[0] );
            scale[0][i] = scale[0][i - 1] * 0.5;

            levelBytes = step[0][i] * levelSize.height;
            imgI[0][i] = (uchar *) ptrA;
            ptrA += levelBytes;

            srcSize.width &= -2;
            srcSize.height &= -2;

            if( !(flags & CV_LKFLOW_PYR_A_READY) )
            {
                result = icvPyrDown_Gauss5x5_8u_C1R( imgI[0][i - 1], step[0][i - 1],
                                                     imgI[0][i], step[0][i],
                                                     srcSize, pyr_down_temp_buffer );
                if( result < 0 )
                    goto func_exit;
                icvPyrDownBorder_8u_CnR( imgI[0][i - 1], step[0][i - 1], size[0][i-1],
                                         imgI[0][i], step[0][i], size[0][i], 1 );
            }

            imgJ[0][i] = (uchar *) ptrB;
            ptrB += levelBytes;

            if( !(flags & CV_LKFLOW_PYR_B_READY) )
            {
                result = icvPyrDown_Gauss5x5_8u_C1R( imgJ[0][i - 1], step[0][i - 1],
                                                     imgJ[0][i], step[0][i],
                                                     srcSize, pyr_down_temp_buffer );
                if( result < 0 )
                    goto func_exit;
                icvPyrDownBorder_8u_CnR( imgJ[0][i - 1], step[0][i - 1], size[0][i-1],
                                         imgJ[0][i], step[0][i], size[0][i], 1 );
            }
        }
    }

  func_exit:
    icvFree( &pyr_down_temp_buffer );

    return CV_OK;
}


/*F///////////////////////////////////////////////////////////////////////////////////////
//    Name: icvCalcOpticalFlowPyrLK_8uC1R ( Lucas & Kanade method,
//                                           modification that uses pyramids )
//    Purpose:
//      Calculates optical flow between two images for certain set of points.
//    Context:
//    Parameters:
//            imgA     - pointer to first frame (time t)
//            imgB     - pointer to second frame (time t+1)
//            imgStep  - full width of the source images in bytes
//            imgSize  - size of the source images
//            pyrA     - buffer for pyramid for the first frame.
//                       if the pointer is not NULL, the buffer must have size enough to
//                       store pyramid (from level 1 to level #<level> (see below))
//                       (imgSize.width*imgSize.height/3 will be enough)).
//            pyrB     - similar to pyrA, but for the second frame.
//                       
//                       for both parameters above the following rules work:
//                           If pointer is 0, the function allocates the buffer internally,
//                           calculates pyramid and releases the buffer after processing.
//                           Else (it should be large enough then) the function calculates
//                           pyramid and stores it in the buffer unless the
//                           CV_LKFLOW_PYR_A[B]_READY flag is set. In both cases
//                           (flag is set or not) the subsequent calls may reuse the calculated
//                           pyramid by setting CV_LKFLOW_PYR_A[B]_READY.
//
//            featuresA - array of points, for which the flow needs to be found
//            count    - number of feature points 
//            winSize  - size of search window on each pyramid level
//            level    - maximal pyramid level number
//                         (if 0, pyramids are not used (single level),
//                          if 1, two levels are used etc.)
//
//            next parameters are arrays of <count> elements.
//            ------------------------------------------------------
//            featuresB - array of 2D points, containing calculated
//                       new positions of input features (in the second image).
//            status   - array, every element of which will be set to 1 if the flow for the
//                       corresponding feature has been found, 0 else.
//            error    - array of double numbers, containing difference between
//                       patches around the original and moved points
//                       (it is optional parameter, can be NULL).
//            ------------------------------------------------------
//            criteria   - specifies when to stop the iteration process of finding flow
//                         for each point on each pyramid level
//
//            flags      - miscellaneous flags:
//                            CV_LKFLOW_PYR_A_READY - pyramid for the first frame
//                                                      is precalculated before call
//                            CV_LKFLOW_PYR_B_READY - pyramid for the second frame
//                                                      is precalculated before call
//                            CV_LKFLOW_INITIAL_GUESSES - featuresB array holds initial
//                                                       guesses about new features'
//                                                       locations before function call.
//    Returns: CV_OK       - all ok
//             CV_OUTOFMEM_ERR - insufficient memory for function work
//             CV_NULLPTR_ERR  - if one of input pointers is NULL
//             CV_BADSIZE_ERR  - wrong input sizes interrelation
//
//    Notes:  For calculating spatial derivatives 3x3 Sobel operator is used.
//            The values of pixels beyond the image are determined using replication mode.
//F*/
static  CvStatus  icvCalcOpticalFlowPyrLK_8uC1R( uchar * imgA,
                                                 uchar * imgB,
                                                 int imgStep,
                                                 CvSize imgSize,
                                                 uchar * pyrA,
                                                 uchar * pyrB,
                                                 CvPoint2D32f * featuresA,
                                                 CvPoint2D32f * featuresB,
                                                 int count,
                                                 CvSize winSize,
                                                 int level,
                                                 char *status,
                                                 float *error,
                                                 CvTermCriteria criteria, int flags )
{
#define MAX_LEVEL 10
#define MAX_ITERS 100

    static const float kerX[] = { -1, 0, 1 }, kerY[] =
    {
    0.09375, 0.3125, 0.09375};  /* 3/32, 10/32, 3/32 */

    uchar *pyr_buffer = 0;
    uchar *buffer = 0;
    int bufferBytes = 0;

    uchar **imgI = 0;
    uchar **imgJ = 0;
    int *step = 0;
    double *scale = 0;
    CvSize* size = 0;

    float *patchI;
    float *patchJ;
    float *Ix;
    float *Iy;

    int i, j, k;
    int x, y;

    CvSize patchSize = cvSize( winSize.width * 2 + 1, winSize.height * 2 + 1 );
    int patchLen = patchSize.width * patchSize.height;
    int patchStep = patchSize.width * sizeof( patchI[0] );

    CvSize srcPatchSize = cvSize( patchSize.width + 2, patchSize.height + 2 );
    int srcPatchLen = srcPatchSize.width * srcPatchSize.height;
    int srcPatchStep = srcPatchSize.width * sizeof( patchI[0] );

    CvStatus result = CV_OK;

    /* check input arguments */
    if( !featuresA || !featuresB )
        return CV_NULLPTR_ERR;
    if( winSize.width <= 1 || winSize.height <= 1 )
        return CV_BADSIZE_ERR;

    if( (flags & ~7) != 0 )
        return CV_BADFLAG_ERR;