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 Renn#define CONV( A, B, C) ( (float)( A + (B<<1) + C ) ) 446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renntypedef struct 466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float xx; 486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float xy; 496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float yy; 506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float xt; 516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float yt; 526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float alpha; /* alpha = 1 / ( 1/lambda + xx + yy ) */ 536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennicvDerProductEx; 556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*F/////////////////////////////////////////////////////////////////////////////////////// 576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Name: icvCalcOpticalFlowHS_8u32fR (Horn & Schunck method ) 586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Purpose: calculate Optical flow for 2 images using Horn & Schunck algorithm 596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Context: 606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Parameters: 616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// imgA - pointer to first frame ROI 626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// imgB - pointer to second frame ROI 636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// imgStep - width of single row of source images in bytes 646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// imgSize - size of the source image ROI 656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// usePrevious - use previous (input) velocity field. 666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// velocityX - pointer to horizontal and 676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// velocityY - vertical components of optical flow ROI 686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// velStep - width of single row of velocity frames in bytes 696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// lambda - Lagrangian multiplier 706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// criteria - criteria of termination processmaximum number of iterations 716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Returns: CV_OK - all ok 736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// CV_OUTOFMEM_ERR - insufficient memory for function work 746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// CV_NULLPTR_ERR - if one of input pointers is NULL 756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// CV_BADSIZE_ERR - wrong input sizes interrelation 766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Notes: 1.Optical flow to be computed for every pixel in ROI 786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 2.For calculating spatial derivatives we use 3x3 Sobel operator. 796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 3.We use the following border mode. 806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// The last row or column is replicated for the border 816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// ( IPL_BORDER_REPLICATE in IPL ). 826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//F*/ 856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic CvStatus CV_STDCALL 866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennicvCalcOpticalFlowHS_8u32fR( uchar* imgA, 876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar* imgB, 886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int imgStep, 896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize imgSize, 906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int usePrevious, 916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float* velocityX, 926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float* velocityY, 936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int velStep, 946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float lambda, 956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvTermCriteria criteria ) 966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Loops indexes */ 986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, j, k, address; 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 imageWidth = imgSize.width; 1086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int imageHeight = imgSize.height; 1096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int ConvLine; 1116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int LastLine; 1126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int BufferSize; 1146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float Ilambda = 1 / lambda; 1166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int iter = 0; 1176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int Stop; 1186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* buffers derivatives product */ 1206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvDerProductEx *II; 1216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float *VelBufX[2]; 1236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float *VelBufY[2]; 1246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* variables for storing number of first pixel of image line */ 1266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int Line1; 1276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int Line2; 1286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int Line3; 1296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int pixNumber; 1316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* auxiliary */ 1336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int NoMem = 0; 1346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Checking bad arguments */ 1366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( imgA == NULL ) 1376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_NULLPTR_ERR; 1386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( imgB == NULL ) 1396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_NULLPTR_ERR; 1406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( imgSize.width <= 0 ) 1426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_BADSIZE_ERR; 1436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( imgSize.height <= 0 ) 1446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_BADSIZE_ERR; 1456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( imgSize.width > imgStep ) 1466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_BADSIZE_ERR; 1476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (velStep & 3) != 0 ) 1496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_BADSIZE_ERR; 1506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn velStep /= 4; 1526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /****************************************************************************************/ 1546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Allocating memory for all buffers */ 1556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /****************************************************************************************/ 1566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( k = 0; k < 2; k++ ) 1576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn MemX[k] = (float *) cvAlloc( (imgSize.height) * sizeof( float )); 1596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( MemX[k] == NULL ) 1616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn NoMem = 1; 1626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn MemY[k] = (float *) cvAlloc( (imgSize.width) * sizeof( float )); 1636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( MemY[k] == NULL ) 1656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn NoMem = 1; 1666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn VelBufX[k] = (float *) cvAlloc( imageWidth * sizeof( float )); 1686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( VelBufX[k] == NULL ) 1706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn NoMem = 1; 1716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn VelBufY[k] = (float *) cvAlloc( imageWidth * sizeof( float )); 1726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( VelBufY[k] == NULL ) 1746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn NoMem = 1; 1756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn BufferSize = imageHeight * imageWidth; 1786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn II = (icvDerProductEx *) cvAlloc( BufferSize * sizeof( icvDerProductEx )); 1806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (II == NULL) ) 1816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn NoMem = 1; 1826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( NoMem ) 1846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( k = 0; k < 2; k++ ) 1866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( MemX[k] ) 1886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &MemX[k] ); 1896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( MemY[k] ) 1916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &MemY[k] ); 1926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( VelBufX[k] ) 1946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &VelBufX[k] ); 1956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( VelBufY[k] ) 1976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &VelBufY[k] ); 1986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( II ) 2006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &II ); 2016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_OUTOFMEM_ERR; 2026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/****************************************************************************************\ 2046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn* Calculate first line of memX and memY * 2056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn\****************************************************************************************/ 2066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn MemY[0][0] = MemY[1][0] = CONV( imgA[0], imgA[0], imgA[1] ); 2076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn MemX[0][0] = MemX[1][0] = CONV( imgA[0], imgA[0], imgA[imgStep] ); 2086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 1; j < imageWidth - 1; j++ ) 2106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn MemY[0][j] = MemY[1][j] = CONV( imgA[j - 1], imgA[j], imgA[j + 1] ); 2126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pixNumber = imgStep; 2156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 1; i < imageHeight - 1; i++ ) 2166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn MemX[0][i] = MemX[1][i] = CONV( imgA[pixNumber - imgStep], 2186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn imgA[pixNumber], imgA[pixNumber + imgStep] ); 2196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pixNumber += imgStep; 2206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn MemY[0][imageWidth - 1] = 2236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn MemY[1][imageWidth - 1] = CONV( imgA[imageWidth - 2], 2246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn imgA[imageWidth - 1], imgA[imageWidth - 1] ); 2256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn MemX[0][imageHeight - 1] = 2276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn MemX[1][imageHeight - 1] = CONV( imgA[pixNumber - imgStep], 2286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn imgA[pixNumber], imgA[pixNumber] ); 2296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/****************************************************************************************\ 2326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn* begin scan image, calc derivatives * 2336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn\****************************************************************************************/ 2346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ConvLine = 0; 2366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Line2 = -imgStep; 2376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn address = 0; 2386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn LastLine = imgStep * (imageHeight - 1); 2396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn while( ConvLine < imageHeight ) 2406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /*Here we calculate derivatives for line of image */ 2426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int memYline = (ConvLine + 1) & 1; 2436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Line2 += imgStep; 2456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Line1 = Line2 - ((Line2 == 0) ? 0 : imgStep); 2466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Line3 = Line2 + ((Line2 == LastLine) ? 0 : imgStep); 2476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Process first pixel */ 2496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ConvX = CONV( imgA[Line1 + 1], imgA[Line2 + 1], imgA[Line3 + 1] ); 2506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ConvY = CONV( imgA[Line3], imgA[Line3], imgA[Line3 + 1] ); 2516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GradY = (ConvY - MemY[memYline][0]) * 0.125f; 2536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GradX = (ConvX - MemX[1][ConvLine]) * 0.125f; 2546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn MemY[memYline][0] = ConvY; 2566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn MemX[1][ConvLine] = ConvX; 2576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GradT = (float) (imgB[Line2] - imgA[Line2]); 2596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn II[address].xx = GradX * GradX; 2616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn II[address].xy = GradX * GradY; 2626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn II[address].yy = GradY * GradY; 2636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn II[address].xt = GradX * GradT; 2646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn II[address].yt = GradY * GradT; 2656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn II[address].alpha = 1 / (Ilambda + II[address].xx + II[address].yy); 2676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn address++; 2686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Process middle of line */ 2706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 1; j < imageWidth - 1; j++ ) 2716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ConvX = CONV( imgA[Line1 + j + 1], imgA[Line2 + j + 1], imgA[Line3 + j + 1] ); 2736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ConvY = CONV( imgA[Line3 + j - 1], imgA[Line3 + j], imgA[Line3 + j + 1] ); 2746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GradY = (ConvY - MemY[memYline][j]) * 0.125f; 2766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GradX = (ConvX - MemX[(j - 1) & 1][ConvLine]) * 0.125f; 2776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn MemY[memYline][j] = ConvY; 2796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn MemX[(j - 1) & 1][ConvLine] = ConvX; 2806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GradT = (float) (imgB[Line2 + j] - imgA[Line2 + j]); 2826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn II[address].xx = GradX * GradX; 2846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn II[address].xy = GradX * GradY; 2856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn II[address].yy = GradY * GradY; 2866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn II[address].xt = GradX * GradT; 2876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn II[address].yt = GradY * GradT; 2886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn II[address].alpha = 1 / (Ilambda + II[address].xx + II[address].yy); 2906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn address++; 2916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Process last pixel of line */ 2936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ConvX = CONV( imgA[Line1 + imageWidth - 1], imgA[Line2 + imageWidth - 1], 2946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn imgA[Line3 + imageWidth - 1] ); 2956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ConvY = CONV( imgA[Line3 + imageWidth - 2], imgA[Line3 + imageWidth - 1], 2976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn imgA[Line3 + imageWidth - 1] ); 2986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GradY = (ConvY - MemY[memYline][imageWidth - 1]) * 0.125f; 3016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GradX = (ConvX - MemX[(imageWidth - 2) & 1][ConvLine]) * 0.125f; 3026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn MemY[memYline][imageWidth - 1] = ConvY; 3046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn GradT = (float) (imgB[Line2 + imageWidth - 1] - imgA[Line2 + imageWidth - 1]); 3066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn II[address].xx = GradX * GradX; 3086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn II[address].xy = GradX * GradY; 3096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn II[address].yy = GradY * GradY; 3106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn II[address].xt = GradX * GradT; 3116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn II[address].yt = GradY * GradT; 3126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn II[address].alpha = 1 / (Ilambda + II[address].xx + II[address].yy); 3146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn address++; 3156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ConvLine++; 3176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/****************************************************************************************\ 3196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn* Prepare initial approximation * 3206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn\****************************************************************************************/ 3216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !usePrevious ) 3226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float *vx = velocityX; 3246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float *vy = velocityY; 3256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < imageHeight; i++ ) 3276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memset( vx, 0, imageWidth * sizeof( float )); 3296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memset( vy, 0, imageWidth * sizeof( float )); 3306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn vx += velStep; 3326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn vy += velStep; 3336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/****************************************************************************************\ 3366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn* Perform iterations * 3376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn\****************************************************************************************/ 3386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn iter = 0; 3396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Stop = 0; 3406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn LastLine = velStep * (imageHeight - 1); 3416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn while( !Stop ) 3426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float Eps = 0; 3446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn address = 0; 3456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn iter++; 3476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/****************************************************************************************\ 3486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn* begin scan velocity and update it * 3496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn\****************************************************************************************/ 3506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Line2 = -velStep; 3516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < imageHeight; i++ ) 3526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Here average velocity */ 3546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float averageX; 3566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float averageY; 3576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float tmp; 3586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Line2 += velStep; 3606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Line1 = Line2 - ((Line2 == 0) ? 0 : velStep); 3616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Line3 = Line2 + ((Line2 == LastLine) ? 0 : velStep); 3626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Process first pixel */ 3636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn averageX = (velocityX[Line2] + 3646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn velocityX[Line2 + 1] + velocityX[Line1] + velocityX[Line3]) / 4; 3656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn averageY = (velocityY[Line2] + 3676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn velocityY[Line2 + 1] + velocityY[Line1] + velocityY[Line3]) / 4; 3686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn VelBufX[i & 1][0] = averageX - 3706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (II[address].xx * averageX + 3716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn II[address].xy * averageY + II[address].xt) * II[address].alpha; 3726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn VelBufY[i & 1][0] = averageY - 3746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (II[address].xy * averageX + 3756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn II[address].yy * averageY + II[address].yt) * II[address].alpha; 3766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* update Epsilon */ 3786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( criteria.type & CV_TERMCRIT_EPS ) 3796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tmp = (float)fabs(velocityX[Line2] - VelBufX[i & 1][0]); 3816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Eps = MAX( tmp, Eps ); 3826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tmp = (float)fabs(velocityY[Line2] - VelBufY[i & 1][0]); 3836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Eps = MAX( tmp, Eps ); 3846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn address++; 3866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Process middle of line */ 3876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 1; j < imageWidth - 1; j++ ) 3886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn averageX = (velocityX[Line2 + j - 1] + 3906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn velocityX[Line2 + j + 1] + 3916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn velocityX[Line1 + j] + velocityX[Line3 + j]) / 4; 3926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn averageY = (velocityY[Line2 + j - 1] + 3936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn velocityY[Line2 + j + 1] + 3946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn velocityY[Line1 + j] + velocityY[Line3 + j]) / 4; 3956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn VelBufX[i & 1][j] = averageX - 3976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (II[address].xx * averageX + 3986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn II[address].xy * averageY + II[address].xt) * II[address].alpha; 3996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn VelBufY[i & 1][j] = averageY - 4016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (II[address].xy * averageX + 4026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn II[address].yy * averageY + II[address].yt) * II[address].alpha; 4036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* update Epsilon */ 4046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( criteria.type & CV_TERMCRIT_EPS ) 4056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tmp = (float)fabs(velocityX[Line2 + j] - VelBufX[i & 1][j]); 4076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Eps = MAX( tmp, Eps ); 4086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tmp = (float)fabs(velocityY[Line2 + j] - VelBufY[i & 1][j]); 4096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Eps = MAX( tmp, Eps ); 4106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn address++; 4126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Process last pixel of line */ 4146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn averageX = (velocityX[Line2 + imageWidth - 2] + 4156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn velocityX[Line2 + imageWidth - 1] + 4166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn velocityX[Line1 + imageWidth - 1] + 4176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn velocityX[Line3 + imageWidth - 1]) / 4; 4186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn averageY = (velocityY[Line2 + imageWidth - 2] + 4206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn velocityY[Line2 + imageWidth - 1] + 4216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn velocityY[Line1 + imageWidth - 1] + 4226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn velocityY[Line3 + imageWidth - 1]) / 4; 4236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn VelBufX[i & 1][imageWidth - 1] = averageX - 4266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (II[address].xx * averageX + 4276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn II[address].xy * averageY + II[address].xt) * II[address].alpha; 4286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn VelBufY[i & 1][imageWidth - 1] = averageY - 4306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (II[address].xy * averageX + 4316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn II[address].yy * averageY + II[address].yt) * II[address].alpha; 4326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* update Epsilon */ 4346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( criteria.type & CV_TERMCRIT_EPS ) 4356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tmp = (float)fabs(velocityX[Line2 + imageWidth - 1] - 4376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn VelBufX[i & 1][imageWidth - 1]); 4386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Eps = MAX( tmp, Eps ); 4396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tmp = (float)fabs(velocityY[Line2 + imageWidth - 1] - 4406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn VelBufY[i & 1][imageWidth - 1]); 4416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Eps = MAX( tmp, Eps ); 4426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn address++; 4446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* store new velocity from old buffer to velocity frame */ 4466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( i > 0 ) 4476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memcpy( &velocityX[Line1], VelBufX[(i - 1) & 1], imageWidth * sizeof( float )); 4496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memcpy( &velocityY[Line1], VelBufY[(i - 1) & 1], imageWidth * sizeof( float )); 4506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } /*for */ 4526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* store new velocity from old buffer to velocity frame */ 4536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memcpy( &velocityX[imageWidth * (imageHeight - 1)], 4546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn VelBufX[(imageHeight - 1) & 1], imageWidth * sizeof( float )); 4556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memcpy( &velocityY[imageWidth * (imageHeight - 1)], 4576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn VelBufY[(imageHeight - 1) & 1], imageWidth * sizeof( float )); 4586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (criteria.type & CV_TERMCRIT_ITER) && (iter == criteria.max_iter) ) 4606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Stop = 1; 4616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (criteria.type & CV_TERMCRIT_EPS) && (Eps < criteria.epsilon) ) 4626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Stop = 1; 4636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Free memory */ 4656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( k = 0; k < 2; k++ ) 4666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &MemX[k] ); 4686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &MemY[k] ); 4696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &VelBufX[k] ); 4706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &VelBufY[k] ); 4716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &II ); 4736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_OK; 4756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} /*icvCalcOpticalFlowHS_8u32fR*/ 4766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*F/////////////////////////////////////////////////////////////////////////////////////// 4796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Name: cvCalcOpticalFlowHS 4806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Purpose: Optical flow implementation 4816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Context: 4826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Parameters: 4836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// srcA, srcB - source image 4846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// velx, vely - destination image 4856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Returns: 4866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 4876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Notes: 4886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//F*/ 4896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL void 4906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvCalcOpticalFlowHS( const void* srcarrA, const void* srcarrB, int usePrevious, 4916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn void* velarrx, void* velarry, 4926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double lambda, CvTermCriteria criteria ) 4936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 4946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvCalcOpticalFlowHS" ); 4956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 4976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat stubA, *srcA = (CvMat*)srcarrA; 4996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat stubB, *srcB = (CvMat*)srcarrB; 5006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat stubx, *velx = (CvMat*)velarrx; 5016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat stuby, *vely = (CvMat*)velarry; 5026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( srcA = cvGetMat( srcA, &stubA )); 5046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( srcB = cvGetMat( srcB, &stubB )); 5056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( velx = cvGetMat( velx, &stubx )); 5076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( vely = cvGetMat( vely, &stuby )); 5086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_ARE_TYPES_EQ( srcA, srcB )) 5106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedFormats, "Source images have different formats" ); 5116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_ARE_TYPES_EQ( velx, vely )) 5136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedFormats, "Destination images have different formats" ); 5146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_ARE_SIZES_EQ( srcA, srcB ) || 5166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn !CV_ARE_SIZES_EQ( velx, vely ) || 5176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn !CV_ARE_SIZES_EQ( srcA, velx )) 5186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "" ); 5196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_MAT_TYPE( srcA->type ) != CV_8UC1 || 5216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_MAT_TYPE( velx->type ) != CV_32FC1 ) 5226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "Source images must have 8uC1 type and " 5236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "destination images must have 32fC1 type" ); 5246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( srcA->step != srcB->step || velx->step != vely->step ) 5266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_BadStep, "source and destination images have different step" ); 5276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn IPPI_CALL( icvCalcOpticalFlowHS_8u32fR( (uchar*)srcA->data.ptr, (uchar*)srcB->data.ptr, 5296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn srcA->step, cvGetMatSize( srcA ), usePrevious, 5306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn velx->data.fl, vely->data.fl, 5316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn velx->step, (float)lambda, criteria )); 5326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 5336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 5346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* End of file. */ 536