+++ /dev/null
-/*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"
-
-typedef struct
-{
- float xx;
- float xy;
- float yy;
- float xt;
- float yt;
-}
-icvDerProduct;
-
-
-#define CONV( A, B, C) ((float)( A + (B<<1) + C ))
-/*F///////////////////////////////////////////////////////////////////////////////////////
-// Name: icvCalcOpticalFlowLK_8u32fR ( Lucas & Kanade method )
-// Purpose: calculate Optical flow for 2 images using Lucas & Kanade 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
-// winSize, // size of the averaging window used for grouping
-// velocityX, // pointer to horizontal and
-// velocityY, // vertical components of optical flow ROI
-// velStep // width of single row of velocity frames in bytes
-//
-// 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*/
-static CvStatus CV_STDCALL
-icvCalcOpticalFlowLK_8u32fR( uchar * imgA,
- uchar * imgB,
- int imgStep,
- CvSize imgSize,
- CvSize winSize,
- float *velocityX,
- float *velocityY, int velStep )
-{
- /* Loops indexes */
- int i, j, k;
-
- /* Gaussian separable kernels */
- float GaussX[16];
- float GaussY[16];
- float *KerX;
- float *KerY;
-
- /* Buffers for Sobel calculations */
- float *MemX[2];
- float *MemY[2];
-
- float ConvX, ConvY;
- float GradX, GradY, GradT;
-
- int winWidth = winSize.width;
- int winHeight = winSize.height;
-
- int imageWidth = imgSize.width;
- int imageHeight = imgSize.height;
-
- int HorRadius = (winWidth - 1) >> 1;
- int VerRadius = (winHeight - 1) >> 1;
-
- int PixelLine;
- int ConvLine;
-
- int BufferAddress;
-
- int BufferHeight = 0;
- int BufferWidth;
- int BufferSize;
-
- /* buffers derivatives product */
- icvDerProduct *II;
-
- /* buffers for gaussian horisontal convolution */
- icvDerProduct *WII;
-
- /* variables for storing number of first pixel of image line */
- int Line1;
- int Line2;
- int Line3;
-
- /* we must have 2*2 linear system coeffs
- | A1B2 B1 | {u} {C1} {0}
- | | { } + { } = { }
- | A2 A1B2 | {v} {C2} {0}
- */
- float A1B2, A2, B1, C1, C2;
-
- int pixNumber;
-
- /* auxiliary */
- int NoMem = 0;
-
- velStep /= sizeof(velocityX[0]);
-
- /* Checking bad arguments */
- if( imgA == NULL )
- return CV_NULLPTR_ERR;
- if( imgB == NULL )
- return CV_NULLPTR_ERR;
-
- if( imageHeight < winHeight )
- return CV_BADSIZE_ERR;
- if( imageWidth < winWidth )
- return CV_BADSIZE_ERR;
-
- if( winHeight >= 16 )
- return CV_BADSIZE_ERR;
- if( winWidth >= 16 )
- return CV_BADSIZE_ERR;
-
- if( !(winHeight & 1) )
- return CV_BADSIZE_ERR;
- if( !(winWidth & 1) )
- return CV_BADSIZE_ERR;
-
- BufferHeight = winHeight;
- BufferWidth = imageWidth;
-
- /****************************************************************************************/
- /* Computing Gaussian coeffs */
- /****************************************************************************************/
- GaussX[0] = 1;
- GaussY[0] = 1;
- for( i = 1; i < winWidth; i++ )
- {
- GaussX[i] = 1;
- for( j = i - 1; j > 0; j-- )
- {
- GaussX[j] += GaussX[j - 1];
- }
- }
- for( i = 1; i < winHeight; i++ )
- {
- GaussY[i] = 1;
- for( j = i - 1; j > 0; j-- )
- {
- GaussY[j] += GaussY[j - 1];
- }
- }
- KerX = &GaussX[HorRadius];
- KerY = &GaussY[VerRadius];
-
- /****************************************************************************************/
- /* Allocating memory for all buffers */
- /****************************************************************************************/
- for( k = 0; k < 2; k++ )
- {
- MemX[k] = (float *) cvAlloc( (imgSize.height) * sizeof( float ));
-
- if( MemX[k] == NULL )
- NoMem = 1;
- MemY[k] = (float *) cvAlloc( (imgSize.width) * sizeof( float ));
-
- if( MemY[k] == NULL )
- NoMem = 1;
- }
-
- BufferSize = BufferHeight * BufferWidth;
-
- II = (icvDerProduct *) cvAlloc( BufferSize * sizeof( icvDerProduct ));
- WII = (icvDerProduct *) cvAlloc( BufferSize * sizeof( icvDerProduct ));
-
-
- if( (II == NULL) || (WII == NULL) )
- NoMem = 1;
-
- if( NoMem )
- {
- for( k = 0; k < 2; k++ )
- {
- if( MemX[k] )
- cvFree( &MemX[k] );
-
- if( MemY[k] )
- cvFree( &MemY[k] );
- }
- if( II )
- cvFree( &II );
- if( WII )
- cvFree( &WII );
-
- 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 and solve system */
- /****************************************************************************************/
-
- PixelLine = -VerRadius;
- ConvLine = 0;
- BufferAddress = -BufferWidth;
-
- while( PixelLine < imageHeight )
- {
- if( ConvLine < imageHeight )
- {
- /*Here we calculate derivatives for line of image */
- int address;
-
- i = ConvLine;
- int L1 = i - 1;
- int L2 = i;
- int L3 = i + 1;
-
- int memYline = L3 & 1;
-
- if( L1 < 0 )
- L1 = 0;
- if( L3 >= imageHeight )
- L3 = imageHeight - 1;
-
- BufferAddress += BufferWidth;
- BufferAddress -= ((BufferAddress >= BufferSize) ? 0xffffffff : 0) & BufferSize;
-
- address = BufferAddress;
-
- Line1 = L1 * imgStep;
- Line2 = L2 * imgStep;
- Line3 = L3 * 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];
- GradX = ConvX - MemX[1][L2];
-
- MemY[memYline][0] = ConvY;
- MemX[1][L2] = 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;
- II[address].yt = GradY * GradT;
- address++;
- /* Process middle of line */
- for( j = 1; j < imageWidth - 1; j++ )
- {
- ConvX = CONV( imgA[Line1 + j + 1], imgA[Line2 + j + 1], imgA[Line3 + j + 1] );
- ConvY = CONV( imgA[Line3 + j - 1], imgA[Line3 + j], imgA[Line3 + j + 1] );
-
- GradY = ConvY - MemY[memYline][j];
- GradX = ConvX - MemX[(j - 1) & 1][L2];
-
- MemY[memYline][j] = ConvY;
- MemX[(j - 1) & 1][L2] = ConvX;
-
- GradT = (float) (imgB[Line2 + j] - imgA[Line2 + j]);
-
- II[address].xx = GradX * GradX;
- II[address].xy = GradX * GradY;
- II[address].yy = GradY * GradY;
- II[address].xt = GradX * GradT;
- II[address].yt = GradY * GradT;
-
- address++;
- }
- /* Process last pixel of line */
- ConvX = CONV( imgA[Line1 + imageWidth - 1], imgA[Line2 + imageWidth - 1],
- imgA[Line3 + imageWidth - 1] );
-
- ConvY = CONV( imgA[Line3 + imageWidth - 2], imgA[Line3 + imageWidth - 1],
- imgA[Line3 + imageWidth - 1] );
-
-
- GradY = ConvY - MemY[memYline][imageWidth - 1];
- GradX = ConvX - MemX[(imageWidth - 2) & 1][L2];
-
- MemY[memYline][imageWidth - 1] = ConvY;
-
- GradT = (float) (imgB[Line2 + imageWidth - 1] - imgA[Line2 + imageWidth - 1]);
-
- II[address].xx = GradX * GradX;
- II[address].xy = GradX * GradY;
- II[address].yy = GradY * GradY;
- II[address].xt = GradX * GradT;
- II[address].yt = GradY * GradT;
- address++;
-
- /* End of derivatives for line */
-
- /****************************************************************************************/
- /* ---------Calculating horizontal convolution of processed line----------------------- */
- /****************************************************************************************/
- address -= BufferWidth;
- /* process first HorRadius pixels */
- for( j = 0; j < HorRadius; j++ )
- {
- int jj;
-
- WII[address].xx = 0;
- WII[address].xy = 0;
- WII[address].yy = 0;
- WII[address].xt = 0;
- WII[address].yt = 0;
-
- for( jj = -j; jj <= HorRadius; jj++ )
- {
- float Ker = KerX[jj];
-
- WII[address].xx += II[address + jj].xx * Ker;
- WII[address].xy += II[address + jj].xy * Ker;
- WII[address].yy += II[address + jj].yy * Ker;
- WII[address].xt += II[address + jj].xt * Ker;
- WII[address].yt += II[address + jj].yt * Ker;
- }
- address++;
- }
- /* process inner part of line */
- for( j = HorRadius; j < imageWidth - HorRadius; j++ )
- {
- int jj;
- float Ker0 = KerX[0];
-
- WII[address].xx = 0;
- WII[address].xy = 0;
- WII[address].yy = 0;
- WII[address].xt = 0;
- WII[address].yt = 0;
-
- for( jj = 1; jj <= HorRadius; jj++ )
- {
- float Ker = KerX[jj];
-
- WII[address].xx += (II[address - jj].xx + II[address + jj].xx) * Ker;
- WII[address].xy += (II[address - jj].xy + II[address + jj].xy) * Ker;
- WII[address].yy += (II[address - jj].yy + II[address + jj].yy) * Ker;
- WII[address].xt += (II[address - jj].xt + II[address + jj].xt) * Ker;
- WII[address].yt += (II[address - jj].yt + II[address + jj].yt) * Ker;
- }
- WII[address].xx += II[address].xx * Ker0;
- WII[address].xy += II[address].xy * Ker0;
- WII[address].yy += II[address].yy * Ker0;
- WII[address].xt += II[address].xt * Ker0;
- WII[address].yt += II[address].yt * Ker0;
-
- address++;
- }
- /* process right side */
- for( j = imageWidth - HorRadius; j < imageWidth; j++ )
- {
- int jj;
-
- WII[address].xx = 0;
- WII[address].xy = 0;
- WII[address].yy = 0;
- WII[address].xt = 0;
- WII[address].yt = 0;
-
- for( jj = -HorRadius; jj < imageWidth - j; jj++ )
- {
- float Ker = KerX[jj];
-
- WII[address].xx += II[address + jj].xx * Ker;
- WII[address].xy += II[address + jj].xy * Ker;
- WII[address].yy += II[address + jj].yy * Ker;
- WII[address].xt += II[address + jj].xt * Ker;
- WII[address].yt += II[address + jj].yt * Ker;
- }
- address++;
- }
- }
-
- /****************************************************************************************/
- /* Calculating velocity line */
- /****************************************************************************************/
- if( PixelLine >= 0 )
- {
- int USpace;
- int BSpace;
- int address;
-
- if( PixelLine < VerRadius )
- USpace = PixelLine;
- else
- USpace = VerRadius;
-
- if( PixelLine >= imageHeight - VerRadius )
- BSpace = imageHeight - PixelLine - 1;
- else
- BSpace = VerRadius;
-
- address = ((PixelLine - USpace) % BufferHeight) * BufferWidth;
- for( j = 0; j < imageWidth; j++ )
- {
- int addr = address;
-
- A1B2 = 0;
- A2 = 0;
- B1 = 0;
- C1 = 0;
- C2 = 0;
-
- for( i = -USpace; i <= BSpace; i++ )
- {
- A2 += WII[addr + j].xx * KerY[i];
- A1B2 += WII[addr + j].xy * KerY[i];
- B1 += WII[addr + j].yy * KerY[i];
- C2 += WII[addr + j].xt * KerY[i];
- C1 += WII[addr + j].yt * KerY[i];
-
- addr += BufferWidth;
- addr -= ((addr >= BufferSize) ? 0xffffffff : 0) & BufferSize;
- }
- /****************************************************************************************\
- * Solve Linear System *
- \****************************************************************************************/
- {
- float delta = (A1B2 * A1B2 - A2 * B1);
-
- if( delta )
- {
- /* system is not singular - solving by Kramer method */
- float deltaX;
- float deltaY;
- float Idelta = 8 / delta;
-
- deltaX = -(C1 * A1B2 - C2 * B1);
- deltaY = -(A1B2 * C2 - A2 * C1);
-
- velocityX[j] = deltaX * Idelta;
- velocityY[j] = deltaY * Idelta;
- }
- else
- {
- /* singular system - find optical flow in gradient direction */
- float Norm = (A1B2 + A2) * (A1B2 + A2) + (B1 + A1B2) * (B1 + A1B2);
-
- if( Norm )
- {
- float IGradNorm = 8 / Norm;
- float temp = -(C1 + C2) * IGradNorm;
-
- velocityX[j] = (A1B2 + A2) * temp;
- velocityY[j] = (B1 + A1B2) * temp;
-
- }
- else
- {
- velocityX[j] = 0;
- velocityY[j] = 0;
- }
- }
- }
- /****************************************************************************************\
- * End of Solving Linear System *
- \****************************************************************************************/
- } /*for */
- velocityX += velStep;
- velocityY += velStep;
- } /*for */
- PixelLine++;
- ConvLine++;
- }
-
- /* Free memory */
- for( k = 0; k < 2; k++ )
- {
- cvFree( &MemX[k] );
- cvFree( &MemY[k] );
- }
- cvFree( &II );
- cvFree( &WII );
-
- return CV_OK;
-} /*icvCalcOpticalFlowLK_8u32fR*/
-
-
-/*F///////////////////////////////////////////////////////////////////////////////////////
-// Name: cvCalcOpticalFlowLK
-// Purpose: Optical flow implementation
-// Context:
-// Parameters:
-// srcA, srcB - source image
-// velx, vely - destination image
-// Returns:
-//
-// Notes:
-//F*/
-CV_IMPL void
-cvCalcOpticalFlowLK( const void* srcarrA, const void* srcarrB, CvSize winSize,
- void* velarrx, void* velarry )
-{
- CV_FUNCNAME( "cvCalcOpticalFlowLK" );
-
- __BEGIN__;
-
- CvMat stubA, *srcA = (CvMat*)srcarrA;
- CvMat stubB, *srcB = (CvMat*)srcarrB;
- CvMat stubx, *velx = (CvMat*)velarrx;
- CvMat stuby, *vely = (CvMat*)velarry;
-
- CV_CALL( srcA = cvGetMat( srcA, &stubA ));
- CV_CALL( srcB = cvGetMat( srcB, &stubB ));
-
- CV_CALL( velx = cvGetMat( velx, &stubx ));
- CV_CALL( vely = cvGetMat( vely, &stuby ));
-
- if( !CV_ARE_TYPES_EQ( srcA, srcB ))
- CV_ERROR( CV_StsUnmatchedFormats, "Source images have different formats" );
-
- if( !CV_ARE_TYPES_EQ( velx, vely ))
- CV_ERROR( CV_StsUnmatchedFormats, "Destination images have different formats" );
-
- if( !CV_ARE_SIZES_EQ( srcA, srcB ) ||
- !CV_ARE_SIZES_EQ( velx, vely ) ||
- !CV_ARE_SIZES_EQ( srcA, velx ))
- CV_ERROR( CV_StsUnmatchedSizes, "" );
-
- if( CV_MAT_TYPE( srcA->type ) != CV_8UC1 ||
- CV_MAT_TYPE( velx->type ) != CV_32FC1 )
- CV_ERROR( CV_StsUnsupportedFormat, "Source images must have 8uC1 type and "
- "destination images must have 32fC1 type" );
-
- if( srcA->step != srcB->step || velx->step != vely->step )
- CV_ERROR( CV_BadStep, "source and destination images have different step" );
-
- IPPI_CALL( icvCalcOpticalFlowLK_8u32fR( (uchar*)srcA->data.ptr, (uchar*)srcB->data.ptr,
- srcA->step, cvGetMatSize( srcA ), winSize,
- velx->data.fl, vely->data.fl, velx->step ));
-
- __END__;
-}
-
-/* End of file. */
projects / opencv / blobdiff