#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_STDCALLicvCalcOpticalFlowLK_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 voidcvCalcOpticalFlowLK( 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. */
