cvlkpyramid.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"
#include <float.h>
#include <stdio.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( const uchar * imgA, const 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 )
{
    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->max_iter = 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 = cvAlign(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 = (int)((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 *)cvAlloc( 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] = (uchar*)imgA;
    imgJ[0][0] = (uchar*)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;

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

        if( !ptrB )
            ptrB = bufPtr;

        levelSize = imgSize;

        /* build pyramids for both frames */
        for( i = 1; i <= level; i++ )
        {
            int levelBytes;
            CvMat prev_level, next_level;

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

            size[0][i] = levelSize;
            step[0][i] = cvAlign( 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;

            if( !(flags & CV_LKFLOW_PYR_A_READY) )
            {
                prev_level = cvMat( size[0][i-1].height, size[0][i-1].width, CV_8UC1 );
                next_level = cvMat( size[0][i].height, size[0][i].width, CV_8UC1 );
                cvSetData( &prev_level, imgI[0][i-1], step[0][i-1] );
                cvSetData( &next_level, imgI[0][i], step[0][i] );
                cvPyrDown( &prev_level, &next_level );
            }

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

            if( !(flags & CV_LKFLOW_PYR_B_READY) )
            {
                prev_level = cvMat( size[0][i-1].height, size[0][i-1].width, CV_8UC1 );
                next_level = cvMat( size[0][i].height, size[0][i].width, CV_8UC1 );
                cvSetData( &prev_level, imgJ[0][i-1], step[0][i-1] );
                cvSetData( &next_level, imgJ[0][i], step[0][i] );
                cvPyrDown( &prev_level, &next_level );
            }
        }
    }

    return CV_OK;
}


/* compute dI/dx and dI/dy */
static void
icvCalcIxIy_32f( const float* src, int src_step, float* dstX, float* dstY, int dst_step,
                 CvSize src_size, const float* smooth_k, float* buffer0 )
{
    int src_width = src_size.width, dst_width = src_size.width-2;
    int x, height = src_size.height - 2;
    float* buffer1 = buffer0 + src_width;

    src_step /= sizeof(src[0]);
    dst_step /= sizeof(dstX[0]);

    for( ; height--; src += src_step, dstX += dst_step, dstY += dst_step )
    {
        const float* src2 = src + src_step;
        const float* src3 = src + src_step*2;

        for( x = 0; x < src_width; x++ )
        {
            float t0 = (src3[x] + src[x])*smooth_k[0] + src2[x]*smooth_k[1];
            float t1 = src3[x] - src[x];
            buffer0[x] = t0; buffer1[x] = t1;
        }

        for( x = 0; x < dst_width; x++ )
        {
            float t0 = buffer0[x+2] - buffer0[x];
            float t1 = (buffer1[x] + buffer1[x+2])*smooth_k[0] + buffer1[x+1]*smooth_k[1];
            dstX[x] = t0; dstY[x] = t1;
        }
    }
}

icvOpticalFlowPyrLKInitAlloc_8u_C1R_t icvOpticalFlowPyrLKInitAlloc_8u_C1R_p = 0;
icvOpticalFlowPyrLKFree_8u_C1R_t icvOpticalFlowPyrLKFree_8u_C1R_p = 0;
icvOpticalFlowPyrLK_8u_C1R_t icvOpticalFlowPyrLK_8u_C1R_p = 0;

static CvStatus
icvCalcOpticalFlowPyrLK_8uC1R( const uchar* imgA, const uchar* imgB,
                               int imgStep, CvSize imgSize,
                               uchar* pyrA, uchar* pyrB,
                               const CvPoint2D32f* featuresA,
                               CvPoint2D32f* featuresB,
                               int count, CvSize winSize,
                               int level, char* status,
                               float *error, CvTermCriteria criteria, int flags )
{
#define MAX_SIZE  100
#define MAX_LEVEL 10
#define MAX_ITERS 100

    static const float smoothKernel[] = { 0.09375, 0.3125, 0.09375 };  /* 3/32, 10/32, 3/32 */

    uchar *pyrBuffer = 0;
    uchar *buffer = 0;
    int bufferBytes = 0;
    float* _error = 0;
    char* _status = 0;

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

    int threadCount = cvGetNumThreads();
    float* _patchI[CV_MAX_THREADS];
    float* _patchJ[CV_MAX_THREADS];
    float* _Ix[CV_MAX_THREADS];
    float* _Iy[CV_MAX_THREADS];

    int i, l;

    CvSize patchSize = cvSize( winSize.width * 2 + 1, winSize.height * 2 + 1 );
    int patchLen = patchSize.width * patchSize.height;
    int srcPatchLen = (patchSize.width + 2)*(patchSize.height + 2);

    CvStatus result = CV_OK;
    void* ipp_optflow_state = 0;

    /* 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;
    if( count <= 0 )
        return CV_BADRANGE_ERR;

    for( i = 0; i < threadCount; i++ )
        _patchI[i] = _patchJ[i] = _Ix[i] = _Iy[i] = 0;

    result = icvInitPyramidalAlgorithm( imgA, imgB, imgStep, imgSize,
                                        pyrA, pyrB, level, &criteria, MAX_ITERS, flags,
                                        &imgI, &imgJ, &step, &size, &scale, &pyrBuffer );

    if( result < 0 )
        goto func_exit;

    if( !status )
    {
        _status = (char*)cvAlloc( count*sizeof(_status[0]) );
        if( !_status )
        {
            result = CV_OUTOFMEM_ERR;
            goto func_exit;
        }
        status = _status;
    }

#if 0
    if( icvOpticalFlowPyrLKInitAlloc_8u_C1R_p &&
        icvOpticalFlowPyrLKFree_8u_C1R_p &&
        icvOpticalFlowPyrLK_8u_C1R_p &&
        winSize.width == winSize.height &&
        icvOpticalFlowPyrLKInitAlloc_8u_C1R_p( &ipp_optflow_state, imgSize,
                                               winSize.width*2+1, cvAlgHintAccurate ) >= 0 )
    {
        CvPyramid ipp_pyrA, ipp_pyrB;
        static const double rate[] = { 1, 0.5, 0.25, 0.125, 0.0625, 0.03125, 0.015625, 0.0078125,
                                       0.00390625, 0.001953125, 0.0009765625, 0.00048828125, 0.000244140625,
                                       0.0001220703125 };
        // initialize pyramid structures
        assert( level < 14 );
        ipp_pyrA.ptr = imgI;
        ipp_pyrB.ptr = imgJ;
        ipp_pyrA.sz = ipp_pyrB.sz = size;
        ipp_pyrA.rate = ipp_pyrB.rate = (double*)rate;
        ipp_pyrA.step = ipp_pyrB.step = step;
        ipp_pyrA.state = ipp_pyrB.state = 0;
        ipp_pyrA.level = ipp_pyrB.level = level;

        if( !error )
        {
            _error = (float*)cvAlloc( count*sizeof(_error[0]) );
            if( !_error )
            {
                result = CV_OUTOFMEM_ERR;
                goto func_exit;
            }
            error = _error;
        }

        for( i = 0; i < count; i++ )
            featuresB[i] = featuresA[i];

        if( icvOpticalFlowPyrLK_8u_C1R_p( &ipp_pyrA, &ipp_pyrB,
            (const float*)featuresA, (float*)featuresB, status, error, count,
            winSize.width*2 + 1, level, criteria.max_iter,
            (float)criteria.epsilon, ipp_optflow_state ) >= 0 )