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#include <float.h> 436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#include <stdio.h> 446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic void 466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennintersect( CvPoint2D32f pt, CvSize win_size, CvSize imgSize, 476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint* min_pt, CvPoint* max_pt ) 486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint ipt; 506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ipt.x = cvFloor( pt.x ); 526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ipt.y = cvFloor( pt.y ); 536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ipt.x -= win_size.width; 556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ipt.y -= win_size.height; 566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn win_size.width = win_size.width * 2 + 1; 586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn win_size.height = win_size.height * 2 + 1; 596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn min_pt->x = MAX( 0, -ipt.x ); 616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn min_pt->y = MAX( 0, -ipt.y ); 626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn max_pt->x = MIN( win_size.width, imgSize.width - ipt.x ); 636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn max_pt->y = MIN( win_size.height, imgSize.height - ipt.y ); 646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic int icvMinimalPyramidSize( CvSize imgSize ) 686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return cvAlign(imgSize.width,8) * imgSize.height / 3; 706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic void 746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennicvInitPyramidalAlgorithm( const CvMat* imgA, const CvMat* imgB, 756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* pyrA, CvMat* pyrB, 766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int level, CvTermCriteria * criteria, 776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int max_iters, int flags, 786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar *** imgI, uchar *** imgJ, 796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int **step, CvSize** size, 806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double **scale, uchar ** buffer ) 816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "icvInitPyramidalAlgorithm" ); 836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const int ALIGN = 8; 876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int pyrBytes, bufferBytes = 0, elem_size; 886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int level1 = level + 1; 896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize imgSize, levelSize; 926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *buffer = 0; 946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *imgI = *imgJ = 0; 956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *step = 0; 966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *scale = 0; 976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *size = 0; 986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* check input arguments */ 1006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( ((flags & CV_LKFLOW_PYR_A_READY) != 0 && !pyrA) || 1016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ((flags & CV_LKFLOW_PYR_B_READY) != 0 && !pyrB) ) 1026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of the precomputed pyramids are missing" ); 1036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( level < 0 ) 1056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "The number of pyramid layers is negative" ); 1066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn switch( criteria->type ) 1086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn case CV_TERMCRIT_ITER: 1106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn criteria->epsilon = 0.f; 1116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 1126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn case CV_TERMCRIT_EPS: 1136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn criteria->max_iter = max_iters; 1146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 1156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn case CV_TERMCRIT_ITER | CV_TERMCRIT_EPS: 1166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 1176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn default: 1186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn assert( 0 ); 1196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadArg, "Invalid termination criteria" ); 1206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* compare squared values */ 1236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn criteria->epsilon *= criteria->epsilon; 1246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* set pointers and step for every level */ 1266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pyrBytes = 0; 1276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn imgSize = cvGetSize(imgA); 1296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn elem_size = CV_ELEM_SIZE(imgA->type); 1306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn levelSize = imgSize; 1316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 1; i < level1; i++ ) 1336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn levelSize.width = (levelSize.width + 1) >> 1; 1356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn levelSize.height = (levelSize.height + 1) >> 1; 1366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int tstep = cvAlign(levelSize.width,ALIGN) * elem_size; 1386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pyrBytes += tstep * levelSize.height; 1396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn assert( pyrBytes <= imgSize.width * imgSize.height * elem_size * 4 / 3 ); 1426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* buffer_size = <size for patches> + <size for pyramids> */ 1446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn bufferBytes = (int)((level1 >= 0) * ((pyrA->data.ptr == 0) + 1456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (pyrB->data.ptr == 0)) * pyrBytes + 1466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (sizeof(imgI[0][0]) * 2 + sizeof(step[0][0]) + 1476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sizeof(size[0][0]) + sizeof(scale[0][0])) * level1); 1486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( *buffer = (uchar *)cvAlloc( bufferBytes )); 1506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *imgI = (uchar **) buffer[0]; 1526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *imgJ = *imgI + level1; 1536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *step = (int *) (*imgJ + level1); 1546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *scale = (double *) (*step + level1); 1556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *size = (CvSize *)(*scale + level1); 1566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn imgI[0][0] = imgA->data.ptr; 1586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn imgJ[0][0] = imgB->data.ptr; 1596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn step[0][0] = imgA->step; 1606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn scale[0][0] = 1; 1616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn size[0][0] = imgSize; 1626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( level > 0 ) 1646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar *bufPtr = (uchar *) (*size + level1); 1666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar *ptrA = pyrA->data.ptr; 1676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar *ptrB = pyrB->data.ptr; 1686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !ptrA ) 1706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ptrA = bufPtr; 1726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn bufPtr += pyrBytes; 1736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !ptrB ) 1766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ptrB = bufPtr; 1776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn levelSize = imgSize; 1796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* build pyramids for both frames */ 1816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 1; i <= level; i++ ) 1826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int levelBytes; 1846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat prev_level, next_level; 1856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn levelSize.width = (levelSize.width + 1) >> 1; 1876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn levelSize.height = (levelSize.height + 1) >> 1; 1886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn size[0][i] = levelSize; 1906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn step[0][i] = cvAlign( levelSize.width, ALIGN ) * elem_size; 1916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn scale[0][i] = scale[0][i - 1] * 0.5; 1926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn levelBytes = step[0][i] * levelSize.height; 1946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn imgI[0][i] = (uchar *) ptrA; 1956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ptrA += levelBytes; 1966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !(flags & CV_LKFLOW_PYR_A_READY) ) 1986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn prev_level = cvMat( size[0][i-1].height, size[0][i-1].width, CV_8UC1 ); 2006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn next_level = cvMat( size[0][i].height, size[0][i].width, CV_8UC1 ); 2016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSetData( &prev_level, imgI[0][i-1], step[0][i-1] ); 2026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSetData( &next_level, imgI[0][i], step[0][i] ); 2036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvPyrDown( &prev_level, &next_level ); 2046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn imgJ[0][i] = (uchar *) ptrB; 2076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ptrB += levelBytes; 2086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !(flags & CV_LKFLOW_PYR_B_READY) ) 2106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn prev_level = cvMat( size[0][i-1].height, size[0][i-1].width, CV_8UC1 ); 2126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn next_level = cvMat( size[0][i].height, size[0][i].width, CV_8UC1 ); 2136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSetData( &prev_level, imgJ[0][i-1], step[0][i-1] ); 2146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSetData( &next_level, imgJ[0][i], step[0][i] ); 2156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvPyrDown( &prev_level, &next_level ); 2166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 2216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 2226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* compute dI/dx and dI/dy */ 2256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic void 2266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennicvCalcIxIy_32f( const float* src, int src_step, float* dstX, float* dstY, int dst_step, 2276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize src_size, const float* smooth_k, float* buffer0 ) 2286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 2296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int src_width = src_size.width, dst_width = src_size.width-2; 2306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int x, height = src_size.height - 2; 2316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float* buffer1 = buffer0 + src_width; 2326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn src_step /= sizeof(src[0]); 2346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst_step /= sizeof(dstX[0]); 2356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( ; height--; src += src_step, dstX += dst_step, dstY += dst_step ) 2376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const float* src2 = src + src_step; 2396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const float* src3 = src + src_step*2; 2406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = 0; x < src_width; x++ ) 2426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float t0 = (src3[x] + src[x])*smooth_k[0] + src2[x]*smooth_k[1]; 2446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float t1 = src3[x] - src[x]; 2456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn buffer0[x] = t0; buffer1[x] = t1; 2466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = 0; x < dst_width; x++ ) 2496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float t0 = buffer0[x+2] - buffer0[x]; 2516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float t1 = (buffer1[x] + buffer1[x+2])*smooth_k[0] + buffer1[x+1]*smooth_k[1]; 2526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dstX[x] = t0; dstY[x] = t1; 2536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 2566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennicvOpticalFlowPyrLKInitAlloc_8u_C1R_t icvOpticalFlowPyrLKInitAlloc_8u_C1R_p = 0; 2596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennicvOpticalFlowPyrLKFree_8u_C1R_t icvOpticalFlowPyrLKFree_8u_C1R_p = 0; 2606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennicvOpticalFlowPyrLK_8u_C1R_t icvOpticalFlowPyrLK_8u_C1R_p = 0; 2616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL void 2646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvCalcOpticalFlowPyrLK( const void* arrA, const void* arrB, 2656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn void* pyrarrA, void* pyrarrB, 2666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvPoint2D32f * featuresA, 2676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f * featuresB, 2686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int count, CvSize winSize, int level, 2696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn char *status, float *error, 2706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvTermCriteria criteria, int flags ) 2716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 2726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar *pyrBuffer = 0; 2736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar *buffer = 0; 2746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float* _error = 0; 2756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn char* _status = 0; 2766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn void* ipp_optflow_state = 0; 2786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvCalcOpticalFlowPyrLK" ); 2806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 2826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const int MAX_ITERS = 100; 2846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat stubA, *imgA = (CvMat*)arrA; 2866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat stubB, *imgB = (CvMat*)arrB; 2876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat pstubA, *pyrA = (CvMat*)pyrarrA; 2886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat pstubB, *pyrB = (CvMat*)pyrarrB; 2896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize imgSize; 2906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn static const float smoothKernel[] = { 0.09375, 0.3125, 0.09375 }; /* 3/32, 10/32, 3/32 */ 2916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int bufferBytes = 0; 2936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar **imgI = 0; 2946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar **imgJ = 0; 2956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int *step = 0; 2966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double *scale = 0; 2976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize* size = 0; 2986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int threadCount = cvGetNumThreads(); 3006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float* _patchI[CV_MAX_THREADS]; 3016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float* _patchJ[CV_MAX_THREADS]; 3026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float* _Ix[CV_MAX_THREADS]; 3036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float* _Iy[CV_MAX_THREADS]; 3046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, l; 3066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize patchSize = cvSize( winSize.width * 2 + 1, winSize.height * 2 + 1 ); 3086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int patchLen = patchSize.width * patchSize.height; 3096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int srcPatchLen = (patchSize.width + 2)*(patchSize.height + 2); 3106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( imgA = cvGetMat( imgA, &stubA )); 3126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( imgB = cvGetMat( imgB, &stubB )); 3136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_MAT_TYPE( imgA->type ) != CV_8UC1 ) 3156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "" ); 3166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_ARE_TYPES_EQ( imgA, imgB )) 3186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedFormats, "" ); 3196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_ARE_SIZES_EQ( imgA, imgB )) 3216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "" ); 3226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( imgA->step != imgB->step ) 3246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "imgA and imgB must have equal steps" ); 3256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn imgSize = cvGetMatSize( imgA ); 3276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( pyrA ) 3296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( pyrA = cvGetMat( pyrA, &pstubA )); 3316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( pyrA->step*pyrA->height < icvMinimalPyramidSize( imgSize ) ) 3336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadArg, "pyramid A has insufficient size" ); 3346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 3366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pyrA = &pstubA; 3386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pyrA->data.ptr = 0; 3396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( pyrB ) 3426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( pyrB = cvGetMat( pyrB, &pstubB )); 3446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( pyrB->step*pyrB->height < icvMinimalPyramidSize( imgSize ) ) 3466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadArg, "pyramid B has insufficient size" ); 3476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 3496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pyrB = &pstubB; 3516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pyrB->data.ptr = 0; 3526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( count == 0 ) 3556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn EXIT; 3566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !featuresA || !featuresB ) 3586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of arrays of point coordinates are missing" ); 3596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( count < 0 ) 3616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "The number of tracked points is negative or zero" ); 3626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( winSize.width <= 1 || winSize.height <= 1 ) 3646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadSize, "Invalid search window size" ); 3656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < threadCount; i++ ) 3676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _patchI[i] = _patchJ[i] = _Ix[i] = _Iy[i] = 0; 3686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( icvInitPyramidalAlgorithm( imgA, imgB, pyrA, pyrB, 3706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn level, &criteria, MAX_ITERS, flags, 3716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &imgI, &imgJ, &step, &size, &scale, &pyrBuffer )); 3726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !status ) 3746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( status = _status = (char*)cvAlloc( count*sizeof(_status[0]) )); 3756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#if 0 3776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( icvOpticalFlowPyrLKInitAlloc_8u_C1R_p && 3786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvOpticalFlowPyrLKFree_8u_C1R_p && 3796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvOpticalFlowPyrLK_8u_C1R_p && 3806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn winSize.width == winSize.height && 3816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvOpticalFlowPyrLKInitAlloc_8u_C1R_p( &ipp_optflow_state, imgSize, 3826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn winSize.width*2+1, cvAlgHintAccurate ) >= 0 ) 3836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPyramid ipp_pyrA, ipp_pyrB; 3856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn static const double rate[] = { 1, 0.5, 0.25, 0.125, 0.0625, 0.03125, 0.015625, 0.0078125, 3866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 0.00390625, 0.001953125, 0.0009765625, 0.00048828125, 0.000244140625, 3876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 0.0001220703125 }; 3886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // initialize pyramid structures 3896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn assert( level < 14 ); 3906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ipp_pyrA.ptr = imgI; 3916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ipp_pyrB.ptr = imgJ; 3926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ipp_pyrA.sz = ipp_pyrB.sz = size; 3936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ipp_pyrA.rate = ipp_pyrB.rate = (double*)rate; 3946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ipp_pyrA.step = ipp_pyrB.step = step; 3956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ipp_pyrA.state = ipp_pyrB.state = 0; 3966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ipp_pyrA.level = ipp_pyrB.level = level; 3976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !error ) 3996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( error = _error = (float*)cvAlloc( count*sizeof(_error[0]) )); 4006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < count; i++ ) 4026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn featuresB[i] = featuresA[i]; 4036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( icvOpticalFlowPyrLK_8u_C1R_p( &ipp_pyrA, &ipp_pyrB, 4056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (const float*)featuresA, (float*)featuresB, status, error, count, 4066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn winSize.width*2 + 1, level, criteria.max_iter, 4076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (float)criteria.epsilon, ipp_optflow_state ) >= 0 ) 4086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < count; i++ ) 4106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn status[i] = status[i] == 0; 4116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn EXIT; 4126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 4156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* buffer_size = <size for patches> + <size for pyramids> */ 4176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn bufferBytes = (srcPatchLen + patchLen * 3) * sizeof( _patchI[0][0] ) * threadCount; 4186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( buffer = (uchar*)cvAlloc( bufferBytes )); 4196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < threadCount; i++ ) 4216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _patchI[i] = i == 0 ? (float*)buffer : _Iy[i-1] + patchLen; 4236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _patchJ[i] = _patchI[i] + srcPatchLen; 4246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _Ix[i] = _patchJ[i] + patchLen; 4256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _Iy[i] = _Ix[i] + patchLen; 4266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memset( status, 1, count ); 4296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( error ) 4306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memset( error, 0, count*sizeof(error[0]) ); 4316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !(flags & CV_LKFLOW_INITIAL_GUESSES) ) 4336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memcpy( featuresB, featuresA, count*sizeof(featuresA[0])); 4346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* do processing from top pyramid level (smallest image) 4366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn to the bottom (original image) */ 4376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( l = level; l >= 0; l-- ) 4386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize levelSize = size[l]; 4406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int levelStep = step[l]; 4416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#ifdef _OPENMP 4446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn #pragma omp parallel for num_threads(threadCount) schedule(dynamic) 4456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif // _OPENMP 4466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* find flow for each given point */ 4476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < count; i++ ) 4486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f v; 4506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint minI, maxI, minJ, maxJ; 4516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize isz, jsz; 4526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int pt_status; 4536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f u; 4546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint prev_minJ = { -1, -1 }, prev_maxJ = { -1, -1 }; 4556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double Gxx = 0, Gxy = 0, Gyy = 0, D = 0, minEig = 0; 4566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float prev_mx = 0, prev_my = 0; 4576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int j, x, y; 4586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int threadIdx = cvGetThreadNum(); 4596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float* patchI = _patchI[threadIdx]; 4606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float* patchJ = _patchJ[threadIdx]; 4616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float* Ix = _Ix[threadIdx]; 4626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float* Iy = _Iy[threadIdx]; 4636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn v.x = featuresB[i].x; 4656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn v.y = featuresB[i].y; 4666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( l < level ) 4676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn v.x += v.x; 4696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn v.y += v.y; 4706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 4726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn v.x = (float)(v.x * scale[l]); 4746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn v.y = (float)(v.y * scale[l]); 4756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pt_status = status[i]; 4786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !pt_status ) 4796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn continue; 4806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn minI = maxI = minJ = maxJ = cvPoint( 0, 0 ); 4826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn u.x = (float) (featuresA[i].x * scale[l]); 4846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn u.y = (float) (featuresA[i].y * scale[l]); 4856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn intersect( u, winSize, levelSize, &minI, &maxI ); 4876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn isz = jsz = cvSize(maxI.x - minI.x + 2, maxI.y - minI.y + 2); 4886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn u.x += (minI.x - (patchSize.width - maxI.x + 1))*0.5f; 4896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn u.y += (minI.y - (patchSize.height - maxI.y + 1))*0.5f; 4906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( isz.width < 3 || isz.height < 3 || 4926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetRectSubPix_8u32f_C1R( imgI[l], levelStep, levelSize, 4936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn patchI, isz.width*sizeof(patchI[0]), isz, u ) < 0 ) 4946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* point is outside the image. take the next */ 4966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn status[i] = 0; 4976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn continue; 4986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCalcIxIy_32f( patchI, isz.width*sizeof(patchI[0]), Ix, Iy, 5016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (isz.width-2)*sizeof(patchI[0]), isz, smoothKernel, patchJ ); 5026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < criteria.max_iter; j++ ) 5046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double bx = 0, by = 0; 5066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float mx, my; 5076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f _v; 5086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn intersect( v, winSize, levelSize, &minJ, &maxJ ); 5106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn minJ.x = MAX( minJ.x, minI.x ); 5126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn minJ.y = MAX( minJ.y, minI.y ); 5136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn maxJ.x = MIN( maxJ.x, maxI.x ); 5156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn maxJ.y = MIN( maxJ.y, maxI.y ); 5166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn jsz = cvSize(maxJ.x - minJ.x, maxJ.y - minJ.y); 5186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _v.x = v.x + (minJ.x - (patchSize.width - maxJ.x + 1))*0.5f; 5206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _v.y = v.y + (minJ.y - (patchSize.height - maxJ.y + 1))*0.5f; 5216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( jsz.width < 1 || jsz.height < 1 || 5236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetRectSubPix_8u32f_C1R( imgJ[l], levelStep, levelSize, patchJ, 5246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn jsz.width*sizeof(patchJ[0]), jsz, _v ) < 0 ) 5256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* point is outside image. take the next */ 5276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pt_status = 0; 5286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 5296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( maxJ.x == prev_maxJ.x && maxJ.y == prev_maxJ.y && 5326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn minJ.x == prev_minJ.x && minJ.y == prev_minJ.y ) 5336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( y = 0; y < jsz.height; y++ ) 5356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const float* pi = patchI + 5376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (y + minJ.y - minI.y + 1)*isz.width + minJ.x - minI.x + 1; 5386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const float* pj = patchJ + y*jsz.width; 5396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const float* ix = Ix + 5406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (y + minJ.y - minI.y)*(isz.width-2) + minJ.x - minI.x; 5416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const float* iy = Iy + (ix - Ix); 5426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = 0; x < jsz.width; x++ ) 5446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double t0 = pi[x] - pj[x]; 5466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn bx += t0 * ix[x]; 5476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn by += t0 * iy[x]; 5486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 5526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Gxx = Gyy = Gxy = 0; 5546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( y = 0; y < jsz.height; y++ ) 5556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const float* pi = patchI + 5576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (y + minJ.y - minI.y + 1)*isz.width + minJ.x - minI.x + 1; 5586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const float* pj = patchJ + y*jsz.width; 5596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const float* ix = Ix + 5606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (y + minJ.y - minI.y)*(isz.width-2) + minJ.x - minI.x; 5616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const float* iy = Iy + (ix - Ix); 5626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = 0; x < jsz.width; x++ ) 5646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double t = pi[x] - pj[x]; 5666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn bx += (double) (t * ix[x]); 5676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn by += (double) (t * iy[x]); 5686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Gxx += ix[x] * ix[x]; 5696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Gxy += ix[x] * iy[x]; 5706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Gyy += iy[x] * iy[x]; 5716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn D = Gxx * Gyy - Gxy * Gxy; 5756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( D < DBL_EPSILON ) 5766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pt_status = 0; 5786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 5796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // Adi Shavit - 2008.05 5826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( flags & CV_LKFLOW_GET_MIN_EIGENVALS ) 5836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn minEig = (Gyy + Gxx - sqrt((Gxx-Gyy)*(Gxx-Gyy) + 4.*Gxy*Gxy))/(2*jsz.height*jsz.width); 5846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn D = 1. / D; 5866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn prev_minJ = minJ; 5886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn prev_maxJ = maxJ; 5896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn mx = (float) ((Gyy * bx - Gxy * by) * D); 5926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn my = (float) ((Gxx * by - Gxy * bx) * D); 5936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn v.x += mx; 5956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn v.y += my; 5966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( mx * mx + my * my < criteria.epsilon ) 5986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 5996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( j > 0 && fabs(mx + prev_mx) < 0.01 && fabs(my + prev_my) < 0.01 ) 6016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn v.x -= mx*0.5f; 6036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn v.y -= my*0.5f; 6046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 6056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn prev_mx = mx; 6076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn prev_my = my; 6086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn featuresB[i] = v; 6116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn status[i] = (char)pt_status; 6126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( l == 0 && error && pt_status ) 6136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* calc error */ 6156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double err = 0; 6166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( flags & CV_LKFLOW_GET_MIN_EIGENVALS ) 6176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn err = minEig; 6186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 6196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( y = 0; y < jsz.height; y++ ) 6216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const float* pi = patchI + 6236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (y + minJ.y - minI.y + 1)*isz.width + minJ.x - minI.x + 1; 6246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const float* pj = patchJ + y*jsz.width; 6256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = 0; x < jsz.width; x++ ) 6276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double t = pi[x] - pj[x]; 6296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn err += t * t; 6306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn err = sqrt(err); 6336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn error[i] = (float)err; 6356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } // end of point processing loop (i) 6376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } // end of pyramid levels loop (l) 6396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 6416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( ipp_optflow_state ) 6436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvOpticalFlowPyrLKFree_8u_C1R_p( ipp_optflow_state ); 6446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &pyrBuffer ); 6466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &buffer ); 6476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &_error ); 6486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &_status ); 6496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 6506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Affine tracking algorithm */ 6536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL void 6556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvCalcAffineFlowPyrLK( const void* arrA, const void* arrB, 6566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn void* pyrarrA, void* pyrarrB, 6576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvPoint2D32f * featuresA, 6586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f * featuresB, 6596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float *matrices, int count, 6606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize winSize, int level, 6616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn char *status, float *error, 6626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvTermCriteria criteria, int flags ) 6636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 6646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const int MAX_ITERS = 100; 6656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn char* _status = 0; 6676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar *buffer = 0; 6686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar *pyr_buffer = 0; 6696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvCalcAffineFlowPyrLK" ); 6716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 6736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat stubA, *imgA = (CvMat*)arrA; 6756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat stubB, *imgB = (CvMat*)arrB; 6766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat pstubA, *pyrA = (CvMat*)pyrarrA; 6776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat pstubB, *pyrB = (CvMat*)pyrarrB; 6786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn static const float smoothKernel[] = { 0.09375, 0.3125, 0.09375 }; /* 3/32, 10/32, 3/32 */ 6806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int bufferBytes = 0; 6826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar **imgI = 0; 6846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar **imgJ = 0; 6856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int *step = 0; 6866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double *scale = 0; 6876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize* size = 0; 6886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float *patchI; 6906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float *patchJ; 6916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float *Ix; 6926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float *Iy; 6936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, j, k, l; 6956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize patchSize = cvSize( winSize.width * 2 + 1, winSize.height * 2 + 1 ); 6976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int patchLen = patchSize.width * patchSize.height; 6986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int patchStep = patchSize.width * sizeof( patchI[0] ); 6996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize srcPatchSize = cvSize( patchSize.width + 2, patchSize.height + 2 ); 7016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int srcPatchLen = srcPatchSize.width * srcPatchSize.height; 7026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int srcPatchStep = srcPatchSize.width * sizeof( patchI[0] ); 7036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize imgSize; 7046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float eps = (float)MIN(winSize.width, winSize.height); 7056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( imgA = cvGetMat( imgA, &stubA )); 7076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( imgB = cvGetMat( imgB, &stubB )); 7086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_MAT_TYPE( imgA->type ) != CV_8UC1 ) 7106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "" ); 7116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_ARE_TYPES_EQ( imgA, imgB )) 7136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedFormats, "" ); 7146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_ARE_SIZES_EQ( imgA, imgB )) 7166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "" ); 7176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( imgA->step != imgB->step ) 7196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "imgA and imgB must have equal steps" ); 7206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !matrices ) 7226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "" ); 7236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn imgSize = cvGetMatSize( imgA ); 7256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( pyrA ) 7276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( pyrA = cvGetMat( pyrA, &pstubA )); 7296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( pyrA->step*pyrA->height < icvMinimalPyramidSize( imgSize ) ) 7316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadArg, "pyramid A has insufficient size" ); 7326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 7346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pyrA = &pstubA; 7366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pyrA->data.ptr = 0; 7376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( pyrB ) 7406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( pyrB = cvGetMat( pyrB, &pstubB )); 7426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( pyrB->step*pyrB->height < icvMinimalPyramidSize( imgSize ) ) 7446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadArg, "pyramid B has insufficient size" ); 7456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 7476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pyrB = &pstubB; 7496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pyrB->data.ptr = 0; 7506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( count == 0 ) 7536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn EXIT; 7546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* check input arguments */ 7566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !featuresA || !featuresB || !matrices ) 7576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "" ); 7586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( winSize.width <= 1 || winSize.height <= 1 ) 7606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "the search window is too small" ); 7616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( count < 0 ) 7636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "" ); 7646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( icvInitPyramidalAlgorithm( imgA, imgB, 7666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pyrA, pyrB, level, &criteria, MAX_ITERS, flags, 7676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &imgI, &imgJ, &step, &size, &scale, &pyr_buffer )); 7686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* buffer_size = <size for patches> + <size for pyramids> */ 7706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn bufferBytes = (srcPatchLen + patchLen*3)*sizeof(patchI[0]) + (36*2 + 6)*sizeof(double); 7716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( buffer = (uchar*)cvAlloc(bufferBytes)); 7736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !status ) 7756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( status = _status = (char*)cvAlloc(count) ); 7766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn patchI = (float *) buffer; 7786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn patchJ = patchI + srcPatchLen; 7796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Ix = patchJ + patchLen; 7806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Iy = Ix + patchLen; 7816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( status ) 7836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memset( status, 1, count ); 7846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !(flags & CV_LKFLOW_INITIAL_GUESSES) ) 7866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memcpy( featuresB, featuresA, count * sizeof( featuresA[0] )); 7886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < count * 4; i += 4 ) 7896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrices[i] = matrices[i + 3] = 1.f; 7916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrices[i + 1] = matrices[i + 2] = 0.f; 7926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < count; i++ ) 7966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn featuresB[i].x = (float)(featuresB[i].x * scale[level] * 0.5); 7986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn featuresB[i].y = (float)(featuresB[i].y * scale[level] * 0.5); 7996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* do processing from top pyramid level (smallest image) 8026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn to the bottom (original image) */ 8036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( l = level; l >= 0; l-- ) 8046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize levelSize = size[l]; 8066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int levelStep = step[l]; 8076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* find flow for each given point at the particular level */ 8096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < count; i++ ) 8106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f u; 8126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float Av[6]; 8136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double G[36]; 8146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double meanI = 0, meanJ = 0; 8156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int x, y; 8166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int pt_status = status[i]; 8176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat mat; 8186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !pt_status ) 8206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn continue; 8216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Av[0] = matrices[i*4]; 8236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Av[1] = matrices[i*4+1]; 8246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Av[3] = matrices[i*4+2]; 8256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Av[4] = matrices[i*4+3]; 8266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Av[2] = featuresB[i].x += featuresB[i].x; 8286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Av[5] = featuresB[i].y += featuresB[i].y; 8296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn u.x = (float) (featuresA[i].x * scale[l]); 8316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn u.y = (float) (featuresA[i].y * scale[l]); 8326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( u.x < -eps || u.x >= levelSize.width+eps || 8346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn u.y < -eps || u.y >= levelSize.height+eps || 8356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetRectSubPix_8u32f_C1R( imgI[l], levelStep, 8366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn levelSize, patchI, srcPatchStep, srcPatchSize, u ) < 0 ) 8376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* point is outside the image. take the next */ 8396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( l == 0 ) 8406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn status[i] = 0; 8416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn continue; 8426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCalcIxIy_32f( patchI, srcPatchStep, Ix, Iy, 8456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (srcPatchSize.width-2)*sizeof(patchI[0]), srcPatchSize, 8466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn smoothKernel, patchJ ); 8476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* repack patchI (remove borders) */ 8496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( k = 0; k < patchSize.height; k++ ) 8506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memcpy( patchI + k * patchSize.width, 8516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn patchI + (k + 1) * srcPatchSize.width + 1, patchStep ); 8526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memset( G, 0, sizeof( G )); 8546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* calculate G matrix */ 8566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( y = -winSize.height, k = 0; y <= winSize.height; y++ ) 8576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = -winSize.width; x <= winSize.width; x++, k++ ) 8596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double ixix = ((double) Ix[k]) * Ix[k]; 8616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double ixiy = ((double) Ix[k]) * Iy[k]; 8626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double iyiy = ((double) Iy[k]) * Iy[k]; 8636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double xx, xy, yy; 8656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn G[0] += ixix; 8676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn G[1] += ixiy; 8686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn G[2] += x * ixix; 8696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn G[3] += y * ixix; 8706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn G[4] += x * ixiy; 8716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn G[5] += y * ixiy; 8726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // G[6] == G[1] 8746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn G[7] += iyiy; 8756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // G[8] == G[4] 8766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // G[9] == G[5] 8776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn G[10] += x * iyiy; 8786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn G[11] += y * iyiy; 8796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn xx = x * x; 8816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn xy = x * y; 8826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn yy = y * y; 8836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // G[12] == G[2] 8856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // G[13] == G[8] == G[4] 8866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn G[14] += xx * ixix; 8876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn G[15] += xy * ixix; 8886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn G[16] += xx * ixiy; 8896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn G[17] += xy * ixiy; 8906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // G[18] == G[3] 8926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // G[19] == G[9] 8936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // G[20] == G[15] 8946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn G[21] += yy * ixix; 8956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // G[22] == G[17] 8966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn G[23] += yy * ixiy; 8976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // G[24] == G[4] 8996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // G[25] == G[10] 9006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // G[26] == G[16] 9016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // G[27] == G[22] 9026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn G[28] += xx * iyiy; 9036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn G[29] += xy * iyiy; 9046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // G[30] == G[5] 9066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // G[31] == G[11] 9076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // G[32] == G[17] 9086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // G[33] == G[23] 9096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // G[34] == G[29] 9106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn G[35] += yy * iyiy; 9116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn meanI += patchI[k]; 9136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn meanI /= patchSize.width*patchSize.height; 9176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn G[8] = G[4]; 9196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn G[9] = G[5]; 9206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn G[22] = G[17]; 9216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // fill part of G below its diagonal 9236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( y = 1; y < 6; y++ ) 9246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = 0; x < y; x++ ) 9256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn G[y * 6 + x] = G[x * 6 + y]; 9266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvInitMatHeader( &mat, 6, 6, CV_64FC1, G ); 9286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( cvInvert( &mat, &mat, CV_SVD ) < 1e-4 ) 9306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* bad matrix. take the next point */ 9326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( l == 0 ) 9336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn status[i] = 0; 9346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn continue; 9356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < criteria.max_iter; j++ ) 9386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double b[6] = {0,0,0,0,0,0}, eta[6]; 9406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double t0, t1, s = 0; 9416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( Av[2] < -eps || Av[2] >= levelSize.width+eps || 9436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Av[5] < -eps || Av[5] >= levelSize.height+eps || 9446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetQuadrangleSubPix_8u32f_C1R( imgJ[l], levelStep, 9456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn levelSize, patchJ, patchStep, patchSize, Av ) < 0 ) 9466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pt_status = 0; 9486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 9496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( y = -winSize.height, k = 0, meanJ = 0; y <= winSize.height; y++ ) 9526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = -winSize.width; x <= winSize.width; x++, k++ ) 9536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn meanJ += patchJ[k]; 9546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn meanJ = meanJ / (patchSize.width * patchSize.height) - meanI; 9566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( y = -winSize.height, k = 0; y <= winSize.height; y++ ) 9586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = -winSize.width; x <= winSize.width; x++, k++ ) 9606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double t = patchI[k] - patchJ[k] + meanJ; 9626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double ixt = Ix[k] * t; 9636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double iyt = Iy[k] * t; 9646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn s += t; 9666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn b[0] += ixt; 9686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn b[1] += iyt; 9696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn b[2] += x * ixt; 9706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn b[3] += y * ixt; 9716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn b[4] += x * iyt; 9726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn b[5] += y * iyt; 9736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvTransformVector_64d( G, b, eta, 6, 6 ); 9776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Av[2] = (float)(Av[2] + Av[0] * eta[0] + Av[1] * eta[1]); 9796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Av[5] = (float)(Av[5] + Av[3] * eta[0] + Av[4] * eta[1]); 9806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t0 = Av[0] * (1 + eta[2]) + Av[1] * eta[4]; 9826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t1 = Av[0] * eta[3] + Av[1] * (1 + eta[5]); 9836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Av[0] = (float)t0; 9846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Av[1] = (float)t1; 9856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t0 = Av[3] * (1 + eta[2]) + Av[4] * eta[4]; 9876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t1 = Av[3] * eta[3] + Av[4] * (1 + eta[5]); 9886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Av[3] = (float)t0; 9896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Av[4] = (float)t1; 9906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( eta[0] * eta[0] + eta[1] * eta[1] < criteria.epsilon ) 9926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 9936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( pt_status != 0 || l == 0 ) 9966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn status[i] = (char)pt_status; 9986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn featuresB[i].x = Av[2]; 9996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn featuresB[i].y = Av[5]; 10006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrices[i*4] = Av[0]; 10026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrices[i*4+1] = Av[1]; 10036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrices[i*4+2] = Av[3]; 10046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrices[i*4+3] = Av[4]; 10056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( pt_status && l == 0 && error ) 10086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* calc error */ 10106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double err = 0; 10116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( y = 0, k = 0; y < patchSize.height; y++ ) 10136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = 0; x < patchSize.width; x++, k++ ) 10156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double t = patchI[k] - patchJ[k] + meanJ; 10176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn err += t * t; 10186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn error[i] = (float)sqrt(err); 10216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 10266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &pyr_buffer ); 10286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &buffer ); 10296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &_status ); 10306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 10316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic void 10356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennicvGetRTMatrix( const CvPoint2D32f* a, const CvPoint2D32f* b, 10366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int count, CvMat* M, int full_affine ) 10376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 10386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( full_affine ) 10396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double sa[36], sb[6]; 10416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat A = cvMat( 6, 6, CV_64F, sa ), B = cvMat( 6, 1, CV_64F, sb ); 10426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat MM = cvMat( 6, 1, CV_64F, M->data.db ); 10436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 10456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memset( sa, 0, sizeof(sa) ); 10476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memset( sb, 0, sizeof(sb) ); 10486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < count; i++ ) 10506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sa[0] += a[i].x*a[i].x; 10526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sa[1] += a[i].y*a[i].x; 10536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sa[2] += a[i].x; 10546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sa[6] += a[i].x*a[i].y; 10566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sa[7] += a[i].y*a[i].y; 10576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sa[8] += a[i].y; 10586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sa[12] += a[i].x; 10606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sa[13] += a[i].y; 10616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sa[14] += 1; 10626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sb[0] += a[i].x*b[i].x; 10646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sb[1] += a[i].y*b[i].x; 10656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sb[2] += b[i].x; 10666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sb[3] += a[i].x*b[i].y; 10676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sb[4] += a[i].y*b[i].y; 10686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sb[5] += b[i].y; 10696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sa[21] = sa[0]; 10726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sa[22] = sa[1]; 10736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sa[23] = sa[2]; 10746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sa[27] = sa[6]; 10756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sa[28] = sa[7]; 10766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sa[29] = sa[8]; 10776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sa[33] = sa[12]; 10786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sa[34] = sa[13]; 10796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sa[35] = sa[14]; 10806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSolve( &A, &B, &MM, CV_SVD ); 10826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 10846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double sa[16], sb[4], m[4], *om = M->data.db; 10866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat A = cvMat( 4, 4, CV_64F, sa ), B = cvMat( 4, 1, CV_64F, sb ); 10876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat MM = cvMat( 4, 1, CV_64F, m ); 10886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 10906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memset( sa, 0, sizeof(sa) ); 10926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memset( sb, 0, sizeof(sb) ); 10936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < count; i++ ) 10956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sa[0] += a[i].x*a[i].x + a[i].y*a[i].y; 10976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sa[1] += 0; 10986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sa[2] += a[i].x; 10996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sa[3] += a[i].y; 11006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sa[4] += 0; 11026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sa[5] += a[i].x*a[i].x + a[i].y*a[i].y; 11036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sa[6] += -a[i].y; 11046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sa[7] += a[i].x; 11056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sa[8] += a[i].x; 11076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sa[9] += -a[i].y; 11086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sa[10] += 1; 11096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sa[11] += 0; 11106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sa[12] += a[i].y; 11126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sa[13] += a[i].x; 11136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sa[14] += 0; 11146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sa[15] += 1; 11156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sb[0] += a[i].x*b[i].x + a[i].y*b[i].y; 11176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sb[1] += a[i].x*b[i].y - a[i].y*b[i].x; 11186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sb[2] += b[i].x; 11196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sb[3] += b[i].y; 11206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSolve( &A, &B, &MM, CV_SVD ); 11236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn om[0] = om[4] = m[0]; 11256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn om[1] = -m[1]; 11266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn om[3] = m[1]; 11276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn om[2] = m[2]; 11286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn om[5] = m[3]; 11296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 11316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL int 11346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvEstimateRigidTransform( const CvArr* _A, const CvArr* _B, CvMat* _M, int full_affine ) 11356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 11366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int result = 0; 11376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const int COUNT = 15; 11396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const int WIDTH = 160, HEIGHT = 120; 11406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const int RANSAC_MAX_ITERS = 100; 11416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const int RANSAC_SIZE0 = 3; 11426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const double MIN_TRIANGLE_SIDE = 20; 11436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const double RANSAC_GOOD_RATIO = 0.5; 11446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int allocated = 1; 11466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *sA = 0, *sB = 0; 11476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f *pA = 0, *pB = 0; 11486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int* good_idx = 0; 11496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn char *status = 0; 11506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* gray = 0; 11516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvEstimateRigidTransform" ); 11536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 11556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat stubA, *A; 11576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat stubB, *B; 11586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize sz0, sz1; 11596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int cn, equal_sizes; 11606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, j, k, k1; 11616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int count_x, count_y, count; 11626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double scale = 1; 11636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvRNG rng = cvRNG(-1); 11646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double m[6]={0}; 11656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat M = cvMat( 2, 3, CV_64F, m ); 11666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int good_count = 0; 11676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( A = cvGetMat( _A, &stubA )); 11696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( B = cvGetMat( _B, &stubB )); 11706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(_M) ) 11726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( _M ? CV_StsBadArg : CV_StsNullPtr, "Output parameter M is not a valid matrix" ); 11736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_ARE_SIZES_EQ( A, B ) ) 11756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Both input images must have the same size" ); 11766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_ARE_TYPES_EQ( A, B ) ) 11786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedFormats, "Both input images must have the same data type" ); 11796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_MAT_TYPE(A->type) == CV_8UC1 || CV_MAT_TYPE(A->type) == CV_8UC3 ) 11816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cn = CV_MAT_CN(A->type); 11836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sz0 = cvGetSize(A); 11846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sz1 = cvSize(WIDTH, HEIGHT); 11856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn scale = MAX( (double)sz1.width/sz0.width, (double)sz1.height/sz0.height ); 11876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn scale = MIN( scale, 1. ); 11886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sz1.width = cvRound( sz0.width * scale ); 11896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sz1.height = cvRound( sz0.height * scale ); 11906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn equal_sizes = sz1.width == sz0.width && sz1.height == sz0.height; 11926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !equal_sizes || cn != 1 ) 11946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( sA = cvCreateMat( sz1.height, sz1.width, CV_8UC1 )); 11966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( sB = cvCreateMat( sz1.height, sz1.width, CV_8UC1 )); 11976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !equal_sizes && cn != 1 ) 11996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( gray = cvCreateMat( sz0.height, sz0.width, CV_8UC1 )); 12006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( gray ) 12026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCvtColor( A, gray, CV_BGR2GRAY ); 12046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvResize( gray, sA, CV_INTER_AREA ); 12056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCvtColor( B, gray, CV_BGR2GRAY ); 12066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvResize( gray, sB, CV_INTER_AREA ); 12076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( cn == 1 ) 12096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvResize( gray, sA, CV_INTER_AREA ); 12116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvResize( gray, sB, CV_INTER_AREA ); 12126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 12146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCvtColor( A, gray, CV_BGR2GRAY ); 12166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvResize( gray, sA, CV_INTER_AREA ); 12176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCvtColor( B, gray, CV_BGR2GRAY ); 12186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &gray ); 12216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn A = sA; 12226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn B = sB; 12236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn count_y = COUNT; 12266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn count_x = cvRound((double)COUNT*sz1.width/sz1.height); 12276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn count = count_x * count_y; 12286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( pA = (CvPoint2D32f*)cvAlloc( count*sizeof(pA[0]) )); 12306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( pB = (CvPoint2D32f*)cvAlloc( count*sizeof(pB[0]) )); 12316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( status = (char*)cvAlloc( count*sizeof(status[0]) )); 12326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0, k = 0; i < count_y; i++ ) 12346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < count_x; j++, k++ ) 12356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pA[k].x = (j+0.5f)*sz1.width/count_x; 12376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pA[k].y = (i+0.5f)*sz1.height/count_y; 12386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // find the corresponding points in B 12416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCalcOpticalFlowPyrLK( A, B, 0, 0, pA, pB, count, cvSize(10,10), 3, 12426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn status, 0, cvTermCriteria(CV_TERMCRIT_ITER,40,0.1), 0 ); 12436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // repack the remained points 12456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0, k = 0; i < count; i++ ) 12466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( status[i] ) 12476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( i > k ) 12496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pA[k] = pA[i]; 12516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pB[k] = pB[i]; 12526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn k++; 12546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn count = k; 12576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( CV_MAT_TYPE(A->type) == CV_32FC2 || CV_MAT_TYPE(A->type) == CV_32SC2 ) 12596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn count = A->cols*A->rows; 12616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_IS_MAT_CONT(A->type & B->type) && CV_MAT_TYPE(A->type) == CV_32FC2 ) 12636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pA = (CvPoint2D32f*)A->data.ptr; 12656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pB = (CvPoint2D32f*)B->data.ptr; 12666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn allocated = 0; 12676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 12696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _pA, _pB; 12716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( pA = (CvPoint2D32f*)cvAlloc( count*sizeof(pA[0]) )); 12736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( pB = (CvPoint2D32f*)cvAlloc( count*sizeof(pB[0]) )); 12746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _pA = cvMat( A->rows, A->cols, CV_32FC2, pA ); 12756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _pB = cvMat( B->rows, B->cols, CV_32FC2, pB ); 12766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( A, &_pA ); 12776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( B, &_pB ); 12786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 12816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "Both input images must have either 8uC1 or 8uC3 type" ); 12826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( good_idx = (int*)cvAlloc( count*sizeof(good_idx[0]) )); 12846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( count < RANSAC_SIZE0 ) 12866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn EXIT; 12876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // RANSAC stuff: 12896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // 1. find the consensus 12906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( k = 0; k < RANSAC_MAX_ITERS; k++ ) 12916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int idx[RANSAC_SIZE0]; 12936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f a[3]; 12946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f b[3]; 12956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memset( a, 0, sizeof(a) ); 12976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memset( b, 0, sizeof(b) ); 12986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // choose random 3 non-complanar points from A & B 13006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < RANSAC_SIZE0; i++ ) 13016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( k1 = 0; k1 < RANSAC_MAX_ITERS; k1++ ) 13036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn idx[i] = cvRandInt(&rng) % count; 13056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < i; j++ ) 13076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( idx[j] == idx[i] ) 13096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 13106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // check that the points are not very close one each other 13116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( fabs(pA[idx[i]].x - pA[idx[j]].x) + 13126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fabs(pA[idx[i]].y - pA[idx[j]].y) < MIN_TRIANGLE_SIDE ) 13136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 13146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( fabs(pB[idx[i]].x - pB[idx[j]].x) + 13156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fabs(pB[idx[i]].y - pB[idx[j]].y) < MIN_TRIANGLE_SIDE ) 13166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 13176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( j < i ) 13206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn continue; 13216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( i+1 == RANSAC_SIZE0 ) 13236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // additional check for non-complanar vectors 13256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[0] = pA[idx[0]]; 13266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[1] = pA[idx[1]]; 13276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[2] = pA[idx[2]]; 13286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn b[0] = pB[idx[0]]; 13306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn b[1] = pB[idx[1]]; 13316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn b[2] = pB[idx[2]]; 13326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( fabs((a[1].x - a[0].x)*(a[2].y - a[0].y) - (a[1].y - a[0].y)*(a[2].x - a[0].x)) < 1 || 13346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fabs((b[1].x - b[0].x)*(b[2].y - b[0].y) - (b[1].y - b[0].y)*(b[2].x - b[0].x)) < 1 ) 13356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn continue; 13366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 13386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( k1 >= RANSAC_MAX_ITERS ) 13416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 13426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( i < RANSAC_SIZE0 ) 13456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn continue; 13466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // estimate the transformation using 3 points 13486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetRTMatrix( a, b, 3, &M, full_affine ); 13496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0, good_count = 0; i < count; i++ ) 13516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( fabs( m[0]*pA[i].x + m[1]*pA[i].y + m[2] - pB[i].x ) + 13536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fabs( m[3]*pA[i].x + m[4]*pA[i].y + m[5] - pB[i].y ) < 8 ) 13546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn good_idx[good_count++] = i; 13556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( good_count >= count*RANSAC_GOOD_RATIO ) 13586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 13596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( k >= RANSAC_MAX_ITERS ) 13626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn EXIT; 13636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( good_count < count ) 13656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < good_count; i++ ) 13676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn j = good_idx[i]; 13696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pA[i] = pA[j]; 13706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pB[i] = pB[j]; 13716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvGetRTMatrix( pA, pB, good_count, &M, full_affine ); 13756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn m[2] /= scale; 13766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn m[5] /= scale; 13776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvConvert( &M, _M )); 13786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn result = 1; 13796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 13816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &sA ); 13836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &sB ); 13846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &pA ); 13856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &pB ); 13866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &status ); 13876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &good_idx ); 13886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &gray ); 13896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return result; 13916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 13926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* End of file. */ 1395