git.maemo.org Git - opencv/blob - cxcore/src/cxmatrix.cpp

   1 /*M///////////////////////////////////////////////////////////////////////////////////////
   2 //
   3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
   4 //
   5 //  By downloading, copying, installing or using the software you agree to this license.
   6 //  If you do not agree to this license, do not download, install,
   7 //  copy or use the software.
   8 //
   9 //
  10 //                        Intel License Agreement
  11 //                For Open Source Computer Vision Library
  12 //
  13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
  14 // Third party copyrights are property of their respective owners.
  15 //
  16 // Redistribution and use in source and binary forms, with or without modification,
  17 // are permitted provided that the following conditions are met:
  18 //
  19 //   * Redistribution's of source code must retain the above copyright notice,
  20 //     this list of conditions and the following disclaimer.
  21 //
  22 //   * Redistribution's in binary form must reproduce the above copyright notice,
  23 //     this list of conditions and the following disclaimer in the documentation
  24 //     and/or other materials provided with the distribution.
  25 //
  26 //   * The name of Intel Corporation may not be used to endorse or promote products
  27 //     derived from this software without specific prior written permission.
  28 //
  29 // This software is provided by the copyright holders and contributors "as is" and
  30 // any express or implied warranties, including, but not limited to, the implied
  31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
  32 // In no event shall the Intel Corporation or contributors be liable for any direct,
  33 // indirect, incidental, special, exemplary, or consequential damages
  34 // (including, but not limited to, procurement of substitute goods or services;
  35 // loss of use, data, or profits; or business interruption) however caused
  36 // and on any theory of liability, whether in contract, strict liability,
  37 // or tort (including negligence or otherwise) arising in any way out of
  38 // the use of this software, even if advised of the possibility of such damage.
  39 //
  40 //M*/
  41
  42 #include "_cxcore.h"
  43
  44 /****************************************************************************************\
  45 *                           [scaled] Identity matrix initialization                      *
  46 \****************************************************************************************/
  47
  48 CV_IMPL void
  49 cvSetIdentity( CvArr* array, CvScalar value )
  50 {
  51     CV_FUNCNAME( "cvSetIdentity" );
  52
  53     __BEGIN__;
  54
  55     CvMat stub, *mat = (CvMat*)array;
  56     CvSize size;
  57     int i, k, len, step;
  58     int type, pix_size;
  59     uchar* data = 0;
  60     double buf[4];
  61
  62     if( !CV_IS_MAT( mat ))
  63     {
  64         int coi = 0;
  65         CV_CALL( mat = cvGetMat( mat, &stub, &coi ));
  66         if( coi != 0 )
  67             CV_ERROR( CV_BadCOI, "coi is not supported" );
  68     }
  69
  70     size = cvGetMatSize( mat );
  71     len = CV_IMIN( size.width, size.height );
  72
  73     type = CV_MAT_TYPE(mat->type);
  74     pix_size = CV_ELEM_SIZE(type);
  75     size.width *= pix_size;
  76
  77     if( CV_IS_MAT_CONT( mat->type ))
  78     {
  79         size.width *= size.height;
  80         size.height = 1;
  81     }
  82
  83     data = mat->data.ptr;
  84     step = mat->step;
  85     if( step == 0 )
  86         step = CV_STUB_STEP;
  87     IPPI_CALL( icvSetZero_8u_C1R( data, step, size ));
  88     step += pix_size;
  89
  90     if( type == CV_32FC1 )
  91     {
  92         float val = (float)value.val[0];
  93         float* _data = (float*)data;
  94         step /= sizeof(_data[0]);
  95         len *= step;
  96
  97         for( i = 0; i < len; i += step )
  98             _data[i] = val;
  99     }
 100     else if( type == CV_64FC1 )
 101     {
 102         double val = value.val[0];
 103         double* _data = (double*)data;
 104         step /= sizeof(_data[0]);
 105         len *= step;
 106
 107         for( i = 0; i < len; i += step )
 108             _data[i] = val;
 109     }
 110     else
 111     {
 112         uchar* val_ptr = (uchar*)buf;
 113         cvScalarToRawData( &value, buf, type, 0 );
 114         len *= step;
 115
 116         for( i = 0; i < len; i += step )
 117             for( k = 0; k < pix_size; k++ )
 118                 data[i+k] = val_ptr[k];
 119     }
 120
 121     __END__;
 122 }
 123
 124
 125 /****************************************************************************************\
 126 *                                    Trace of the matrix                                 *
 127 \****************************************************************************************/
 128
 129 CV_IMPL CvScalar
 130 cvTrace( const CvArr* array )
 131 {
 132     CvScalar sum = {{0,0,0,0}};
 133
 134     CV_FUNCNAME( "cvTrace" );
 135
 136     __BEGIN__;
 137
 138     CvMat stub, *mat = 0;
 139
 140     if( CV_IS_MAT( array ))
 141     {
 142         mat = (CvMat*)array;
 143         int type = CV_MAT_TYPE(mat->type);
 144         int size = MIN(mat->rows,mat->cols);
 145         uchar* data = mat->data.ptr;
 146
 147         if( type == CV_32FC1 )
 148         {
 149             int step = mat->step + sizeof(float);
 150
 151             for( ; size--; data += step )
 152                 sum.val[0] += *(float*)data;
 153             EXIT;
 154         }
 155
 156         if( type == CV_64FC1 )
 157         {
 158             int step = mat->step + sizeof(double);
 159
 160             for( ; size--; data += step )
 161                 sum.val[0] += *(double*)data;
 162             EXIT;
 163         }
 164     }
 165
 166     CV_CALL( mat = cvGetDiag( array, &stub ));
 167     CV_CALL( sum = cvSum( mat ));
 168
 169     __END__;
 170
 171     return sum;
 172 }
 173
 174
 175 /****************************************************************************************\
 176 *                                     Matrix transpose                                   *
 177 \****************************************************************************************/
 178
 179 /////////////////// macros for inplace transposition of square matrix ////////////////////
 180
 181 #define ICV_DEF_TRANSP_INP_CASE_C1( \
 182     arrtype, len )                  \
 183 {                                   \
 184     arrtype* arr1 = arr;            \
 185     step /= sizeof(arr[0]);         \
 186                                     \
 187     while( --len )                  \
 188     {                               \
 189         arr += step, arr1++;        \
 190         arrtype* arr2 = arr;        \
 191         arrtype* arr3 = arr1;       \
 192                                     \
 193         do                          \
 194         {                           \
 195             arrtype t0 = arr2[0];   \
 196             arrtype t1 = arr3[0];   \
 197             arr2[0] = t1;           \
 198             arr3[0] = t0;           \
 199                                     \
 200             arr2++;                 \
 201             arr3 += step;           \
 202         }                           \
 203         while( arr2 != arr3  );     \
 204     }                               \
 205 }
 206
 207
 208 #define ICV_DEF_TRANSP_INP_CASE_C3( \
 209     arrtype, len )                  \
 210 {                                   \
 211     arrtype* arr1 = arr;            \
 212     int y;                          \
 213     step /= sizeof(arr[0]);         \
 214                                     \
 215     for( y = 1; y < len; y++ )      \
 216     {                               \
 217         arr += step, arr1 += 3;     \
 218         arrtype* arr2 = arr;        \
 219         arrtype* arr3 = arr1;       \
 220                                     \
 221         for( ; arr2!=arr3; arr2+=3, \
 222                         arr3+=step )\
 223         {                           \
 224             arrtype t0 = arr2[0];   \
 225             arrtype t1 = arr3[0];   \
 226             arr2[0] = t1;           \
 227             arr3[0] = t0;           \
 228             t0 = arr2[1];           \
 229             t1 = arr3[1];           \
 230             arr2[1] = t1;           \
 231             arr3[1] = t0;           \
 232             t0 = arr2[2];           \
 233             t1 = arr3[2];           \
 234             arr2[2] = t1;           \
 235             arr3[2] = t0;           \
 236         }                           \
 237     }                               \
 238 }
 239
 240
 241 #define ICV_DEF_TRANSP_INP_CASE_C4( \
 242     arrtype, len )                  \
 243 {                                   \
 244     arrtype* arr1 = arr;            \
 245     int y;                          \
 246     step /= sizeof(arr[0]);         \
 247                                     \
 248     for( y = 1; y < len; y++ )      \
 249     {                               \
 250         arr += step, arr1 += 4;     \
 251         arrtype* arr2 = arr;        \
 252         arrtype* arr3 = arr1;       \
 253                                     \
 254         for( ; arr2!=arr3; arr2+=4, \
 255                         arr3+=step )\
 256         {                           \
 257             arrtype t0 = arr2[0];   \
 258             arrtype t1 = arr3[0];   \
 259             arr2[0] = t1;           \
 260             arr3[0] = t0;           \
 261             t0 = arr2[1];           \
 262             t1 = arr3[1];           \
 263             arr2[1] = t1;           \
 264             arr3[1] = t0;           \
 265             t0 = arr2[2];           \
 266             t1 = arr3[2];           \
 267             arr2[2] = t1;           \
 268             arr3[2] = t0;           \
 269             t0 = arr2[3];           \
 270             t1 = arr3[3];           \
 271             arr2[3] = t1;           \
 272             arr3[3] = t0;           \
 273         }                           \
 274     }                               \
 275 }
 276
 277
 278 //////////////// macros for non-inplace transposition of rectangular matrix //////////////
 279
 280 #define ICV_DEF_TRANSP_CASE_C1( arrtype )       \
 281 {                                               \
 282     int x, y;                                   \
 283     srcstep /= sizeof(src[0]);                  \
 284     dststep /= sizeof(dst[0]);                  \
 285                                                 \
 286     for( y = 0; y <= size.height - 2; y += 2,   \
 287                 src += 2*srcstep, dst += 2 )    \
 288     {                                           \
 289         const arrtype* src1 = src + srcstep;    \
 290         arrtype* dst1 = dst;                    \
 291                                                 \
 292         for( x = 0; x <= size.width - 2;        \
 293                 x += 2, dst1 += dststep )       \
 294         {                                       \
 295             arrtype t0 = src[x];                \
 296             arrtype t1 = src1[x];               \
 297             dst1[0] = t0;                       \
 298             dst1[1] = t1;                       \
 299             dst1 += dststep;                    \
 300                                                 \
 301             t0 = src[x + 1];                    \
 302             t1 = src1[x + 1];                   \
 303             dst1[0] = t0;                       \
 304             dst1[1] = t1;                       \
 305         }                                       \
 306                                                 \
 307         if( x < size.width )                    \
 308         {                                       \
 309             arrtype t0 = src[x];                \
 310             arrtype t1 = src1[x];               \
 311             dst1[0] = t0;                       \
 312             dst1[1] = t1;                       \
 313         }                                       \
 314     }                                           \
 315                                                 \
 316     if( y < size.height )                       \
 317     {                                           \
 318         arrtype* dst1 = dst;                    \
 319         for( x = 0; x <= size.width - 2;        \
 320                 x += 2, dst1 += 2*dststep )     \
 321         {                                       \
 322             arrtype t0 = src[x];                \
 323             arrtype t1 = src[x + 1];            \
 324             dst1[0] = t0;                       \
 325             dst1[dststep] = t1;                 \
 326         }                                       \
 327                                                 \
 328         if( x < size.width )                    \
 329         {                                       \
 330             arrtype t0 = src[x];                \
 331             dst1[0] = t0;                       \
 332         }                                       \
 333     }                                           \
 334 }
 335
 336
 337 #define ICV_DEF_TRANSP_CASE_C3( arrtype )       \
 338 {                                               \
 339     size.width *= 3;                            \
 340     srcstep /= sizeof(src[0]);                  \
 341     dststep /= sizeof(dst[0]);                  \
 342                                                 \
 343     for( ; size.height--; src+=srcstep, dst+=3 )\
 344     {                                           \
 345         int x;                                  \
 346         arrtype* dst1 = dst;                    \
 347                                                 \
 348         for( x = 0; x < size.width; x += 3,     \
 349                             dst1 += dststep )   \
 350         {                                       \
 351             arrtype t0 = src[x];                \
 352             arrtype t1 = src[x + 1];            \
 353             arrtype t2 = src[x + 2];            \
 354                                                 \
 355             dst1[0] = t0;                       \
 356             dst1[1] = t1;                       \
 357             dst1[2] = t2;                       \
 358         }                                       \
 359     }                                           \
 360 }
 361
 362
 363 #define ICV_DEF_TRANSP_CASE_C4( arrtype )       \
 364 {                                               \
 365     size.width *= 4;                            \
 366     srcstep /= sizeof(src[0]);                  \
 367     dststep /= sizeof(dst[0]);                  \
 368                                                 \
 369     for( ; size.height--; src+=srcstep, dst+=4 )\
 370     {                                           \
 371         int x;                                  \
 372         arrtype* dst1 = dst;                    \
 373                                                 \
 374         for( x = 0; x < size.width; x += 4,     \
 375                             dst1 += dststep )   \
 376         {                                       \
 377             arrtype t0 = src[x];                \
 378             arrtype t1 = src[x + 1];            \
 379                                                 \
 380             dst1[0] = t0;                       \
 381             dst1[1] = t1;                       \
 382                                                 \
 383             t0 = src[x + 2];                    \
 384             t1 = src[x + 3];                    \
 385                                                 \
 386             dst1[2] = t0;                       \
 387             dst1[3] = t1;                       \
 388         }                                       \
 389     }                                           \
 390 }
 391
 392
 393 #define ICV_DEF_TRANSP_INP_FUNC( flavor, arrtype, cn )      \
 394 static CvStatus CV_STDCALL                                  \
 395 icvTranspose_##flavor( arrtype* arr, int step, CvSize size )\
 396 {                                                           \
 397     assert( size.width == size.height );                    \
 398                                                             \
 399     ICV_DEF_TRANSP_INP_CASE_C##cn( arrtype, size.width )    \
 400     return CV_OK;                                           \
 401 }
 402
 403
 404 #define ICV_DEF_TRANSP_FUNC( flavor, arrtype, cn )          \
 405 static CvStatus CV_STDCALL                                  \
 406 icvTranspose_##flavor( const arrtype* src, int srcstep,     \
 407                     arrtype* dst, int dststep, CvSize size )\
 408 {                                                           \
 409     ICV_DEF_TRANSP_CASE_C##cn( arrtype )                    \
 410     return CV_OK;                                           \
 411 }
 412
 413
 414 ICV_DEF_TRANSP_INP_FUNC( 8u_C1IR, uchar, 1 )
 415 ICV_DEF_TRANSP_INP_FUNC( 8u_C2IR, ushort, 1 )
 416 ICV_DEF_TRANSP_INP_FUNC( 8u_C3IR, uchar, 3 )
 417 ICV_DEF_TRANSP_INP_FUNC( 16u_C2IR, int, 1 )
 418 ICV_DEF_TRANSP_INP_FUNC( 16u_C3IR, ushort, 3 )
 419 ICV_DEF_TRANSP_INP_FUNC( 32s_C2IR, int64, 1 )
 420 ICV_DEF_TRANSP_INP_FUNC( 32s_C3IR, int, 3 )
 421 ICV_DEF_TRANSP_INP_FUNC( 64s_C2IR, int, 4 )
 422 ICV_DEF_TRANSP_INP_FUNC( 64s_C3IR, int64, 3 )
 423 ICV_DEF_TRANSP_INP_FUNC( 64s_C4IR, int64, 4 )
 424
 425
 426 ICV_DEF_TRANSP_FUNC( 8u_C1R, uchar, 1 )
 427 ICV_DEF_TRANSP_FUNC( 8u_C2R, ushort, 1 )
 428 ICV_DEF_TRANSP_FUNC( 8u_C3R, uchar, 3 )
 429 ICV_DEF_TRANSP_FUNC( 16u_C2R, int, 1 )
 430 ICV_DEF_TRANSP_FUNC( 16u_C3R, ushort, 3 )
 431 ICV_DEF_TRANSP_FUNC( 32s_C2R, int64, 1 )
 432 ICV_DEF_TRANSP_FUNC( 32s_C3R, int, 3 )
 433 ICV_DEF_TRANSP_FUNC( 64s_C2R, int, 4 )
 434 ICV_DEF_TRANSP_FUNC( 64s_C3R, int64, 3 )
 435 ICV_DEF_TRANSP_FUNC( 64s_C4R, int64, 4 )
 436
 437 CV_DEF_INIT_PIXSIZE_TAB_2D( Transpose, R )
 438 CV_DEF_INIT_PIXSIZE_TAB_2D( Transpose, IR )
 439
 440 CV_IMPL void
 441 cvTranspose( const CvArr* srcarr, CvArr* dstarr )
 442 {
 443     static CvBtFuncTable tab, inp_tab;
 444     static int inittab = 0;
 445
 446     CV_FUNCNAME( "cvTranspose" );
 447
 448     __BEGIN__;
 449
 450     CvMat sstub, *src = (CvMat*)srcarr;
 451     CvMat dstub, *dst = (CvMat*)dstarr;
 452     CvSize size;
 453     int type, pix_size;
 454
 455     if( !inittab )
 456     {
 457         icvInitTransposeIRTable( &inp_tab );
 458         icvInitTransposeRTable( &tab );
 459         inittab = 1;
 460     }
 461
 462     if( !CV_IS_MAT( src ))
 463     {
 464         int coi = 0;
 465         CV_CALL( src = cvGetMat( src, &sstub, &coi ));
 466         if( coi != 0 )
 467             CV_ERROR( CV_BadCOI, "coi is not supported" );
 468     }
 469
 470     type = CV_MAT_TYPE( src->type );
 471     pix_size = CV_ELEM_SIZE(type);
 472     size = cvGetMatSize( src );
 473
 474     if( dstarr == srcarr )
 475     {
 476         dst = src;
 477     }
 478     else
 479     {
 480         if( !CV_IS_MAT( dst ))
 481         {
 482             int coi = 0;
 483             CV_CALL( dst = cvGetMat( dst, &dstub, &coi ));
 484
 485             if( coi != 0 )
 486             CV_ERROR( CV_BadCOI, "coi is not supported" );
 487         }
 488
 489         if( !CV_ARE_TYPES_EQ( src, dst ))
 490             CV_ERROR( CV_StsUnmatchedFormats, "" );
 491
 492         if( size.width != dst->height || size.height != dst->width )
 493             CV_ERROR( CV_StsUnmatchedSizes, "" );
 494     }
 495
 496     if( src->data.ptr == dst->data.ptr )
 497     {
 498         if( size.width == size.height )
 499         {
 500             CvFunc2D_1A func = (CvFunc2D_1A)(inp_tab.fn_2d[pix_size]);
 501
 502             if( !func )
 503                 CV_ERROR( CV_StsUnsupportedFormat, "" );
 504
 505             IPPI_CALL( func( src->data.ptr, src->step, size ));
 506         }
 507         else
 508         {
 509             if( size.width != 1 && size.height != 1 )
 510                 CV_ERROR( CV_StsBadSize,
 511                     "Rectangular matrix can not be transposed inplace" );
 512
 513             if( !CV_IS_MAT_CONT( src->type & dst->type ))
 514                 CV_ERROR( CV_StsBadFlag, "In case of inplace column/row transposition "
 515                                        "both source and destination must be continuous" );
 516
 517             if( dst == src )
 518             {
 519                 int t;
 520                 CV_SWAP( dst->width, dst->height, t );
 521                 dst->step = dst->height == 1 ? 0 : pix_size;
 522             }
 523         }
 524     }
 525     else
 526     {
 527         CvFunc2D_2A func = (CvFunc2D_2A)(tab.fn_2d[pix_size]);
 528
 529         if( !func )
 530             CV_ERROR( CV_StsUnsupportedFormat, "" );
 531
 532         IPPI_CALL( func( src->data.ptr, src->step,
 533                          dst->data.ptr, dst->step, size ));
 534     }
 535
 536     __END__;
 537 }
 538
 539
 540 /****************************************************************************************\
 541 *                              LU decomposition/back substitution                        *
 542 \****************************************************************************************/
 543
 544 #define arrtype float
 545 #define temptype double
 546
 547 typedef  CvStatus (CV_STDCALL * CvLUDecompFunc)( double* A, int stepA, CvSize sizeA,
 548                                                  void* B, int stepB, CvSize sizeB,
 549                                                  double* det );
 550
 551 typedef  CvStatus (CV_STDCALL * CvLUBackFunc)( double* A, int stepA, CvSize sizeA,
 552                                                void* B, int stepB, CvSize sizeB );
 553
 554
 555 #define ICV_DEF_LU_DECOMP_FUNC( flavor, arrtype )                               \
 556 static CvStatus CV_STDCALL                                                      \
 557 icvLUDecomp_##flavor( double* A, int stepA, CvSize sizeA,                       \
 558                       arrtype* B, int stepB, CvSize sizeB, double* _det )       \
 559 {                                                                               \
 560     int n = sizeA.width;                                                        \
 561     int m = 0, i;                                                               \
 562     double det = 1;                                                             \
 563                                                                                 \
 564     assert( sizeA.width == sizeA.height );                                      \
 565                                                                                 \
 566     if( B )                                                                     \
 567     {                                                                           \
 568         assert( sizeA.height == sizeB.height );                                 \
 569         m = sizeB.width;                                                        \
 570     }                                                                           \
 571     stepA /= sizeof(A[0]);                                                      \
 572     stepB /= sizeof(B[0]);                                                      \
 573                                                                                 \
 574     for( i = 0; i < n; i++, A += stepA, B += stepB )                            \
 575     {                                                                           \
 576         int j, k = i;                                                           \
 577         double* tA = A;                                                         \
 578         arrtype* tB = 0;                                                        \
 579         double kval = fabs(A[i]), tval;                                         \
 580                                                                                 \
 581         /* find the pivot element */                                            \
 582         for( j = i + 1; j < n; j++ )                                            \
 583         {                                                                       \
 584             tA += stepA;                                                        \
 585             tval = fabs(tA[i]);                                                 \
 586                                                                                 \
 587             if( tval > kval )                                                   \
 588             {                                                                   \
 589                 kval = tval;                                                    \
 590                 k = j;                                                          \
 591             }                                                                   \
 592         }                                                                       \
 593                                                                                 \
 594         if( kval == 0 )                                                         \
 595         {                                                                       \
 596             det = 0;                                                            \
 597             break;                                                              \
 598         }                                                                       \
 599                                                                                 \
 600         /* swap rows */                                                         \
 601         if( k != i )                                                            \
 602         {                                                                       \
 603             tA = A + stepA*(k - i);                                             \
 604             det = -det;                                                         \
 605                                                                                 \
 606             for( j = i; j < n; j++ )                                            \
 607             {                                                                   \
 608                 double t;                                                       \
 609                 CV_SWAP( A[j], tA[j], t );                                      \
 610             }                                                                   \
 611                                                                                 \
 612             if( m > 0 )                                                         \
 613             {                                                                   \
 614                 tB = B + stepB*(k - i);                                         \
 615                                                                                 \
 616                 for( j = 0; j < m; j++ )                                        \
 617                 {                                                               \
 618                     arrtype t = B[j];                                           \
 619                     CV_SWAP( B[j], tB[j], t );                                  \
 620                 }                                                               \
 621             }                                                                   \
 622         }                                                                       \
 623                                                                                 \
 624         tval = 1./A[i];                                                         \
 625         det *= A[i];                                                            \
 626         tA = A;                                                                 \
 627         tB = B;                                                                 \
 628         A[i] = tval; /* to replace division with multiplication in LUBack */    \
 629                                                                                 \
 630         /* update matrix and the right side of the system */                    \
 631         for( j = i + 1; j < n; j++ )                                            \
 632         {                                                                       \
 633             tA += stepA;                                                        \
 634             tB += stepB;                                                        \
 635             double alpha = -tA[i]*tval;                                         \
 636                                                                                 \
 637             for( k = i + 1; k < n; k++ )                                        \
 638                 tA[k] = tA[k] + alpha*A[k];                                     \
 639                                                                                 \
 640             if( m > 0 )                                                         \
 641                 for( k = 0; k < m; k++ )                                        \
 642                     tB[k] = (arrtype)(tB[k] + alpha*B[k]);                      \
 643         }                                                                       \
 644     }                                                                           \
 645                                                                                 \
 646     if( _det )                                                                  \
 647         *_det = det;                                                            \
 648                                                                                 \
 649     return CV_OK;                                                               \
 650 }
 651
 652
 653 ICV_DEF_LU_DECOMP_FUNC( 32f, float )
 654 ICV_DEF_LU_DECOMP_FUNC( 64f, double )
 655
 656
 657 #define ICV_DEF_LU_BACK_FUNC( flavor, arrtype )                                 \
 658 static CvStatus CV_STDCALL                                                      \
 659 icvLUBack_##flavor( double* A, int stepA, CvSize sizeA,                         \
 660                     arrtype* B, int stepB, CvSize sizeB )                       \
 661 {                                                                               \
 662     int n = sizeA.width;                                                        \
 663     int m = sizeB.width, i;                                                     \
 664                                                                                 \
 665     assert( m > 0 && sizeA.width == sizeA.height &&                             \
 666             sizeA.height == sizeB.height );                                     \
 667     stepA /= sizeof(A[0]);                                                      \
 668     stepB /= sizeof(B[0]);                                                      \
 669                                                                                 \
 670     A += stepA*(n - 1);                                                         \
 671     B += stepB*(n - 1);                                                         \
 672                                                                                 \
 673     for( i = n - 1; i >= 0; i--, A -= stepA )                                   \
 674     {                                                                           \
 675         int j, k;                                                               \
 676         for( j = 0; j < m; j++ )                                                \
 677         {                                                                       \
 678             arrtype* tB = B + j;                                                \
 679             double x = 0;                                                       \
 680                                                                                 \
 681             for( k = n - 1; k > i; k--, tB -= stepB )                           \
 682                 x += A[k]*tB[0];                                                \
 683                                                                                 \
 684             tB[0] = (arrtype)((tB[0] - x)*A[i]);                                \
 685         }                                                                       \
 686     }                                                                           \
 687                                                                                 \
 688     return CV_OK;                                                               \
 689 }
 690
 691
 692 ICV_DEF_LU_BACK_FUNC( 32f, float )
 693 ICV_DEF_LU_BACK_FUNC( 64f, double )
 694
 695 static CvFuncTable lu_decomp_tab, lu_back_tab;
 696 static int lu_inittab = 0;
 697
 698 static void icvInitLUTable( CvFuncTable* decomp_tab,
 699                             CvFuncTable* back_tab )
 700 {
 701     decomp_tab->fn_2d[0] = (void*)icvLUDecomp_32f;
 702     decomp_tab->fn_2d[1] = (void*)icvLUDecomp_64f;
 703     back_tab->fn_2d[0] = (void*)icvLUBack_32f;
 704     back_tab->fn_2d[1] = (void*)icvLUBack_64f;
 705 }
 706
 707
 708
 709 /****************************************************************************************\
 710 *                                 Determinant of the matrix                              *
 711 \****************************************************************************************/
 712
 713 #define det2(m)   (m(0,0)*m(1,1) - m(0,1)*m(1,0))
 714 #define det3(m)   (m(0,0)*(m(1,1)*m(2,2) - m(1,2)*m(2,1)) -  \
 715                    m(0,1)*(m(1,0)*m(2,2) - m(1,2)*m(2,0)) +  \
 716                    m(0,2)*(m(1,0)*m(2,1) - m(1,1)*m(2,0)))
 717
 718 CV_IMPL double
 719 cvDet( const CvArr* arr )
 720 {
 721     double result = 0;
 722     uchar* buffer = 0;
 723     int local_alloc = 0;
 724
 725     CV_FUNCNAME( "cvDet" );
 726
 727     __BEGIN__;
 728
 729     CvMat stub, *mat = (CvMat*)arr;
 730     int type;
 731
 732     if( !CV_IS_MAT( mat ))
 733     {
 734         CV_CALL( mat = cvGetMat( mat, &stub ));
 735     }
 736
 737     type = CV_MAT_TYPE( mat->type );
 738
 739     if( mat->width != mat->height )
 740         CV_ERROR( CV_StsBadSize, "The matrix must be square" );
 741
 742     #define Mf( y, x ) ((float*)(m + y*step))[x]
 743     #define Md( y, x ) ((double*)(m + y*step))[x]
 744
 745     if( mat->width == 2 )
 746     {
 747         uchar* m = mat->data.ptr;
 748         int step = mat->step;
 749
 750         if( type == CV_32FC1 )
 751         {
 752             result = det2(Mf);
 753         }
 754         else if( type == CV_64FC1 )
 755         {
 756             result = det2(Md);
 757         }
 758         else
 759         {
 760             CV_ERROR( CV_StsUnsupportedFormat, "" );
 761         }
 762     }
 763     else if( mat->width == 3 )
 764     {
 765         uchar* m = mat->data.ptr;
 766         int step = mat->step;
 767
 768         if( type == CV_32FC1 )
 769         {
 770             result = det3(Mf);
 771         }
 772         else if( type == CV_64FC1 )
 773         {
 774             result = det3(Md);
 775         }
 776         else
 777         {
 778             CV_ERROR( CV_StsUnsupportedFormat, "" );
 779         }
 780     }
 781     else if( mat->width == 1 )
 782     {
 783         if( type == CV_32FC1 )
 784         {
 785             result = mat->data.fl[0];
 786         }
 787         else if( type == CV_64FC1 )
 788         {
 789             result = mat->data.db[0];
 790         }
 791         else
 792         {
 793             CV_ERROR( CV_StsUnsupportedFormat, "" );
 794         }
 795     }
 796     else
 797     {
 798         CvLUDecompFunc decomp_func;
 799         CvSize size = cvGetMatSize( mat );
 800         const int worktype = CV_64FC1;
 801         int buf_size = size.width*size.height*CV_ELEM_SIZE(worktype);
 802         CvMat tmat;
 803
 804         if( !lu_inittab )
 805         {
 806             icvInitLUTable( &lu_decomp_tab, &lu_back_tab );
 807             lu_inittab = 1;
 808         }
 809
 810         if( CV_MAT_CN( type ) != 1 || CV_MAT_DEPTH( type ) < CV_32F )
 811             CV_ERROR( CV_StsUnsupportedFormat, "" );
 812
 813         if( size.width <= CV_MAX_LOCAL_MAT_SIZE )
 814         {
 815             buffer = (uchar*)cvStackAlloc( buf_size );
 816             local_alloc = 1;
 817         }
 818         else
 819         {
 820             CV_CALL( buffer = (uchar*)cvAlloc( buf_size ));
 821         }
 822
 823         CV_CALL( cvInitMatHeader( &tmat, size.height, size.width, worktype, buffer ));
 824         if( type == worktype )
 825         {
 826                 CV_CALL( cvCopy( mat, &tmat ));
 827         }
 828         else
 829             CV_CALL( cvConvert( mat, &tmat ));
 830
 831         decomp_func = (CvLUDecompFunc)(lu_decomp_tab.fn_2d[CV_MAT_DEPTH(worktype)-CV_32F]);
 832         assert( decomp_func );
 833
 834         IPPI_CALL( decomp_func( tmat.data.db, tmat.step, size, 0, 0, size, &result ));
 835     }
 836
 837     #undef Mf
 838     #undef Md
 839
 840     /*icvCheckVector_64f( &result, 1 );*/
 841
 842     __END__;
 843
 844     if( buffer && !local_alloc )
 845         cvFree( &buffer );
 846
 847     return result;
 848 }
 849
 850
 851
 852 /****************************************************************************************\
 853 *                          Inverse (or pseudo-inverse) of the matrix                     *
 854 \****************************************************************************************/
 855
 856 #define Sf( y, x ) ((float*)(srcdata + y*srcstep))[x]
 857 #define Sd( y, x ) ((double*)(srcdata + y*srcstep))[x]
 858 #define Df( y, x ) ((float*)(dstdata + y*dststep))[x]
 859 #define Dd( y, x ) ((double*)(dstdata + y*dststep))[x]
 860
 861 CV_IMPL double
 862 cvInvert( const CvArr* srcarr, CvArr* dstarr, int method )
 863 {
 864     CvMat* u = 0;
 865     CvMat* v = 0;
 866     CvMat* w = 0;
 867
 868     uchar* buffer = 0;
 869     int local_alloc = 0;
 870     double result = 0;
 871
 872     CV_FUNCNAME( "cvInvert" );
 873
 874     __BEGIN__;
 875
 876     CvMat sstub, *src = (CvMat*)srcarr;
 877     CvMat dstub, *dst = (CvMat*)dstarr;
 878     int type;
 879
 880     if( !CV_IS_MAT( src ))
 881         CV_CALL( src = cvGetMat( src, &sstub ));
 882
 883     if( !CV_IS_MAT( dst ))
 884         CV_CALL( dst = cvGetMat( dst, &dstub ));
 885
 886     type = CV_MAT_TYPE( src->type );
 887
 888     if( method == CV_SVD || method == CV_SVD_SYM )
 889     {
 890         int n = MIN(src->rows,src->cols);
 891         if( method == CV_SVD_SYM && src->rows != src->cols )
 892             CV_ERROR( CV_StsBadSize, "CV_SVD_SYM method is used for non-square matrix" );
 893
 894         CV_CALL( u = cvCreateMat( n, src->rows, src->type ));
 895         if( method != CV_SVD_SYM )
 896             CV_CALL( v = cvCreateMat( n, src->cols, src->type ));
 897         CV_CALL( w = cvCreateMat( n, 1, src->type ));
 898         CV_CALL( cvSVD( src, w, u, v, CV_SVD_U_T + CV_SVD_V_T ));
 899
 900         if( type == CV_32FC1 )
 901             result = w->data.fl[0] >= FLT_EPSILON ?
 902                      w->data.fl[w->rows-1]/w->data.fl[0] : 0;
 903         else
 904             result = w->data.db[0] >= FLT_EPSILON ?
 905                      w->data.db[w->rows-1]/w->data.db[0] : 0;
 906
 907         CV_CALL( cvSVBkSb( w, u, v ? v : u, 0, dst, CV_SVD_U_T + CV_SVD_V_T ));
 908         EXIT;
 909     }
 910     else if( method != CV_LU )
 911         CV_ERROR( CV_StsBadArg, "Unknown inversion method" );
 912
 913     if( !CV_ARE_TYPES_EQ( src, dst ))
 914         CV_ERROR( CV_StsUnmatchedFormats, "" );
 915
 916     if( src->width != src->height )
 917         CV_ERROR( CV_StsBadSize, "The matrix must be square" );
 918
 919     if( !CV_ARE_SIZES_EQ( src, dst ))
 920         CV_ERROR( CV_StsUnmatchedSizes, "" );
 921
 922     if( type != CV_32FC1 && type != CV_64FC1 )
 923         CV_ERROR( CV_StsUnsupportedFormat, "" );
 924
 925     if( src->width <= 3 )
 926     {
 927         uchar* srcdata = src->data.ptr;
 928         uchar* dstdata = dst->data.ptr;
 929         int srcstep = src->step;
 930         int dststep = dst->step;
 931
 932         if( src->width == 2 )
 933         {
 934             if( type == CV_32FC1 )
 935             {
 936                 double d = det2(Sf);
 937                 if( d != 0. )
 938                 {
 939                     double t0, t1;
 940                     result = d;
 941                     d = 1./d;
 942                     t0 = Sf(0,0)*d;
 943                     t1 = Sf(1,1)*d;
 944                     Df(1,1) = (float)t0;
 945                     Df(0,0) = (float)t1;
 946                     t0 = -Sf(0,1)*d;
 947                     t1 = -Sf(1,0)*d;
 948                     Df(0,1) = (float)t0;
 949                     Df(1,0) = (float)t1;
 950                 }
 951             }
 952             else
 953             {
 954                 double d = det2(Sd);
 955                 if( d != 0. )
 956                 {
 957                     double t0, t1;
 958                     result = d;
 959                     d = 1./d;
 960                     t0 = Sd(0,0)*d;
 961                     t1 = Sd(1,1)*d;
 962                     Dd(1,1) = t0;
 963                     Dd(0,0) = t1;
 964                     t0 = -Sd(0,1)*d;
 965                     t1 = -Sd(1,0)*d;
 966                     Dd(0,1) = t0;
 967                     Dd(1,0) = t1;
 968                 }
 969             }
 970         }
 971         else if( src->width == 3 )
 972         {
 973             if( type == CV_32FC1 )
 974             {
 975                 double d = det3(Sf);
 976                 if( d != 0. )
 977                 {
 978                     float t[9];
 979                     result = d;
 980                     d = 1./d;
 981
 982                     t[0] = (float)((Sf(1,1) * Sf(2,2) - Sf(1,2) * Sf(2,1)) * d);
 983                     t[1] = (float)((Sf(0,2) * Sf(2,1) - Sf(0,1) * Sf(2,2)) * d);
 984                     t[2] = (float)((Sf(0,1) * Sf(1,2) - Sf(0,2) * Sf(1,1)) * d);
 985
 986                     t[3] = (float)((Sf(1,2) * Sf(2,0) - Sf(1,0) * Sf(2,2)) * d);
 987                     t[4] = (float)((Sf(0,0) * Sf(2,2) - Sf(0,2) * Sf(2,0)) * d);
 988                     t[5] = (float)((Sf(0,2) * Sf(1,0) - Sf(0,0) * Sf(1,2)) * d);
 989
 990                     t[6] = (float)((Sf(1,0) * Sf(2,1) - Sf(1,1) * Sf(2,0)) * d);
 991                     t[7] = (float)((Sf(0,1) * Sf(2,0) - Sf(0,0) * Sf(2,1)) * d);
 992                     t[8] = (float)((Sf(0,0) * Sf(1,1) - Sf(0,1) * Sf(1,0)) * d);
 993
 994                     Df(0,0) = t[0]; Df(0,1) = t[1]; Df(0,2) = t[2];
 995                     Df(1,0) = t[3]; Df(1,1) = t[4]; Df(1,2) = t[5];
 996                     Df(2,0) = t[6]; Df(2,1) = t[7]; Df(2,2) = t[8];
 997                 }
 998             }
 999             else
1000             {
1001                 double d = det3(Sd);
1002                 if( d != 0. )
1003                 {
1004                     double t[9];
1005                     result = d;
1006                     d = 1./d;
1007
1008                     t[0] = (Sd(1,1) * Sd(2,2) - Sd(1,2) * Sd(2,1)) * d;
1009                     t[1] = (Sd(0,2) * Sd(2,1) - Sd(0,1) * Sd(2,2)) * d;
1010                     t[2] = (Sd(0,1) * Sd(1,2) - Sd(0,2) * Sd(1,1)) * d;
1011
1012                     t[3] = (Sd(1,2) * Sd(2,0) - Sd(1,0) * Sd(2,2)) * d;
1013                     t[4] = (Sd(0,0) * Sd(2,2) - Sd(0,2) * Sd(2,0)) * d;
1014                     t[5] = (Sd(0,2) * Sd(1,0) - Sd(0,0) * Sd(1,2)) * d;
1015
1016                     t[6] = (Sd(1,0) * Sd(2,1) - Sd(1,1) * Sd(2,0)) * d;
1017                     t[7] = (Sd(0,1) * Sd(2,0) - Sd(0,0) * Sd(2,1)) * d;
1018                     t[8] = (Sd(0,0) * Sd(1,1) - Sd(0,1) * Sd(1,0)) * d;
1019
1020                     Dd(0,0) = t[0]; Dd(0,1) = t[1]; Dd(0,2) = t[2];
1021                     Dd(1,0) = t[3]; Dd(1,1) = t[4]; Dd(1,2) = t[5];
1022                     Dd(2,0) = t[6]; Dd(2,1) = t[7]; Dd(2,2) = t[8];
1023                 }
1024             }
1025         }
1026         else
1027         {
1028             assert( src->width == 1 );
1029
1030             if( type == CV_32FC1 )
1031             {
1032                 double d = Sf(0,0);
1033                 if( d != 0. )
1034                 {
1035                     result = d;
1036                     Df(0,0) = (float)(1./d);
1037                 }
1038             }
1039             else
1040             {
1041                 double d = Sd(0,0);
1042                 if( d != 0. )
1043                 {
1044                     result = d;
1045                     Dd(0,0) = 1./d;
1046                 }
1047             }
1048         }
1049     }
1050     else
1051     {
1052         CvLUDecompFunc decomp_func;
1053         CvLUBackFunc back_func;
1054         CvSize size = cvGetMatSize( src );
1055         const int worktype = CV_64FC1;
1056         int buf_size = size.width*size.height*CV_ELEM_SIZE(worktype);
1057         CvMat tmat;
1058
1059         if( !lu_inittab )
1060         {
1061             icvInitLUTable( &lu_decomp_tab, &lu_back_tab );
1062             lu_inittab = 1;
1063         }
1064
1065         if( size.width <= CV_MAX_LOCAL_MAT_SIZE )
1066         {
1067             buffer = (uchar*)cvStackAlloc( buf_size );
1068             local_alloc = 1;
1069         }
1070         else
1071         {
1072             CV_CALL( buffer = (uchar*)cvAlloc( buf_size ));
1073         }
1074
1075         CV_CALL( cvInitMatHeader( &tmat, size.height, size.width, worktype, buffer ));
1076         if( type == worktype )
1077         {
1078             CV_CALL( cvCopy( src, &tmat ));
1079         }
1080         else
1081             CV_CALL( cvConvert( src, &tmat ));
1082         CV_CALL( cvSetIdentity( dst ));
1083
1084         decomp_func = (CvLUDecompFunc)(lu_decomp_tab.fn_2d[CV_MAT_DEPTH(type)-CV_32F]);
1085         back_func = (CvLUBackFunc)(lu_back_tab.fn_2d[CV_MAT_DEPTH(type)-CV_32F]);
1086         assert( decomp_func && back_func );
1087
1088         IPPI_CALL( decomp_func( tmat.data.db, tmat.step, size,
1089                                 dst->data.ptr, dst->step, size, &result ));
1090
1091         if( result != 0 )
1092         {
1093             IPPI_CALL( back_func( tmat.data.db, tmat.step, size,
1094                                   dst->data.ptr, dst->step, size ));
1095         }
1096     }
1097
1098     if( !result )
1099         CV_CALL( cvSetZero( dst ));
1100
1101     __END__;
1102
1103     if( buffer && !local_alloc )
1104         cvFree( &buffer );
1105
1106     if( u || v || w )
1107     {
1108         cvReleaseMat( &u );
1109         cvReleaseMat( &v );
1110         cvReleaseMat( &w );
1111     }
1112
1113     return result;
1114 }
1115
1116
1117 /****************************************************************************************\
1118 *                               Linear system [least-squares] solution                   *
1119 \****************************************************************************************/
1120
1121 CV_IMPL int
1122 cvSolve( const CvArr* A, const CvArr* b, CvArr* x, int method )
1123 {
1124     CvMat* u = 0;
1125     CvMat* v = 0;
1126     CvMat* w = 0;
1127
1128     uchar* buffer = 0;
1129     int local_alloc = 0;
1130     int result = 1;
1131
1132     CV_FUNCNAME( "cvSolve" );
1133
1134     __BEGIN__;
1135
1136     CvMat sstub, *src = (CvMat*)A;
1137     CvMat dstub, *dst = (CvMat*)x;
1138     CvMat bstub, *src2 = (CvMat*)b;
1139     int type;
1140
1141     if( !CV_IS_MAT( src ))
1142         CV_CALL( src = cvGetMat( src, &sstub ));
1143
1144     if( !CV_IS_MAT( src2 ))
1145         CV_CALL( src2 = cvGetMat( src2, &bstub ));
1146
1147     if( !CV_IS_MAT( dst ))
1148         CV_CALL( dst = cvGetMat( dst, &dstub ));
1149
1150     if( method == CV_SVD || method == CV_SVD_SYM )
1151     {
1152         int n = MIN(src->rows,src->cols);
1153
1154         if( method == CV_SVD_SYM && src->rows != src->cols )
1155             CV_ERROR( CV_StsBadSize, "CV_SVD_SYM method is used for non-square matrix" );
1156
1157         CV_CALL( u = cvCreateMat( n, src->rows, src->type ));
1158         if( method != CV_SVD_SYM )
1159             CV_CALL( v = cvCreateMat( n, src->cols, src->type ));
1160         CV_CALL( w = cvCreateMat( n, 1, src->type ));
1161         CV_CALL( cvSVD( src, w, u, v, CV_SVD_U_T + CV_SVD_V_T ));
1162         CV_CALL( cvSVBkSb( w, u, v ? v : u, src2, dst, CV_SVD_U_T + CV_SVD_V_T ));
1163         EXIT;
1164     }
1165     else if( method != CV_LU )
1166         CV_ERROR( CV_StsBadArg, "Unknown inversion method" );
1167
1168     type = CV_MAT_TYPE( src->type );
1169
1170     if( !CV_ARE_TYPES_EQ( src, dst ) || !CV_ARE_TYPES_EQ( src, src2 ))
1171         CV_ERROR( CV_StsUnmatchedFormats, "" );
1172
1173     if( src->width != src->height )
1174         CV_ERROR( CV_StsBadSize, "The matrix must be square" );
1175
1176     if( !CV_ARE_SIZES_EQ( src2, dst ) || src->width != src2->height )
1177         CV_ERROR( CV_StsUnmatchedSizes, "" );
1178
1179     if( type != CV_32FC1 && type != CV_64FC1 )
1180         CV_ERROR( CV_StsUnsupportedFormat, "" );
1181
1182     // check case of a single equation and small matrix
1183     if( src->width <= 3 && src2->width == 1 )
1184     {
1185         #define bf(y) ((float*)(bdata + y*src2step))[0]
1186         #define bd(y) ((double*)(bdata + y*src2step))[0]
1187
1188         uchar* srcdata = src->data.ptr;
1189         uchar* bdata = src2->data.ptr;
1190         uchar* dstdata = dst->data.ptr;
1191         int srcstep = src->step;
1192         int src2step = src2->step;
1193         int dststep = dst->step;
1194
1195         if( src->width == 2 )
1196         {
1197             if( type == CV_32FC1 )
1198             {
1199                 double d = det2(Sf);
1200                 if( d != 0. )
1201                 {
1202                     float t;
1203                     d = 1./d;
1204                     t = (float)((bf(0)*Sf(1,1) - bf(1)*Sf(0,1))*d);
1205                     Df(1,0) = (float)((bf(1)*Sf(0,0) - bf(0)*Sf(1,0))*d);
1206                     Df(0,0) = t;
1207                 }
1208                 else
1209                     result = 0;
1210             }
1211             else
1212             {
1213                 double d = det2(Sd);
1214                 if( d != 0. )
1215                 {
1216                     double t;
1217                     d = 1./d;
1218                     t = (bd(0)*Sd(1,1) - bd(1)*Sd(0,1))*d;
1219                     Dd(1,0) = (bd(1)*Sd(0,0) - bd(0)*Sd(1,0))*d;
1220                     Dd(0,0) = t;
1221                 }
1222                 else
1223                     result = 0;
1224             }
1225         }
1226         else if( src->width == 3 )
1227         {
1228             if( type == CV_32FC1 )
1229             {
1230                 double d = det3(Sf);
1231                 if( d != 0. )
1232                 {
1233                     float t[3];
1234                     d = 1./d;
1235
1236                     t[0] = (float)(d*
1237                            (bf(0)*(Sf(1,1)*Sf(2,2) - Sf(1,2)*Sf(2,1)) -
1238                             Sf(0,1)*(bf(1)*Sf(2,2) - Sf(1,2)*bf(2)) +
1239                             Sf(0,2)*(bf(1)*Sf(2,1) - Sf(1,1)*bf(2))));
1240
1241                     t[1] = (float)(d*
1242                            (Sf(0,0)*(bf(1)*Sf(2,2) - Sf(1,2)*bf(2)) -
1243                             bf(0)*(Sf(1,0)*Sf(2,2) - Sf(1,2)*Sf(2,0)) +
1244                             Sf(0,2)*(Sf(1,0)*bf(2) - bf(1)*Sf(2,0))));
1245
1246                     t[2] = (float)(d*
1247                            (Sf(0,0)*(Sf(1,1)*bf(2) - bf(1)*Sf(2,1)) -
1248                             Sf(0,1)*(Sf(1,0)*bf(2) - bf(1)*Sf(2,0)) +
1249                             bf(0)*(Sf(1,0)*Sf(2,1) - Sf(1,1)*Sf(2,0))));
1250
1251                     Df(0,0) = t[0];
1252                     Df(1,0) = t[1];
1253                     Df(2,0) = t[2];
1254                 }
1255                 else
1256                     result = 0;
1257             }
1258             else
1259             {
1260                 double d = det3(Sd);
1261                 if( d != 0. )
1262                 {
1263                     double t[9];
1264
1265                     d = 1./d;
1266
1267                     t[0] = ((Sd(1,1) * Sd(2,2) - Sd(1,2) * Sd(2,1))*bd(0) +
1268                             (Sd(0,2) * Sd(2,1) - Sd(0,1) * Sd(2,2))*bd(1) +
1269                             (Sd(0,1) * Sd(1,2) - Sd(0,2) * Sd(1,1))*bd(2))*d;
1270
1271                     t[1] = ((Sd(1,2) * Sd(2,0) - Sd(1,0) * Sd(2,2))*bd(0) +
1272                             (Sd(0,0) * Sd(2,2) - Sd(0,2) * Sd(2,0))*bd(1) +
1273                             (Sd(0,2) * Sd(1,0) - Sd(0,0) * Sd(1,2))*bd(2))*d;
1274
1275                     t[2] = ((Sd(1,0) * Sd(2,1) - Sd(1,1) * Sd(2,0))*bd(0) +
1276                             (Sd(0,1) * Sd(2,0) - Sd(0,0) * Sd(2,1))*bd(1) +
1277                             (Sd(0,0) * Sd(1,1) - Sd(0,1) * Sd(1,0))*bd(2))*d;
1278
1279                     Dd(0,0) = t[0];
1280                     Dd(1,0) = t[1];
1281                     Dd(2,0) = t[2];
1282                 }
1283                 else
1284                     result = 0;
1285             }
1286         }
1287         else
1288         {
1289             assert( src->width == 1 );
1290
1291             if( type == CV_32FC1 )
1292             {
1293                 double d = Sf(0,0);
1294                 if( d != 0. )
1295                     Df(0,0) = (float)(bf(0)/d);
1296                 else
1297                     result = 0;
1298             }
1299             else
1300             {
1301                 double d = Sd(0,0);
1302                 if( d != 0. )
1303                     Dd(0,0) = (bd(0)/d);
1304                 else
1305                     result = 0;
1306             }
1307         }
1308     }
1309     else
1310     {
1311         CvLUDecompFunc decomp_func;
1312         CvLUBackFunc back_func;
1313         CvSize size = cvGetMatSize( src );
1314         CvSize dstsize = cvGetMatSize( dst );
1315         int worktype = CV_64FC1;
1316         int buf_size = size.width*size.height*CV_ELEM_SIZE(worktype);
1317         double d = 0;
1318         CvMat tmat;
1319
1320         if( !lu_inittab )
1321         {
1322             icvInitLUTable( &lu_decomp_tab, &lu_back_tab );
1323             lu_inittab = 1;
1324         }
1325
1326         if( size.width <= CV_MAX_LOCAL_MAT_SIZE )
1327         {
1328             buffer = (uchar*)cvStackAlloc( buf_size );
1329             local_alloc = 1;
1330         }
1331         else
1332         {
1333             CV_CALL( buffer = (uchar*)cvAlloc( buf_size ));
1334         }
1335
1336         CV_CALL( cvInitMatHeader( &tmat, size.height, size.width, worktype, buffer ));
1337         if( type == worktype )
1338         {
1339             CV_CALL( cvCopy( src, &tmat ));
1340         }
1341         else
1342             CV_CALL( cvConvert( src, &tmat ));
1343
1344         if( src2->data.ptr != dst->data.ptr )
1345         {
1346             CV_CALL( cvCopy( src2, dst ));
1347         }
1348
1349         decomp_func = (CvLUDecompFunc)(lu_decomp_tab.fn_2d[CV_MAT_DEPTH(type)-CV_32F]);
1350         back_func = (CvLUBackFunc)(lu_back_tab.fn_2d[CV_MAT_DEPTH(type)-CV_32F]);
1351         assert( decomp_func && back_func );
1352
1353         IPPI_CALL( decomp_func( tmat.data.db, tmat.step, size,
1354                                 dst->data.ptr, dst->step, dstsize, &d ));
1355
1356         if( d != 0 )
1357         {
1358             IPPI_CALL( back_func( tmat.data.db, tmat.step, size,
1359                                   dst->data.ptr, dst->step, dstsize ));
1360         }
1361         else
1362             result = 0;
1363     }
1364
1365     if( !result )
1366         CV_CALL( cvSetZero( dst ));
1367
1368     __END__;
1369
1370     if( buffer && !local_alloc )
1371         cvFree( &buffer );
1372
1373     if( u || v || w )
1374     {
1375         cvReleaseMat( &u );
1376         cvReleaseMat( &v );
1377         cvReleaseMat( &w );
1378     }
1379
1380     return result;
1381 }
1382
1383
1384
1385 /****************************************************************************************\
1386 *                               3D vector cross-product                                  *
1387 \****************************************************************************************/
1388
1389 CV_IMPL void
1390 cvCrossProduct( const CvArr* srcAarr, const CvArr* srcBarr, CvArr* dstarr )
1391 {
1392     CV_FUNCNAME( "cvCrossProduct" );
1393
1394     __BEGIN__;
1395
1396     CvMat stubA, *srcA = (CvMat*)srcAarr;
1397     CvMat stubB, *srcB = (CvMat*)srcBarr;
1398     CvMat dstub, *dst = (CvMat*)dstarr;
1399     int type;
1400
1401     if( !CV_IS_MAT(srcA))
1402         CV_CALL( srcA = cvGetMat( srcA, &stubA ));
1403
1404     type = CV_MAT_TYPE( srcA->type );
1405
1406     if( srcA->width*srcA->height*CV_MAT_CN(type) != 3 )
1407         CV_ERROR( CV_StsBadArg, "All the input arrays must be continuous 3-vectors" );
1408
1409     if( !srcB || !dst )
1410         CV_ERROR( CV_StsNullPtr, "" );
1411
1412     if( (srcA->type & ~CV_MAT_CONT_FLAG) == (srcB->type & ~CV_MAT_CONT_FLAG) &&
1413         (srcA->type & ~CV_MAT_CONT_FLAG) == (dst->type & ~CV_MAT_CONT_FLAG) )
1414     {
1415         if( !srcB->data.ptr || !dst->data.ptr )
1416             CV_ERROR( CV_StsNullPtr, "" );
1417     }
1418     else
1419     {
1420         if( !CV_IS_MAT(srcB))
1421             CV_CALL( srcB = cvGetMat( srcB, &stubB ));
1422
1423         if( !CV_IS_MAT(dst))
1424             CV_CALL( dst = cvGetMat( dst, &dstub ));
1425
1426         if( !CV_ARE_TYPES_EQ( srcA, srcB ) ||
1427             !CV_ARE_TYPES_EQ( srcB, dst ))
1428             CV_ERROR( CV_StsUnmatchedFormats, "" );
1429     }
1430
1431     if( !CV_ARE_SIZES_EQ( srcA, srcB ) || !CV_ARE_SIZES_EQ( srcB, dst ))
1432         CV_ERROR( CV_StsUnmatchedSizes, "" );
1433
1434     if( CV_MAT_DEPTH(type) == CV_32F )
1435     {
1436         float* dstdata = (float*)(dst->data.ptr);
1437         const float* src1data = (float*)(srcA->data.ptr);
1438         const float* src2data = (float*)(srcB->data.ptr);
1439
1440         if( CV_IS_MAT_CONT(srcA->type & srcB->type & dst->type) )
1441         {
1442             dstdata[2] = src1data[0] * src2data[1] - src1data[1] * src2data[0];
1443             dstdata[0] = src1data[1] * src2data[2] - src1data[2] * src2data[1];
1444             dstdata[1] = src1data[2] * src2data[0] - src1data[0] * src2data[2];
1445         }
1446         else
1447         {
1448             int step1 = srcA->step ? srcA->step/sizeof(src1data[0]) : 1;
1449             int step2 = srcB->step ? srcB->step/sizeof(src1data[0]) : 1;
1450             int step = dst->step ? dst->step/sizeof(src1data[0]) : 1;
1451
1452             dstdata[2*step] = src1data[0] * src2data[step2] - src1data[step1] * src2data[0];
1453             dstdata[0] = src1data[step1] * src2data[step2*2] - src1data[step1*2] * src2data[step2];
1454             dstdata[step] = src1data[step1*2] * src2data[0] - src1data[0] * src2data[step2*2];
1455         }
1456     }
1457     else if( CV_MAT_DEPTH(type) == CV_64F )
1458     {
1459         double* dstdata = (double*)(dst->data.ptr);
1460         const double* src1data = (double*)(srcA->data.ptr);
1461         const double* src2data = (double*)(srcB->data.ptr);
1462
1463         if( CV_IS_MAT_CONT(srcA->type & srcB->type & dst->type) )
1464         {
1465             dstdata[2] = src1data[0] * src2data[1] - src1data[1] * src2data[0];
1466             dstdata[0] = src1data[1] * src2data[2] - src1data[2] * src2data[1];
1467             dstdata[1] = src1data[2] * src2data[0] - src1data[0] * src2data[2];
1468         }
1469         else
1470         {
1471             int step1 = srcA->step ? srcA->step/sizeof(src1data[0]) : 1;
1472             int step2 = srcB->step ? srcB->step/sizeof(src1data[0]) : 1;
1473             int step = dst->step ? dst->step/sizeof(src1data[0]) : 1;
1474
1475             dstdata[2*step] = src1data[0] * src2data[step2] - src1data[step1] * src2data[0];
1476             dstdata[0] = src1data[step1] * src2data[step2*2] - src1data[step1*2] * src2data[step2];
1477             dstdata[step] = src1data[step1*2] * src2data[0] - src1data[0] * src2data[step2*2];
1478         }
1479     }
1480     else
1481     {
1482         CV_ERROR( CV_StsUnsupportedFormat, "" );
1483     }
1484
1485     __END__;
1486 }
1487
1488
1489 CV_IMPL void
1490 cvCalcPCA( const CvArr* data_arr, CvArr* avg_arr, CvArr* eigenvals, CvArr* eigenvects, int flags )
1491 {
1492     CvMat* tmp_avg = 0;
1493     CvMat* tmp_avg_r = 0;
1494     CvMat* tmp_cov = 0;
1495     CvMat* tmp_evals = 0;
1496     CvMat* tmp_evects = 0;
1497     CvMat* tmp_evects2 = 0;
1498     CvMat* tmp_data = 0;
1499
1500     CV_FUNCNAME( "cvCalcPCA" );
1501
1502     __BEGIN__;
1503
1504     CvMat stub, *data = (CvMat*)data_arr;
1505     CvMat astub, *avg = (CvMat*)avg_arr;
1506     CvMat evalstub, *evals = (CvMat*)eigenvals;
1507     CvMat evectstub, *evects = (CvMat*)eigenvects;
1508     int covar_flags = CV_COVAR_SCALE;
1509     int i, len, in_count, count, out_count;
1510
1511     if( !CV_IS_MAT(data) )
1512         CV_CALL( data = cvGetMat( data, &stub ));
1513
1514     if( !CV_IS_MAT(avg) )
1515         CV_CALL( avg = cvGetMat( avg, &astub ));
1516
1517     if( !CV_IS_MAT(evals) )
1518         CV_CALL( evals = cvGetMat( evals, &evalstub ));
1519
1520     if( !CV_IS_MAT(evects) )
1521         CV_CALL( evects = cvGetMat( evects, &evectstub ));
1522
1523     if( CV_MAT_CN(data->type) != 1 || CV_MAT_CN(avg->type) != 1 ||
1524         CV_MAT_CN(evals->type) != 1 || CV_MAT_CN(evects->type) != 1 )
1525         CV_ERROR( CV_StsUnsupportedFormat, "All the input and output arrays must be 1-channel" );
1526
1527     if( CV_MAT_DEPTH(avg->type) < CV_32F || !CV_ARE_DEPTHS_EQ(avg, evals) ||
1528         !CV_ARE_DEPTHS_EQ(avg, evects) )
1529         CV_ERROR( CV_StsUnsupportedFormat, "All the output arrays must have the same type, 32fC1 or 64fC1" );
1530
1531     if( flags & CV_PCA_DATA_AS_COL )
1532     {
1533         len = data->rows;
1534         in_count = data->cols;
1535         covar_flags |= CV_COVAR_COLS;
1536
1537         if( avg->cols != 1 || avg->rows != len )
1538             CV_ERROR( CV_StsBadSize,
1539             "The mean (average) vector should be data->rows x 1 when CV_PCA_DATA_AS_COL is used" );
1540
1541         CV_CALL( tmp_avg = cvCreateMat( len, 1, CV_64F ));
1542     }
1543     else
1544     {
1545         len = data->cols;
1546         in_count = data->rows;
1547         covar_flags |= CV_COVAR_ROWS;
1548
1549         if( avg->rows != 1 || avg->cols != len )
1550             CV_ERROR( CV_StsBadSize,
1551             "The mean (average) vector should be 1 x data->cols when CV_PCA_DATA_AS_ROW is used" );
1552
1553         CV_CALL( tmp_avg = cvCreateMat( 1, len, CV_64F ));
1554     }
1555
1556     count = MIN(len, in_count);
1557     out_count = evals->cols + evals->rows - 1;
1558
1559     if( (evals->cols != 1 && evals->rows != 1) || out_count > count )
1560         CV_ERROR( CV_StsBadSize,
1561         "The array of eigenvalues must be 1d vector containing "
1562         "no more than min(data->rows,data->cols) elements" );
1563
1564     if( evects->cols != len || evects->rows != out_count )
1565         CV_ERROR( CV_StsBadSize,
1566         "The matrix of eigenvalues must have the same number of columns as the input vector length "
1567         "and the same number of rows as the number of eigenvalues" );
1568
1569     // "scrambled" way to compute PCA (when cols(A)>rows(A)):
1570     // 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
1571     if( len <= in_count )
1572         covar_flags |= CV_COVAR_NORMAL;
1573
1574     if( flags & CV_PCA_USE_AVG ){
1575         covar_flags |= CV_COVAR_USE_AVG;
1576                 CV_CALL( cvConvert( avg, tmp_avg ) );
1577         }
1578
1579     CV_CALL( tmp_cov = cvCreateMat( count, count, CV_64F ));
1580     CV_CALL( tmp_evals = cvCreateMat( 1, count, CV_64F ));
1581     CV_CALL( tmp_evects = cvCreateMat( count, count, CV_64F ));
1582
1583     CV_CALL( cvCalcCovarMatrix( &data_arr, 0, tmp_cov, tmp_avg, covar_flags ));
1584     CV_CALL( cvSVD( tmp_cov, tmp_evals, tmp_evects, 0, CV_SVD_MODIFY_A + CV_SVD_U_T ));
1585     tmp_evects->rows = out_count;
1586     tmp_evals->cols = out_count;
1587     cvZero( evects );
1588     cvZero( evals );
1589
1590     if( covar_flags & CV_COVAR_NORMAL )
1591     {
1592         CV_CALL( cvConvert( tmp_evects, evects ));
1593     }
1594     else
1595     {
1596         // CV_PCA_DATA_AS_ROW: cols(A)>rows(A). x=A'*y -> x'=y'*A
1597         // CV_PCA_DATA_AS_COL: rows(A)>cols(A). x=A''*y -> x'=y'*A'
1598         int block_count = 0;
1599
1600         CV_CALL( tmp_data = cvCreateMat( count, count, CV_64F ));
1601         CV_CALL( tmp_avg_r = cvCreateMat( count, count, CV_64F ));
1602         CV_CALL( tmp_evects2 = cvCreateMat( out_count, count, CV_64F ));
1603
1604         for( i = 0; i < len; i += block_count )
1605         {
1606             CvMat data_part, tdata_part, part, dst_part, avg_part, tmp_avg_part;
1607             int gemm_flags;
1608
1609             block_count = MIN( count, len - i );
1610
1611             if( flags & CV_PCA_DATA_AS_COL )
1612             {
1613                 cvGetRows( data, &data_part, i, i + block_count );
1614                 cvGetRows( tmp_data, &tdata_part, 0, block_count );
1615                 cvGetRows( tmp_avg, &avg_part, i, i + block_count );
1616                 cvGetRows( tmp_avg_r, &tmp_avg_part, 0, block_count );
1617                 gemm_flags = CV_GEMM_B_T;
1618             }
1619             else
1620             {
1621                 cvGetCols( data, &data_part, i, i + block_count );
1622                 cvGetCols( tmp_data, &tdata_part, 0, block_count );
1623                 cvGetCols( tmp_avg, &avg_part, i, i + block_count );
1624                 cvGetCols( tmp_avg_r, &tmp_avg_part, 0, block_count );
1625                 gemm_flags = 0;
1626             }
1627
1628             cvGetCols( tmp_evects2, &part, 0, block_count );
1629             cvGetCols( evects, &dst_part, i, i + block_count );
1630
1631             cvConvert( &data_part, &tdata_part );
1632             cvRepeat( &avg_part, &tmp_avg_part );
1633             cvSub( &tdata_part, &tmp_avg_part, &tdata_part );
1634             cvGEMM( tmp_evects, &tdata_part, 1, 0, 0, &part, gemm_flags );
1635             cvConvert( &part, &dst_part );
1636         }
1637
1638         // normalize eigenvectors
1639         for( i = 0; i < out_count; i++ )
1640         {
1641             CvMat ei;
1642             cvGetRow( evects, &ei, i );
1643                         cvNormalize( &ei, &ei );
1644         }
1645     }
1646
1647     if( tmp_evals->rows != evals->rows )
1648         cvReshape( tmp_evals, tmp_evals, 1, evals->rows );
1649     cvConvert( tmp_evals, evals );
1650     cvConvert( tmp_avg, avg );
1651
1652     __END__;
1653
1654     cvReleaseMat( &tmp_avg );
1655     cvReleaseMat( &tmp_avg_r );
1656     cvReleaseMat( &tmp_cov );
1657     cvReleaseMat( &tmp_evals );
1658     cvReleaseMat( &tmp_evects );
1659     cvReleaseMat( &tmp_evects2 );
1660     cvReleaseMat( &tmp_data );
1661 }
1662
1663
1664 CV_IMPL void
1665 cvProjectPCA( const CvArr* data_arr, const CvArr* avg_arr,
1666               const CvArr* eigenvects, CvArr* result_arr )
1667 {
1668     uchar* buffer = 0;
1669     int local_alloc = 0;
1670
1671     CV_FUNCNAME( "cvProjectPCA" );
1672
1673     __BEGIN__;
1674
1675     CvMat stub, *data = (CvMat*)data_arr;
1676     CvMat astub, *avg = (CvMat*)avg_arr;
1677     CvMat evectstub, *evects = (CvMat*)eigenvects;
1678     CvMat rstub, *result = (CvMat*)result_arr;
1679     CvMat avg_repeated;
1680     int i, len, in_count;
1681     int gemm_flags, as_cols, convert_data;
1682     int block_count0, block_count, buf_size, elem_size;
1683     uchar* tmp_data_ptr;
1684
1685     if( !CV_IS_MAT(data) )
1686         CV_CALL( data = cvGetMat( data, &stub ));
1687
1688     if( !CV_IS_MAT(avg) )
1689         CV_CALL( avg = cvGetMat( avg, &astub ));
1690
1691     if( !CV_IS_MAT(evects) )
1692         CV_CALL( evects = cvGetMat( evects, &evectstub ));
1693
1694     if( !CV_IS_MAT(result) )
1695         CV_CALL( result = cvGetMat( result, &rstub ));
1696
1697     if( CV_MAT_CN(data->type) != 1 || CV_MAT_CN(avg->type) != 1 )
1698         CV_ERROR( CV_StsUnsupportedFormat, "All the input and output arrays must be 1-channel" );
1699
1700     if( CV_MAT_TYPE(avg->type) != CV_32FC1 && CV_MAT_TYPE(avg->type) != CV_64FC1 ||
1701         !CV_ARE_TYPES_EQ(avg, evects) || !CV_ARE_TYPES_EQ(avg, result) )
1702         CV_ERROR( CV_StsUnsupportedFormat,
1703         "All the input and output arrays (except for data) must have the same type, 32fC1 or 64fC1" );
1704
1705     if( (avg->cols != 1 || avg->rows != data->rows) &&
1706         (avg->rows != 1 || avg->cols != data->cols) )
1707         CV_ERROR( CV_StsBadSize,
1708         "The mean (average) vector should be either 1 x data->cols or data->rows x 1" );
1709
1710     if( avg->cols == 1 )
1711     {
1712         len = data->rows;
1713         in_count = data->cols;
1714
1715         gemm_flags = CV_GEMM_A_T + CV_GEMM_B_T;
1716         as_cols = 1;
1717     }
1718     else
1719     {
1720         len = data->cols;
1721         in_count = data->rows;
1722
1723         gemm_flags = CV_GEMM_B_T;
1724         as_cols = 0;
1725     }
1726
1727     if( evects->cols != len )
1728         CV_ERROR( CV_StsUnmatchedSizes,
1729         "Eigenvectors must be stored as rows and be of the same size as input vectors" );
1730
1731     if( result->cols > evects->rows )
1732         CV_ERROR( CV_StsOutOfRange,
1733         "The output matrix of coefficients must have the number of columns "
1734         "less than or equal to the number of eigenvectors (number of rows in eigenvectors matrix)" );
1735
1736     evects = cvGetRows( evects, &evectstub, 0, result->cols );
1737
1738     block_count0 = (1 << 16)/len;
1739     block_count0 = MAX( block_count0, 4 );
1740     block_count0 = MIN( block_count0, in_count );
1741     elem_size = CV_ELEM_SIZE(avg->type);
1742     convert_data = CV_MAT_DEPTH(data->type) < CV_MAT_DEPTH(avg->type);
1743
1744     buf_size = block_count0*len*((block_count0 > 1) + 1)*elem_size;
1745
1746     if( buf_size < CV_MAX_LOCAL_SIZE )
1747     {
1748         buffer = (uchar*)cvStackAlloc( buf_size );
1749         local_alloc = 1;
1750     }
1751     else
1752         CV_CALL( buffer = (uchar*)cvAlloc( buf_size ));
1753
1754     tmp_data_ptr = buffer;
1755     if( block_count0 > 1 )
1756     {
1757         avg_repeated = cvMat( as_cols ? len : block_count0,
1758                               as_cols ? block_count0 : len, avg->type, buffer );
1759         cvRepeat( avg, &avg_repeated );
1760         tmp_data_ptr += block_count0*len*elem_size;
1761     }
1762     else
1763         avg_repeated = *avg;
1764
1765     for( i = 0; i < in_count; i += block_count )
1766     {
1767         CvMat data_part, norm_data, avg_part, *src = &data_part, out_part;
1768
1769         block_count = MIN( block_count0, in_count - i );
1770         if( as_cols )
1771         {
1772             cvGetCols( data, &data_part, i, i + block_count );
1773             cvGetCols( &avg_repeated, &avg_part, 0, block_count );
1774             norm_data = cvMat( len, block_count, avg->type, tmp_data_ptr );
1775         }
1776         else
1777         {
1778             cvGetRows( data, &data_part, i, i + block_count );
1779             cvGetRows( &avg_repeated, &avg_part, 0, block_count );
1780             norm_data = cvMat( block_count, len, avg->type, tmp_data_ptr );
1781         }
1782
1783         if( convert_data )
1784         {
1785             cvConvert( src, &norm_data );
1786             src = &norm_data;
1787         }
1788
1789         cvSub( src, &avg_part, &norm_data );
1790
1791         cvGetRows( result, &out_part, i, i + block_count );
1792         cvGEMM( &norm_data, evects, 1, 0, 0, &out_part, gemm_flags );
1793     }
1794
1795     __END__;
1796
1797     if( !local_alloc )
1798         cvFree( &buffer );
1799 }
1800
1801
1802 CV_IMPL void
1803 cvBackProjectPCA( const CvArr* proj_arr, const CvArr* avg_arr,
1804                   const CvArr* eigenvects, CvArr* result_arr )
1805 {
1806     uchar* buffer = 0;
1807     int local_alloc = 0;
1808
1809     CV_FUNCNAME( "cvProjectPCA" );
1810
1811     __BEGIN__;
1812
1813     CvMat pstub, *data = (CvMat*)proj_arr;
1814     CvMat astub, *avg = (CvMat*)avg_arr;
1815     CvMat evectstub, *evects = (CvMat*)eigenvects;
1816     CvMat rstub, *result = (CvMat*)result_arr;
1817     CvMat avg_repeated;
1818     int i, len, in_count, as_cols;
1819     int block_count0, block_count, buf_size, elem_size;
1820
1821     if( !CV_IS_MAT(data) )
1822         CV_CALL( data = cvGetMat( data, &pstub ));
1823
1824     if( !CV_IS_MAT(avg) )
1825         CV_CALL( avg = cvGetMat( avg, &astub ));
1826
1827     if( !CV_IS_MAT(evects) )
1828         CV_CALL( evects = cvGetMat( evects, &evectstub ));
1829
1830     if( !CV_IS_MAT(result) )
1831         CV_CALL( result = cvGetMat( result, &rstub ));
1832
1833     if( CV_MAT_TYPE(avg->type) != CV_32FC1 && CV_MAT_TYPE(avg->type) != CV_64FC1 ||
1834         !CV_ARE_TYPES_EQ(avg, data) || !CV_ARE_TYPES_EQ(avg, evects) || !CV_ARE_TYPES_EQ(avg, result) )
1835         CV_ERROR( CV_StsUnsupportedFormat,
1836         "All the input and output arrays must have the same type, 32fC1 or 64fC1" );
1837
1838     if( (avg->cols != 1 || avg->rows != result->rows) &&
1839         (avg->rows != 1 || avg->cols != result->cols) )
1840         CV_ERROR( CV_StsBadSize,
1841         "The mean (average) vector should be either 1 x result->cols or result->rows x 1" );
1842
1843     if( avg->cols == 1 )
1844     {
1845         len = result->rows;
1846         in_count = result->cols;
1847         as_cols = 1;
1848     }
1849     else
1850     {
1851         len = result->cols;
1852         in_count = result->rows;
1853         as_cols = 0;
1854     }
1855
1856     if( evects->cols != len )
1857         CV_ERROR( CV_StsUnmatchedSizes,
1858         "Eigenvectors must be stored as rows and be of the same size as the output vectors" );
1859
1860     if( data->cols > evects->rows )
1861         CV_ERROR( CV_StsOutOfRange,
1862         "The input matrix of coefficients must have the number of columns "
1863         "less than or equal to the number of eigenvectors (number of rows in eigenvectors matrix)" );
1864
1865     evects = cvGetRows( evects, &evectstub, 0, data->cols );
1866
1867     block_count0 = (1 << 16)/len;
1868     block_count0 = MAX( block_count0, 4 );
1869     block_count0 = MIN( block_count0, in_count );
1870     elem_size = CV_ELEM_SIZE(avg->type);
1871
1872     buf_size = block_count0*len*(block_count0 > 1)*elem_size;
1873
1874     if( buf_size < CV_MAX_LOCAL_SIZE )
1875     {
1876         buffer = (uchar*)cvStackAlloc( MAX(buf_size,16) );
1877         local_alloc = 1;
1878     }
1879     else
1880         CV_CALL( buffer = (uchar*)cvAlloc( buf_size ));
1881
1882     if( block_count0 > 1 )
1883     {
1884         avg_repeated = cvMat( as_cols ? len : block_count0,
1885                               as_cols ? block_count0 : len, avg->type, buffer );
1886         cvRepeat( avg, &avg_repeated );
1887     }
1888     else
1889         avg_repeated = *avg;
1890
1891     for( i = 0; i < in_count; i += block_count )
1892     {
1893         CvMat data_part, avg_part, out_part;
1894
1895         block_count = MIN( block_count0, in_count - i );
1896         cvGetRows( data, &data_part, i, i + block_count );
1897
1898         if( as_cols )
1899         {
1900             cvGetCols( result, &out_part, i, i + block_count );
1901             cvGetCols( &avg_repeated, &avg_part, 0, block_count );
1902             cvGEMM( evects, &data_part, 1, &avg_part, 1, &out_part, CV_GEMM_A_T + CV_GEMM_B_T );
1903         }
1904         else
1905         {
1906             cvGetRows( result, &out_part, i, i + block_count );
1907             cvGetRows( &avg_repeated, &avg_part, 0, block_count );
1908             cvGEMM( &data_part, evects, 1, &avg_part, 1, &out_part, 0 );
1909         }
1910     }
1911
1912     __END__;
1913
1914     if( !local_alloc )
1915         cvFree( &buffer );
1916 }
1917
1918
1919 /* End of file. */