16acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*M/////////////////////////////////////////////////////////////////////////////////////// 26acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 36acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 46acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 56acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// By downloading, copying, installing or using the software you agree to this license. 66acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// If you do not agree to this license, do not download, install, 76acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// copy or use the software. 86acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 96acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Intel License Agreement 116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// For Open Source Computer Vision Library 126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Copyright (C) 2000, Intel Corporation, all rights reserved. 146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Third party copyrights are property of their respective owners. 156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Redistribution and use in source and binary forms, with or without modification, 176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// are permitted provided that the following conditions are met: 186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * Redistribution's of source code must retain the above copyright notice, 206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// this list of conditions and the following disclaimer. 216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * Redistribution's in binary form must reproduce the above copyright notice, 236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// this list of conditions and the following disclaimer in the documentation 246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// and/or other materials provided with the distribution. 256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * The name of Intel Corporation may not be used to endorse or promote products 276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// derived from this software without specific prior written permission. 286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// This software is provided by the copyright holders and contributors "as is" and 306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// any express or implied warranties, including, but not limited to, the implied 316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// warranties of merchantability and fitness for a particular purpose are disclaimed. 326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// In no event shall the Intel Corporation or contributors be liable for any direct, 336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// indirect, incidental, special, exemplary, or consequential damages 346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// (including, but not limited to, procurement of substitute goods or services; 356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// loss of use, data, or profits; or business interruption) however caused 366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// and on any theory of liability, whether in contract, strict liability, 376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// or tort (including negligence or otherwise) arising in any way out of 386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// the use of this software, even if advised of the possibility of such damage. 396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//M*/ 416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#include "_cv.h" 426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renntypedef struct 446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float xx; 466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float xy; 476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float yy; 486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float xt; 496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float yt; 506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennicvDerProduct; 526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define CONV( A, B, C) ((float)( A + (B<<1) + C )) 556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*F/////////////////////////////////////////////////////////////////////////////////////// 566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Name: icvCalcOpticalFlowLK_8u32fR ( Lucas & Kanade method ) 576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Purpose: calculate Optical flow for 2 images using Lucas & Kanade algorithm 586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Context: 596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Parameters: 606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// imgA, // pointer to first frame ROI 616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// imgB, // pointer to second frame ROI 626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// imgStep, // width of single row of source images in bytes 636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// imgSize, // size of the source image ROI 646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// winSize, // size of the averaging window used for grouping 656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// velocityX, // pointer to horizontal and 666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// velocityY, // vertical components of optical flow ROI 676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// velStep // width of single row of velocity frames in bytes 686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Returns: CV_OK - all ok 706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// CV_OUTOFMEM_ERR - insufficient memory for function work 716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// CV_NULLPTR_ERR - if one of input pointers is NULL 726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// CV_BADSIZE_ERR - wrong input sizes interrelation 736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Notes: 1.Optical flow to be computed for every pixel in ROI 756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 2.For calculating spatial derivatives we use 3x3 Sobel operator. 766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 3.We use the following border mode. 776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// The last row or column is replicated for the border 786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// ( IPL_BORDER_REPLICATE in IPL ). 796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//F*/ 826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic CvStatus CV_STDCALL 836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennicvCalcOpticalFlowLK_8u32fR( uchar * imgA, 846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar * imgB, 856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int imgStep, 866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize imgSize, 876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize winSize, 886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float *velocityX, 896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float *velocityY, int velStep ) 906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Loops indexes */ 926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, j, k; 936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Gaussian separable kernels */ 956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float GaussX[16]; 966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float GaussY[16]; 976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float *KerX; 986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float *KerY; 996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Buffers for Sobel calculations */ 1016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float *MemX[2]; 1026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float *MemY[2]; 1036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float ConvX, ConvY; 1056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float GradX, GradY, GradT; 1066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int winWidth = winSize.width; 1086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int winHeight = winSize.height; 1096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int imageWidth = imgSize.width; 1116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int imageHeight = imgSize.height; 1126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int HorRadius = (winWidth - 1) >> 1; 1146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int VerRadius = (winHeight - 1) >> 1; 1156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int PixelLine; 1176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int ConvLine; 1186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int BufferAddress; 1206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int BufferHeight = 0; 1226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int BufferWidth; 1236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int BufferSize; 1246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* buffers derivatives product */ 1266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvDerProduct *II; 1276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* buffers for gaussian horisontal convolution */ 1296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvDerProduct *WII; 1306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* variables for storing number of first pixel of image line */ 1326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int Line1; 1336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int Line2; 1346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int Line3; 1356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* we must have 2*2 linear system coeffs 1376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn | A1B2 B1 | {u} {C1} {0} 1386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn | | { } + { } = { } 1396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn | A2 A1B2 | {v} {C2} {0} 1406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn */ 1416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float A1B2, A2, B1, C1, C2; 1426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int pixNumber; 1446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* auxiliary */ 1466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int NoMem = 0; 1476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn velStep /= sizeof(velocityX[0]); 1496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Checking bad arguments */ 1516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( imgA == NULL ) 1526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_NULLPTR_ERR; 1536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( imgB == NULL ) 1546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_NULLPTR_ERR; 1556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( imageHeight < winHeight ) 1576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_BADSIZE_ERR; 1586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( imageWidth < winWidth ) 1596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_BADSIZE_ERR; 1606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( winHeight >= 16 ) 1626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_BADSIZE_ERR; 1636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( winWidth >= 16 ) 1646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_BADSIZE_ERR; 1656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !(winHeight & 1) ) 1676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_BADSIZE_ERR; 1686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !(winWidth & 1) ) 1696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_BADSIZE_ERR; 1706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn BufferHeight = winHeight; 1726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn BufferWidth = imageWidth; 1736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /****************************************************************************************/ 1756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Computing Gaussian coeffs */ 1766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /****************************************************************************************/ 1776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GaussX[0] = 1; 1786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GaussY[0] = 1; 1796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 1; i < winWidth; i++ ) 1806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GaussX[i] = 1; 1826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = i - 1; j > 0; j-- ) 1836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GaussX[j] += GaussX[j - 1]; 1856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 1; i < winHeight; i++ ) 1886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GaussY[i] = 1; 1906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = i - 1; j > 0; j-- ) 1916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GaussY[j] += GaussY[j - 1]; 1936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn KerX = &GaussX[HorRadius]; 1966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn KerY = &GaussY[VerRadius]; 1976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /****************************************************************************************/ 1996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Allocating memory for all buffers */ 2006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /****************************************************************************************/ 2016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( k = 0; k < 2; k++ ) 2026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn MemX[k] = (float *) cvAlloc( (imgSize.height) * sizeof( float )); 2046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( MemX[k] == NULL ) 2066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn NoMem = 1; 2076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn MemY[k] = (float *) cvAlloc( (imgSize.width) * sizeof( float )); 2086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( MemY[k] == NULL ) 2106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn NoMem = 1; 2116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn BufferSize = BufferHeight * BufferWidth; 2146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn II = (icvDerProduct *) cvAlloc( BufferSize * sizeof( icvDerProduct )); 2166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn WII = (icvDerProduct *) cvAlloc( BufferSize * sizeof( icvDerProduct )); 2176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (II == NULL) || (WII == NULL) ) 2206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn NoMem = 1; 2216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( NoMem ) 2236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( k = 0; k < 2; k++ ) 2256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( MemX[k] ) 2276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &MemX[k] ); 2286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( MemY[k] ) 2306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &MemY[k] ); 2316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( II ) 2336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &II ); 2346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( WII ) 2356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &WII ); 2366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_OUTOFMEM_ERR; 2386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /****************************************************************************************/ 2416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Calculate first line of memX and memY */ 2426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /****************************************************************************************/ 2436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn MemY[0][0] = MemY[1][0] = CONV( imgA[0], imgA[0], imgA[1] ); 2446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn MemX[0][0] = MemX[1][0] = CONV( imgA[0], imgA[0], imgA[imgStep] ); 2456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 1; j < imageWidth - 1; j++ ) 2476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn MemY[0][j] = MemY[1][j] = CONV( imgA[j - 1], imgA[j], imgA[j + 1] ); 2496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pixNumber = imgStep; 2526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 1; i < imageHeight - 1; i++ ) 2536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn MemX[0][i] = MemX[1][i] = CONV( imgA[pixNumber - imgStep], 2556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn imgA[pixNumber], imgA[pixNumber + imgStep] ); 2566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pixNumber += imgStep; 2576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn MemY[0][imageWidth - 1] = 2606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn MemY[1][imageWidth - 1] = CONV( imgA[imageWidth - 2], 2616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn imgA[imageWidth - 1], imgA[imageWidth - 1] ); 2626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn MemX[0][imageHeight - 1] = 2646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn MemX[1][imageHeight - 1] = CONV( imgA[pixNumber - imgStep], 2656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn imgA[pixNumber], imgA[pixNumber] ); 2666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /****************************************************************************************/ 2696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* begin scan image, calc derivatives and solve system */ 2706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /****************************************************************************************/ 2716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn PixelLine = -VerRadius; 2736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ConvLine = 0; 2746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn BufferAddress = -BufferWidth; 2756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn while( PixelLine < imageHeight ) 2776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( ConvLine < imageHeight ) 2796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /*Here we calculate derivatives for line of image */ 2816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int address; 2826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn i = ConvLine; 2846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int L1 = i - 1; 2856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int L2 = i; 2866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int L3 = i + 1; 2876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int memYline = L3 & 1; 2896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( L1 < 0 ) 2916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn L1 = 0; 2926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( L3 >= imageHeight ) 2936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn L3 = imageHeight - 1; 2946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn BufferAddress += BufferWidth; 2966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn BufferAddress -= ((BufferAddress >= BufferSize) ? 0xffffffff : 0) & BufferSize; 2976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn address = BufferAddress; 2996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Line1 = L1 * imgStep; 3016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Line2 = L2 * imgStep; 3026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Line3 = L3 * imgStep; 3036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Process first pixel */ 3056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ConvX = CONV( imgA[Line1 + 1], imgA[Line2 + 1], imgA[Line3 + 1] ); 3066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ConvY = CONV( imgA[Line3], imgA[Line3], imgA[Line3 + 1] ); 3076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GradY = ConvY - MemY[memYline][0]; 3096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GradX = ConvX - MemX[1][L2]; 3106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn MemY[memYline][0] = ConvY; 3126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn MemX[1][L2] = ConvX; 3136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GradT = (float) (imgB[Line2] - imgA[Line2]); 3156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn II[address].xx = GradX * GradX; 3176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn II[address].xy = GradX * GradY; 3186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn II[address].yy = GradY * GradY; 3196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn II[address].xt = GradX * GradT; 3206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn II[address].yt = GradY * GradT; 3216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn address++; 3226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Process middle of line */ 3236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 1; j < imageWidth - 1; j++ ) 3246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ConvX = CONV( imgA[Line1 + j + 1], imgA[Line2 + j + 1], imgA[Line3 + j + 1] ); 3266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ConvY = CONV( imgA[Line3 + j - 1], imgA[Line3 + j], imgA[Line3 + j + 1] ); 3276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GradY = ConvY - MemY[memYline][j]; 3296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GradX = ConvX - MemX[(j - 1) & 1][L2]; 3306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn MemY[memYline][j] = ConvY; 3326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn MemX[(j - 1) & 1][L2] = ConvX; 3336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GradT = (float) (imgB[Line2 + j] - imgA[Line2 + j]); 3356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn II[address].xx = GradX * GradX; 3376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn II[address].xy = GradX * GradY; 3386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn II[address].yy = GradY * GradY; 3396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn II[address].xt = GradX * GradT; 3406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn II[address].yt = GradY * GradT; 3416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn address++; 3436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Process last pixel of line */ 3456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ConvX = CONV( imgA[Line1 + imageWidth - 1], imgA[Line2 + imageWidth - 1], 3466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn imgA[Line3 + imageWidth - 1] ); 3476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ConvY = CONV( imgA[Line3 + imageWidth - 2], imgA[Line3 + imageWidth - 1], 3496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn imgA[Line3 + imageWidth - 1] ); 3506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GradY = ConvY - MemY[memYline][imageWidth - 1]; 3536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GradX = ConvX - MemX[(imageWidth - 2) & 1][L2]; 3546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn MemY[memYline][imageWidth - 1] = ConvY; 3566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GradT = (float) (imgB[Line2 + imageWidth - 1] - imgA[Line2 + imageWidth - 1]); 3586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn II[address].xx = GradX * GradX; 3606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn II[address].xy = GradX * GradY; 3616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn II[address].yy = GradY * GradY; 3626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn II[address].xt = GradX * GradT; 3636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn II[address].yt = GradY * GradT; 3646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn address++; 3656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* End of derivatives for line */ 3676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /****************************************************************************************/ 3696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* ---------Calculating horizontal convolution of processed line----------------------- */ 3706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /****************************************************************************************/ 3716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn address -= BufferWidth; 3726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* process first HorRadius pixels */ 3736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < HorRadius; j++ ) 3746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int jj; 3766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn WII[address].xx = 0; 3786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn WII[address].xy = 0; 3796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn WII[address].yy = 0; 3806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn WII[address].xt = 0; 3816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn WII[address].yt = 0; 3826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( jj = -j; jj <= HorRadius; jj++ ) 3846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float Ker = KerX[jj]; 3866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn WII[address].xx += II[address + jj].xx * Ker; 3886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn WII[address].xy += II[address + jj].xy * Ker; 3896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn WII[address].yy += II[address + jj].yy * Ker; 3906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn WII[address].xt += II[address + jj].xt * Ker; 3916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn WII[address].yt += II[address + jj].yt * Ker; 3926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn address++; 3946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* process inner part of line */ 3966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = HorRadius; j < imageWidth - HorRadius; j++ ) 3976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int jj; 3996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float Ker0 = KerX[0]; 4006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn WII[address].xx = 0; 4026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn WII[address].xy = 0; 4036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn WII[address].yy = 0; 4046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn WII[address].xt = 0; 4056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn WII[address].yt = 0; 4066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( jj = 1; jj <= HorRadius; jj++ ) 4086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float Ker = KerX[jj]; 4106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn WII[address].xx += (II[address - jj].xx + II[address + jj].xx) * Ker; 4126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn WII[address].xy += (II[address - jj].xy + II[address + jj].xy) * Ker; 4136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn WII[address].yy += (II[address - jj].yy + II[address + jj].yy) * Ker; 4146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn WII[address].xt += (II[address - jj].xt + II[address + jj].xt) * Ker; 4156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn WII[address].yt += (II[address - jj].yt + II[address + jj].yt) * Ker; 4166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn WII[address].xx += II[address].xx * Ker0; 4186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn WII[address].xy += II[address].xy * Ker0; 4196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn WII[address].yy += II[address].yy * Ker0; 4206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn WII[address].xt += II[address].xt * Ker0; 4216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn WII[address].yt += II[address].yt * Ker0; 4226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn address++; 4246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* process right side */ 4266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = imageWidth - HorRadius; j < imageWidth; j++ ) 4276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int jj; 4296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn WII[address].xx = 0; 4316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn WII[address].xy = 0; 4326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn WII[address].yy = 0; 4336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn WII[address].xt = 0; 4346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn WII[address].yt = 0; 4356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( jj = -HorRadius; jj < imageWidth - j; jj++ ) 4376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float Ker = KerX[jj]; 4396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn WII[address].xx += II[address + jj].xx * Ker; 4416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn WII[address].xy += II[address + jj].xy * Ker; 4426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn WII[address].yy += II[address + jj].yy * Ker; 4436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn WII[address].xt += II[address + jj].xt * Ker; 4446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn WII[address].yt += II[address + jj].yt * Ker; 4456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn address++; 4476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /****************************************************************************************/ 4516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Calculating velocity line */ 4526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /****************************************************************************************/ 4536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( PixelLine >= 0 ) 4546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int USpace; 4566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int BSpace; 4576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int address; 4586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( PixelLine < VerRadius ) 4606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn USpace = PixelLine; 4616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 4626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn USpace = VerRadius; 4636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( PixelLine >= imageHeight - VerRadius ) 4656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn BSpace = imageHeight - PixelLine - 1; 4666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 4676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn BSpace = VerRadius; 4686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn address = ((PixelLine - USpace) % BufferHeight) * BufferWidth; 4706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < imageWidth; j++ ) 4716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int addr = address; 4736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn A1B2 = 0; 4756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn A2 = 0; 4766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn B1 = 0; 4776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn C1 = 0; 4786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn C2 = 0; 4796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = -USpace; i <= BSpace; i++ ) 4816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn A2 += WII[addr + j].xx * KerY[i]; 4836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn A1B2 += WII[addr + j].xy * KerY[i]; 4846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn B1 += WII[addr + j].yy * KerY[i]; 4856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn C2 += WII[addr + j].xt * KerY[i]; 4866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn C1 += WII[addr + j].yt * KerY[i]; 4876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn addr += BufferWidth; 4896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn addr -= ((addr >= BufferSize) ? 0xffffffff : 0) & BufferSize; 4906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /****************************************************************************************\ 4926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn * Solve Linear System * 4936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn \****************************************************************************************/ 4946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float delta = (A1B2 * A1B2 - A2 * B1); 4966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( delta ) 4986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* system is not singular - solving by Kramer method */ 5006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float deltaX; 5016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float deltaY; 5026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float Idelta = 8 / delta; 5036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn deltaX = -(C1 * A1B2 - C2 * B1); 5056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn deltaY = -(A1B2 * C2 - A2 * C1); 5066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn velocityX[j] = deltaX * Idelta; 5086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn velocityY[j] = deltaY * Idelta; 5096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 5116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* singular system - find optical flow in gradient direction */ 5136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float Norm = (A1B2 + A2) * (A1B2 + A2) + (B1 + A1B2) * (B1 + A1B2); 5146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( Norm ) 5166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float IGradNorm = 8 / Norm; 5186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float temp = -(C1 + C2) * IGradNorm; 5196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn velocityX[j] = (A1B2 + A2) * temp; 5216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn velocityY[j] = (B1 + A1B2) * temp; 5226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 5256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn velocityX[j] = 0; 5276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn velocityY[j] = 0; 5286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /****************************************************************************************\ 5326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn * End of Solving Linear System * 5336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn \****************************************************************************************/ 5346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } /*for */ 5356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn velocityX += velStep; 5366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn velocityY += velStep; 5376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } /*for */ 5386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn PixelLine++; 5396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ConvLine++; 5406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Free memory */ 5436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( k = 0; k < 2; k++ ) 5446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &MemX[k] ); 5466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &MemY[k] ); 5476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &II ); 5496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &WII ); 5506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_OK; 5526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} /*icvCalcOpticalFlowLK_8u32fR*/ 5536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*F/////////////////////////////////////////////////////////////////////////////////////// 5566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Name: cvCalcOpticalFlowLK 5576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Purpose: Optical flow implementation 5586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Context: 5596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Parameters: 5606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// srcA, srcB - source image 5616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// velx, vely - destination image 5626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Returns: 5636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 5646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Notes: 5656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//F*/ 5666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL void 5676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvCalcOpticalFlowLK( const void* srcarrA, const void* srcarrB, CvSize winSize, 5686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn void* velarrx, void* velarry ) 5696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 5706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvCalcOpticalFlowLK" ); 5716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 5736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat stubA, *srcA = (CvMat*)srcarrA; 5756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat stubB, *srcB = (CvMat*)srcarrB; 5766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat stubx, *velx = (CvMat*)velarrx; 5776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat stuby, *vely = (CvMat*)velarry; 5786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( srcA = cvGetMat( srcA, &stubA )); 5806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( srcB = cvGetMat( srcB, &stubB )); 5816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( velx = cvGetMat( velx, &stubx )); 5836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( vely = cvGetMat( vely, &stuby )); 5846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_ARE_TYPES_EQ( srcA, srcB )) 5866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedFormats, "Source images have different formats" ); 5876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_ARE_TYPES_EQ( velx, vely )) 5896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedFormats, "Destination images have different formats" ); 5906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_ARE_SIZES_EQ( srcA, srcB ) || 5926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn !CV_ARE_SIZES_EQ( velx, vely ) || 5936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn !CV_ARE_SIZES_EQ( srcA, velx )) 5946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "" ); 5956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_MAT_TYPE( srcA->type ) != CV_8UC1 || 5976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_MAT_TYPE( velx->type ) != CV_32FC1 ) 5986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "Source images must have 8uC1 type and " 5996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "destination images must have 32fC1 type" ); 6006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( srcA->step != srcB->step || velx->step != vely->step ) 6026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_BadStep, "source and destination images have different step" ); 6036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn IPPI_CALL( icvCalcOpticalFlowLK_8u32fR( (uchar*)srcA->data.ptr, (uchar*)srcB->data.ptr, 6056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn srcA->step, cvGetMatSize( srcA ), winSize, 6066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn velx->data.fl, vely->data.fl, velx->step )); 6076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 6096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 6106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* End of file. */ 612