cvoptflowhs.cpp.svn-base

来自「非结构化路识别」· SVN-BASE 代码 · 共 547 行 · 第 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"

#define CONV( A, B, C)  ( (float)( A +  (B<<1)  + C ) )

typedef struct
{
    float xx;
    float xy;
    float yy;
    float xt;
    float yt;
    float alpha;                /* alpha = 1 / ( 1/lambda + xx + yy ) */
}
icvDerProductEx;


IPCVAPI( CvStatus , icvCalcOpticalFlowHS_8u32fR,( uchar*  imgA,
                                                  uchar*  imgB,
                                                  int     imgStep,
                                                  CvSize imgSize,
                                                  int     usePrevious,
                                                  float*  velocityX,
                                                  float*  velocityY,
                                                  int     velStep,
                                                  float   lambda,
                                                  CvTermCriteria criteria ))


/*F///////////////////////////////////////////////////////////////////////////////////////
//    Name: icvCalcOpticalFlowHS_8u32fR (Horn & Schunck method )
//    Purpose: calculate Optical flow for 2 images using Horn & Schunck algorithm
//    Context:
//    Parameters:
//            imgA          -  pointer to first frame ROI
//            imgB          -  pointer to second frame ROI
//            imgStep       -  width of single row of source images in bytes
//            imgSize       -  size of the source image ROI
//            usePrevious   - use previous (input) velocity field.
//            velocityX     - pointer to horizontal and
//            velocityY     - vertical components of optical flow ROI
//            velStep       - width of single row of velocity frames in bytes
//            lambda        - Lagrangian multiplier
//            criteria      - criteria of termination processmaximum number of iterations
//
//    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:  1.Optical flow to be computed for every pixel in ROI
//            2.For calculating spatial derivatives we use 3x3 Sobel operator.
//            3.We use the following border mode.
//              The last row or column is replicated for the border
//              ( IPL_BORDER_REPLICATE in IPL ).
//
//
//F*/
IPCVAPI_IMPL( CvStatus, icvCalcOpticalFlowHS_8u32fR, (uchar * imgA,
                                                      uchar * imgB,
                                                      int imgStep,
                                                      CvSize imgSize,
                                                      int usePrevious,
                                                      float *velocityX,
                                                      float *velocityY,
                                                      int velStep,
                                                      float lambda, CvTermCriteria criteria) )
{
    /* Loops indexes */
    int i, j, k, address;

    /* Buffers for Sobel calculations */
    float *MemX[2];
    float *MemY[2];

    float ConvX, ConvY;
    float GradX, GradY, GradT;

    int imageWidth = imgSize.width;
    int imageHeight = imgSize.height;

    int ConvLine;
    int LastLine;

    int BufferSize;

    float Ilambda = 1 / lambda;
    int iter = 0;
    int Stop;

    /* buffers derivatives product */
    icvDerProductEx *II;

    float *VelBufX[2];
    float *VelBufY[2];

    /* variables for storing number of first pixel of image line */
    int Line1;
    int Line2;
    int Line3;

    int pixNumber;

    /* auxiliary */
    int NoMem = 0;

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

    if( imgSize.width <= 0 )
        return CV_BADSIZE_ERR;
    if( imgSize.height <= 0 )
        return CV_BADSIZE_ERR;
    if( imgSize.width > imgStep )
        return CV_BADSIZE_ERR;

    if( (velStep & 3) != 0 )
        return CV_BADSIZE_ERR;

    velStep /= 4;

    /****************************************************************************************/
    /* Allocating memory for all buffers                                                    */
    /****************************************************************************************/
    for( k = 0; k < 2; k++ )
    {
        MemX[k] = (float *) icvAlloc( (imgSize.height) * sizeof( float ));

        if( MemX[k] == NULL )
            NoMem = 1;
        MemY[k] = (float *) icvAlloc( (imgSize.width) * sizeof( float ));

        if( MemY[k] == NULL )
            NoMem = 1;

        VelBufX[k] = (float *) icvAlloc( imageWidth * sizeof( float ));

        if( VelBufX[k] == NULL )
            NoMem = 1;
        VelBufY[k] = (float *) icvAlloc( imageWidth * sizeof( float ));

        if( VelBufY[k] == NULL )
            NoMem = 1;
    }

    BufferSize = imageHeight * imageWidth;

    II = (icvDerProductEx *) icvAlloc( BufferSize * sizeof( icvDerProductEx ));
    if( (II == NULL) )
        NoMem = 1;

    if( NoMem )
    {
        for( k = 0; k < 2; k++ )
        {
            if( MemX[k] )
                icvFree( (void **) &MemX[k] );

            if( MemY[k] )
                icvFree( (void **) &MemY[k] );

            if( VelBufX[k] )
                icvFree( (void **) &VelBufX[k] );

            if( VelBufY[k] )
                icvFree( (void **) &VelBufY[k] );
        }
        if( II )
            icvFree( (void **) &II );
        return CV_OUTOFMEM_ERR;
    }
/****************************************************************************************\
*         Calculate first line of memX and memY                                          *
\****************************************************************************************/
    MemY[0][0] = MemY[1][0] = CONV( imgA[0], imgA[0], imgA[1] );
    MemX[0][0] = MemX[1][0] = CONV( imgA[0], imgA[0], imgA[imgStep] );

    for( j = 1; j < imageWidth - 1; j++ )
    {
        MemY[0][j] = MemY[1][j] = CONV( imgA[j - 1], imgA[j], imgA[j + 1] );
    }

    pixNumber = imgStep;
    for( i = 1; i < imageHeight - 1; i++ )
    {
        MemX[0][i] = MemX[1][i] = CONV( imgA[pixNumber - imgStep],
                                        imgA[pixNumber], imgA[pixNumber + imgStep] );
        pixNumber += imgStep;
    }

    MemY[0][imageWidth - 1] =
        MemY[1][imageWidth - 1] = CONV( imgA[imageWidth - 2],
                                        imgA[imageWidth - 1], imgA[imageWidth - 1] );

    MemX[0][imageHeight - 1] =
        MemX[1][imageHeight - 1] = CONV( imgA[pixNumber - imgStep],
                                         imgA[pixNumber], imgA[pixNumber] );


/****************************************************************************************\
*     begin scan image, calc derivatives                                                 *
\****************************************************************************************/

    ConvLine = 0;
    Line2 = -imgStep;
    address = 0;
    LastLine = imgStep * (imageHeight - 1);
    while( ConvLine < imageHeight )
    {
        /*Here we calculate derivatives for line of image */
        int memYline = (ConvLine + 1) & 1;

        Line2 += imgStep;
        Line1 = Line2 - ((Line2 == 0) ? 0 : imgStep);
        Line3 = Line2 + ((Line2 == LastLine) ? 0 : imgStep);

        /* Process first pixel */
        ConvX = CONV( imgA[Line1 + 1], imgA[Line2 + 1], imgA[Line3 + 1] );
        ConvY = CONV( imgA[Line3], imgA[Line3], imgA[Line3 + 1] );

        GradY = (ConvY - MemY[memYline][0]) * 0.125f;
        GradX = (ConvX - MemX[1][ConvLine]) * 0.125f;

        MemY[memYline][0] = ConvY;
        MemX[1][ConvLine] = ConvX;

        GradT = (float) (imgB[Line2] - imgA[Line2]);

        II[address].xx = GradX * GradX;
        II[address].xy = GradX * GradY;
        II[address].yy = GradY * GradY;
        II[address].xt = GradX * GradT;