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 426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#include "_cv.h" 436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* 456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Finds L1 norm between two blocks. 466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn*/ 476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic int 486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennicvCmpBlocksL1_8u_C1( const uchar * vec1, const uchar * vec2, int len ) 496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, sum = 0; 516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i <= len - 4; i += 4 ) 536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int t0 = abs(vec1[i] - vec2[i]); 556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int t1 = abs(vec1[i + 1] - vec2[i + 1]); 566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int t2 = abs(vec1[i + 2] - vec2[i + 2]); 576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int t3 = abs(vec1[i + 3] - vec2[i + 3]); 586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sum += t0 + t1 + t2 + t3; 606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( ; i < len; i++ ) 636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int t0 = abs(vec1[i] - vec2[i]); 656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sum += t0; 666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return sum; 696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic void 736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennicvCopyBM_8u_C1R( const uchar* src, int src_step, 746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar* dst, int dst_step, CvSize size ) 756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( ; size.height--; src += src_step, dst += dst_step ) 776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memcpy( dst, src, size.width ); 786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*F/////////////////////////////////////////////////////////////////////////////////////// 826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Name: icvCalcOpticalFlowBM_8u32fR 836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Purpose: calculate Optical flow for 2 images using block matching algorithm 846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Context: 856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Parameters: 866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// imgA, // pointer to first frame ROI 876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// imgB, // pointer to second frame ROI 886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// imgStep, // full width of input images in bytes 896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// imgSize, // size of the image 906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// blockSize, // size of basic blocks which are compared 916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// shiftSize, // coordinates increments. 926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// maxRange, // size of the scanned neighborhood. 936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// usePrevious, // flag of using previous velocity field 946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// velocityX, // pointer to ROI of horizontal and 956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// velocityY, // vertical components of optical flow 966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// velStep); // full width of velocity frames in bytes 976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Returns: CV_OK or error code 986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Notes: 996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//F*/ 1006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define SMALL_DIFF 2 1016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define BIG_DIFF 128 1026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic CvStatus CV_STDCALL 1046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennicvCalcOpticalFlowBM_8u32fR( uchar * imgA, uchar * imgB, 1056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int imgStep, CvSize imgSize, 1066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize blockSize, CvSize shiftSize, 1076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize maxRange, int usePrev, 1086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float *velocityX, float *velocityY, 1096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int velStep ) 1106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 1116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const float back = 1.f / (float) (1 << 16); 1126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* scanning scheme coordinates */ 1146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint *ss = 0; 1166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int ss_count = 0; 1176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int stand_accept_level = blockSize.height * blockSize.width * SMALL_DIFF; 1196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int stand_escape_level = blockSize.height * blockSize.width * BIG_DIFF; 1206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, j; 1226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int *int_velocityX = (int *) velocityX; 1246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int *int_velocityY = (int *) velocityY; 1256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* if image sizes can't be divided by block sizes then right blocks will */ 1276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* have not full width - BorderWidth */ 1286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* and bottom blocks will */ 1296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* have not full height - BorderHeight */ 1306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int BorderWidth; 1316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int BorderHeight; 1326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int CurrentWidth; 1346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int CurrentHeight; 1356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int NumberBlocksX; 1376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int NumberBlocksY; 1386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int Y1 = 0; 1406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int X1 = 0; 1416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int DownStep = blockSize.height * imgStep; 1436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar *blockA = 0; 1456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar *blockB = 0; 1466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar *blockZ = 0; 1476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int blSize = blockSize.width * blockSize.height; 1486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int bufferSize = cvAlign(blSize + 9,16); 1496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int cmpSize = cvAlign(blSize,4); 1506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int patch_ofs = blSize & -8; 1516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int64 patch_mask = (((int64) 1) << (blSize - patch_ofs * 8)) - 1; 1526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn velStep /= sizeof(velocityX[0]); 1546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( patch_ofs == blSize ) 1566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn patch_mask = (int64) - 1; 1576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/****************************************************************************************\ 1596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn* Checking bad arguments * 1606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn\****************************************************************************************/ 1616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( imgA == NULL ) 1626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_NULLPTR_ERR; 1636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( imgB == NULL ) 1646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_NULLPTR_ERR; 1656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/****************************************************************************************\ 1676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn* Allocate buffers * 1686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn\****************************************************************************************/ 1696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn blockA = (uchar *) cvAlloc( bufferSize * 3 ); 1706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !blockA ) 1716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_OUTOFMEM_ERR; 1726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn blockB = blockA + bufferSize; 1746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn blockZ = blockB + bufferSize; 1756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memset( blockZ, 0, bufferSize ); 1776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ss = (CvPoint *) cvAlloc( (2 * maxRange.width + 1) * (2 * maxRange.height + 1) * 1796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sizeof( CvPoint )); 1806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !ss ) 1816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &blockA ); 1836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_OUTOFMEM_ERR; 1846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/****************************************************************************************\ 1876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn* Calculate scanning scheme * 1886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn\****************************************************************************************/ 1896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int X_shift_count = maxRange.width / shiftSize.width; 1916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int Y_shift_count = maxRange.height / shiftSize.height; 1926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int min_count = MIN( X_shift_count, Y_shift_count ); 1936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* cycle by neighborhood rings */ 1956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* scanning scheme is 1966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn . 9 10 11 12 1986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn . 8 1 2 13 1996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn . 7 * 3 14 2006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn . 6 5 4 15 2016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20 19 18 17 16 2026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn */ 2036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < min_count; i++ ) 2056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* four cycles along sides */ 2076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int y = -(i + 1) * shiftSize.height; 2086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int x = -(i + 1) * shiftSize.width; 2096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* upper side */ 2116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = -i; j <= i + 1; j++, ss_count++ ) 2126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x += shiftSize.width; 2146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ss[ss_count].x = x; 2156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ss[ss_count].y = y; 2166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* right side */ 2196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = -i; j <= i + 1; j++, ss_count++ ) 2206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y += shiftSize.height; 2226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ss[ss_count].x = x; 2236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ss[ss_count].y = y; 2246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* bottom side */ 2276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = -i; j <= i + 1; j++, ss_count++ ) 2286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x -= shiftSize.width; 2306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ss[ss_count].x = x; 2316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ss[ss_count].y = y; 2326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* left side */ 2356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = -i; j <= i + 1; j++, ss_count++ ) 2366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y -= shiftSize.height; 2386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ss[ss_count].x = x; 2396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ss[ss_count].y = y; 2406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* the rest part */ 2446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( X_shift_count < Y_shift_count ) 2456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int xleft = -min_count * shiftSize.width; 2476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* cycle by neighbor rings */ 2496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = min_count; i < Y_shift_count; i++ ) 2506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* two cycles by x */ 2526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int y = -(i + 1) * shiftSize.height; 2536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int x = xleft; 2546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* upper side */ 2566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = -X_shift_count; j <= X_shift_count; j++, ss_count++ ) 2576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ss[ss_count].x = x; 2596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ss[ss_count].y = y; 2606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x += shiftSize.width; 2616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x = xleft; 2646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y = -y; 2656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* bottom side */ 2666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = -X_shift_count; j <= X_shift_count; j++, ss_count++ ) 2676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ss[ss_count].x = x; 2696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ss[ss_count].y = y; 2706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x += shiftSize.width; 2716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( X_shift_count > Y_shift_count ) 2756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int yupper = -min_count * shiftSize.height; 2776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* cycle by neighbor rings */ 2796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = min_count; i < X_shift_count; i++ ) 2806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* two cycles by y */ 2826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int x = -(i + 1) * shiftSize.width; 2836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int y = yupper; 2846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* left side */ 2866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = -Y_shift_count; j <= Y_shift_count; j++, ss_count++ ) 2876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ss[ss_count].x = x; 2896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ss[ss_count].y = y; 2906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y += shiftSize.height; 2916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y = yupper; 2946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x = -x; 2956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* right side */ 2966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = -Y_shift_count; j <= Y_shift_count; j++, ss_count++ ) 2976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ss[ss_count].x = x; 2996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ss[ss_count].y = y; 3006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y += shiftSize.height; 3016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/****************************************************************************************\ 3086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn* Calculate some neeeded variables * 3096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn\****************************************************************************************/ 3106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Calculate number of full blocks */ 3116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn NumberBlocksX = (int) imgSize.width / blockSize.width; 3126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn NumberBlocksY = (int) imgSize.height / blockSize.height; 3136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* add 1 if not full border blocks exist */ 3156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn BorderWidth = imgSize.width % blockSize.width; 3166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( BorderWidth ) 3176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn NumberBlocksX++; 3186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 3196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn BorderWidth = blockSize.width; 3206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn BorderHeight = imgSize.height % blockSize.height; 3226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( BorderHeight ) 3236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn NumberBlocksY++; 3246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 3256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn BorderHeight = blockSize.height; 3266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/****************************************************************************************\ 3286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn* Round input velocities integer searching area center position * 3296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn\****************************************************************************************/ 3306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( usePrev ) 3316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float *velxf = velocityX, *velyf = velocityY; 3336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int* velx = (int*)velocityX, *vely = (int*)velocityY; 3346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < NumberBlocksY; i++, velxf += velStep, velyf += velStep, 3366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn velx += velStep, vely += velStep ) 3376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < NumberBlocksX; j++ ) 3396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int vx = cvRound( velxf[j] ), vy = cvRound( velyf[j] ); 3416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn velx[j] = vx; vely[j] = vy; 3426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/****************************************************************************************\ 3466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn* Main loop * 3476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn\****************************************************************************************/ 3486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Y1 = 0; 3496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < NumberBlocksY; i++ ) 3506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* calculate height of current block */ 3526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CurrentHeight = (i == NumberBlocksY - 1) ? BorderHeight : blockSize.height; 3536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn X1 = 0; 3546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < NumberBlocksX; j++ ) 3566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int accept_level; 3586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int escape_level; 3596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int blDist; 3616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int VelocityX = 0; 3636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int VelocityY = 0; 3646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int offX = 0, offY = 0; 3666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int CountDirection = 1; 3686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int main_flag = i < NumberBlocksY - 1 && j < NumberBlocksX - 1; 3706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize CurSize; 3716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* calculate width of current block */ 3736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CurrentWidth = (j == NumberBlocksX - 1) ? BorderWidth : blockSize.width; 3746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* compute initial offset */ 3766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( usePrev ) 3776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn offX = int_velocityX[j]; 3796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn offY = int_velocityY[j]; 3806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CurSize.width = CurrentWidth; 3836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CurSize.height = CurrentHeight; 3846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( main_flag ) 3866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCopyBM_8u_C1R( imgA + X1, imgStep, blockA, 3886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CurSize.width, CurSize ); 3896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCopyBM_8u_C1R( imgB + (Y1 + offY)*imgStep + (X1 + offX), 3906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn imgStep, blockB, CurSize.width, CurSize ); 3916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *((int64 *) (blockA + patch_ofs)) &= patch_mask; 3936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *((int64 *) (blockB + patch_ofs)) &= patch_mask; 3946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 3966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memset( blockA, 0, bufferSize ); 3986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memset( blockB, 0, bufferSize ); 3996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCopyBM_8u_C1R( imgA + X1, imgStep, blockA, blockSize.width, CurSize ); 4016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCopyBM_8u_C1R( imgB + (Y1 + offY) * imgStep + (X1 + offX), imgStep, 4026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn blockB, blockSize.width, CurSize ); 4036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !main_flag ) 4066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int tmp = CurSize.width * CurSize.height; 4086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn accept_level = tmp * SMALL_DIFF; 4106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn escape_level = tmp * BIG_DIFF; 4116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 4136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn accept_level = stand_accept_level; 4156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn escape_level = stand_escape_level; 4166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn blDist = icvCmpBlocksL1_8u_C1( blockA, blockB, cmpSize ); 4196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( blDist > accept_level ) 4216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int k; 4236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int VelX = 0; 4246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int VelY = 0; 4256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* walk around basic block */ 4276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* cycle for neighborhood */ 4296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( k = 0; k < ss_count; k++ ) 4306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int tmpDist; 4326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int Y2 = Y1 + offY + ss[k].y; 4346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int X2 = X1 + offX + ss[k].x; 4356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* if we break upper border */ 4376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( Y2 < 0 ) 4386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn continue; 4406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* if we break bottom border */ 4426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( Y2 + CurrentHeight >= imgSize.height ) 4436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn continue; 4456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* if we break left border */ 4476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( X2 < 0 ) 4486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn continue; 4506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* if we break right border */ 4526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( X2 + CurrentWidth >= imgSize.width ) 4536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn continue; 4556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( main_flag ) 4586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCopyBM_8u_C1R( imgB + Y2 * imgStep + X2, 4606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn imgStep, blockB, CurSize.width, CurSize ); 4616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *((int64 *) (blockB + patch_ofs)) &= patch_mask; 4636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 4656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memset( blockB, 0, bufferSize ); 4676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCopyBM_8u_C1R( imgB + Y1 * imgStep + X1, imgStep, 4686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn blockB, blockSize.width, CurSize ); 4696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tmpDist = icvCmpBlocksL1_8u_C1( blockA, blockB, cmpSize ); 4726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( tmpDist < accept_level ) 4746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn VelX = ss[k].x; 4766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn VelY = ss[k].y; 4776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; /*for */ 4786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( tmpDist < blDist ) 4806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn blDist = tmpDist; 4826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn VelX = ss[k].x; 4836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn VelY = ss[k].y; 4846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CountDirection = 1; 4856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( tmpDist == blDist ) 4876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn VelX += ss[k].x; 4896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn VelY += ss[k].y; 4906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CountDirection++; 4916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( blDist > escape_level ) 4946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn VelX = VelY = 0; 4966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CountDirection = 1; 4976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CountDirection > 1 ) 4996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int temp = CountDirection == 2 ? 1 << 15 : ((1 << 16) / CountDirection); 5016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn VelocityX = VelX * temp; 5036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn VelocityY = VelY * temp; 5046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 5066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn VelocityX = VelX << 16; 5086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn VelocityY = VelY << 16; 5096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } /*if */ 5116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int_velocityX[j] = VelocityX + (offX << 16); 5136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int_velocityY[j] = VelocityY + (offY << 16); 5146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn X1 += blockSize.width; 5166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } /*for */ 5186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int_velocityX += velStep; 5196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int_velocityY += velStep; 5206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn imgA += DownStep; 5226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Y1 += blockSize.height; 5236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } /*for */ 5246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/****************************************************************************************\ 5266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn* Converting fixed point velocities to floating point * 5276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn\****************************************************************************************/ 5286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float *velxf = velocityX, *velyf = velocityY; 5306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int* velx = (int*)velocityX, *vely = (int*)velocityY; 5316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < NumberBlocksY; i++, velxf += velStep, velyf += velStep, 5336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn velx += velStep, vely += velStep ) 5346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < NumberBlocksX; j++ ) 5366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float vx = (float)velx[j]*back, vy = (float)vely[j]*back; 5386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn velxf[j] = vx; velyf[j] = vy; 5396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &ss ); 5446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &blockA ); 5456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_OK; 5476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} /*cvCalcOpticalFlowBM_8u */ 5486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*F/////////////////////////////////////////////////////////////////////////////////////// 5516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Name: cvCalcOpticalFlowBM 5526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Purpose: Optical flow implementation 5536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Context: 5546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Parameters: 5556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// srcA, srcB - source image 5566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// velx, vely - destination image 5576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Returns: 5586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 5596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Notes: 5606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//F*/ 5616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL void 5626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvCalcOpticalFlowBM( const void* srcarrA, const void* srcarrB, 5636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize blockSize, CvSize shiftSize, 5646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize maxRange, int usePrevious, 5656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn void* velarrx, void* velarry ) 5666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 5676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvCalcOpticalFlowBM" ); 5686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 5706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat stubA, *srcA = (CvMat*)srcarrA; 5726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat stubB, *srcB = (CvMat*)srcarrB; 5736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat stubx, *velx = (CvMat*)velarrx; 5746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat stuby, *vely = (CvMat*)velarry; 5756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( srcA = cvGetMat( srcA, &stubA )); 5776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( srcB = cvGetMat( srcB, &stubB )); 5786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( velx = cvGetMat( velx, &stubx )); 5806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( vely = cvGetMat( vely, &stuby )); 5816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_ARE_TYPES_EQ( srcA, srcB )) 5836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedFormats, "Source images have different formats" ); 5846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_ARE_TYPES_EQ( velx, vely )) 5866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedFormats, "Destination images have different formats" ); 5876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_ARE_SIZES_EQ( srcA, srcB ) || 5896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn !CV_ARE_SIZES_EQ( velx, vely ) || 5906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (unsigned)(velx->width*blockSize.width - srcA->width) >= (unsigned)blockSize.width || 5916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (unsigned)(velx->height*blockSize.height - srcA->height) >= (unsigned)blockSize.height ) 5926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "" ); 5936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_MAT_TYPE( srcA->type ) != CV_8UC1 || 5956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_MAT_TYPE( velx->type ) != CV_32FC1 ) 5966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "Source images must have 8uC1 type and " 5976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "destination images must have 32fC1 type" ); 5986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( srcA->step != srcB->step || velx->step != vely->step ) 6006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_BadStep, "two source or two destination images have different steps" ); 6016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn IPPI_CALL( icvCalcOpticalFlowBM_8u32fR( (uchar*)srcA->data.ptr, (uchar*)srcB->data.ptr, 6036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn srcA->step, cvGetMatSize( srcA ), blockSize, 6046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn shiftSize, maxRange, usePrevious, 6056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn velx->data.fl, vely->data.fl, velx->step )); 6066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 6076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 6086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* End of file. */ 611