+++ /dev/null
-/*M///////////////////////////////////////////////////////////////////////////////////////
-//
-// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
-//
-// By downloading, copying, installing or using the software you agree to this license.
-// If you do not agree to this license, do not download, install,
-// copy or use the software.
-//
-//
-// Intel License Agreement
-// For Open Source Computer Vision Library
-//
-// Copyright (C) 2000, Intel Corporation, all rights reserved.
-// Third party copyrights are property of their respective owners.
-//
-// Redistribution and use in source and binary forms, with or without modification,
-// are permitted provided that the following conditions are met:
-//
-// * Redistribution's of source code must retain the above copyright notice,
-// this list of conditions and the following disclaimer.
-//
-// * Redistribution's in binary form must reproduce the above copyright notice,
-// this list of conditions and the following disclaimer in the documentation
-// and/or other materials provided with the distribution.
-//
-// * The name of Intel Corporation may not be used to endorse or promote products
-// derived from this software without specific prior written permission.
-//
-// This software is provided by the copyright holders and contributors "as is" and
-// any express or implied warranties, including, but not limited to, the implied
-// warranties of merchantability and fitness for a particular purpose are disclaimed.
-// In no event shall the Intel Corporation or contributors be liable for any direct,
-// indirect, incidental, special, exemplary, or consequential damages
-// (including, but not limited to, procurement of substitute goods or services;
-// loss of use, data, or profits; or business interruption) however caused
-// and on any theory of liability, whether in contract, strict liability,
-// or tort (including negligence or otherwise) arising in any way out of
-// the use of this software, even if advised of the possibility of such damage.
-//
-//M*/
-
-#include "_cxcore.h"
-
-/****************************************************************************************\
-* [scaled] Identity matrix initialization *
-\****************************************************************************************/
-
-CV_IMPL void
-cvSetIdentity( CvArr* array, CvScalar value )
-{
- CV_FUNCNAME( "cvSetIdentity" );
-
- __BEGIN__;
-
- CvMat stub, *mat = (CvMat*)array;
- CvSize size;
- int i, k, len, step;
- int type, pix_size;
- uchar* data = 0;
- double buf[4];
-
- if( !CV_IS_MAT( mat ))
- {
- int coi = 0;
- CV_CALL( mat = cvGetMat( mat, &stub, &coi ));
- if( coi != 0 )
- CV_ERROR( CV_BadCOI, "coi is not supported" );
- }
-
- size = cvGetMatSize( mat );
- len = CV_IMIN( size.width, size.height );
-
- type = CV_MAT_TYPE(mat->type);
- pix_size = CV_ELEM_SIZE(type);
- size.width *= pix_size;
-
- if( CV_IS_MAT_CONT( mat->type ))
- {
- size.width *= size.height;
- size.height = 1;
- }
-
- data = mat->data.ptr;
- step = mat->step;
- if( step == 0 )
- step = CV_STUB_STEP;
- IPPI_CALL( icvSetZero_8u_C1R( data, step, size ));
- step += pix_size;
-
- if( type == CV_32FC1 )
- {
- float val = (float)value.val[0];
- float* _data = (float*)data;
- step /= sizeof(_data[0]);
- len *= step;
-
- for( i = 0; i < len; i += step )
- _data[i] = val;
- }
- else if( type == CV_64FC1 )
- {
- double val = value.val[0];
- double* _data = (double*)data;
- step /= sizeof(_data[0]);
- len *= step;
-
- for( i = 0; i < len; i += step )
- _data[i] = val;
- }
- else
- {
- uchar* val_ptr = (uchar*)buf;
- cvScalarToRawData( &value, buf, type, 0 );
- len *= step;
-
- for( i = 0; i < len; i += step )
- for( k = 0; k < pix_size; k++ )
- data[i+k] = val_ptr[k];
- }
-
- __END__;
-}
-
-
-/****************************************************************************************\
-* Trace of the matrix *
-\****************************************************************************************/
-
-CV_IMPL CvScalar
-cvTrace( const CvArr* array )
-{
- CvScalar sum = {{0,0,0,0}};
-
- CV_FUNCNAME( "cvTrace" );
-
- __BEGIN__;
-
- CvMat stub, *mat = 0;
-
- if( CV_IS_MAT( array ))
- {
- mat = (CvMat*)array;
- int type = CV_MAT_TYPE(mat->type);
- int size = MIN(mat->rows,mat->cols);
- uchar* data = mat->data.ptr;
-
- if( type == CV_32FC1 )
- {
- int step = mat->step + sizeof(float);
-
- for( ; size--; data += step )
- sum.val[0] += *(float*)data;
- EXIT;
- }
-
- if( type == CV_64FC1 )
- {
- int step = mat->step + sizeof(double);
-
- for( ; size--; data += step )
- sum.val[0] += *(double*)data;
- EXIT;
- }
- }
-
- CV_CALL( mat = cvGetDiag( array, &stub ));
- CV_CALL( sum = cvSum( mat ));
-
- __END__;
-
- return sum;
-}
-
-
-/****************************************************************************************\
-* Matrix transpose *
-\****************************************************************************************/
-
-/////////////////// macros for inplace transposition of square matrix ////////////////////
-
-#define ICV_DEF_TRANSP_INP_CASE_C1( \
- arrtype, len ) \
-{ \
- arrtype* arr1 = arr; \
- step /= sizeof(arr[0]); \
- \
- while( --len ) \
- { \
- arr += step, arr1++; \
- arrtype* arr2 = arr; \
- arrtype* arr3 = arr1; \
- \
- do \
- { \
- arrtype t0 = arr2[0]; \
- arrtype t1 = arr3[0]; \
- arr2[0] = t1; \
- arr3[0] = t0; \
- \
- arr2++; \
- arr3 += step; \
- } \
- while( arr2 != arr3 ); \
- } \
-}
-
-
-#define ICV_DEF_TRANSP_INP_CASE_C3( \
- arrtype, len ) \
-{ \
- arrtype* arr1 = arr; \
- int y; \
- step /= sizeof(arr[0]); \
- \
- for( y = 1; y < len; y++ ) \
- { \
- arr += step, arr1 += 3; \
- arrtype* arr2 = arr; \
- arrtype* arr3 = arr1; \
- \
- for( ; arr2!=arr3; arr2+=3, \
- arr3+=step )\
- { \
- arrtype t0 = arr2[0]; \
- arrtype t1 = arr3[0]; \
- arr2[0] = t1; \
- arr3[0] = t0; \
- t0 = arr2[1]; \
- t1 = arr3[1]; \
- arr2[1] = t1; \
- arr3[1] = t0; \
- t0 = arr2[2]; \
- t1 = arr3[2]; \
- arr2[2] = t1; \
- arr3[2] = t0; \
- } \
- } \
-}
-
-
-#define ICV_DEF_TRANSP_INP_CASE_C4( \
- arrtype, len ) \
-{ \
- arrtype* arr1 = arr; \
- int y; \
- step /= sizeof(arr[0]); \
- \
- for( y = 1; y < len; y++ ) \
- { \
- arr += step, arr1 += 4; \
- arrtype* arr2 = arr; \
- arrtype* arr3 = arr1; \
- \
- for( ; arr2!=arr3; arr2+=4, \
- arr3+=step )\
- { \
- arrtype t0 = arr2[0]; \
- arrtype t1 = arr3[0]; \
- arr2[0] = t1; \
- arr3[0] = t0; \
- t0 = arr2[1]; \
- t1 = arr3[1]; \
- arr2[1] = t1; \
- arr3[1] = t0; \
- t0 = arr2[2]; \
- t1 = arr3[2]; \
- arr2[2] = t1; \
- arr3[2] = t0; \
- t0 = arr2[3]; \
- t1 = arr3[3]; \
- arr2[3] = t1; \
- arr3[3] = t0; \
- } \
- } \
-}
-
-
-//////////////// macros for non-inplace transposition of rectangular matrix //////////////
-
-#define ICV_DEF_TRANSP_CASE_C1( arrtype ) \
-{ \
- int x, y; \
- srcstep /= sizeof(src[0]); \
- dststep /= sizeof(dst[0]); \
- \
- for( y = 0; y <= size.height - 2; y += 2, \
- src += 2*srcstep, dst += 2 ) \
- { \
- const arrtype* src1 = src + srcstep; \
- arrtype* dst1 = dst; \
- \
- for( x = 0; x <= size.width - 2; \
- x += 2, dst1 += dststep ) \
- { \
- arrtype t0 = src[x]; \
- arrtype t1 = src1[x]; \
- dst1[0] = t0; \
- dst1[1] = t1; \
- dst1 += dststep; \
- \
- t0 = src[x + 1]; \
- t1 = src1[x + 1]; \
- dst1[0] = t0; \
- dst1[1] = t1; \
- } \
- \
- if( x < size.width ) \
- { \
- arrtype t0 = src[x]; \
- arrtype t1 = src1[x]; \
- dst1[0] = t0; \
- dst1[1] = t1; \
- } \
- } \
- \
- if( y < size.height ) \
- { \
- arrtype* dst1 = dst; \
- for( x = 0; x <= size.width - 2; \
- x += 2, dst1 += 2*dststep ) \
- { \
- arrtype t0 = src[x]; \
- arrtype t1 = src[x + 1]; \
- dst1[0] = t0; \
- dst1[dststep] = t1; \
- } \
- \
- if( x < size.width ) \
- { \
- arrtype t0 = src[x]; \
- dst1[0] = t0; \
- } \
- } \
-}
-
-
-#define ICV_DEF_TRANSP_CASE_C3( arrtype ) \
-{ \
- size.width *= 3; \
- srcstep /= sizeof(src[0]); \
- dststep /= sizeof(dst[0]); \
- \
- for( ; size.height--; src+=srcstep, dst+=3 )\
- { \
- int x; \
- arrtype* dst1 = dst; \
- \
- for( x = 0; x < size.width; x += 3, \
- dst1 += dststep ) \
- { \
- arrtype t0 = src[x]; \
- arrtype t1 = src[x + 1]; \
- arrtype t2 = src[x + 2]; \
- \
- dst1[0] = t0; \
- dst1[1] = t1; \
- dst1[2] = t2; \
- } \
- } \
-}
-
-
-#define ICV_DEF_TRANSP_CASE_C4( arrtype ) \
-{ \
- size.width *= 4; \
- srcstep /= sizeof(src[0]); \
- dststep /= sizeof(dst[0]); \
- \
- for( ; size.height--; src+=srcstep, dst+=4 )\
- { \
- int x; \
- arrtype* dst1 = dst; \
- \
- for( x = 0; x < size.width; x += 4, \
- dst1 += dststep ) \
- { \
- arrtype t0 = src[x]; \
- arrtype t1 = src[x + 1]; \
- \
- dst1[0] = t0; \
- dst1[1] = t1; \
- \
- t0 = src[x + 2]; \
- t1 = src[x + 3]; \
- \
- dst1[2] = t0; \
- dst1[3] = t1; \
- } \
- } \
-}
-
-
-#define ICV_DEF_TRANSP_INP_FUNC( flavor, arrtype, cn ) \
-static CvStatus CV_STDCALL \
-icvTranspose_##flavor( arrtype* arr, int step, CvSize size )\
-{ \
- assert( size.width == size.height ); \
- \
- ICV_DEF_TRANSP_INP_CASE_C##cn( arrtype, size.width ) \
- return CV_OK; \
-}
-
-
-#define ICV_DEF_TRANSP_FUNC( flavor, arrtype, cn ) \
-static CvStatus CV_STDCALL \
-icvTranspose_##flavor( const arrtype* src, int srcstep, \
- arrtype* dst, int dststep, CvSize size )\
-{ \
- ICV_DEF_TRANSP_CASE_C##cn( arrtype ) \
- return CV_OK; \
-}
-
-
-ICV_DEF_TRANSP_INP_FUNC( 8u_C1IR, uchar, 1 )
-ICV_DEF_TRANSP_INP_FUNC( 8u_C2IR, ushort, 1 )
-ICV_DEF_TRANSP_INP_FUNC( 8u_C3IR, uchar, 3 )
-ICV_DEF_TRANSP_INP_FUNC( 16u_C2IR, int, 1 )
-ICV_DEF_TRANSP_INP_FUNC( 16u_C3IR, ushort, 3 )
-ICV_DEF_TRANSP_INP_FUNC( 32s_C2IR, int64, 1 )
-ICV_DEF_TRANSP_INP_FUNC( 32s_C3IR, int, 3 )
-ICV_DEF_TRANSP_INP_FUNC( 64s_C2IR, int, 4 )
-ICV_DEF_TRANSP_INP_FUNC( 64s_C3IR, int64, 3 )
-ICV_DEF_TRANSP_INP_FUNC( 64s_C4IR, int64, 4 )
-
-
-ICV_DEF_TRANSP_FUNC( 8u_C1R, uchar, 1 )
-ICV_DEF_TRANSP_FUNC( 8u_C2R, ushort, 1 )
-ICV_DEF_TRANSP_FUNC( 8u_C3R, uchar, 3 )
-ICV_DEF_TRANSP_FUNC( 16u_C2R, int, 1 )
-ICV_DEF_TRANSP_FUNC( 16u_C3R, ushort, 3 )
-ICV_DEF_TRANSP_FUNC( 32s_C2R, int64, 1 )
-ICV_DEF_TRANSP_FUNC( 32s_C3R, int, 3 )
-ICV_DEF_TRANSP_FUNC( 64s_C2R, int, 4 )
-ICV_DEF_TRANSP_FUNC( 64s_C3R, int64, 3 )
-ICV_DEF_TRANSP_FUNC( 64s_C4R, int64, 4 )
-
-CV_DEF_INIT_PIXSIZE_TAB_2D( Transpose, R )
-CV_DEF_INIT_PIXSIZE_TAB_2D( Transpose, IR )
-
-CV_IMPL void
-cvTranspose( const CvArr* srcarr, CvArr* dstarr )
-{
- static CvBtFuncTable tab, inp_tab;
- static int inittab = 0;
-
- CV_FUNCNAME( "cvTranspose" );
-
- __BEGIN__;
-
- CvMat sstub, *src = (CvMat*)srcarr;
- CvMat dstub, *dst = (CvMat*)dstarr;
- CvSize size;
- int type, pix_size;
-
- if( !inittab )
- {
- icvInitTransposeIRTable( &inp_tab );
- icvInitTransposeRTable( &tab );
- inittab = 1;
- }
-
- if( !CV_IS_MAT( src ))
- {
- int coi = 0;
- CV_CALL( src = cvGetMat( src, &sstub, &coi ));
- if( coi != 0 )
- CV_ERROR( CV_BadCOI, "coi is not supported" );
- }
-
- type = CV_MAT_TYPE( src->type );
- pix_size = CV_ELEM_SIZE(type);
- size = cvGetMatSize( src );
-
- if( dstarr == srcarr )
- {
- dst = src;
- }
- else
- {
- if( !CV_IS_MAT( dst ))
- {
- int coi = 0;
- CV_CALL( dst = cvGetMat( dst, &dstub, &coi ));
-
- if( coi != 0 )
- CV_ERROR( CV_BadCOI, "coi is not supported" );
- }
-
- if( !CV_ARE_TYPES_EQ( src, dst ))
- CV_ERROR( CV_StsUnmatchedFormats, "" );
-
- if( size.width != dst->height || size.height != dst->width )
- CV_ERROR( CV_StsUnmatchedSizes, "" );
- }
-
- if( src->data.ptr == dst->data.ptr )
- {
- if( size.width == size.height )
- {
- CvFunc2D_1A func = (CvFunc2D_1A)(inp_tab.fn_2d[pix_size]);
-
- if( !func )
- CV_ERROR( CV_StsUnsupportedFormat, "" );
-
- IPPI_CALL( func( src->data.ptr, src->step, size ));
- }
- else
- {
- if( size.width != 1 && size.height != 1 )
- CV_ERROR( CV_StsBadSize,
- "Rectangular matrix can not be transposed inplace" );
-
- if( !CV_IS_MAT_CONT( src->type & dst->type ))
- CV_ERROR( CV_StsBadFlag, "In case of inplace column/row transposition "
- "both source and destination must be continuous" );
-
- if( dst == src )
- {
- int t;
- CV_SWAP( dst->width, dst->height, t );
- dst->step = dst->height == 1 ? 0 : pix_size;
- }
- }
- }
- else
- {
- CvFunc2D_2A func = (CvFunc2D_2A)(tab.fn_2d[pix_size]);
-
- if( !func )
- CV_ERROR( CV_StsUnsupportedFormat, "" );
-
- IPPI_CALL( func( src->data.ptr, src->step,
- dst->data.ptr, dst->step, size ));
- }
-
- __END__;
-}
-
-
-/****************************************************************************************\
-* LU decomposition/back substitution *
-\****************************************************************************************/
-
-#define arrtype float
-#define temptype double
-
-typedef CvStatus (CV_STDCALL * CvLUDecompFunc)( double* A, int stepA, CvSize sizeA,
- void* B, int stepB, CvSize sizeB,
- double* det );
-
-typedef CvStatus (CV_STDCALL * CvLUBackFunc)( double* A, int stepA, CvSize sizeA,
- void* B, int stepB, CvSize sizeB );
-
-
-#define ICV_DEF_LU_DECOMP_FUNC( flavor, arrtype ) \
-static CvStatus CV_STDCALL \
-icvLUDecomp_##flavor( double* A, int stepA, CvSize sizeA, \
- arrtype* B, int stepB, CvSize sizeB, double* _det ) \
-{ \
- int n = sizeA.width; \
- int m = 0, i; \
- double det = 1; \
- \
- assert( sizeA.width == sizeA.height ); \
- \
- if( B ) \
- { \
- assert( sizeA.height == sizeB.height ); \
- m = sizeB.width; \
- } \
- stepA /= sizeof(A[0]); \
- stepB /= sizeof(B[0]); \
- \
- for( i = 0; i < n; i++, A += stepA, B += stepB ) \
- { \
- int j, k = i; \
- double* tA = A; \
- arrtype* tB = 0; \
- double kval = fabs(A[i]), tval; \
- \
- /* find the pivot element */ \
- for( j = i + 1; j < n; j++ ) \
- { \
- tA += stepA; \
- tval = fabs(tA[i]); \
- \
- if( tval > kval ) \
- { \
- kval = tval; \
- k = j; \
- } \
- } \
- \
- if( kval == 0 ) \
- { \
- det = 0; \
- break; \
- } \
- \
- /* swap rows */ \
- if( k != i ) \
- { \
- tA = A + stepA*(k - i); \
- det = -det; \
- \
- for( j = i; j < n; j++ ) \
- { \
- double t; \
- CV_SWAP( A[j], tA[j], t ); \
- } \
- \
- if( m > 0 ) \
- { \
- tB = B + stepB*(k - i); \
- \
- for( j = 0; j < m; j++ ) \
- { \
- arrtype t = B[j]; \
- CV_SWAP( B[j], tB[j], t ); \
- } \
- } \
- } \
- \
- tval = 1./A[i]; \
- det *= A[i]; \
- tA = A; \
- tB = B; \
- A[i] = tval; /* to replace division with multiplication in LUBack */ \
- \
- /* update matrix and the right side of the system */ \
- for( j = i + 1; j < n; j++ ) \
- { \
- tA += stepA; \
- tB += stepB; \
- double alpha = -tA[i]*tval; \
- \
- for( k = i + 1; k < n; k++ ) \
- tA[k] = tA[k] + alpha*A[k]; \
- \
- if( m > 0 ) \
- for( k = 0; k < m; k++ ) \
- tB[k] = (arrtype)(tB[k] + alpha*B[k]); \
- } \
- } \
- \
- if( _det ) \
- *_det = det; \
- \
- return CV_OK; \
-}
-
-
-ICV_DEF_LU_DECOMP_FUNC( 32f, float )
-ICV_DEF_LU_DECOMP_FUNC( 64f, double )
-
-
-#define ICV_DEF_LU_BACK_FUNC( flavor, arrtype ) \
-static CvStatus CV_STDCALL \
-icvLUBack_##flavor( double* A, int stepA, CvSize sizeA, \
- arrtype* B, int stepB, CvSize sizeB ) \
-{ \
- int n = sizeA.width; \
- int m = sizeB.width, i; \
- \
- assert( m > 0 && sizeA.width == sizeA.height && \
- sizeA.height == sizeB.height ); \
- stepA /= sizeof(A[0]); \
- stepB /= sizeof(B[0]); \
- \
- A += stepA*(n - 1); \
- B += stepB*(n - 1); \
- \
- for( i = n - 1; i >= 0; i--, A -= stepA ) \
- { \
- int j, k; \
- for( j = 0; j < m; j++ ) \
- { \
- arrtype* tB = B + j; \
- double x = 0; \
- \
- for( k = n - 1; k > i; k--, tB -= stepB ) \
- x += A[k]*tB[0]; \
- \
- tB[0] = (arrtype)((tB[0] - x)*A[i]); \
- } \
- } \
- \
- return CV_OK; \
-}
-
-
-ICV_DEF_LU_BACK_FUNC( 32f, float )
-ICV_DEF_LU_BACK_FUNC( 64f, double )
-
-static CvFuncTable lu_decomp_tab, lu_back_tab;
-static int lu_inittab = 0;
-
-static void icvInitLUTable( CvFuncTable* decomp_tab,
- CvFuncTable* back_tab )
-{
- decomp_tab->fn_2d[0] = (void*)icvLUDecomp_32f;
- decomp_tab->fn_2d[1] = (void*)icvLUDecomp_64f;
- back_tab->fn_2d[0] = (void*)icvLUBack_32f;
- back_tab->fn_2d[1] = (void*)icvLUBack_64f;
-}
-
-
-
-/****************************************************************************************\
-* Determinant of the matrix *
-\****************************************************************************************/
-
-#define det2(m) (m(0,0)*m(1,1) - m(0,1)*m(1,0))
-#define det3(m) (m(0,0)*(m(1,1)*m(2,2) - m(1,2)*m(2,1)) - \
- m(0,1)*(m(1,0)*m(2,2) - m(1,2)*m(2,0)) + \
- m(0,2)*(m(1,0)*m(2,1) - m(1,1)*m(2,0)))
-
-CV_IMPL double
-cvDet( const CvArr* arr )
-{
- double result = 0;
- uchar* buffer = 0;
- int local_alloc = 0;
-
- CV_FUNCNAME( "cvDet" );
-
- __BEGIN__;
-
- CvMat stub, *mat = (CvMat*)arr;
- int type;
-
- if( !CV_IS_MAT( mat ))
- {
- CV_CALL( mat = cvGetMat( mat, &stub ));
- }
-
- type = CV_MAT_TYPE( mat->type );
-
- if( mat->width != mat->height )
- CV_ERROR( CV_StsBadSize, "The matrix must be square" );
-
- #define Mf( y, x ) ((float*)(m + y*step))[x]
- #define Md( y, x ) ((double*)(m + y*step))[x]
-
- if( mat->width == 2 )
- {
- uchar* m = mat->data.ptr;
- int step = mat->step;
-
- if( type == CV_32FC1 )
- {
- result = det2(Mf);
- }
- else if( type == CV_64FC1 )
- {
- result = det2(Md);
- }
- else
- {
- CV_ERROR( CV_StsUnsupportedFormat, "" );
- }
- }
- else if( mat->width == 3 )
- {
- uchar* m = mat->data.ptr;
- int step = mat->step;
-
- if( type == CV_32FC1 )
- {
- result = det3(Mf);
- }
- else if( type == CV_64FC1 )
- {
- result = det3(Md);
- }
- else
- {
- CV_ERROR( CV_StsUnsupportedFormat, "" );
- }
- }
- else if( mat->width == 1 )
- {
- if( type == CV_32FC1 )
- {
- result = mat->data.fl[0];
- }
- else if( type == CV_64FC1 )
- {
- result = mat->data.db[0];
- }
- else
- {
- CV_ERROR( CV_StsUnsupportedFormat, "" );
- }
- }
- else
- {
- CvLUDecompFunc decomp_func;
- CvSize size = cvGetMatSize( mat );
- const int worktype = CV_64FC1;
- int buf_size = size.width*size.height*CV_ELEM_SIZE(worktype);
- CvMat tmat;
-
- if( !lu_inittab )
- {
- icvInitLUTable( &lu_decomp_tab, &lu_back_tab );
- lu_inittab = 1;
- }
-
- if( CV_MAT_CN( type ) != 1 || CV_MAT_DEPTH( type ) < CV_32F )
- CV_ERROR( CV_StsUnsupportedFormat, "" );
-
- if( size.width <= CV_MAX_LOCAL_MAT_SIZE )
- {
- buffer = (uchar*)cvStackAlloc( buf_size );
- local_alloc = 1;
- }
- else
- {
- CV_CALL( buffer = (uchar*)cvAlloc( buf_size ));
- }
-
- CV_CALL( cvInitMatHeader( &tmat, size.height, size.width, worktype, buffer ));
- if( type == worktype )
- {
- CV_CALL( cvCopy( mat, &tmat ));
- }
- else
- CV_CALL( cvConvert( mat, &tmat ));
-
- decomp_func = (CvLUDecompFunc)(lu_decomp_tab.fn_2d[CV_MAT_DEPTH(worktype)-CV_32F]);
- assert( decomp_func );
-
- IPPI_CALL( decomp_func( tmat.data.db, tmat.step, size, 0, 0, size, &result ));
- }
-
- #undef Mf
- #undef Md
-
- /*icvCheckVector_64f( &result, 1 );*/
-
- __END__;
-
- if( buffer && !local_alloc )
- cvFree( &buffer );
-
- return result;
-}
-
-
-
-/****************************************************************************************\
-* Inverse (or pseudo-inverse) of the matrix *
-\****************************************************************************************/
-
-#define Sf( y, x ) ((float*)(srcdata + y*srcstep))[x]
-#define Sd( y, x ) ((double*)(srcdata + y*srcstep))[x]
-#define Df( y, x ) ((float*)(dstdata + y*dststep))[x]
-#define Dd( y, x ) ((double*)(dstdata + y*dststep))[x]
-
-CV_IMPL double
-cvInvert( const CvArr* srcarr, CvArr* dstarr, int method )
-{
- CvMat* u = 0;
- CvMat* v = 0;
- CvMat* w = 0;
-
- uchar* buffer = 0;
- int local_alloc = 0;
- double result = 0;
-
- CV_FUNCNAME( "cvInvert" );
-
- __BEGIN__;
-
- CvMat sstub, *src = (CvMat*)srcarr;
- CvMat dstub, *dst = (CvMat*)dstarr;
- int type;
-
- if( !CV_IS_MAT( src ))
- CV_CALL( src = cvGetMat( src, &sstub ));
-
- if( !CV_IS_MAT( dst ))
- CV_CALL( dst = cvGetMat( dst, &dstub ));
-
- type = CV_MAT_TYPE( src->type );
-
- if( method == CV_SVD || method == CV_SVD_SYM )
- {
- int n = MIN(src->rows,src->cols);
- if( method == CV_SVD_SYM && src->rows != src->cols )
- CV_ERROR( CV_StsBadSize, "CV_SVD_SYM method is used for non-square matrix" );
-
- CV_CALL( u = cvCreateMat( n, src->rows, src->type ));
- if( method != CV_SVD_SYM )
- CV_CALL( v = cvCreateMat( n, src->cols, src->type ));
- CV_CALL( w = cvCreateMat( n, 1, src->type ));
- CV_CALL( cvSVD( src, w, u, v, CV_SVD_U_T + CV_SVD_V_T ));
-
- if( type == CV_32FC1 )
- result = w->data.fl[0] >= FLT_EPSILON ?
- w->data.fl[w->rows-1]/w->data.fl[0] : 0;
- else
- result = w->data.db[0] >= FLT_EPSILON ?
- w->data.db[w->rows-1]/w->data.db[0] : 0;
-
- CV_CALL( cvSVBkSb( w, u, v ? v : u, 0, dst, CV_SVD_U_T + CV_SVD_V_T ));
- EXIT;
- }
- else if( method != CV_LU )
- CV_ERROR( CV_StsBadArg, "Unknown inversion method" );
-
- if( !CV_ARE_TYPES_EQ( src, dst ))
- CV_ERROR( CV_StsUnmatchedFormats, "" );
-
- if( src->width != src->height )
- CV_ERROR( CV_StsBadSize, "The matrix must be square" );
-
- if( !CV_ARE_SIZES_EQ( src, dst ))
- CV_ERROR( CV_StsUnmatchedSizes, "" );
-
- if( type != CV_32FC1 && type != CV_64FC1 )
- CV_ERROR( CV_StsUnsupportedFormat, "" );
-
- if( src->width <= 3 )
- {
- uchar* srcdata = src->data.ptr;
- uchar* dstdata = dst->data.ptr;
- int srcstep = src->step;
- int dststep = dst->step;
-
- if( src->width == 2 )
- {
- if( type == CV_32FC1 )
- {
- double d = det2(Sf);
- if( d != 0. )
- {
- double t0, t1;
- result = d;
- d = 1./d;
- t0 = Sf(0,0)*d;
- t1 = Sf(1,1)*d;
- Df(1,1) = (float)t0;
- Df(0,0) = (float)t1;
- t0 = -Sf(0,1)*d;
- t1 = -Sf(1,0)*d;
- Df(0,1) = (float)t0;
- Df(1,0) = (float)t1;
- }
- }
- else
- {
- double d = det2(Sd);
- if( d != 0. )
- {
- double t0, t1;
- result = d;
- d = 1./d;
- t0 = Sd(0,0)*d;
- t1 = Sd(1,1)*d;
- Dd(1,1) = t0;
- Dd(0,0) = t1;
- t0 = -Sd(0,1)*d;
- t1 = -Sd(1,0)*d;
- Dd(0,1) = t0;
- Dd(1,0) = t1;
- }
- }
- }
- else if( src->width == 3 )
- {
- if( type == CV_32FC1 )
- {
- double d = det3(Sf);
- if( d != 0. )
- {
- float t[9];
- result = d;
- d = 1./d;
-
- t[0] = (float)((Sf(1,1) * Sf(2,2) - Sf(1,2) * Sf(2,1)) * d);
- t[1] = (float)((Sf(0,2) * Sf(2,1) - Sf(0,1) * Sf(2,2)) * d);
- t[2] = (float)((Sf(0,1) * Sf(1,2) - Sf(0,2) * Sf(1,1)) * d);
-
- t[3] = (float)((Sf(1,2) * Sf(2,0) - Sf(1,0) * Sf(2,2)) * d);
- t[4] = (float)((Sf(0,0) * Sf(2,2) - Sf(0,2) * Sf(2,0)) * d);
- t[5] = (float)((Sf(0,2) * Sf(1,0) - Sf(0,0) * Sf(1,2)) * d);
-
- t[6] = (float)((Sf(1,0) * Sf(2,1) - Sf(1,1) * Sf(2,0)) * d);
- t[7] = (float)((Sf(0,1) * Sf(2,0) - Sf(0,0) * Sf(2,1)) * d);
- t[8] = (float)((Sf(0,0) * Sf(1,1) - Sf(0,1) * Sf(1,0)) * d);
-
- Df(0,0) = t[0]; Df(0,1) = t[1]; Df(0,2) = t[2];
- Df(1,0) = t[3]; Df(1,1) = t[4]; Df(1,2) = t[5];
- Df(2,0) = t[6]; Df(2,1) = t[7]; Df(2,2) = t[8];
- }
- }
- else
- {
- double d = det3(Sd);
- if( d != 0. )
- {
- double t[9];
- result = d;
- d = 1./d;
-
- t[0] = (Sd(1,1) * Sd(2,2) - Sd(1,2) * Sd(2,1)) * d;
- t[1] = (Sd(0,2) * Sd(2,1) - Sd(0,1) * Sd(2,2)) * d;
- t[2] = (Sd(0,1) * Sd(1,2) - Sd(0,2) * Sd(1,1)) * d;
-
- t[3] = (Sd(1,2) * Sd(2,0) - Sd(1,0) * Sd(2,2)) * d;
- t[4] = (Sd(0,0) * Sd(2,2) - Sd(0,2) * Sd(2,0)) * d;
- t[5] = (Sd(0,2) * Sd(1,0) - Sd(0,0) * Sd(1,2)) * d;
-
- t[6] = (Sd(1,0) * Sd(2,1) - Sd(1,1) * Sd(2,0)) * d;
- t[7] = (Sd(0,1) * Sd(2,0) - Sd(0,0) * Sd(2,1)) * d;
- t[8] = (Sd(0,0) * Sd(1,1) - Sd(0,1) * Sd(1,0)) * d;
-
- Dd(0,0) = t[0]; Dd(0,1) = t[1]; Dd(0,2) = t[2];
- Dd(1,0) = t[3]; Dd(1,1) = t[4]; Dd(1,2) = t[5];
- Dd(2,0) = t[6]; Dd(2,1) = t[7]; Dd(2,2) = t[8];
- }
- }
- }
- else
- {
- assert( src->width == 1 );
-
- if( type == CV_32FC1 )
- {
- double d = Sf(0,0);
- if( d != 0. )
- {
- result = d;
- Df(0,0) = (float)(1./d);
- }
- }
- else
- {
- double d = Sd(0,0);
- if( d != 0. )
- {
- result = d;
- Dd(0,0) = 1./d;
- }
- }
- }
- }
- else
- {
- CvLUDecompFunc decomp_func;
- CvLUBackFunc back_func;
- CvSize size = cvGetMatSize( src );
- const int worktype = CV_64FC1;
- int buf_size = size.width*size.height*CV_ELEM_SIZE(worktype);
- CvMat tmat;
-
- if( !lu_inittab )
- {
- icvInitLUTable( &lu_decomp_tab, &lu_back_tab );
- lu_inittab = 1;
- }
-
- if( size.width <= CV_MAX_LOCAL_MAT_SIZE )
- {
- buffer = (uchar*)cvStackAlloc( buf_size );
- local_alloc = 1;
- }
- else
- {
- CV_CALL( buffer = (uchar*)cvAlloc( buf_size ));
- }
-
- CV_CALL( cvInitMatHeader( &tmat, size.height, size.width, worktype, buffer ));
- if( type == worktype )
- {
- CV_CALL( cvCopy( src, &tmat ));
- }
- else
- CV_CALL( cvConvert( src, &tmat ));
- CV_CALL( cvSetIdentity( dst ));
-
- decomp_func = (CvLUDecompFunc)(lu_decomp_tab.fn_2d[CV_MAT_DEPTH(type)-CV_32F]);
- back_func = (CvLUBackFunc)(lu_back_tab.fn_2d[CV_MAT_DEPTH(type)-CV_32F]);
- assert( decomp_func && back_func );
-
- IPPI_CALL( decomp_func( tmat.data.db, tmat.step, size,
- dst->data.ptr, dst->step, size, &result ));
-
- if( result != 0 )
- {
- IPPI_CALL( back_func( tmat.data.db, tmat.step, size,
- dst->data.ptr, dst->step, size ));
- }
- }
-
- if( !result )
- CV_CALL( cvSetZero( dst ));
-
- __END__;
-
- if( buffer && !local_alloc )
- cvFree( &buffer );
-
- if( u || v || w )
- {
- cvReleaseMat( &u );
- cvReleaseMat( &v );
- cvReleaseMat( &w );
- }
-
- return result;
-}
-
-
-/****************************************************************************************\
-* Linear system [least-squares] solution *
-\****************************************************************************************/
-
-CV_IMPL int
-cvSolve( const CvArr* A, const CvArr* b, CvArr* x, int method )
-{
- CvMat* u = 0;
- CvMat* v = 0;
- CvMat* w = 0;
-
- uchar* buffer = 0;
- int local_alloc = 0;
- int result = 1;
-
- CV_FUNCNAME( "cvSolve" );
-
- __BEGIN__;
-
- CvMat sstub, *src = (CvMat*)A;
- CvMat dstub, *dst = (CvMat*)x;
- CvMat bstub, *src2 = (CvMat*)b;
- int type;
-
- if( !CV_IS_MAT( src ))
- CV_CALL( src = cvGetMat( src, &sstub ));
-
- if( !CV_IS_MAT( src2 ))
- CV_CALL( src2 = cvGetMat( src2, &bstub ));
-
- if( !CV_IS_MAT( dst ))
- CV_CALL( dst = cvGetMat( dst, &dstub ));
-
- if( method == CV_SVD || method == CV_SVD_SYM )
- {
- int n = MIN(src->rows,src->cols);
-
- if( method == CV_SVD_SYM && src->rows != src->cols )
- CV_ERROR( CV_StsBadSize, "CV_SVD_SYM method is used for non-square matrix" );
-
- CV_CALL( u = cvCreateMat( n, src->rows, src->type ));
- if( method != CV_SVD_SYM )
- CV_CALL( v = cvCreateMat( n, src->cols, src->type ));
- CV_CALL( w = cvCreateMat( n, 1, src->type ));
- CV_CALL( cvSVD( src, w, u, v, CV_SVD_U_T + CV_SVD_V_T ));
- CV_CALL( cvSVBkSb( w, u, v ? v : u, src2, dst, CV_SVD_U_T + CV_SVD_V_T ));
- EXIT;
- }
- else if( method != CV_LU )
- CV_ERROR( CV_StsBadArg, "Unknown inversion method" );
-
- type = CV_MAT_TYPE( src->type );
-
- if( !CV_ARE_TYPES_EQ( src, dst ) || !CV_ARE_TYPES_EQ( src, src2 ))
- CV_ERROR( CV_StsUnmatchedFormats, "" );
-
- if( src->width != src->height )
- CV_ERROR( CV_StsBadSize, "The matrix must be square" );
-
- if( !CV_ARE_SIZES_EQ( src2, dst ) || src->width != src2->height )
- CV_ERROR( CV_StsUnmatchedSizes, "" );
-
- if( type != CV_32FC1 && type != CV_64FC1 )
- CV_ERROR( CV_StsUnsupportedFormat, "" );
-
- // check case of a single equation and small matrix
- if( src->width <= 3 && src2->width == 1 )
- {
- #define bf(y) ((float*)(bdata + y*src2step))[0]
- #define bd(y) ((double*)(bdata + y*src2step))[0]
-
- uchar* srcdata = src->data.ptr;
- uchar* bdata = src2->data.ptr;
- uchar* dstdata = dst->data.ptr;
- int srcstep = src->step;
- int src2step = src2->step;
- int dststep = dst->step;
-
- if( src->width == 2 )
- {
- if( type == CV_32FC1 )
- {
- double d = det2(Sf);
- if( d != 0. )
- {
- float t;
- d = 1./d;
- t = (float)((bf(0)*Sf(1,1) - bf(1)*Sf(0,1))*d);
- Df(1,0) = (float)((bf(1)*Sf(0,0) - bf(0)*Sf(1,0))*d);
- Df(0,0) = t;
- }
- else
- result = 0;
- }
- else
- {
- double d = det2(Sd);
- if( d != 0. )
- {
- double t;
- d = 1./d;
- t = (bd(0)*Sd(1,1) - bd(1)*Sd(0,1))*d;
- Dd(1,0) = (bd(1)*Sd(0,0) - bd(0)*Sd(1,0))*d;
- Dd(0,0) = t;
- }
- else
- result = 0;
- }
- }
- else if( src->width == 3 )
- {
- if( type == CV_32FC1 )
- {
- double d = det3(Sf);
- if( d != 0. )
- {
- float t[3];
- d = 1./d;
-
- t[0] = (float)(d*
- (bf(0)*(Sf(1,1)*Sf(2,2) - Sf(1,2)*Sf(2,1)) -
- Sf(0,1)*(bf(1)*Sf(2,2) - Sf(1,2)*bf(2)) +
- Sf(0,2)*(bf(1)*Sf(2,1) - Sf(1,1)*bf(2))));
-
- t[1] = (float)(d*
- (Sf(0,0)*(bf(1)*Sf(2,2) - Sf(1,2)*bf(2)) -
- bf(0)*(Sf(1,0)*Sf(2,2) - Sf(1,2)*Sf(2,0)) +
- Sf(0,2)*(Sf(1,0)*bf(2) - bf(1)*Sf(2,0))));
-
- t[2] = (float)(d*
- (Sf(0,0)*(Sf(1,1)*bf(2) - bf(1)*Sf(2,1)) -
- Sf(0,1)*(Sf(1,0)*bf(2) - bf(1)*Sf(2,0)) +
- bf(0)*(Sf(1,0)*Sf(2,1) - Sf(1,1)*Sf(2,0))));
-
- Df(0,0) = t[0];
- Df(1,0) = t[1];
- Df(2,0) = t[2];
- }
- else
- result = 0;
- }
- else
- {
- double d = det3(Sd);
- if( d != 0. )
- {
- double t[9];
-
- d = 1./d;
-
- t[0] = ((Sd(1,1) * Sd(2,2) - Sd(1,2) * Sd(2,1))*bd(0) +
- (Sd(0,2) * Sd(2,1) - Sd(0,1) * Sd(2,2))*bd(1) +
- (Sd(0,1) * Sd(1,2) - Sd(0,2) * Sd(1,1))*bd(2))*d;
-
- t[1] = ((Sd(1,2) * Sd(2,0) - Sd(1,0) * Sd(2,2))*bd(0) +
- (Sd(0,0) * Sd(2,2) - Sd(0,2) * Sd(2,0))*bd(1) +
- (Sd(0,2) * Sd(1,0) - Sd(0,0) * Sd(1,2))*bd(2))*d;
-
- t[2] = ((Sd(1,0) * Sd(2,1) - Sd(1,1) * Sd(2,0))*bd(0) +
- (Sd(0,1) * Sd(2,0) - Sd(0,0) * Sd(2,1))*bd(1) +
- (Sd(0,0) * Sd(1,1) - Sd(0,1) * Sd(1,0))*bd(2))*d;
-
- Dd(0,0) = t[0];
- Dd(1,0) = t[1];
- Dd(2,0) = t[2];
- }
- else
- result = 0;
- }
- }
- else
- {
- assert( src->width == 1 );
-
- if( type == CV_32FC1 )
- {
- double d = Sf(0,0);
- if( d != 0. )
- Df(0,0) = (float)(bf(0)/d);
- else
- result = 0;
- }
- else
- {
- double d = Sd(0,0);
- if( d != 0. )
- Dd(0,0) = (bd(0)/d);
- else
- result = 0;
- }
- }
- }
- else
- {
- CvLUDecompFunc decomp_func;
- CvLUBackFunc back_func;
- CvSize size = cvGetMatSize( src );
- CvSize dstsize = cvGetMatSize( dst );
- int worktype = CV_64FC1;
- int buf_size = size.width*size.height*CV_ELEM_SIZE(worktype);
- double d = 0;
- CvMat tmat;
-
- if( !lu_inittab )
- {
- icvInitLUTable( &lu_decomp_tab, &lu_back_tab );
- lu_inittab = 1;
- }
-
- if( size.width <= CV_MAX_LOCAL_MAT_SIZE )
- {
- buffer = (uchar*)cvStackAlloc( buf_size );
- local_alloc = 1;
- }
- else
- {
- CV_CALL( buffer = (uchar*)cvAlloc( buf_size ));
- }
-
- CV_CALL( cvInitMatHeader( &tmat, size.height, size.width, worktype, buffer ));
- if( type == worktype )
- {
- CV_CALL( cvCopy( src, &tmat ));
- }
- else
- CV_CALL( cvConvert( src, &tmat ));
-
- if( src2->data.ptr != dst->data.ptr )
- {
- CV_CALL( cvCopy( src2, dst ));
- }
-
- decomp_func = (CvLUDecompFunc)(lu_decomp_tab.fn_2d[CV_MAT_DEPTH(type)-CV_32F]);
- back_func = (CvLUBackFunc)(lu_back_tab.fn_2d[CV_MAT_DEPTH(type)-CV_32F]);
- assert( decomp_func && back_func );
-
- IPPI_CALL( decomp_func( tmat.data.db, tmat.step, size,
- dst->data.ptr, dst->step, dstsize, &d ));
-
- if( d != 0 )
- {
- IPPI_CALL( back_func( tmat.data.db, tmat.step, size,
- dst->data.ptr, dst->step, dstsize ));
- }
- else
- result = 0;
- }
-
- if( !result )
- CV_CALL( cvSetZero( dst ));
-
- __END__;
-
- if( buffer && !local_alloc )
- cvFree( &buffer );
-
- if( u || v || w )
- {
- cvReleaseMat( &u );
- cvReleaseMat( &v );
- cvReleaseMat( &w );
- }
-
- return result;
-}
-
-
-
-/****************************************************************************************\
-* 3D vector cross-product *
-\****************************************************************************************/
-
-CV_IMPL void
-cvCrossProduct( const CvArr* srcAarr, const CvArr* srcBarr, CvArr* dstarr )
-{
- CV_FUNCNAME( "cvCrossProduct" );
-
- __BEGIN__;
-
- CvMat stubA, *srcA = (CvMat*)srcAarr;
- CvMat stubB, *srcB = (CvMat*)srcBarr;
- CvMat dstub, *dst = (CvMat*)dstarr;
- int type;
-
- if( !CV_IS_MAT(srcA))
- CV_CALL( srcA = cvGetMat( srcA, &stubA ));
-
- type = CV_MAT_TYPE( srcA->type );
-
- if( srcA->width*srcA->height*CV_MAT_CN(type) != 3 )
- CV_ERROR( CV_StsBadArg, "All the input arrays must be continuous 3-vectors" );
-
- if( !srcB || !dst )
- CV_ERROR( CV_StsNullPtr, "" );
-
- if( (srcA->type & ~CV_MAT_CONT_FLAG) == (srcB->type & ~CV_MAT_CONT_FLAG) &&
- (srcA->type & ~CV_MAT_CONT_FLAG) == (dst->type & ~CV_MAT_CONT_FLAG) )
- {
- if( !srcB->data.ptr || !dst->data.ptr )
- CV_ERROR( CV_StsNullPtr, "" );
- }
- else
- {
- if( !CV_IS_MAT(srcB))
- CV_CALL( srcB = cvGetMat( srcB, &stubB ));
-
- if( !CV_IS_MAT(dst))
- CV_CALL( dst = cvGetMat( dst, &dstub ));
-
- if( !CV_ARE_TYPES_EQ( srcA, srcB ) ||
- !CV_ARE_TYPES_EQ( srcB, dst ))
- CV_ERROR( CV_StsUnmatchedFormats, "" );
- }
-
- if( !CV_ARE_SIZES_EQ( srcA, srcB ) || !CV_ARE_SIZES_EQ( srcB, dst ))
- CV_ERROR( CV_StsUnmatchedSizes, "" );
-
- if( CV_MAT_DEPTH(type) == CV_32F )
- {
- float* dstdata = (float*)(dst->data.ptr);
- const float* src1data = (float*)(srcA->data.ptr);
- const float* src2data = (float*)(srcB->data.ptr);
-
- if( CV_IS_MAT_CONT(srcA->type & srcB->type & dst->type) )
- {
- dstdata[2] = src1data[0] * src2data[1] - src1data[1] * src2data[0];
- dstdata[0] = src1data[1] * src2data[2] - src1data[2] * src2data[1];
- dstdata[1] = src1data[2] * src2data[0] - src1data[0] * src2data[2];
- }
- else
- {
- int step1 = srcA->step ? srcA->step/sizeof(src1data[0]) : 1;
- int step2 = srcB->step ? srcB->step/sizeof(src1data[0]) : 1;
- int step = dst->step ? dst->step/sizeof(src1data[0]) : 1;
-
- dstdata[2*step] = src1data[0] * src2data[step2] - src1data[step1] * src2data[0];
- dstdata[0] = src1data[step1] * src2data[step2*2] - src1data[step1*2] * src2data[step2];
- dstdata[step] = src1data[step1*2] * src2data[0] - src1data[0] * src2data[step2*2];
- }
- }
- else if( CV_MAT_DEPTH(type) == CV_64F )
- {
- double* dstdata = (double*)(dst->data.ptr);
- const double* src1data = (double*)(srcA->data.ptr);
- const double* src2data = (double*)(srcB->data.ptr);
-
- if( CV_IS_MAT_CONT(srcA->type & srcB->type & dst->type) )
- {
- dstdata[2] = src1data[0] * src2data[1] - src1data[1] * src2data[0];
- dstdata[0] = src1data[1] * src2data[2] - src1data[2] * src2data[1];
- dstdata[1] = src1data[2] * src2data[0] - src1data[0] * src2data[2];
- }
- else
- {
- int step1 = srcA->step ? srcA->step/sizeof(src1data[0]) : 1;
- int step2 = srcB->step ? srcB->step/sizeof(src1data[0]) : 1;
- int step = dst->step ? dst->step/sizeof(src1data[0]) : 1;
-
- dstdata[2*step] = src1data[0] * src2data[step2] - src1data[step1] * src2data[0];
- dstdata[0] = src1data[step1] * src2data[step2*2] - src1data[step1*2] * src2data[step2];
- dstdata[step] = src1data[step1*2] * src2data[0] - src1data[0] * src2data[step2*2];
- }
- }
- else
- {
- CV_ERROR( CV_StsUnsupportedFormat, "" );
- }
-
- __END__;
-}
-
-
-CV_IMPL void
-cvCalcPCA( const CvArr* data_arr, CvArr* avg_arr, CvArr* eigenvals, CvArr* eigenvects, int flags )
-{
- CvMat* tmp_avg = 0;
- CvMat* tmp_avg_r = 0;
- CvMat* tmp_cov = 0;
- CvMat* tmp_evals = 0;
- CvMat* tmp_evects = 0;
- CvMat* tmp_evects2 = 0;
- CvMat* tmp_data = 0;
-
- CV_FUNCNAME( "cvCalcPCA" );
-
- __BEGIN__;
-
- CvMat stub, *data = (CvMat*)data_arr;
- CvMat astub, *avg = (CvMat*)avg_arr;
- CvMat evalstub, *evals = (CvMat*)eigenvals;
- CvMat evectstub, *evects = (CvMat*)eigenvects;
- int covar_flags = CV_COVAR_SCALE;
- int i, len, in_count, count, out_count;
-
- if( !CV_IS_MAT(data) )
- CV_CALL( data = cvGetMat( data, &stub ));
-
- if( !CV_IS_MAT(avg) )
- CV_CALL( avg = cvGetMat( avg, &astub ));
-
- if( !CV_IS_MAT(evals) )
- CV_CALL( evals = cvGetMat( evals, &evalstub ));
-
- if( !CV_IS_MAT(evects) )
- CV_CALL( evects = cvGetMat( evects, &evectstub ));
-
- if( CV_MAT_CN(data->type) != 1 || CV_MAT_CN(avg->type) != 1 ||
- CV_MAT_CN(evals->type) != 1 || CV_MAT_CN(evects->type) != 1 )
- CV_ERROR( CV_StsUnsupportedFormat, "All the input and output arrays must be 1-channel" );
-
- if( CV_MAT_DEPTH(avg->type) < CV_32F || !CV_ARE_DEPTHS_EQ(avg, evals) ||
- !CV_ARE_DEPTHS_EQ(avg, evects) )
- CV_ERROR( CV_StsUnsupportedFormat, "All the output arrays must have the same type, 32fC1 or 64fC1" );
-
- if( flags & CV_PCA_DATA_AS_COL )
- {
- len = data->rows;
- in_count = data->cols;
- covar_flags |= CV_COVAR_COLS;
-
- if( avg->cols != 1 || avg->rows != len )
- CV_ERROR( CV_StsBadSize,
- "The mean (average) vector should be data->rows x 1 when CV_PCA_DATA_AS_COL is used" );
-
- CV_CALL( tmp_avg = cvCreateMat( len, 1, CV_64F ));
- }
- else
- {
- len = data->cols;
- in_count = data->rows;
- covar_flags |= CV_COVAR_ROWS;
-
- if( avg->rows != 1 || avg->cols != len )
- CV_ERROR( CV_StsBadSize,
- "The mean (average) vector should be 1 x data->cols when CV_PCA_DATA_AS_ROW is used" );
-
- CV_CALL( tmp_avg = cvCreateMat( 1, len, CV_64F ));
- }
-
- count = MIN(len, in_count);
- out_count = evals->cols + evals->rows - 1;
-
- if( (evals->cols != 1 && evals->rows != 1) || out_count > count )
- CV_ERROR( CV_StsBadSize,
- "The array of eigenvalues must be 1d vector containing "
- "no more than min(data->rows,data->cols) elements" );
-
- if( evects->cols != len || evects->rows != out_count )
- CV_ERROR( CV_StsBadSize,
- "The matrix of eigenvalues must have the same number of columns as the input vector length "
- "and the same number of rows as the number of eigenvalues" );
-
- // "scrambled" way to compute PCA (when cols(A)>rows(A)):
- // 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
- if( len <= in_count )
- covar_flags |= CV_COVAR_NORMAL;
-
- if( flags & CV_PCA_USE_AVG ){
- covar_flags |= CV_COVAR_USE_AVG;
- CV_CALL( cvConvert( avg, tmp_avg ) );
- }
-
- CV_CALL( tmp_cov = cvCreateMat( count, count, CV_64F ));
- CV_CALL( tmp_evals = cvCreateMat( 1, count, CV_64F ));
- CV_CALL( tmp_evects = cvCreateMat( count, count, CV_64F ));
-
- CV_CALL( cvCalcCovarMatrix( &data_arr, 0, tmp_cov, tmp_avg, covar_flags ));
- CV_CALL( cvSVD( tmp_cov, tmp_evals, tmp_evects, 0, CV_SVD_MODIFY_A + CV_SVD_U_T ));
- tmp_evects->rows = out_count;
- tmp_evals->cols = out_count;
- cvZero( evects );
- cvZero( evals );
-
- if( covar_flags & CV_COVAR_NORMAL )
- {
- CV_CALL( cvConvert( tmp_evects, evects ));
- }
- else
- {
- // CV_PCA_DATA_AS_ROW: cols(A)>rows(A). x=A'*y -> x'=y'*A
- // CV_PCA_DATA_AS_COL: rows(A)>cols(A). x=A''*y -> x'=y'*A'
- int block_count = 0;
-
- CV_CALL( tmp_data = cvCreateMat( count, count, CV_64F ));
- CV_CALL( tmp_avg_r = cvCreateMat( count, count, CV_64F ));
- CV_CALL( tmp_evects2 = cvCreateMat( out_count, count, CV_64F ));
-
- for( i = 0; i < len; i += block_count )
- {
- CvMat data_part, tdata_part, part, dst_part, avg_part, tmp_avg_part;
- int gemm_flags;
-
- block_count = MIN( count, len - i );
-
- if( flags & CV_PCA_DATA_AS_COL )
- {
- cvGetRows( data, &data_part, i, i + block_count );
- cvGetRows( tmp_data, &tdata_part, 0, block_count );
- cvGetRows( tmp_avg, &avg_part, i, i + block_count );
- cvGetRows( tmp_avg_r, &tmp_avg_part, 0, block_count );
- gemm_flags = CV_GEMM_B_T;
- }
- else
- {
- cvGetCols( data, &data_part, i, i + block_count );
- cvGetCols( tmp_data, &tdata_part, 0, block_count );
- cvGetCols( tmp_avg, &avg_part, i, i + block_count );
- cvGetCols( tmp_avg_r, &tmp_avg_part, 0, block_count );
- gemm_flags = 0;
- }
-
- cvGetCols( tmp_evects2, &part, 0, block_count );
- cvGetCols( evects, &dst_part, i, i + block_count );
-
- cvConvert( &data_part, &tdata_part );
- cvRepeat( &avg_part, &tmp_avg_part );
- cvSub( &tdata_part, &tmp_avg_part, &tdata_part );
- cvGEMM( tmp_evects, &tdata_part, 1, 0, 0, &part, gemm_flags );
- cvConvert( &part, &dst_part );
- }
-
- // normalize eigenvectors
- for( i = 0; i < out_count; i++ )
- {
- CvMat ei;
- cvGetRow( evects, &ei, i );
- cvNormalize( &ei, &ei );
- }
- }
-
- if( tmp_evals->rows != evals->rows )
- cvReshape( tmp_evals, tmp_evals, 1, evals->rows );
- cvConvert( tmp_evals, evals );
- cvConvert( tmp_avg, avg );
-
- __END__;
-
- cvReleaseMat( &tmp_avg );
- cvReleaseMat( &tmp_avg_r );
- cvReleaseMat( &tmp_cov );
- cvReleaseMat( &tmp_evals );
- cvReleaseMat( &tmp_evects );
- cvReleaseMat( &tmp_evects2 );
- cvReleaseMat( &tmp_data );
-}
-
-
-CV_IMPL void
-cvProjectPCA( const CvArr* data_arr, const CvArr* avg_arr,
- const CvArr* eigenvects, CvArr* result_arr )
-{
- uchar* buffer = 0;
- int local_alloc = 0;
-
- CV_FUNCNAME( "cvProjectPCA" );
-
- __BEGIN__;
-
- CvMat stub, *data = (CvMat*)data_arr;
- CvMat astub, *avg = (CvMat*)avg_arr;
- CvMat evectstub, *evects = (CvMat*)eigenvects;
- CvMat rstub, *result = (CvMat*)result_arr;
- CvMat avg_repeated;
- int i, len, in_count;
- int gemm_flags, as_cols, convert_data;
- int block_count0, block_count, buf_size, elem_size;
- uchar* tmp_data_ptr;
-
- if( !CV_IS_MAT(data) )
- CV_CALL( data = cvGetMat( data, &stub ));
-
- if( !CV_IS_MAT(avg) )
- CV_CALL( avg = cvGetMat( avg, &astub ));
-
- if( !CV_IS_MAT(evects) )
- CV_CALL( evects = cvGetMat( evects, &evectstub ));
-
- if( !CV_IS_MAT(result) )
- CV_CALL( result = cvGetMat( result, &rstub ));
-
- if( CV_MAT_CN(data->type) != 1 || CV_MAT_CN(avg->type) != 1 )
- CV_ERROR( CV_StsUnsupportedFormat, "All the input and output arrays must be 1-channel" );
-
- if( CV_MAT_TYPE(avg->type) != CV_32FC1 && CV_MAT_TYPE(avg->type) != CV_64FC1 ||
- !CV_ARE_TYPES_EQ(avg, evects) || !CV_ARE_TYPES_EQ(avg, result) )
- CV_ERROR( CV_StsUnsupportedFormat,
- "All the input and output arrays (except for data) must have the same type, 32fC1 or 64fC1" );
-
- if( (avg->cols != 1 || avg->rows != data->rows) &&
- (avg->rows != 1 || avg->cols != data->cols) )
- CV_ERROR( CV_StsBadSize,
- "The mean (average) vector should be either 1 x data->cols or data->rows x 1" );
-
- if( avg->cols == 1 )
- {
- len = data->rows;
- in_count = data->cols;
-
- gemm_flags = CV_GEMM_A_T + CV_GEMM_B_T;
- as_cols = 1;
- }
- else
- {
- len = data->cols;
- in_count = data->rows;
-
- gemm_flags = CV_GEMM_B_T;
- as_cols = 0;
- }
-
- if( evects->cols != len )
- CV_ERROR( CV_StsUnmatchedSizes,
- "Eigenvectors must be stored as rows and be of the same size as input vectors" );
-
- if( result->cols > evects->rows )
- CV_ERROR( CV_StsOutOfRange,
- "The output matrix of coefficients must have the number of columns "
- "less than or equal to the number of eigenvectors (number of rows in eigenvectors matrix)" );
-
- evects = cvGetRows( evects, &evectstub, 0, result->cols );
-
- block_count0 = (1 << 16)/len;
- block_count0 = MAX( block_count0, 4 );
- block_count0 = MIN( block_count0, in_count );
- elem_size = CV_ELEM_SIZE(avg->type);
- convert_data = CV_MAT_DEPTH(data->type) < CV_MAT_DEPTH(avg->type);
-
- buf_size = block_count0*len*((block_count0 > 1) + 1)*elem_size;
-
- if( buf_size < CV_MAX_LOCAL_SIZE )
- {
- buffer = (uchar*)cvStackAlloc( buf_size );
- local_alloc = 1;
- }
- else
- CV_CALL( buffer = (uchar*)cvAlloc( buf_size ));
-
- tmp_data_ptr = buffer;
- if( block_count0 > 1 )
- {
- avg_repeated = cvMat( as_cols ? len : block_count0,
- as_cols ? block_count0 : len, avg->type, buffer );
- cvRepeat( avg, &avg_repeated );
- tmp_data_ptr += block_count0*len*elem_size;
- }
- else
- avg_repeated = *avg;
-
- for( i = 0; i < in_count; i += block_count )
- {
- CvMat data_part, norm_data, avg_part, *src = &data_part, out_part;
-
- block_count = MIN( block_count0, in_count - i );
- if( as_cols )
- {
- cvGetCols( data, &data_part, i, i + block_count );
- cvGetCols( &avg_repeated, &avg_part, 0, block_count );
- norm_data = cvMat( len, block_count, avg->type, tmp_data_ptr );
- }
- else
- {
- cvGetRows( data, &data_part, i, i + block_count );
- cvGetRows( &avg_repeated, &avg_part, 0, block_count );
- norm_data = cvMat( block_count, len, avg->type, tmp_data_ptr );
- }
-
- if( convert_data )
- {
- cvConvert( src, &norm_data );
- src = &norm_data;
- }
-
- cvSub( src, &avg_part, &norm_data );
-
- cvGetRows( result, &out_part, i, i + block_count );
- cvGEMM( &norm_data, evects, 1, 0, 0, &out_part, gemm_flags );
- }
-
- __END__;
-
- if( !local_alloc )
- cvFree( &buffer );
-}
-
-
-CV_IMPL void
-cvBackProjectPCA( const CvArr* proj_arr, const CvArr* avg_arr,
- const CvArr* eigenvects, CvArr* result_arr )
-{
- uchar* buffer = 0;
- int local_alloc = 0;
-
- CV_FUNCNAME( "cvProjectPCA" );
-
- __BEGIN__;
-
- CvMat pstub, *data = (CvMat*)proj_arr;
- CvMat astub, *avg = (CvMat*)avg_arr;
- CvMat evectstub, *evects = (CvMat*)eigenvects;
- CvMat rstub, *result = (CvMat*)result_arr;
- CvMat avg_repeated;
- int i, len, in_count, as_cols;
- int block_count0, block_count, buf_size, elem_size;
-
- if( !CV_IS_MAT(data) )
- CV_CALL( data = cvGetMat( data, &pstub ));
-
- if( !CV_IS_MAT(avg) )
- CV_CALL( avg = cvGetMat( avg, &astub ));
-
- if( !CV_IS_MAT(evects) )
- CV_CALL( evects = cvGetMat( evects, &evectstub ));
-
- if( !CV_IS_MAT(result) )
- CV_CALL( result = cvGetMat( result, &rstub ));
-
- if( CV_MAT_TYPE(avg->type) != CV_32FC1 && CV_MAT_TYPE(avg->type) != CV_64FC1 ||
- !CV_ARE_TYPES_EQ(avg, data) || !CV_ARE_TYPES_EQ(avg, evects) || !CV_ARE_TYPES_EQ(avg, result) )
- CV_ERROR( CV_StsUnsupportedFormat,
- "All the input and output arrays must have the same type, 32fC1 or 64fC1" );
-
- if( (avg->cols != 1 || avg->rows != result->rows) &&
- (avg->rows != 1 || avg->cols != result->cols) )
- CV_ERROR( CV_StsBadSize,
- "The mean (average) vector should be either 1 x result->cols or result->rows x 1" );
-
- if( avg->cols == 1 )
- {
- len = result->rows;
- in_count = result->cols;
- as_cols = 1;
- }
- else
- {
- len = result->cols;
- in_count = result->rows;
- as_cols = 0;
- }
-
- if( evects->cols != len )
- CV_ERROR( CV_StsUnmatchedSizes,
- "Eigenvectors must be stored as rows and be of the same size as the output vectors" );
-
- if( data->cols > evects->rows )
- CV_ERROR( CV_StsOutOfRange,
- "The input matrix of coefficients must have the number of columns "
- "less than or equal to the number of eigenvectors (number of rows in eigenvectors matrix)" );
-
- evects = cvGetRows( evects, &evectstub, 0, data->cols );
-
- block_count0 = (1 << 16)/len;
- block_count0 = MAX( block_count0, 4 );
- block_count0 = MIN( block_count0, in_count );
- elem_size = CV_ELEM_SIZE(avg->type);
-
- buf_size = block_count0*len*(block_count0 > 1)*elem_size;
-
- if( buf_size < CV_MAX_LOCAL_SIZE )
- {
- buffer = (uchar*)cvStackAlloc( MAX(buf_size,16) );
- local_alloc = 1;
- }
- else
- CV_CALL( buffer = (uchar*)cvAlloc( buf_size ));
-
- if( block_count0 > 1 )
- {
- avg_repeated = cvMat( as_cols ? len : block_count0,
- as_cols ? block_count0 : len, avg->type, buffer );
- cvRepeat( avg, &avg_repeated );
- }
- else
- avg_repeated = *avg;
-
- for( i = 0; i < in_count; i += block_count )
- {
- CvMat data_part, avg_part, out_part;
-
- block_count = MIN( block_count0, in_count - i );
- cvGetRows( data, &data_part, i, i + block_count );
-
- if( as_cols )
- {
- cvGetCols( result, &out_part, i, i + block_count );
- cvGetCols( &avg_repeated, &avg_part, 0, block_count );
- cvGEMM( evects, &data_part, 1, &avg_part, 1, &out_part, CV_GEMM_A_T + CV_GEMM_B_T );
- }
- else
- {
- cvGetRows( result, &out_part, i, i + block_count );
- cvGetRows( &avg_repeated, &avg_part, 0, block_count );
- cvGEMM( &data_part, evects, 1, &avg_part, 1, &out_part, 0 );
- }
- }
-
- __END__;
-
- if( !local_alloc )
- cvFree( &buffer );
-}
-
-
-/* End of file. */
projects / opencv / blobdiff