600 lines
21 KiB
C++
600 lines
21 KiB
C++
|
/*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 "precomp.hpp"
|
||
|
|
|
||
|
|
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 )
|
||
|
|
{
|
||
|
|
CvMat stubA, *srcA = cvGetMat( srcarrA, &stubA );
|
||
|
|
CvMat stubB, *srcB = cvGetMat( srcarrB, &stubB );
|
||
|
|
CvMat stubx, *velx = cvGetMat( velarrx, &stubx );
|
||
|
|
CvMat stuby, *vely = cvGetMat( velarry, &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 of file. */
|