16acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*M/////////////////////////////////////////////////////////////////////////////////////// 26acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 36acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 46acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 56acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// By downloading, copying, installing or using the software you agree to this license. 66acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// If you do not agree to this license, do not download, install, 76acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// copy or use the software. 86acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 96acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Intel License Agreement 116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// For Open Source Computer Vision Library 126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Copyright (C) 2000, Intel Corporation, all rights reserved. 146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Third party copyrights are property of their respective owners. 156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Redistribution and use in source and binary forms, with or without modification, 176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// are permitted provided that the following conditions are met: 186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * Redistribution's of source code must retain the above copyright notice, 206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// this list of conditions and the following disclaimer. 216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * Redistribution's in binary form must reproduce the above copyright notice, 236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// this list of conditions and the following disclaimer in the documentation 246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// and/or other materials provided with the distribution. 256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * The name of Intel Corporation may not be used to endorse or promote products 276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// derived from this software without specific prior written permission. 286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// This software is provided by the copyright holders and contributors "as is" and 306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// any express or implied warranties, including, but not limited to, the implied 316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// warranties of merchantability and fitness for a particular purpose are disclaimed. 326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// In no event shall the Intel Corporation or contributors be liable for any direct, 336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// indirect, incidental, special, exemplary, or consequential damages 346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// (including, but not limited to, procurement of substitute goods or services; 356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// loss of use, data, or profits; or business interruption) however caused 366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// and on any theory of liability, whether in contract, strict liability, 376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// or tort (including negligence or otherwise) arising in any way out of 386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// the use of this software, even if advised of the possibility of such damage. 396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//M*/ 416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#include "_cxcore.h" 436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/****************************************************************************************\ 456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn* [scaled] Identity matrix initialization * 466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn\****************************************************************************************/ 476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL void 496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvSetIdentity( CvArr* array, CvScalar value ) 506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvSetIdentity" ); 526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat stub, *mat = (CvMat*)array; 566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize size; 576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, k, len, step; 586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int type, pix_size; 596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar* data = 0; 606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double buf[4]; 616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT( mat )) 636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int coi = 0; 656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( mat = cvGetMat( mat, &stub, &coi )); 666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( coi != 0 ) 676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_BadCOI, "coi is not supported" ); 686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn size = cvGetMatSize( mat ); 716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn len = CV_IMIN( size.width, size.height ); 726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn type = CV_MAT_TYPE(mat->type); 746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pix_size = CV_ELEM_SIZE(type); 756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn size.width *= pix_size; 766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_IS_MAT_CONT( mat->type )) 786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn size.width *= size.height; 806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn size.height = 1; 816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn data = mat->data.ptr; 846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn step = mat->step; 856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( step == 0 ) 866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn step = CV_STUB_STEP; 876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn IPPI_CALL( icvSetZero_8u_C1R( data, step, size )); 886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn step += pix_size; 896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( type == CV_32FC1 ) 916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float val = (float)value.val[0]; 936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float* _data = (float*)data; 946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn step /= sizeof(_data[0]); 956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn len *= step; 966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < len; i += step ) 986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _data[i] = val; 996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( type == CV_64FC1 ) 1016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double val = value.val[0]; 1036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double* _data = (double*)data; 1046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn step /= sizeof(_data[0]); 1056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn len *= step; 1066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < len; i += step ) 1086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _data[i] = val; 1096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 1116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar* val_ptr = (uchar*)buf; 1136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvScalarToRawData( &value, buf, type, 0 ); 1146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn len *= step; 1156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < len; i += step ) 1176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( k = 0; k < pix_size; k++ ) 1186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn data[i+k] = val_ptr[k]; 1196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 1226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 1236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/****************************************************************************************\ 1266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn* Trace of the matrix * 1276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn\****************************************************************************************/ 1286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL CvScalar 1306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvTrace( const CvArr* array ) 1316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 1326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvScalar sum = {{0,0,0,0}}; 1336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvTrace" ); 1356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 1376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat stub, *mat = 0; 1396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_IS_MAT( array )) 1416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn mat = (CvMat*)array; 1436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int type = CV_MAT_TYPE(mat->type); 1446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int size = MIN(mat->rows,mat->cols); 1456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar* data = mat->data.ptr; 1466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( type == CV_32FC1 ) 1486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int step = mat->step + sizeof(float); 1506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( ; size--; data += step ) 1526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sum.val[0] += *(float*)data; 1536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn EXIT; 1546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( type == CV_64FC1 ) 1576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int step = mat->step + sizeof(double); 1596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( ; size--; data += step ) 1616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sum.val[0] += *(double*)data; 1626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn EXIT; 1636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( mat = cvGetDiag( array, &stub )); 1676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( sum = cvSum( mat )); 1686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 1706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return sum; 1726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 1736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/****************************************************************************************\ 1766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn* Matrix transpose * 1776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn\****************************************************************************************/ 1786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/////////////////// macros for inplace transposition of square matrix //////////////////// 1806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define ICV_DEF_TRANSP_INP_CASE_C1( \ 1826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arrtype, len ) \ 1836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ \ 1846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arrtype* arr1 = arr; \ 1856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn step /= sizeof(arr[0]); \ 1866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn \ 1876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn while( --len ) \ 1886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { \ 1896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arr += step, arr1++; \ 1906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arrtype* arr2 = arr; \ 1916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arrtype* arr3 = arr1; \ 1926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn \ 1936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn do \ 1946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { \ 1956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arrtype t0 = arr2[0]; \ 1966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arrtype t1 = arr3[0]; \ 1976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arr2[0] = t1; \ 1986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arr3[0] = t0; \ 1996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn \ 2006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arr2++; \ 2016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arr3 += step; \ 2026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } \ 2036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn while( arr2 != arr3 ); \ 2046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } \ 2056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 2066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define ICV_DEF_TRANSP_INP_CASE_C3( \ 2096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arrtype, len ) \ 2106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ \ 2116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arrtype* arr1 = arr; \ 2126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int y; \ 2136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn step /= sizeof(arr[0]); \ 2146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn \ 2156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( y = 1; y < len; y++ ) \ 2166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { \ 2176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arr += step, arr1 += 3; \ 2186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arrtype* arr2 = arr; \ 2196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arrtype* arr3 = arr1; \ 2206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn \ 2216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( ; arr2!=arr3; arr2+=3, \ 2226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arr3+=step )\ 2236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { \ 2246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arrtype t0 = arr2[0]; \ 2256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arrtype t1 = arr3[0]; \ 2266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arr2[0] = t1; \ 2276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arr3[0] = t0; \ 2286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t0 = arr2[1]; \ 2296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t1 = arr3[1]; \ 2306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arr2[1] = t1; \ 2316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arr3[1] = t0; \ 2326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t0 = arr2[2]; \ 2336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t1 = arr3[2]; \ 2346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arr2[2] = t1; \ 2356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arr3[2] = t0; \ 2366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } \ 2376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } \ 2386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 2396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define ICV_DEF_TRANSP_INP_CASE_C4( \ 2426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arrtype, len ) \ 2436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ \ 2446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arrtype* arr1 = arr; \ 2456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int y; \ 2466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn step /= sizeof(arr[0]); \ 2476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn \ 2486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( y = 1; y < len; y++ ) \ 2496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { \ 2506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arr += step, arr1 += 4; \ 2516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arrtype* arr2 = arr; \ 2526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arrtype* arr3 = arr1; \ 2536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn \ 2546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( ; arr2!=arr3; arr2+=4, \ 2556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arr3+=step )\ 2566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { \ 2576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arrtype t0 = arr2[0]; \ 2586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arrtype t1 = arr3[0]; \ 2596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arr2[0] = t1; \ 2606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arr3[0] = t0; \ 2616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t0 = arr2[1]; \ 2626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t1 = arr3[1]; \ 2636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arr2[1] = t1; \ 2646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arr3[1] = t0; \ 2656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t0 = arr2[2]; \ 2666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t1 = arr3[2]; \ 2676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arr2[2] = t1; \ 2686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arr3[2] = t0; \ 2696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t0 = arr2[3]; \ 2706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t1 = arr3[3]; \ 2716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arr2[3] = t1; \ 2726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arr3[3] = t0; \ 2736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } \ 2746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } \ 2756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 2766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//////////////// macros for non-inplace transposition of rectangular matrix ////////////// 2796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define ICV_DEF_TRANSP_CASE_C1( arrtype ) \ 2816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ \ 2826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int x, y; \ 2836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn srcstep /= sizeof(src[0]); \ 2846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dststep /= sizeof(dst[0]); \ 2856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn \ 2866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( y = 0; y <= size.height - 2; y += 2, \ 2876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn src += 2*srcstep, dst += 2 ) \ 2886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { \ 2896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const arrtype* src1 = src + srcstep; \ 2906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arrtype* dst1 = dst; \ 2916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn \ 2926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = 0; x <= size.width - 2; \ 2936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x += 2, dst1 += dststep ) \ 2946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { \ 2956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arrtype t0 = src[x]; \ 2966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arrtype t1 = src1[x]; \ 2976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst1[0] = t0; \ 2986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst1[1] = t1; \ 2996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst1 += dststep; \ 3006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn \ 3016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t0 = src[x + 1]; \ 3026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t1 = src1[x + 1]; \ 3036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst1[0] = t0; \ 3046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst1[1] = t1; \ 3056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } \ 3066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn \ 3076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( x < size.width ) \ 3086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { \ 3096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arrtype t0 = src[x]; \ 3106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arrtype t1 = src1[x]; \ 3116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst1[0] = t0; \ 3126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst1[1] = t1; \ 3136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } \ 3146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } \ 3156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn \ 3166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( y < size.height ) \ 3176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { \ 3186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arrtype* dst1 = dst; \ 3196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = 0; x <= size.width - 2; \ 3206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x += 2, dst1 += 2*dststep ) \ 3216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { \ 3226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arrtype t0 = src[x]; \ 3236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arrtype t1 = src[x + 1]; \ 3246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst1[0] = t0; \ 3256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst1[dststep] = t1; \ 3266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } \ 3276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn \ 3286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( x < size.width ) \ 3296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { \ 3306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arrtype t0 = src[x]; \ 3316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst1[0] = t0; \ 3326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } \ 3336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } \ 3346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 3356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define ICV_DEF_TRANSP_CASE_C3( arrtype ) \ 3386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ \ 3396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn size.width *= 3; \ 3406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn srcstep /= sizeof(src[0]); \ 3416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dststep /= sizeof(dst[0]); \ 3426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn \ 3436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( ; size.height--; src+=srcstep, dst+=3 )\ 3446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { \ 3456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int x; \ 3466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arrtype* dst1 = dst; \ 3476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn \ 3486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = 0; x < size.width; x += 3, \ 3496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst1 += dststep ) \ 3506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { \ 3516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arrtype t0 = src[x]; \ 3526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arrtype t1 = src[x + 1]; \ 3536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arrtype t2 = src[x + 2]; \ 3546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn \ 3556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst1[0] = t0; \ 3566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst1[1] = t1; \ 3576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst1[2] = t2; \ 3586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } \ 3596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } \ 3606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 3616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define ICV_DEF_TRANSP_CASE_C4( arrtype ) \ 3646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ \ 3656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn size.width *= 4; \ 3666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn srcstep /= sizeof(src[0]); \ 3676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dststep /= sizeof(dst[0]); \ 3686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn \ 3696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( ; size.height--; src+=srcstep, dst+=4 )\ 3706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { \ 3716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int x; \ 3726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arrtype* dst1 = dst; \ 3736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn \ 3746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = 0; x < size.width; x += 4, \ 3756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst1 += dststep ) \ 3766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { \ 3776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arrtype t0 = src[x]; \ 3786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arrtype t1 = src[x + 1]; \ 3796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn \ 3806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst1[0] = t0; \ 3816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst1[1] = t1; \ 3826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn \ 3836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t0 = src[x + 2]; \ 3846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t1 = src[x + 3]; \ 3856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn \ 3866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst1[2] = t0; \ 3876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst1[3] = t1; \ 3886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } \ 3896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } \ 3906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 3916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define ICV_DEF_TRANSP_INP_FUNC( flavor, arrtype, cn ) \ 3946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic CvStatus CV_STDCALL \ 3956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennicvTranspose_##flavor( arrtype* arr, int step, CvSize size )\ 3966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ \ 3976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn assert( size.width == size.height ); \ 3986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn \ 3996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ICV_DEF_TRANSP_INP_CASE_C##cn( arrtype, size.width ) \ 4006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_OK; \ 4016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 4026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define ICV_DEF_TRANSP_FUNC( flavor, arrtype, cn ) \ 4056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic CvStatus CV_STDCALL \ 4066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennicvTranspose_##flavor( const arrtype* src, int srcstep, \ 4076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arrtype* dst, int dststep, CvSize size )\ 4086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ \ 4096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ICV_DEF_TRANSP_CASE_C##cn( arrtype ) \ 4106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_OK; \ 4116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 4126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennICV_DEF_TRANSP_INP_FUNC( 8u_C1IR, uchar, 1 ) 4156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennICV_DEF_TRANSP_INP_FUNC( 8u_C2IR, ushort, 1 ) 4166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennICV_DEF_TRANSP_INP_FUNC( 8u_C3IR, uchar, 3 ) 4176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennICV_DEF_TRANSP_INP_FUNC( 16u_C2IR, int, 1 ) 4186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennICV_DEF_TRANSP_INP_FUNC( 16u_C3IR, ushort, 3 ) 4196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennICV_DEF_TRANSP_INP_FUNC( 32s_C2IR, int64, 1 ) 4206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennICV_DEF_TRANSP_INP_FUNC( 32s_C3IR, int, 3 ) 4216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennICV_DEF_TRANSP_INP_FUNC( 64s_C2IR, int, 4 ) 4226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennICV_DEF_TRANSP_INP_FUNC( 64s_C3IR, int64, 3 ) 4236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennICV_DEF_TRANSP_INP_FUNC( 64s_C4IR, int64, 4 ) 4246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennICV_DEF_TRANSP_FUNC( 8u_C1R, uchar, 1 ) 4276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennICV_DEF_TRANSP_FUNC( 8u_C2R, ushort, 1 ) 4286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennICV_DEF_TRANSP_FUNC( 8u_C3R, uchar, 3 ) 4296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennICV_DEF_TRANSP_FUNC( 16u_C2R, int, 1 ) 4306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennICV_DEF_TRANSP_FUNC( 16u_C3R, ushort, 3 ) 4316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennICV_DEF_TRANSP_FUNC( 32s_C2R, int64, 1 ) 4326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennICV_DEF_TRANSP_FUNC( 32s_C3R, int, 3 ) 4336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennICV_DEF_TRANSP_FUNC( 64s_C2R, int, 4 ) 4346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennICV_DEF_TRANSP_FUNC( 64s_C3R, int64, 3 ) 4356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennICV_DEF_TRANSP_FUNC( 64s_C4R, int64, 4 ) 4366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_DEF_INIT_PIXSIZE_TAB_2D( Transpose, R ) 4386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_DEF_INIT_PIXSIZE_TAB_2D( Transpose, IR ) 4396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL void 4416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvTranspose( const CvArr* srcarr, CvArr* dstarr ) 4426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 4436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn static CvBtFuncTable tab, inp_tab; 4446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn static int inittab = 0; 4456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvTranspose" ); 4476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 4496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat sstub, *src = (CvMat*)srcarr; 4516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat dstub, *dst = (CvMat*)dstarr; 4526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize size; 4536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int type, pix_size; 4546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !inittab ) 4566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvInitTransposeIRTable( &inp_tab ); 4586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvInitTransposeRTable( &tab ); 4596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn inittab = 1; 4606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT( src )) 4636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int coi = 0; 4656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( src = cvGetMat( src, &sstub, &coi )); 4666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( coi != 0 ) 4676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_BadCOI, "coi is not supported" ); 4686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn type = CV_MAT_TYPE( src->type ); 4716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pix_size = CV_ELEM_SIZE(type); 4726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn size = cvGetMatSize( src ); 4736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dstarr == srcarr ) 4756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst = src; 4776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 4796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT( dst )) 4816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int coi = 0; 4836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( dst = cvGetMat( dst, &dstub, &coi )); 4846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( coi != 0 ) 4866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_BadCOI, "coi is not supported" ); 4876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_ARE_TYPES_EQ( src, dst )) 4906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedFormats, "" ); 4916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( size.width != dst->height || size.height != dst->width ) 4936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "" ); 4946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( src->data.ptr == dst->data.ptr ) 4976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( size.width == size.height ) 4996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvFunc2D_1A func = (CvFunc2D_1A)(inp_tab.fn_2d[pix_size]); 5016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !func ) 5036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "" ); 5046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn IPPI_CALL( func( src->data.ptr, src->step, size )); 5066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 5086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( size.width != 1 && size.height != 1 ) 5106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadSize, 5116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "Rectangular matrix can not be transposed inplace" ); 5126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT_CONT( src->type & dst->type )) 5146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadFlag, "In case of inplace column/row transposition " 5156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "both source and destination must be continuous" ); 5166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dst == src ) 5186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int t; 5206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_SWAP( dst->width, dst->height, t ); 5216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst->step = dst->height == 1 ? 0 : pix_size; 5226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 5266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvFunc2D_2A func = (CvFunc2D_2A)(tab.fn_2d[pix_size]); 5286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !func ) 5306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "" ); 5316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn IPPI_CALL( func( src->data.ptr, src->step, 5336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst->data.ptr, dst->step, size )); 5346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 5376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 5386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/****************************************************************************************\ 5416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn* LU decomposition/back substitution * 5426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn\****************************************************************************************/ 5436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL void 5456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvCompleteSymm( CvMat* matrix, int LtoR ) 5466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 5476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvCompleteSymm" ); 5486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 5506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, j, nrows; 5526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ASSERT( CV_IS_MAT(matrix) && matrix->rows == matrix->cols ); 5546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn nrows = matrix->rows; 5566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_MAT_TYPE(matrix->type) == CV_32FC1 || CV_MAT_TYPE(matrix->type) == CV_32SC1 ) 5586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int* data = matrix->data.i; 5606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int step = matrix->step/sizeof(data[0]); 5616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int j0 = 0, j1 = nrows; 5626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < nrows; i++ ) 5636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !LtoR ) j1 = i; else j0 = i+1; 5656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = j0; j < j1; j++ ) 5666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn data[i*step + j] = data[j*step + i]; 5676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( CV_MAT_TYPE(matrix->type) == CV_64FC1 ) 5706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double* data = matrix->data.db; 5726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int step = matrix->step/sizeof(data[0]); 5736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int j0 = 0, j1 = nrows; 5746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < nrows; i++ ) 5756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !LtoR ) j1 = i; else j0 = i+1; 5776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = j0; j < j1; j++ ) 5786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn data[i*step + j] = data[j*step + i]; 5796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 5826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "" ); 5836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 5856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 5866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/****************************************************************************************\ 5896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn* LU decomposition/back substitution * 5906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn\****************************************************************************************/ 5916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define arrtype float 5936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define temptype double 5946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renntypedef CvStatus (CV_STDCALL * CvLUDecompFunc)( double* A, int stepA, CvSize sizeA, 5966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn void* B, int stepB, CvSize sizeB, 5976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double* det ); 5986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renntypedef CvStatus (CV_STDCALL * CvLUBackFunc)( double* A, int stepA, CvSize sizeA, 6006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn void* B, int stepB, CvSize sizeB ); 6016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define ICV_DEF_LU_DECOMP_FUNC( flavor, arrtype ) \ 6046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic CvStatus CV_STDCALL \ 6056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennicvLUDecomp_##flavor( double* A, int stepA, CvSize sizeA, \ 6066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arrtype* B, int stepB, CvSize sizeB, double* _det ) \ 6076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ \ 6086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int n = sizeA.width; \ 6096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int m = 0, i; \ 6106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double det = 1; \ 6116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn \ 6126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn assert( sizeA.width == sizeA.height ); \ 6136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn \ 6146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( B ) \ 6156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { \ 6166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn assert( sizeA.height == sizeB.height ); \ 6176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn m = sizeB.width; \ 6186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } \ 6196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stepA /= sizeof(A[0]); \ 6206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stepB /= sizeof(B[0]); \ 6216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn \ 6226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < n; i++, A += stepA, B += stepB ) \ 6236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { \ 6246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int j, k = i; \ 6256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double* tA = A; \ 6266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arrtype* tB = 0; \ 6276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double kval = fabs(A[i]), tval; \ 6286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn \ 6296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* find the pivot element */ \ 6306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = i + 1; j < n; j++ ) \ 6316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { \ 6326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tA += stepA; \ 6336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tval = fabs(tA[i]); \ 6346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn \ 6356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( tval > kval ) \ 6366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { \ 6376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn kval = tval; \ 6386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn k = j; \ 6396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } \ 6406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } \ 6416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn \ 6426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( kval == 0 ) \ 6436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { \ 6446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn det = 0; \ 6456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; \ 6466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } \ 6476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn \ 6486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* swap rows */ \ 6496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( k != i ) \ 6506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { \ 6516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tA = A + stepA*(k - i); \ 6526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn det = -det; \ 6536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn \ 6546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = i; j < n; j++ ) \ 6556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { \ 6566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double t; \ 6576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_SWAP( A[j], tA[j], t ); \ 6586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } \ 6596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn \ 6606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( m > 0 ) \ 6616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { \ 6626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tB = B + stepB*(k - i); \ 6636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn \ 6646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < m; j++ ) \ 6656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { \ 6666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arrtype t = B[j]; \ 6676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_SWAP( B[j], tB[j], t ); \ 6686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } \ 6696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } \ 6706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } \ 6716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn \ 6726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tval = 1./A[i]; \ 6736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn det *= A[i]; \ 6746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tA = A; \ 6756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tB = B; \ 6766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn A[i] = tval; /* to replace division with multiplication in LUBack */ \ 6776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn \ 6786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* update matrix and the right side of the system */ \ 6796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = i + 1; j < n; j++ ) \ 6806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { \ 6816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tA += stepA; \ 6826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tB += stepB; \ 6836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double alpha = -tA[i]*tval; \ 6846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn \ 6856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( k = i + 1; k < n; k++ ) \ 6866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tA[k] = tA[k] + alpha*A[k]; \ 6876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn \ 6886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( m > 0 ) \ 6896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( k = 0; k < m; k++ ) \ 6906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tB[k] = (arrtype)(tB[k] + alpha*B[k]); \ 6916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } \ 6926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } \ 6936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn \ 6946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( _det ) \ 6956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *_det = det; \ 6966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn \ 6976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_OK; \ 6986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 6996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennICV_DEF_LU_DECOMP_FUNC( 32f, float ) 7026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennICV_DEF_LU_DECOMP_FUNC( 64f, double ) 7036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define ICV_DEF_LU_BACK_FUNC( flavor, arrtype ) \ 7066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic CvStatus CV_STDCALL \ 7076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennicvLUBack_##flavor( double* A, int stepA, CvSize sizeA, \ 7086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arrtype* B, int stepB, CvSize sizeB ) \ 7096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ \ 7106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int n = sizeA.width; \ 7116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int m = sizeB.width, i; \ 7126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn \ 7136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn assert( m > 0 && sizeA.width == sizeA.height && \ 7146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sizeA.height == sizeB.height ); \ 7156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stepA /= sizeof(A[0]); \ 7166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stepB /= sizeof(B[0]); \ 7176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn \ 7186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn A += stepA*(n - 1); \ 7196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn B += stepB*(n - 1); \ 7206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn \ 7216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = n - 1; i >= 0; i--, A -= stepA ) \ 7226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { \ 7236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int j, k; \ 7246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < m; j++ ) \ 7256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { \ 7266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn arrtype* tB = B + j; \ 7276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double x = 0; \ 7286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn \ 7296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( k = n - 1; k > i; k--, tB -= stepB ) \ 7306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x += A[k]*tB[0]; \ 7316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn \ 7326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tB[0] = (arrtype)((tB[0] - x)*A[i]); \ 7336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } \ 7346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } \ 7356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn \ 7366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_OK; \ 7376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 7386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennICV_DEF_LU_BACK_FUNC( 32f, float ) 7416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennICV_DEF_LU_BACK_FUNC( 64f, double ) 7426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic CvFuncTable lu_decomp_tab, lu_back_tab; 7446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic int lu_inittab = 0; 7456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic void icvInitLUTable( CvFuncTable* decomp_tab, 7476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvFuncTable* back_tab ) 7486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 7496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn decomp_tab->fn_2d[0] = (void*)icvLUDecomp_32f; 7506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn decomp_tab->fn_2d[1] = (void*)icvLUDecomp_64f; 7516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn back_tab->fn_2d[0] = (void*)icvLUBack_32f; 7526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn back_tab->fn_2d[1] = (void*)icvLUBack_64f; 7536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 7546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/****************************************************************************************\ 7586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn* Determinant of the matrix * 7596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn\****************************************************************************************/ 7606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define det2(m) (m(0,0)*m(1,1) - m(0,1)*m(1,0)) 7626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define det3(m) (m(0,0)*(m(1,1)*m(2,2) - m(1,2)*m(2,1)) - \ 7636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn m(0,1)*(m(1,0)*m(2,2) - m(1,2)*m(2,0)) + \ 7646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn m(0,2)*(m(1,0)*m(2,1) - m(1,1)*m(2,0))) 7656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL double 7676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvDet( const CvArr* arr ) 7686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 7696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double result = 0; 7706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar* buffer = 0; 7716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int local_alloc = 0; 7726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvDet" ); 7746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 7766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat stub, *mat = (CvMat*)arr; 7786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int type; 7796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT( mat )) 7816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( mat = cvGetMat( mat, &stub )); 7836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn type = CV_MAT_TYPE( mat->type ); 7866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( mat->width != mat->height ) 7886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadSize, "The matrix must be square" ); 7896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn #define Mf( y, x ) ((float*)(m + y*step))[x] 7916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn #define Md( y, x ) ((double*)(m + y*step))[x] 7926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( mat->width == 2 ) 7946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar* m = mat->data.ptr; 7966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int step = mat->step; 7976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( type == CV_32FC1 ) 7996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn result = det2(Mf); 8016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( type == CV_64FC1 ) 8036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn result = det2(Md); 8056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 8076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "" ); 8096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( mat->width == 3 ) 8126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar* m = mat->data.ptr; 8146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int step = mat->step; 8156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( type == CV_32FC1 ) 8176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn result = det3(Mf); 8196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( type == CV_64FC1 ) 8216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn result = det3(Md); 8236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 8256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "" ); 8276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( mat->width == 1 ) 8306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( type == CV_32FC1 ) 8326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn result = mat->data.fl[0]; 8346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( type == CV_64FC1 ) 8366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn result = mat->data.db[0]; 8386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 8406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "" ); 8426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 8456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvLUDecompFunc decomp_func; 8476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize size = cvGetMatSize( mat ); 8486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const int worktype = CV_64FC1; 8496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int buf_size = size.width*size.height*CV_ELEM_SIZE(worktype); 8506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat tmat; 8516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !lu_inittab ) 8536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvInitLUTable( &lu_decomp_tab, &lu_back_tab ); 8556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn lu_inittab = 1; 8566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_MAT_CN( type ) != 1 || CV_MAT_DEPTH( type ) < CV_32F ) 8596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "" ); 8606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( size.width <= CV_MAX_LOCAL_MAT_SIZE ) 8626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn buffer = (uchar*)cvStackAlloc( buf_size ); 8646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn local_alloc = 1; 8656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 8676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( buffer = (uchar*)cvAlloc( buf_size )); 8696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvInitMatHeader( &tmat, size.height, size.width, worktype, buffer )); 8726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( type == worktype ) 8736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvCopy( mat, &tmat )); 8756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 8776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvConvert( mat, &tmat )); 8786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn decomp_func = (CvLUDecompFunc)(lu_decomp_tab.fn_2d[CV_MAT_DEPTH(worktype)-CV_32F]); 8806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn assert( decomp_func ); 8816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn IPPI_CALL( decomp_func( tmat.data.db, tmat.step, size, 0, 0, size, &result )); 8836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn #undef Mf 8866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn #undef Md 8876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /*icvCheckVector_64f( &result, 1 );*/ 8896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 8916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( buffer && !local_alloc ) 8936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &buffer ); 8946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return result; 8966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 8976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/****************************************************************************************\ 9016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn* Inverse (or pseudo-inverse) of the matrix * 9026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn\****************************************************************************************/ 9036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define Sf( y, x ) ((float*)(srcdata + y*srcstep))[x] 9056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define Sd( y, x ) ((double*)(srcdata + y*srcstep))[x] 9066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define Df( y, x ) ((float*)(dstdata + y*dststep))[x] 9076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define Dd( y, x ) ((double*)(dstdata + y*dststep))[x] 9086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL double 9106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvInvert( const CvArr* srcarr, CvArr* dstarr, int method ) 9116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 9126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* u = 0; 9136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* v = 0; 9146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* w = 0; 9156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar* buffer = 0; 9176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int local_alloc = 0; 9186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double result = 0; 9196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvInvert" ); 9216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 9236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat sstub, *src = (CvMat*)srcarr; 9256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat dstub, *dst = (CvMat*)dstarr; 9266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int type; 9276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT( src )) 9296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( src = cvGetMat( src, &sstub )); 9306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT( dst )) 9326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( dst = cvGetMat( dst, &dstub )); 9336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn type = CV_MAT_TYPE( src->type ); 9356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( method == CV_SVD || method == CV_SVD_SYM ) 9376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int n = MIN(src->rows,src->cols); 9396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( method == CV_SVD_SYM && src->rows != src->cols ) 9406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadSize, "CV_SVD_SYM method is used for non-square matrix" ); 9416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( u = cvCreateMat( n, src->rows, src->type )); 9436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( method != CV_SVD_SYM ) 9446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( v = cvCreateMat( n, src->cols, src->type )); 9456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( w = cvCreateMat( n, 1, src->type )); 9466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvSVD( src, w, u, v, CV_SVD_U_T + CV_SVD_V_T )); 9476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( type == CV_32FC1 ) 9496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn result = w->data.fl[0] >= FLT_EPSILON ? 9506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn w->data.fl[w->rows-1]/w->data.fl[0] : 0; 9516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 9526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn result = w->data.db[0] >= FLT_EPSILON ? 9536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn w->data.db[w->rows-1]/w->data.db[0] : 0; 9546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvSVBkSb( w, u, v ? v : u, 0, dst, CV_SVD_U_T + CV_SVD_V_T )); 9566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn EXIT; 9576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( method != CV_LU ) 9596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadArg, "Unknown inversion method" ); 9606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_ARE_TYPES_EQ( src, dst )) 9626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedFormats, "" ); 9636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( src->width != src->height ) 9656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadSize, "The matrix must be square" ); 9666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_ARE_SIZES_EQ( src, dst )) 9686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "" ); 9696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( type != CV_32FC1 && type != CV_64FC1 ) 9716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "" ); 9726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( src->width <= 3 ) 9746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar* srcdata = src->data.ptr; 9766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar* dstdata = dst->data.ptr; 9776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int srcstep = src->step; 9786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int dststep = dst->step; 9796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( src->width == 2 ) 9816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( type == CV_32FC1 ) 9836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double d = det2(Sf); 9856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( d != 0. ) 9866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double t0, t1; 9886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn result = d; 9896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d = 1./d; 9906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t0 = Sf(0,0)*d; 9916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t1 = Sf(1,1)*d; 9926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Df(1,1) = (float)t0; 9936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Df(0,0) = (float)t1; 9946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t0 = -Sf(0,1)*d; 9956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t1 = -Sf(1,0)*d; 9966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Df(0,1) = (float)t0; 9976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Df(1,0) = (float)t1; 9986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 10016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double d = det2(Sd); 10036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( d != 0. ) 10046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double t0, t1; 10066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn result = d; 10076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d = 1./d; 10086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t0 = Sd(0,0)*d; 10096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t1 = Sd(1,1)*d; 10106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Dd(1,1) = t0; 10116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Dd(0,0) = t1; 10126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t0 = -Sd(0,1)*d; 10136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t1 = -Sd(1,0)*d; 10146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Dd(0,1) = t0; 10156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Dd(1,0) = t1; 10166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( src->width == 3 ) 10206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( type == CV_32FC1 ) 10226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double d = det3(Sf); 10246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( d != 0. ) 10256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float t[9]; 10276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn result = d; 10286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d = 1./d; 10296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t[0] = (float)((Sf(1,1) * Sf(2,2) - Sf(1,2) * Sf(2,1)) * d); 10316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t[1] = (float)((Sf(0,2) * Sf(2,1) - Sf(0,1) * Sf(2,2)) * d); 10326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t[2] = (float)((Sf(0,1) * Sf(1,2) - Sf(0,2) * Sf(1,1)) * d); 10336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t[3] = (float)((Sf(1,2) * Sf(2,0) - Sf(1,0) * Sf(2,2)) * d); 10356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t[4] = (float)((Sf(0,0) * Sf(2,2) - Sf(0,2) * Sf(2,0)) * d); 10366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t[5] = (float)((Sf(0,2) * Sf(1,0) - Sf(0,0) * Sf(1,2)) * d); 10376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t[6] = (float)((Sf(1,0) * Sf(2,1) - Sf(1,1) * Sf(2,0)) * d); 10396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t[7] = (float)((Sf(0,1) * Sf(2,0) - Sf(0,0) * Sf(2,1)) * d); 10406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t[8] = (float)((Sf(0,0) * Sf(1,1) - Sf(0,1) * Sf(1,0)) * d); 10416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Df(0,0) = t[0]; Df(0,1) = t[1]; Df(0,2) = t[2]; 10436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Df(1,0) = t[3]; Df(1,1) = t[4]; Df(1,2) = t[5]; 10446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Df(2,0) = t[6]; Df(2,1) = t[7]; Df(2,2) = t[8]; 10456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 10486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double d = det3(Sd); 10506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( d != 0. ) 10516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double t[9]; 10536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn result = d; 10546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d = 1./d; 10556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t[0] = (Sd(1,1) * Sd(2,2) - Sd(1,2) * Sd(2,1)) * d; 10576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t[1] = (Sd(0,2) * Sd(2,1) - Sd(0,1) * Sd(2,2)) * d; 10586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t[2] = (Sd(0,1) * Sd(1,2) - Sd(0,2) * Sd(1,1)) * d; 10596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t[3] = (Sd(1,2) * Sd(2,0) - Sd(1,0) * Sd(2,2)) * d; 10616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t[4] = (Sd(0,0) * Sd(2,2) - Sd(0,2) * Sd(2,0)) * d; 10626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t[5] = (Sd(0,2) * Sd(1,0) - Sd(0,0) * Sd(1,2)) * d; 10636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t[6] = (Sd(1,0) * Sd(2,1) - Sd(1,1) * Sd(2,0)) * d; 10656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t[7] = (Sd(0,1) * Sd(2,0) - Sd(0,0) * Sd(2,1)) * d; 10666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t[8] = (Sd(0,0) * Sd(1,1) - Sd(0,1) * Sd(1,0)) * d; 10676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Dd(0,0) = t[0]; Dd(0,1) = t[1]; Dd(0,2) = t[2]; 10696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Dd(1,0) = t[3]; Dd(1,1) = t[4]; Dd(1,2) = t[5]; 10706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Dd(2,0) = t[6]; Dd(2,1) = t[7]; Dd(2,2) = t[8]; 10716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 10756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn assert( src->width == 1 ); 10776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( type == CV_32FC1 ) 10796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double d = Sf(0,0); 10816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( d != 0. ) 10826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn result = d; 10846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Df(0,0) = (float)(1./d); 10856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 10886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double d = Sd(0,0); 10906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( d != 0. ) 10916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn result = d; 10936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Dd(0,0) = 1./d; 10946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 10996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvLUDecompFunc decomp_func; 11016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvLUBackFunc back_func; 11026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize size = cvGetMatSize( src ); 11036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const int worktype = CV_64FC1; 11046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int buf_size = size.width*size.height*CV_ELEM_SIZE(worktype); 11056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat tmat; 11066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !lu_inittab ) 11086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvInitLUTable( &lu_decomp_tab, &lu_back_tab ); 11106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn lu_inittab = 1; 11116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( size.width <= CV_MAX_LOCAL_MAT_SIZE ) 11146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn buffer = (uchar*)cvStackAlloc( buf_size ); 11166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn local_alloc = 1; 11176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 11196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( buffer = (uchar*)cvAlloc( buf_size )); 11216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvInitMatHeader( &tmat, size.height, size.width, worktype, buffer )); 11246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( type == worktype ) 11256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvCopy( src, &tmat )); 11276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 11296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvConvert( src, &tmat )); 11306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvSetIdentity( dst )); 11316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn decomp_func = (CvLUDecompFunc)(lu_decomp_tab.fn_2d[CV_MAT_DEPTH(type)-CV_32F]); 11336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn back_func = (CvLUBackFunc)(lu_back_tab.fn_2d[CV_MAT_DEPTH(type)-CV_32F]); 11346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn assert( decomp_func && back_func ); 11356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn IPPI_CALL( decomp_func( tmat.data.db, tmat.step, size, 11376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst->data.ptr, dst->step, size, &result )); 11386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( result != 0 ) 11406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn IPPI_CALL( back_func( tmat.data.db, tmat.step, size, 11426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst->data.ptr, dst->step, size )); 11436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !result ) 11476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvSetZero( dst )); 11486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 11506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( buffer && !local_alloc ) 11526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &buffer ); 11536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( u || v || w ) 11556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &u ); 11576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &v ); 11586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &w ); 11596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return result; 11626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 11636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/****************************************************************************************\ 11666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn* Linear system [least-squares] solution * 11676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn\****************************************************************************************/ 11686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic void 11706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennicvLSQ( const CvMat* A, const CvMat* B, CvMat* X ) 11716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 11726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* AtA = 0; 11736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* AtB = 0; 11746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* W = 0; 11756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* V = 0; 11766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "icvLSQ" ); 11786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 11806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(A) || !CV_IS_MAT(B) || !CV_IS_MAT(X) ) 11826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadArg, "Some of required arguments is not a valid matrix" ); 11836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn AtA = cvCreateMat( A->cols, A->cols, A->type ); 11856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn AtB = cvCreateMat( A->cols, 1, A->type ); 11866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn W = cvCreateMat( A->cols, 1, A->type ); 11876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn V = cvCreateMat( A->cols, A->cols, A->type ); 11886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMulTransposed( A, AtA, 1 ); 11906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGEMM( A, B, 1, 0, 0, AtB, CV_GEMM_A_T ); 11916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSVD( AtA, W, 0, V, CV_SVD_MODIFY_A + CV_SVD_V_T ); 11926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSVBkSb( W, V, V, AtB, X, CV_SVD_U_T + CV_SVD_V_T ); 11936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 11956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &AtA ); 11976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &AtB ); 11986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &W ); 11996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &V ); 12006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 12016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL int 12036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvSolve( const CvArr* A, const CvArr* b, CvArr* x, int method ) 12046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 12056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* u = 0; 12066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* v = 0; 12076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* w = 0; 12086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar* buffer = 0; 12106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int local_alloc = 0; 12116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int result = 1; 12126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvSolve" ); 12146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 12166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat sstub, *src = (CvMat*)A; 12186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat dstub, *dst = (CvMat*)x; 12196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat bstub, *src2 = (CvMat*)b; 12206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int type; 12216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT( src )) 12236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( src = cvGetMat( src, &sstub )); 12246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT( src2 )) 12266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( src2 = cvGetMat( src2, &bstub )); 12276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT( dst )) 12296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( dst = cvGetMat( dst, &dstub )); 12306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( method & CV_LSQ ) 12326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvLSQ( src, src2, dst ); 12346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn EXIT; 12356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( method == CV_SVD || method == CV_SVD_SYM ) 12386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int n = MIN(src->rows,src->cols); 12406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( method == CV_SVD_SYM && src->rows != src->cols ) 12426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadSize, "CV_SVD_SYM method is used for non-square matrix" ); 12436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( u = cvCreateMat( n, src->rows, src->type )); 12456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( method != CV_SVD_SYM ) 12466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( v = cvCreateMat( n, src->cols, src->type )); 12476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( w = cvCreateMat( n, 1, src->type )); 12486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvSVD( src, w, u, v, CV_SVD_U_T + CV_SVD_V_T )); 12496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvSVBkSb( w, u, v ? v : u, src2, dst, CV_SVD_U_T + CV_SVD_V_T )); 12506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn EXIT; 12516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( method != CV_LU ) 12536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadArg, "Unknown inversion method" ); 12546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn type = CV_MAT_TYPE( src->type ); 12566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_ARE_TYPES_EQ( src, dst ) || !CV_ARE_TYPES_EQ( src, src2 )) 12586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedFormats, "" ); 12596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( src->width != src->height ) 12616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadSize, "The matrix must be square" ); 12626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_ARE_SIZES_EQ( src2, dst ) || src->width != src2->height ) 12646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "" ); 12656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( type != CV_32FC1 && type != CV_64FC1 ) 12676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "" ); 12686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // check case of a single equation and small matrix 12706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( src->width <= 3 && src2->width == 1 ) 12716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn #define bf(y) ((float*)(bdata + y*src2step))[0] 12736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn #define bd(y) ((double*)(bdata + y*src2step))[0] 12746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar* srcdata = src->data.ptr; 12766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar* bdata = src2->data.ptr; 12776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar* dstdata = dst->data.ptr; 12786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int srcstep = src->step; 12796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int src2step = src2->step; 12806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int dststep = dst->step; 12816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( src->width == 2 ) 12836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( type == CV_32FC1 ) 12856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double d = det2(Sf); 12876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( d != 0. ) 12886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float t; 12906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d = 1./d; 12916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t = (float)((bf(0)*Sf(1,1) - bf(1)*Sf(0,1))*d); 12926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Df(1,0) = (float)((bf(1)*Sf(0,0) - bf(0)*Sf(1,0))*d); 12936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Df(0,0) = t; 12946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 12966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn result = 0; 12976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 12996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double d = det2(Sd); 13016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( d != 0. ) 13026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double t; 13046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d = 1./d; 13056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t = (bd(0)*Sd(1,1) - bd(1)*Sd(0,1))*d; 13066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Dd(1,0) = (bd(1)*Sd(0,0) - bd(0)*Sd(1,0))*d; 13076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Dd(0,0) = t; 13086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 13106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn result = 0; 13116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( src->width == 3 ) 13146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( type == CV_32FC1 ) 13166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double d = det3(Sf); 13186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( d != 0. ) 13196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float t[3]; 13216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d = 1./d; 13226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t[0] = (float)(d* 13246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (bf(0)*(Sf(1,1)*Sf(2,2) - Sf(1,2)*Sf(2,1)) - 13256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Sf(0,1)*(bf(1)*Sf(2,2) - Sf(1,2)*bf(2)) + 13266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Sf(0,2)*(bf(1)*Sf(2,1) - Sf(1,1)*bf(2)))); 13276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t[1] = (float)(d* 13296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (Sf(0,0)*(bf(1)*Sf(2,2) - Sf(1,2)*bf(2)) - 13306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn bf(0)*(Sf(1,0)*Sf(2,2) - Sf(1,2)*Sf(2,0)) + 13316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Sf(0,2)*(Sf(1,0)*bf(2) - bf(1)*Sf(2,0)))); 13326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t[2] = (float)(d* 13346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (Sf(0,0)*(Sf(1,1)*bf(2) - bf(1)*Sf(2,1)) - 13356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Sf(0,1)*(Sf(1,0)*bf(2) - bf(1)*Sf(2,0)) + 13366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn bf(0)*(Sf(1,0)*Sf(2,1) - Sf(1,1)*Sf(2,0)))); 13376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Df(0,0) = t[0]; 13396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Df(1,0) = t[1]; 13406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Df(2,0) = t[2]; 13416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 13436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn result = 0; 13446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 13466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double d = det3(Sd); 13486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( d != 0. ) 13496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double t[9]; 13516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d = 1./d; 13536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t[0] = ((Sd(1,1) * Sd(2,2) - Sd(1,2) * Sd(2,1))*bd(0) + 13556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (Sd(0,2) * Sd(2,1) - Sd(0,1) * Sd(2,2))*bd(1) + 13566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (Sd(0,1) * Sd(1,2) - Sd(0,2) * Sd(1,1))*bd(2))*d; 13576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t[1] = ((Sd(1,2) * Sd(2,0) - Sd(1,0) * Sd(2,2))*bd(0) + 13596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (Sd(0,0) * Sd(2,2) - Sd(0,2) * Sd(2,0))*bd(1) + 13606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (Sd(0,2) * Sd(1,0) - Sd(0,0) * Sd(1,2))*bd(2))*d; 13616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t[2] = ((Sd(1,0) * Sd(2,1) - Sd(1,1) * Sd(2,0))*bd(0) + 13636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (Sd(0,1) * Sd(2,0) - Sd(0,0) * Sd(2,1))*bd(1) + 13646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (Sd(0,0) * Sd(1,1) - Sd(0,1) * Sd(1,0))*bd(2))*d; 13656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Dd(0,0) = t[0]; 13676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Dd(1,0) = t[1]; 13686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Dd(2,0) = t[2]; 13696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 13716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn result = 0; 13726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 13756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn assert( src->width == 1 ); 13776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( type == CV_32FC1 ) 13796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double d = Sf(0,0); 13816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( d != 0. ) 13826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Df(0,0) = (float)(bf(0)/d); 13836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 13846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn result = 0; 13856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 13876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double d = Sd(0,0); 13896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( d != 0. ) 13906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Dd(0,0) = (bd(0)/d); 13916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 13926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn result = 0; 13936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 13976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvLUDecompFunc decomp_func; 13996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvLUBackFunc back_func; 14006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize size = cvGetMatSize( src ); 14016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize dstsize = cvGetMatSize( dst ); 14026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int worktype = CV_64FC1; 14036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int buf_size = size.width*size.height*CV_ELEM_SIZE(worktype); 14046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double d = 0; 14056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat tmat; 14066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !lu_inittab ) 14086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 14096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvInitLUTable( &lu_decomp_tab, &lu_back_tab ); 14106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn lu_inittab = 1; 14116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 14126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( size.width <= CV_MAX_LOCAL_MAT_SIZE ) 14146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 14156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn buffer = (uchar*)cvStackAlloc( buf_size ); 14166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn local_alloc = 1; 14176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 14186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 14196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 14206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( buffer = (uchar*)cvAlloc( buf_size )); 14216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 14226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvInitMatHeader( &tmat, size.height, size.width, worktype, buffer )); 14246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( type == worktype ) 14256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 14266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvCopy( src, &tmat )); 14276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 14286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 14296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvConvert( src, &tmat )); 14306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( src2->data.ptr != dst->data.ptr ) 14326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 14336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvCopy( src2, dst )); 14346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 14356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn decomp_func = (CvLUDecompFunc)(lu_decomp_tab.fn_2d[CV_MAT_DEPTH(type)-CV_32F]); 14376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn back_func = (CvLUBackFunc)(lu_back_tab.fn_2d[CV_MAT_DEPTH(type)-CV_32F]); 14386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn assert( decomp_func && back_func ); 14396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn IPPI_CALL( decomp_func( tmat.data.db, tmat.step, size, 14416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst->data.ptr, dst->step, dstsize, &d )); 14426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( d != 0 ) 14446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 14456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn IPPI_CALL( back_func( tmat.data.db, tmat.step, size, 14466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst->data.ptr, dst->step, dstsize )); 14476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 14486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 14496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn result = 0; 14506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 14516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !result ) 14536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvSetZero( dst )); 14546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 14566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( buffer && !local_alloc ) 14586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &buffer ); 14596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( u || v || w ) 14616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 14626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &u ); 14636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &v ); 14646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &w ); 14656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 14666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return result; 14686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 14696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/****************************************************************************************\ 14736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn* 3D vector cross-product * 14746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn\****************************************************************************************/ 14756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL void 14776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvCrossProduct( const CvArr* srcAarr, const CvArr* srcBarr, CvArr* dstarr ) 14786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 14796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvCrossProduct" ); 14806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 14826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat stubA, *srcA = (CvMat*)srcAarr; 14846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat stubB, *srcB = (CvMat*)srcBarr; 14856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat dstub, *dst = (CvMat*)dstarr; 14866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int type; 14876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(srcA)) 14896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( srcA = cvGetMat( srcA, &stubA )); 14906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn type = CV_MAT_TYPE( srcA->type ); 14926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( srcA->width*srcA->height*CV_MAT_CN(type) != 3 ) 14946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadArg, "All the input arrays must be continuous 3-vectors" ); 14956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !srcB || !dst ) 14976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "" ); 14986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (srcA->type & ~CV_MAT_CONT_FLAG) == (srcB->type & ~CV_MAT_CONT_FLAG) && 15006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (srcA->type & ~CV_MAT_CONT_FLAG) == (dst->type & ~CV_MAT_CONT_FLAG) ) 15016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 15026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !srcB->data.ptr || !dst->data.ptr ) 15036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "" ); 15046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 15056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 15066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 15076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(srcB)) 15086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( srcB = cvGetMat( srcB, &stubB )); 15096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(dst)) 15116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( dst = cvGetMat( dst, &dstub )); 15126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_ARE_TYPES_EQ( srcA, srcB ) || 15146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn !CV_ARE_TYPES_EQ( srcB, dst )) 15156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedFormats, "" ); 15166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 15176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_ARE_SIZES_EQ( srcA, srcB ) || !CV_ARE_SIZES_EQ( srcB, dst )) 15196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "" ); 15206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_MAT_DEPTH(type) == CV_32F ) 15226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 15236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float* dstdata = (float*)(dst->data.ptr); 15246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const float* src1data = (float*)(srcA->data.ptr); 15256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const float* src2data = (float*)(srcB->data.ptr); 15266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_IS_MAT_CONT(srcA->type & srcB->type & dst->type) ) 15286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 15296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dstdata[2] = src1data[0] * src2data[1] - src1data[1] * src2data[0]; 15306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dstdata[0] = src1data[1] * src2data[2] - src1data[2] * src2data[1]; 15316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dstdata[1] = src1data[2] * src2data[0] - src1data[0] * src2data[2]; 15326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 15336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 15346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 15356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int step1 = srcA->step ? srcA->step/sizeof(src1data[0]) : 1; 15366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int step2 = srcB->step ? srcB->step/sizeof(src1data[0]) : 1; 15376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int step = dst->step ? dst->step/sizeof(src1data[0]) : 1; 15386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dstdata[2*step] = src1data[0] * src2data[step2] - src1data[step1] * src2data[0]; 15406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dstdata[0] = src1data[step1] * src2data[step2*2] - src1data[step1*2] * src2data[step2]; 15416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dstdata[step] = src1data[step1*2] * src2data[0] - src1data[0] * src2data[step2*2]; 15426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 15436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 15446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( CV_MAT_DEPTH(type) == CV_64F ) 15456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 15466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double* dstdata = (double*)(dst->data.ptr); 15476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const double* src1data = (double*)(srcA->data.ptr); 15486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const double* src2data = (double*)(srcB->data.ptr); 15496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_IS_MAT_CONT(srcA->type & srcB->type & dst->type) ) 15516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 15526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dstdata[2] = src1data[0] * src2data[1] - src1data[1] * src2data[0]; 15536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dstdata[0] = src1data[1] * src2data[2] - src1data[2] * src2data[1]; 15546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dstdata[1] = src1data[2] * src2data[0] - src1data[0] * src2data[2]; 15556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 15566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 15576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 15586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int step1 = srcA->step ? srcA->step/sizeof(src1data[0]) : 1; 15596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int step2 = srcB->step ? srcB->step/sizeof(src1data[0]) : 1; 15606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int step = dst->step ? dst->step/sizeof(src1data[0]) : 1; 15616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dstdata[2*step] = src1data[0] * src2data[step2] - src1data[step1] * src2data[0]; 15636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dstdata[0] = src1data[step1] * src2data[step2*2] - src1data[step1*2] * src2data[step2]; 15646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dstdata[step] = src1data[step1*2] * src2data[0] - src1data[0] * src2data[step2*2]; 15656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 15666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 15676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 15686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 15696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "" ); 15706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 15716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 15736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 15746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL void 15776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvCalcPCA( const CvArr* data_arr, CvArr* avg_arr, CvArr* eigenvals, CvArr* eigenvects, int flags ) 15786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 15796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* tmp_avg = 0; 15806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* tmp_avg_r = 0; 15816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* tmp_cov = 0; 15826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* tmp_evals = 0; 15836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* tmp_evects = 0; 15846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* tmp_evects2 = 0; 15856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* tmp_data = 0; 15866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvCalcPCA" ); 15886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 15906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat stub, *data = (CvMat*)data_arr; 15926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat astub, *avg = (CvMat*)avg_arr; 15936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat evalstub, *evals = (CvMat*)eigenvals; 15946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat evectstub, *evects = (CvMat*)eigenvects; 15956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int covar_flags = CV_COVAR_SCALE; 15966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, len, in_count, count, out_count; 15976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(data) ) 15996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( data = cvGetMat( data, &stub )); 16006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(avg) ) 16026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( avg = cvGetMat( avg, &astub )); 16036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(evals) ) 16056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( evals = cvGetMat( evals, &evalstub )); 16066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(evects) ) 16086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( evects = cvGetMat( evects, &evectstub )); 16096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_MAT_CN(data->type) != 1 || CV_MAT_CN(avg->type) != 1 || 16116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_MAT_CN(evals->type) != 1 || CV_MAT_CN(evects->type) != 1 ) 16126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "All the input and output arrays must be 1-channel" ); 16136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_MAT_DEPTH(avg->type) < CV_32F || !CV_ARE_DEPTHS_EQ(avg, evals) || 16156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn !CV_ARE_DEPTHS_EQ(avg, evects) ) 16166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "All the output arrays must have the same type, 32fC1 or 64fC1" ); 16176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( flags & CV_PCA_DATA_AS_COL ) 16196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 16206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn len = data->rows; 16216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn in_count = data->cols; 16226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn covar_flags |= CV_COVAR_COLS; 16236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( avg->cols != 1 || avg->rows != len ) 16256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadSize, 16266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "The mean (average) vector should be data->rows x 1 when CV_PCA_DATA_AS_COL is used" ); 16276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( tmp_avg = cvCreateMat( len, 1, CV_64F )); 16296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 16306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 16316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 16326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn len = data->cols; 16336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn in_count = data->rows; 16346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn covar_flags |= CV_COVAR_ROWS; 16356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( avg->rows != 1 || avg->cols != len ) 16376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadSize, 16386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "The mean (average) vector should be 1 x data->cols when CV_PCA_DATA_AS_ROW is used" ); 16396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( tmp_avg = cvCreateMat( 1, len, CV_64F )); 16416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 16426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn count = MIN(len, in_count); 16446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn out_count = evals->cols + evals->rows - 1; 16456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (evals->cols != 1 && evals->rows != 1) || out_count > count ) 16476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadSize, 16486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "The array of eigenvalues must be 1d vector containing " 16496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "no more than min(data->rows,data->cols) elements" ); 16506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( evects->cols != len || evects->rows != out_count ) 16526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadSize, 16536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "The matrix of eigenvalues must have the same number of columns as the input vector length " 16546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "and the same number of rows as the number of eigenvalues" ); 16556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // "scrambled" way to compute PCA (when cols(A)>rows(A)): 16576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // B = A'A; B*x=b*x; C = AA'; C*y=c*y -> AA'*y=c*y -> A'A*(A'*y)=c*(A'*y) -> c = b, x=A'*y 16586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( len <= in_count ) 16596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn covar_flags |= CV_COVAR_NORMAL; 16606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( flags & CV_PCA_USE_AVG ){ 16626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn covar_flags |= CV_COVAR_USE_AVG; 16636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvConvert( avg, tmp_avg ) ); 16646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 16656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( tmp_cov = cvCreateMat( count, count, CV_64F )); 16676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( tmp_evals = cvCreateMat( 1, count, CV_64F )); 16686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( tmp_evects = cvCreateMat( count, count, CV_64F )); 16696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvCalcCovarMatrix( &data_arr, 0, tmp_cov, tmp_avg, covar_flags )); 16716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvSVD( tmp_cov, tmp_evals, tmp_evects, 0, CV_SVD_MODIFY_A + CV_SVD_U_T )); 16726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tmp_evects->rows = out_count; 16736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tmp_evals->cols = out_count; 16746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvZero( evects ); 16756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvZero( evals ); 16766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( covar_flags & CV_COVAR_NORMAL ) 16786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 16796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvConvert( tmp_evects, evects )); 16806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 16816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 16826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 16836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // CV_PCA_DATA_AS_ROW: cols(A)>rows(A). x=A'*y -> x'=y'*A 16846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // CV_PCA_DATA_AS_COL: rows(A)>cols(A). x=A''*y -> x'=y'*A' 16856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int block_count = 0; 16866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( tmp_data = cvCreateMat( count, count, CV_64F )); 16886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( tmp_avg_r = cvCreateMat( count, count, CV_64F )); 16896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( tmp_evects2 = cvCreateMat( out_count, count, CV_64F )); 16906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < len; i += block_count ) 16926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 16936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat data_part, tdata_part, part, dst_part, avg_part, tmp_avg_part; 16946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int gemm_flags; 16956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn block_count = MIN( count, len - i ); 16976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( flags & CV_PCA_DATA_AS_COL ) 16996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 17006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetRows( data, &data_part, i, i + block_count ); 17016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetRows( tmp_data, &tdata_part, 0, block_count ); 17026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetRows( tmp_avg, &avg_part, i, i + block_count ); 17036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetRows( tmp_avg_r, &tmp_avg_part, 0, block_count ); 17046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn gemm_flags = CV_GEMM_B_T; 17056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 17066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 17076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 17086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetCols( data, &data_part, i, i + block_count ); 17096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetCols( tmp_data, &tdata_part, 0, block_count ); 17106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetCols( tmp_avg, &avg_part, i, i + block_count ); 17116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetCols( tmp_avg_r, &tmp_avg_part, 0, block_count ); 17126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn gemm_flags = 0; 17136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 17146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetCols( tmp_evects2, &part, 0, block_count ); 17166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetCols( evects, &dst_part, i, i + block_count ); 17176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( &data_part, &tdata_part ); 17196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvRepeat( &avg_part, &tmp_avg_part ); 17206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSub( &tdata_part, &tmp_avg_part, &tdata_part ); 17216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGEMM( tmp_evects, &tdata_part, 1, 0, 0, &part, gemm_flags ); 17226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( &part, &dst_part ); 17236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 17246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // normalize eigenvectors 17266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < out_count; i++ ) 17276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 17286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat ei; 17296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetRow( evects, &ei, i ); 17306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvNormalize( &ei, &ei ); 17316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 17326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 17336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( tmp_evals->rows != evals->rows ) 17356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReshape( tmp_evals, tmp_evals, 1, evals->rows ); 17366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( tmp_evals, evals ); 17376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( tmp_avg, avg ); 17386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 17406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &tmp_avg ); 17426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &tmp_avg_r ); 17436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &tmp_cov ); 17446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &tmp_evals ); 17456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &tmp_evects ); 17466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &tmp_evects2 ); 17476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &tmp_data ); 17486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 17496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL void 17526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvProjectPCA( const CvArr* data_arr, const CvArr* avg_arr, 17536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvArr* eigenvects, CvArr* result_arr ) 17546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 17556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar* buffer = 0; 17566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int local_alloc = 0; 17576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvProjectPCA" ); 17596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 17616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat stub, *data = (CvMat*)data_arr; 17636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat astub, *avg = (CvMat*)avg_arr; 17646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat evectstub, *evects = (CvMat*)eigenvects; 17656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat rstub, *result = (CvMat*)result_arr; 17666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat avg_repeated; 17676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, len, in_count; 17686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int gemm_flags, as_cols, convert_data; 17696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int block_count0, block_count, buf_size, elem_size; 17706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar* tmp_data_ptr; 17716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(data) ) 17736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( data = cvGetMat( data, &stub )); 17746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(avg) ) 17766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( avg = cvGetMat( avg, &astub )); 17776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(evects) ) 17796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( evects = cvGetMat( evects, &evectstub )); 17806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(result) ) 17826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( result = cvGetMat( result, &rstub )); 17836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_MAT_CN(data->type) != 1 || CV_MAT_CN(avg->type) != 1 ) 17856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "All the input and output arrays must be 1-channel" ); 17866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (CV_MAT_TYPE(avg->type) != CV_32FC1 && CV_MAT_TYPE(avg->type) != CV_64FC1) || 17886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn !CV_ARE_TYPES_EQ(avg, evects) || !CV_ARE_TYPES_EQ(avg, result) ) 17896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, 17906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "All the input and output arrays (except for data) must have the same type, 32fC1 or 64fC1" ); 17916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (avg->cols != 1 || avg->rows != data->rows) && 17936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (avg->rows != 1 || avg->cols != data->cols) ) 17946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadSize, 17956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "The mean (average) vector should be either 1 x data->cols or data->rows x 1" ); 17966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( avg->cols == 1 ) 17986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 17996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn len = data->rows; 18006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn in_count = data->cols; 18016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn gemm_flags = CV_GEMM_A_T + CV_GEMM_B_T; 18036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn as_cols = 1; 18046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 18056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 18066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 18076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn len = data->cols; 18086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn in_count = data->rows; 18096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn gemm_flags = CV_GEMM_B_T; 18116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn as_cols = 0; 18126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 18136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( evects->cols != len ) 18156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, 18166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "Eigenvectors must be stored as rows and be of the same size as input vectors" ); 18176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( result->cols > evects->rows ) 18196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, 18206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "The output matrix of coefficients must have the number of columns " 18216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "less than or equal to the number of eigenvectors (number of rows in eigenvectors matrix)" ); 18226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn evects = cvGetRows( evects, &evectstub, 0, result->cols ); 18246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn block_count0 = (1 << 16)/len; 18266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn block_count0 = MAX( block_count0, 4 ); 18276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn block_count0 = MIN( block_count0, in_count ); 18286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn elem_size = CV_ELEM_SIZE(avg->type); 18296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn convert_data = CV_MAT_DEPTH(data->type) < CV_MAT_DEPTH(avg->type); 18306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn buf_size = block_count0*len*((block_count0 > 1) + 1)*elem_size; 18326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( buf_size < CV_MAX_LOCAL_SIZE ) 18346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 18356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn buffer = (uchar*)cvStackAlloc( buf_size ); 18366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn local_alloc = 1; 18376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 18386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 18396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( buffer = (uchar*)cvAlloc( buf_size )); 18406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tmp_data_ptr = buffer; 18426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( block_count0 > 1 ) 18436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 18446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn avg_repeated = cvMat( as_cols ? len : block_count0, 18456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn as_cols ? block_count0 : len, avg->type, buffer ); 18466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvRepeat( avg, &avg_repeated ); 18476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tmp_data_ptr += block_count0*len*elem_size; 18486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 18496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 18506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn avg_repeated = *avg; 18516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < in_count; i += block_count ) 18536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 18546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat data_part, norm_data, avg_part, *src = &data_part, out_part; 18556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn block_count = MIN( block_count0, in_count - i ); 18576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( as_cols ) 18586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 18596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetCols( data, &data_part, i, i + block_count ); 18606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetCols( &avg_repeated, &avg_part, 0, block_count ); 18616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn norm_data = cvMat( len, block_count, avg->type, tmp_data_ptr ); 18626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 18636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 18646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 18656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetRows( data, &data_part, i, i + block_count ); 18666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetRows( &avg_repeated, &avg_part, 0, block_count ); 18676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn norm_data = cvMat( block_count, len, avg->type, tmp_data_ptr ); 18686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 18696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( convert_data ) 18716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 18726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( src, &norm_data ); 18736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn src = &norm_data; 18746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 18756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSub( src, &avg_part, &norm_data ); 18776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetRows( result, &out_part, i, i + block_count ); 18796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGEMM( &norm_data, evects, 1, 0, 0, &out_part, gemm_flags ); 18806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 18816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 18836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !local_alloc ) 18856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &buffer ); 18866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 18876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL void 18906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvBackProjectPCA( const CvArr* proj_arr, const CvArr* avg_arr, 18916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvArr* eigenvects, CvArr* result_arr ) 18926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 18936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar* buffer = 0; 18946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int local_alloc = 0; 18956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvProjectPCA" ); 18976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 18996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat pstub, *data = (CvMat*)proj_arr; 19016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat astub, *avg = (CvMat*)avg_arr; 19026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat evectstub, *evects = (CvMat*)eigenvects; 19036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat rstub, *result = (CvMat*)result_arr; 19046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat avg_repeated; 19056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, len, in_count, as_cols; 19066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int block_count0, block_count, buf_size, elem_size; 19076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(data) ) 19096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( data = cvGetMat( data, &pstub )); 19106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(avg) ) 19126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( avg = cvGetMat( avg, &astub )); 19136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(evects) ) 19156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( evects = cvGetMat( evects, &evectstub )); 19166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(result) ) 19186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( result = cvGetMat( result, &rstub )); 19196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (CV_MAT_TYPE(avg->type) != CV_32FC1 && CV_MAT_TYPE(avg->type) != CV_64FC1) || 19216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn !CV_ARE_TYPES_EQ(avg, data) || !CV_ARE_TYPES_EQ(avg, evects) || !CV_ARE_TYPES_EQ(avg, result) ) 19226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, 19236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "All the input and output arrays must have the same type, 32fC1 or 64fC1" ); 19246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (avg->cols != 1 || avg->rows != result->rows) && 19266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (avg->rows != 1 || avg->cols != result->cols) ) 19276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadSize, 19286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "The mean (average) vector should be either 1 x result->cols or result->rows x 1" ); 19296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( avg->cols == 1 ) 19316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 19326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn len = result->rows; 19336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn in_count = result->cols; 19346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn as_cols = 1; 19356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 19366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 19376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 19386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn len = result->cols; 19396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn in_count = result->rows; 19406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn as_cols = 0; 19416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 19426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( evects->cols != len ) 19446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, 19456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "Eigenvectors must be stored as rows and be of the same size as the output vectors" ); 19466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( data->cols > evects->rows ) 19486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, 19496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "The input matrix of coefficients must have the number of columns " 19506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "less than or equal to the number of eigenvectors (number of rows in eigenvectors matrix)" ); 19516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn evects = cvGetRows( evects, &evectstub, 0, data->cols ); 19536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn block_count0 = (1 << 16)/len; 19556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn block_count0 = MAX( block_count0, 4 ); 19566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn block_count0 = MIN( block_count0, in_count ); 19576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn elem_size = CV_ELEM_SIZE(avg->type); 19586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn buf_size = block_count0*len*(block_count0 > 1)*elem_size; 19606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( buf_size < CV_MAX_LOCAL_SIZE ) 19626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 19636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn buffer = (uchar*)cvStackAlloc( MAX(buf_size,16) ); 19646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn local_alloc = 1; 19656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 19666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 19676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( buffer = (uchar*)cvAlloc( buf_size )); 19686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( block_count0 > 1 ) 19706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 19716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn avg_repeated = cvMat( as_cols ? len : block_count0, 19726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn as_cols ? block_count0 : len, avg->type, buffer ); 19736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvRepeat( avg, &avg_repeated ); 19746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 19756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 19766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn avg_repeated = *avg; 19776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < in_count; i += block_count ) 19796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 19806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat data_part, avg_part, out_part; 19816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn block_count = MIN( block_count0, in_count - i ); 19836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetRows( data, &data_part, i, i + block_count ); 19846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( as_cols ) 19866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 19876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetCols( result, &out_part, i, i + block_count ); 19886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetCols( &avg_repeated, &avg_part, 0, block_count ); 19896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGEMM( evects, &data_part, 1, &avg_part, 1, &out_part, CV_GEMM_A_T + CV_GEMM_B_T ); 19906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 19916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 19926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 19936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetRows( result, &out_part, i, i + block_count ); 19946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetRows( &avg_repeated, &avg_part, 0, block_count ); 19956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGEMM( &data_part, evects, 1, &avg_part, 1, &out_part, 0 ); 19966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 19976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 19986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 20006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !local_alloc ) 20026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &buffer ); 20036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 20046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* End of file. */ 2007