modules/core/src/lapack.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 //                           License Agreement
  11 //                For Open Source Computer Vision Library
  12 //
  13 // Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
  14 // Copyright (C) 2009, Willow Garage Inc., all rights reserved.
  15 // Third party copyrights are property of their respective owners.
  16 //
  17 // Redistribution and use in source and binary forms, with or without modification,
  18 // are permitted provided that the following conditions are met:
  19 //
  20 //   * Redistribution's of source code must retain the above copyright notice,
  21 //     this list of conditions and the following disclaimer.
  22 //
  23 //   * Redistribution's in binary form must reproduce the above copyright notice,
  24 //     this list of conditions and the following disclaimer in the documentation
  25 //     and/or other materials provided with the distribution.
  26 //
  27 //   * The name of the copyright holders may not be used to endorse or promote products
  28 //     derived from this software without specific prior written permission.
  29 //
  30 // This software is provided by the copyright holders and contributors "as is" and
  31 // any express or implied warranties, including, but not limited to, the implied
  32 // warranties of merchantability and fitness for a particular purpose are disclaimed.
  33 // In no event shall the Intel Corporation or contributors be liable for any direct,
  34 // indirect, incidental, special, exemplary, or consequential damages
  35 // (including, but not limited to, procurement of substitute goods or services;
  36 // loss of use, data, or profits; or business interruption) however caused
  37 // and on any theory of liability, whether in contract, strict liability,
  38 // or tort (including negligence or otherwise) arising in any way out of
  39 // the use of this software, even if advised of the possibility of such damage.
  40 //
  41 //M*/
  42
  43 #include "precomp.hpp"
  44
  45 #if defined _M_IX86 && defined _MSC_VER && _MSC_VER < 1700
  46 #pragma float_control(precise, on)
  47 #endif
  48
  49 namespace cv
  50 {
  51
  52 /****************************************************************************************\
  53 *                     LU & Cholesky implementation for small matrices                    *
  54 \****************************************************************************************/
  55
  56 template<typename _Tp> static inline int
  57 LUImpl(_Tp* A, size_t astep, int m, _Tp* b, size_t bstep, int n)
  58 {
  59     int i, j, k, p = 1;
  60     astep /= sizeof(A[0]);
  61     bstep /= sizeof(b[0]);
  62
  63     for( i = 0; i < m; i++ )
  64     {
  65         k = i;
  66
  67         for( j = i+1; j < m; j++ )
  68             if( std::abs(A[j*astep + i]) > std::abs(A[k*astep + i]) )
  69                 k = j;
  70
  71         if( std::abs(A[k*astep + i]) < std::numeric_limits<_Tp>::epsilon() )
  72             return 0;
  73
  74         if( k != i )
  75         {
  76             for( j = i; j < m; j++ )
  77                 std::swap(A[i*astep + j], A[k*astep + j]);
  78             if( b )
  79                 for( j = 0; j < n; j++ )
  80                     std::swap(b[i*bstep + j], b[k*bstep + j]);
  81             p = -p;
  82         }
  83
  84         _Tp d = -1/A[i*astep + i];
  85
  86         for( j = i+1; j < m; j++ )
  87         {
  88             _Tp alpha = A[j*astep + i]*d;
  89
  90             for( k = i+1; k < m; k++ )
  91                 A[j*astep + k] += alpha*A[i*astep + k];
  92
  93             if( b )
  94                 for( k = 0; k < n; k++ )
  95                     b[j*bstep + k] += alpha*b[i*bstep + k];
  96         }
  97
  98         A[i*astep + i] = -d;
  99     }
 100
 101     if( b )
 102     {
 103         for( i = m-1; i >= 0; i-- )
 104             for( j = 0; j < n; j++ )
 105             {
 106                 _Tp s = b[i*bstep + j];
 107                 for( k = i+1; k < m; k++ )
 108                     s -= A[i*astep + k]*b[k*bstep + j];
 109                 b[i*bstep + j] = s*A[i*astep + i];
 110             }
 111     }
 112
 113     return p;
 114 }
 115
 116
 117 int LU(float* A, size_t astep, int m, float* b, size_t bstep, int n)
 118 {
 119     return LUImpl(A, astep, m, b, bstep, n);
 120 }
 121
 122
 123 int LU(double* A, size_t astep, int m, double* b, size_t bstep, int n)
 124 {
 125     return LUImpl(A, astep, m, b, bstep, n);
 126 }
 127
 128
 129 template<typename _Tp> static inline bool
 130 CholImpl(_Tp* A, size_t astep, int m, _Tp* b, size_t bstep, int n)
 131 {
 132     _Tp* L = A;
 133     int i, j, k;
 134     double s;
 135     astep /= sizeof(A[0]);
 136     bstep /= sizeof(b[0]);
 137
 138     for( i = 0; i < m; i++ )
 139     {
 140         for( j = 0; j < i; j++ )
 141         {
 142             s = A[i*astep + j];
 143             for( k = 0; k < j; k++ )
 144                 s -= L[i*astep + k]*L[j*astep + k];
 145             L[i*astep + j] = (_Tp)(s*L[j*astep + j]);
 146         }
 147         s = A[i*astep + i];
 148         for( k = 0; k < j; k++ )
 149         {
 150             double t = L[i*astep + k];
 151             s -= t*t;
 152         }
 153         if( s < std::numeric_limits<_Tp>::epsilon() )
 154             return false;
 155         L[i*astep + i] = (_Tp)(1./std::sqrt(s));
 156     }
 157
 158     if( !b )
 159         return true;
 160
 161     // LLt x = b
 162     // 1: L y = b
 163     // 2. Lt x = y
 164
 165     /*
 166      [ L00             ]  y0   b0
 167      [ L10 L11         ]  y1 = b1
 168      [ L20 L21 L22     ]  y2   b2
 169      [ L30 L31 L32 L33 ]  y3   b3
 170
 171      [ L00 L10 L20 L30 ]  x0   y0
 172      [     L11 L21 L31 ]  x1 = y1
 173      [         L22 L32 ]  x2   y2
 174      [             L33 ]  x3   y3
 175     */
 176
 177     for( i = 0; i < m; i++ )
 178     {
 179         for( j = 0; j < n; j++ )
 180         {
 181             s = b[i*bstep + j];
 182             for( k = 0; k < i; k++ )
 183                 s -= L[i*astep + k]*b[k*bstep + j];
 184             b[i*bstep + j] = (_Tp)(s*L[i*astep + i]);
 185         }
 186     }
 187
 188     for( i = m-1; i >= 0; i-- )
 189     {
 190         for( j = 0; j < n; j++ )
 191         {
 192             s = b[i*bstep + j];
 193             for( k = m-1; k > i; k-- )
 194                 s -= L[k*astep + i]*b[k*bstep + j];
 195             b[i*bstep + j] = (_Tp)(s*L[i*astep + i]);
 196         }
 197     }
 198
 199     return true;
 200 }
 201
 202
 203 bool Cholesky(float* A, size_t astep, int m, float* b, size_t bstep, int n)
 204 {
 205     return CholImpl(A, astep, m, b, bstep, n);
 206 }
 207
 208 bool Cholesky(double* A, size_t astep, int m, double* b, size_t bstep, int n)
 209 {
 210     return CholImpl(A, astep, m, b, bstep, n);
 211 }
 212
 213
 214 template<typename _Tp> static inline _Tp hypot(_Tp a, _Tp b)
 215 {
 216     a = std::abs(a);
 217     b = std::abs(b);
 218     if( a > b )
 219     {
 220         b /= a;
 221         return a*std::sqrt(1 + b*b);
 222     }
 223     if( b > 0 )
 224     {
 225         a /= b;
 226         return b*std::sqrt(1 + a*a);
 227     }
 228     return 0;
 229 }
 230
 231
 232 template<typename _Tp> bool
 233 JacobiImpl_( _Tp* A, size_t astep, _Tp* W, _Tp* V, size_t vstep, int n, uchar* buf )
 234 {
 235     const _Tp eps = std::numeric_limits<_Tp>::epsilon();
 236     int i, j, k, m;
 237
 238     astep /= sizeof(A[0]);
 239     if( V )
 240     {
 241         vstep /= sizeof(V[0]);
 242         for( i = 0; i < n; i++ )
 243         {
 244             for( j = 0; j < n; j++ )
 245                 V[i*vstep + j] = (_Tp)0;
 246             V[i*vstep + i] = (_Tp)1;
 247         }
 248     }
 249
 250     int iters, maxIters = n*n*30;
 251
 252     int* indR = (int*)alignPtr(buf, sizeof(int));
 253     int* indC = indR + n;
 254     _Tp mv = (_Tp)0;
 255
 256     for( k = 0; k < n; k++ )
 257     {
 258         W[k] = A[(astep + 1)*k];
 259         if( k < n - 1 )
 260         {
 261             for( m = k+1, mv = std::abs(A[astep*k + m]), i = k+2; i < n; i++ )
 262             {
 263                 _Tp val = std::abs(A[astep*k+i]);
 264                 if( mv < val )
 265                     mv = val, m = i;
 266             }
 267             indR[k] = m;
 268         }
 269         if( k > 0 )
 270         {
 271             for( m = 0, mv = std::abs(A[k]), i = 1; i < k; i++ )
 272             {
 273                 _Tp val = std::abs(A[astep*i+k]);
 274                 if( mv < val )
 275                     mv = val, m = i;
 276             }
 277             indC[k] = m;
 278         }
 279     }
 280
 281     if( n > 1 ) for( iters = 0; iters < maxIters; iters++ )
 282     {
 283         // find index (k,l) of pivot p
 284         for( k = 0, mv = std::abs(A[indR[0]]), i = 1; i < n-1; i++ )
 285         {
 286             _Tp val = std::abs(A[astep*i + indR[i]]);
 287             if( mv < val )
 288                 mv = val, k = i;
 289         }
 290         int l = indR[k];
 291         for( i = 1; i < n; i++ )
 292         {
 293             _Tp val = std::abs(A[astep*indC[i] + i]);
 294             if( mv < val )
 295                 mv = val, k = indC[i], l = i;
 296         }
 297
 298         _Tp p = A[astep*k + l];
 299         if( std::abs(p) <= eps )
 300             break;
 301         _Tp y = (_Tp)((W[l] - W[k])*0.5);
 302         _Tp t = std::abs(y) + hypot(p, y);
 303         _Tp s = hypot(p, t);
 304         _Tp c = t/s;
 305         s = p/s; t = (p/t)*p;
 306         if( y < 0 )
 307             s = -s, t = -t;
 308         A[astep*k + l] = 0;
 309
 310         W[k] -= t;
 311         W[l] += t;
 312
 313         _Tp a0, b0;
 314
 315 #undef rotate
 316 #define rotate(v0, v1) a0 = v0, b0 = v1, v0 = a0*c - b0*s, v1 = a0*s + b0*c
 317
 318         // rotate rows and columns k and l
 319         for( i = 0; i < k; i++ )
 320             rotate(A[astep*i+k], A[astep*i+l]);
 321         for( i = k+1; i < l; i++ )
 322             rotate(A[astep*k+i], A[astep*i+l]);
 323         for( i = l+1; i < n; i++ )
 324             rotate(A[astep*k+i], A[astep*l+i]);
 325
 326         // rotate eigenvectors
 327         if( V )
 328             for( i = 0; i < n; i++ )
 329                 rotate(V[vstep*k+i], V[vstep*l+i]);
 330
 331 #undef rotate
 332
 333         for( j = 0; j < 2; j++ )
 334         {
 335             int idx = j == 0 ? k : l;
 336             if( idx < n - 1 )
 337             {
 338                 for( m = idx+1, mv = std::abs(A[astep*idx + m]), i = idx+2; i < n; i++ )
 339                 {
 340                     _Tp val = std::abs(A[astep*idx+i]);
 341                     if( mv < val )
 342                         mv = val, m = i;
 343                 }
 344                 indR[idx] = m;
 345             }
 346             if( idx > 0 )
 347             {
 348                 for( m = 0, mv = std::abs(A[idx]), i = 1; i < idx; i++ )
 349                 {
 350                     _Tp val = std::abs(A[astep*i+idx]);
 351                     if( mv < val )
 352                         mv = val, m = i;
 353                 }
 354                 indC[idx] = m;
 355             }
 356         }
 357     }
 358
 359     // sort eigenvalues & eigenvectors
 360     for( k = 0; k < n-1; k++ )
 361     {
 362         m = k;
 363         for( i = k+1; i < n; i++ )
 364         {
 365             if( W[m] < W[i] )
 366                 m = i;
 367         }
 368         if( k != m )
 369         {
 370             std::swap(W[m], W[k]);
 371             if( V )
 372                 for( i = 0; i < n; i++ )
 373                     std::swap(V[vstep*m + i], V[vstep*k + i]);
 374         }
 375     }
 376
 377     return true;
 378 }
 379
 380 static bool Jacobi( float* S, size_t sstep, float* e, float* E, size_t estep, int n, uchar* buf )
 381 {
 382     return JacobiImpl_(S, sstep, e, E, estep, n, buf);
 383 }
 384
 385 static bool Jacobi( double* S, size_t sstep, double* e, double* E, size_t estep, int n, uchar* buf )
 386 {
 387     return JacobiImpl_(S, sstep, e, E, estep, n, buf);
 388 }
 389
 390
 391 template<typename T> struct VBLAS
 392 {
 393     int dot(const T*, const T*, int, T*) const { return 0; }
 394     int givens(T*, T*, int, T, T) const { return 0; }
 395     int givensx(T*, T*, int, T, T, T*, T*) const { return 0; }
 396 };
 397
 398 #if CV_SSE2
 399 template<> inline int VBLAS<float>::dot(const float* a, const float* b, int n, float* result) const
 400 {
 401     if( n < 8 )
 402         return 0;
 403     int k = 0;
 404     __m128 s0 = _mm_setzero_ps(), s1 = _mm_setzero_ps();
 405     for( ; k <= n - 8; k += 8 )
 406     {
 407         __m128 a0 = _mm_load_ps(a + k), a1 = _mm_load_ps(a + k + 4);
 408         __m128 b0 = _mm_load_ps(b + k), b1 = _mm_load_ps(b + k + 4);
 409
 410         s0 = _mm_add_ps(s0, _mm_mul_ps(a0, b0));
 411         s1 = _mm_add_ps(s1, _mm_mul_ps(a1, b1));
 412     }
 413     s0 = _mm_add_ps(s0, s1);
 414     float sbuf[4];
 415     _mm_storeu_ps(sbuf, s0);
 416     *result = sbuf[0] + sbuf[1] + sbuf[2] + sbuf[3];
 417     return k;
 418 }
 419
 420
 421 template<> inline int VBLAS<float>::givens(float* a, float* b, int n, float c, float s) const
 422 {
 423     if( n < 4 )
 424         return 0;
 425     int k = 0;
 426     __m128 c4 = _mm_set1_ps(c), s4 = _mm_set1_ps(s);
 427     for( ; k <= n - 4; k += 4 )
 428     {
 429         __m128 a0 = _mm_load_ps(a + k);
 430         __m128 b0 = _mm_load_ps(b + k);
 431         __m128 t0 = _mm_add_ps(_mm_mul_ps(a0, c4), _mm_mul_ps(b0, s4));
 432         __m128 t1 = _mm_sub_ps(_mm_mul_ps(b0, c4), _mm_mul_ps(a0, s4));
 433         _mm_store_ps(a + k, t0);
 434         _mm_store_ps(b + k, t1);
 435     }
 436     return k;
 437 }
 438
 439
 440 template<> inline int VBLAS<float>::givensx(float* a, float* b, int n, float c, float s,
 441                                              float* anorm, float* bnorm) const
 442 {
 443     if( n < 4 )
 444         return 0;
 445     int k = 0;
 446     __m128 c4 = _mm_set1_ps(c), s4 = _mm_set1_ps(s);
 447     __m128 sa = _mm_setzero_ps(), sb = _mm_setzero_ps();
 448     for( ; k <= n - 4; k += 4 )
 449     {
 450         __m128 a0 = _mm_load_ps(a + k);
 451         __m128 b0 = _mm_load_ps(b + k);
 452         __m128 t0 = _mm_add_ps(_mm_mul_ps(a0, c4), _mm_mul_ps(b0, s4));
 453         __m128 t1 = _mm_sub_ps(_mm_mul_ps(b0, c4), _mm_mul_ps(a0, s4));
 454         _mm_store_ps(a + k, t0);
 455         _mm_store_ps(b + k, t1);
 456         sa = _mm_add_ps(sa, _mm_mul_ps(t0, t0));
 457         sb = _mm_add_ps(sb, _mm_mul_ps(t1, t1));
 458     }
 459     float abuf[4], bbuf[4];
 460     _mm_storeu_ps(abuf, sa);
 461     _mm_storeu_ps(bbuf, sb);
 462     *anorm = abuf[0] + abuf[1] + abuf[2] + abuf[3];
 463     *bnorm = bbuf[0] + bbuf[1] + bbuf[2] + bbuf[3];
 464     return k;
 465 }
 466
 467
 468 template<> inline int VBLAS<double>::dot(const double* a, const double* b, int n, double* result) const
 469 {
 470     if( n < 4 )
 471         return 0;
 472     int k = 0;
 473     __m128d s0 = _mm_setzero_pd(), s1 = _mm_setzero_pd();
 474     for( ; k <= n - 4; k += 4 )
 475     {
 476         __m128d a0 = _mm_load_pd(a + k), a1 = _mm_load_pd(a + k + 2);
 477         __m128d b0 = _mm_load_pd(b + k), b1 = _mm_load_pd(b + k + 2);
 478
 479         s0 = _mm_add_pd(s0, _mm_mul_pd(a0, b0));
 480         s1 = _mm_add_pd(s1, _mm_mul_pd(a1, b1));
 481     }
 482     s0 = _mm_add_pd(s0, s1);
 483     double sbuf[2];
 484     _mm_storeu_pd(sbuf, s0);
 485     *result = sbuf[0] + sbuf[1];
 486     return k;
 487 }
 488
 489
 490 template<> inline int VBLAS<double>::givens(double* a, double* b, int n, double c, double s) const
 491 {
 492     int k = 0;
 493     __m128d c2 = _mm_set1_pd(c), s2 = _mm_set1_pd(s);
 494     for( ; k <= n - 2; k += 2 )
 495     {
 496         __m128d a0 = _mm_load_pd(a + k);
 497         __m128d b0 = _mm_load_pd(b + k);
 498         __m128d t0 = _mm_add_pd(_mm_mul_pd(a0, c2), _mm_mul_pd(b0, s2));
 499         __m128d t1 = _mm_sub_pd(_mm_mul_pd(b0, c2), _mm_mul_pd(a0, s2));
 500         _mm_store_pd(a + k, t0);
 501         _mm_store_pd(b + k, t1);
 502     }
 503     return k;
 504 }
 505
 506
 507 template<> inline int VBLAS<double>::givensx(double* a, double* b, int n, double c, double s,
 508                                               double* anorm, double* bnorm) const
 509 {
 510     int k = 0;
 511     __m128d c2 = _mm_set1_pd(c), s2 = _mm_set1_pd(s);
 512     __m128d sa = _mm_setzero_pd(), sb = _mm_setzero_pd();
 513     for( ; k <= n - 2; k += 2 )
 514     {
 515         __m128d a0 = _mm_load_pd(a + k);
 516         __m128d b0 = _mm_load_pd(b + k);
 517         __m128d t0 = _mm_add_pd(_mm_mul_pd(a0, c2), _mm_mul_pd(b0, s2));
 518         __m128d t1 = _mm_sub_pd(_mm_mul_pd(b0, c2), _mm_mul_pd(a0, s2));
 519         _mm_store_pd(a + k, t0);
 520         _mm_store_pd(b + k, t1);
 521         sa = _mm_add_pd(sa, _mm_mul_pd(t0, t0));
 522         sb = _mm_add_pd(sb, _mm_mul_pd(t1, t1));
 523     }
 524     double abuf[2], bbuf[2];
 525     _mm_storeu_pd(abuf, sa);
 526     _mm_storeu_pd(bbuf, sb);
 527     *anorm = abuf[0] + abuf[1];
 528     *bnorm = bbuf[0] + bbuf[1];
 529     return k;
 530 }
 531 #endif
 532
 533 template<typename _Tp> void
 534 JacobiSVDImpl_(_Tp* At, size_t astep, _Tp* _W, _Tp* Vt, size_t vstep,
 535                int m, int n, int n1, double minval, _Tp eps)
 536 {
 537     VBLAS<_Tp> vblas;
 538     AutoBuffer<double> Wbuf(n);
 539     double* W = Wbuf;
 540     int i, j, k, iter, max_iter = std::max(m, 30);
 541     _Tp c, s;
 542     double sd;
 543     astep /= sizeof(At[0]);
 544     vstep /= sizeof(Vt[0]);
 545
 546     for( i = 0; i < n; i++ )
 547     {
 548         for( k = 0, sd = 0; k < m; k++ )
 549         {
 550             _Tp t = At[i*astep + k];
 551             sd += (double)t*t;
 552         }
 553         W[i] = sd;
 554
 555         if( Vt )
 556         {
 557             for( k = 0; k < n; k++ )
 558                 Vt[i*vstep + k] = 0;
 559             Vt[i*vstep + i] = 1;
 560         }
 561     }
 562
 563     for( iter = 0; iter < max_iter; iter++ )
 564     {
 565         bool changed = false;
 566
 567         for( i = 0; i < n-1; i++ )
 568             for( j = i+1; j < n; j++ )
 569             {
 570                 _Tp *Ai = At + i*astep, *Aj = At + j*astep;
 571                 double a = W[i], p = 0, b = W[j];
 572
 573                 for( k = 0; k < m; k++ )
 574                     p += (double)Ai[k]*Aj[k];
 575
 576                 if( std::abs(p) <= eps*std::sqrt((double)a*b) )
 577                     continue;
 578
 579                 p *= 2;
 580                 double beta = a - b, gamma = hypot((double)p, beta);
 581                 if( beta < 0 )
 582                 {
 583                     double delta = (gamma - beta)*0.5;
 584                     s = (_Tp)std::sqrt(delta/gamma);
 585                     c = (_Tp)(p/(gamma*s*2));
 586                 }
 587                 else
 588                 {
 589                     c = (_Tp)std::sqrt((gamma + beta)/(gamma*2));
 590                     s = (_Tp)(p/(gamma*c*2));
 591                 }
 592
 593                 a = b = 0;
 594                 for( k = 0; k < m; k++ )
 595                 {
 596                     _Tp t0 = c*Ai[k] + s*Aj[k];
 597                     _Tp t1 = -s*Ai[k] + c*Aj[k];
 598                     Ai[k] = t0; Aj[k] = t1;
 599
 600                     a += (double)t0*t0; b += (double)t1*t1;
 601                 }
 602                 W[i] = a; W[j] = b;
 603
 604                 changed = true;
 605
 606                 if( Vt )
 607                 {
 608                     _Tp *Vi = Vt + i*vstep, *Vj = Vt + j*vstep;
 609                     k = vblas.givens(Vi, Vj, n, c, s);
 610
 611                     for( ; k < n; k++ )
 612                     {
 613                         _Tp t0 = c*Vi[k] + s*Vj[k];
 614                         _Tp t1 = -s*Vi[k] + c*Vj[k];
 615                         Vi[k] = t0; Vj[k] = t1;
 616                     }
 617                 }
 618             }
 619         if( !changed )
 620             break;
 621     }
 622
 623     for( i = 0; i < n; i++ )
 624     {
 625         for( k = 0, sd = 0; k < m; k++ )
 626         {
 627             _Tp t = At[i*astep + k];
 628             sd += (double)t*t;
 629         }
 630         W[i] = std::sqrt(sd);
 631     }
 632
 633     for( i = 0; i < n-1; i++ )
 634     {
 635         j = i;
 636         for( k = i+1; k < n; k++ )
 637         {
 638             if( W[j] < W[k] )
 639                 j = k;
 640         }
 641         if( i != j )
 642         {
 643             std::swap(W[i], W[j]);
 644             if( Vt )
 645             {
 646                 for( k = 0; k < m; k++ )
 647                     std::swap(At[i*astep + k], At[j*astep + k]);
 648
 649                 for( k = 0; k < n; k++ )
 650                     std::swap(Vt[i*vstep + k], Vt[j*vstep + k]);
 651             }
 652         }
 653     }
 654
 655     for( i = 0; i < n; i++ )
 656         _W[i] = (_Tp)W[i];
 657
 658     if( !Vt )
 659         return;
 660
 661     RNG rng(0x12345678);
 662     for( i = 0; i < n1; i++ )
 663     {
 664         sd = i < n ? W[i] : 0;
 665
 666         while( sd <= minval )
 667         {
 668             // if we got a zero singular value, then in order to get the corresponding left singular vector
 669             // we generate a random vector, project it to the previously computed left singular vectors,
 670             // subtract the projection and normalize the difference.
 671             const _Tp val0 = (_Tp)(1./m);
 672             for( k = 0; k < m; k++ )
 673             {
 674                 _Tp val = (rng.next() & 256) != 0 ? val0 : -val0;
 675                 At[i*astep + k] = val;
 676             }
 677             for( iter = 0; iter < 2; iter++ )
 678             {
 679                 for( j = 0; j < i; j++ )
 680                 {
 681                     sd = 0;
 682                     for( k = 0; k < m; k++ )
 683                         sd += At[i*astep + k]*At[j*astep + k];
 684                     _Tp asum = 0;
 685                     for( k = 0; k < m; k++ )
 686                     {
 687                         _Tp t = (_Tp)(At[i*astep + k] - sd*At[j*astep + k]);
 688                         At[i*astep + k] = t;
 689                         asum += std::abs(t);
 690                     }
 691                     asum = asum ? 1/asum : 0;
 692                     for( k = 0; k < m; k++ )
 693                         At[i*astep + k] *= asum;
 694                 }
 695             }
 696             sd = 0;
 697             for( k = 0; k < m; k++ )
 698             {
 699                 _Tp t = At[i*astep + k];
 700                 sd += (double)t*t;
 701             }
 702             sd = std::sqrt(sd);
 703         }
 704
 705         s = (_Tp)(1/sd);
 706         for( k = 0; k < m; k++ )
 707             At[i*astep + k] *= s;
 708     }
 709 }
 710
 711
 712 static void JacobiSVD(float* At, size_t astep, float* W, float* Vt, size_t vstep, int m, int n, int n1=-1)
 713 {
 714     JacobiSVDImpl_(At, astep, W, Vt, vstep, m, n, !Vt ? 0 : n1 < 0 ? n : n1, FLT_MIN, FLT_EPSILON*2);
 715 }
 716
 717 static void JacobiSVD(double* At, size_t astep, double* W, double* Vt, size_t vstep, int m, int n, int n1=-1)
 718 {
 719     JacobiSVDImpl_(At, astep, W, Vt, vstep, m, n, !Vt ? 0 : n1 < 0 ? n : n1, DBL_MIN, DBL_EPSILON*10);
 720 }
 721
 722 /* y[0:m,0:n] += diag(a[0:1,0:m]) * x[0:m,0:n] */
 723 template<typename T1, typename T2, typename T3> static void
 724 MatrAXPY( int m, int n, const T1* x, int dx,
 725          const T2* a, int inca, T3* y, int dy )
 726 {
 727     int i, j;
 728     for( i = 0; i < m; i++, x += dx, y += dy )
 729     {
 730         T2 s = a[i*inca];
 731         j=0;
 732          #if CV_ENABLE_UNROLLED
 733         for(; j <= n - 4; j += 4 )
 734         {
 735             T3 t0 = (T3)(y[j]   + s*x[j]);
 736             T3 t1 = (T3)(y[j+1] + s*x[j+1]);
 737             y[j]   = t0;
 738             y[j+1] = t1;
 739             t0 = (T3)(y[j+2] + s*x[j+2]);
 740             t1 = (T3)(y[j+3] + s*x[j+3]);
 741             y[j+2] = t0;
 742             y[j+3] = t1;
 743         }
 744         #endif
 745         for( ; j < n; j++ )
 746             y[j] = (T3)(y[j] + s*x[j]);
 747     }
 748 }
 749
 750 template<typename T> static void
 751 SVBkSbImpl_( int m, int n, const T* w, int incw,
 752        const T* u, int ldu, bool uT,
 753        const T* v, int ldv, bool vT,
 754        const T* b, int ldb, int nb,
 755        T* x, int ldx, double* buffer, T eps )
 756 {
 757     double threshold = 0;
 758     int udelta0 = uT ? ldu : 1, udelta1 = uT ? 1 : ldu;
 759     int vdelta0 = vT ? ldv : 1, vdelta1 = vT ? 1 : ldv;
 760     int i, j, nm = std::min(m, n);
 761
 762     if( !b )
 763         nb = m;
 764
 765     for( i = 0; i < n; i++ )
 766         for( j = 0; j < nb; j++ )
 767             x[i*ldx + j] = 0;
 768
 769     for( i = 0; i < nm; i++ )
 770         threshold += w[i*incw];
 771     threshold *= eps;
 772
 773     // v * inv(w) * uT * b
 774     for( i = 0; i < nm; i++, u += udelta0, v += vdelta0 )
 775     {
 776         double wi = w[i*incw];
 777         if( (double)std::abs(wi) <= threshold )
 778             continue;
 779         wi = 1/wi;
 780
 781         if( nb == 1 )
 782         {
 783             double s = 0;
 784             if( b )
 785                 for( j = 0; j < m; j++ )
 786                     s += u[j*udelta1]*b[j*ldb];
 787             else
 788                 s = u[0];
 789             s *= wi;
 790
 791             for( j = 0; j < n; j++ )
 792                 x[j*ldx] = (T)(x[j*ldx] + s*v[j*vdelta1]);
 793         }
 794         else
 795         {
 796             if( b )
 797             {
 798                 for( j = 0; j < nb; j++ )
 799                     buffer[j] = 0;
 800                 MatrAXPY( m, nb, b, ldb, u, udelta1, buffer, 0 );
 801                 for( j = 0; j < nb; j++ )
 802                     buffer[j] *= wi;
 803             }
 804             else
 805             {
 806                 for( j = 0; j < nb; j++ )
 807                     buffer[j] = u[j*udelta1]*wi;
 808             }
 809             MatrAXPY( n, nb, buffer, 0, v, vdelta1, x, ldx );
 810         }
 811     }
 812 }
 813
 814 static void
 815 SVBkSb( int m, int n, const float* w, size_t wstep,
 816         const float* u, size_t ustep, bool uT,
 817         const float* v, size_t vstep, bool vT,
 818         const float* b, size_t bstep, int nb,
 819         float* x, size_t xstep, uchar* buffer )
 820 {
 821     SVBkSbImpl_(m, n, w, wstep ? (int)(wstep/sizeof(w[0])) : 1,
 822                 u, (int)(ustep/sizeof(u[0])), uT,
 823                 v, (int)(vstep/sizeof(v[0])), vT,
 824                 b, (int)(bstep/sizeof(b[0])), nb,
 825                 x, (int)(xstep/sizeof(x[0])),
 826                 (double*)alignPtr(buffer, sizeof(double)), (float)(DBL_EPSILON*2) );
 827 }
 828
 829 static void
 830 SVBkSb( int m, int n, const double* w, size_t wstep,
 831        const double* u, size_t ustep, bool uT,
 832        const double* v, size_t vstep, bool vT,
 833        const double* b, size_t bstep, int nb,
 834        double* x, size_t xstep, uchar* buffer )
 835 {
 836     SVBkSbImpl_(m, n, w, wstep ? (int)(wstep/sizeof(w[0])) : 1,
 837                 u, (int)(ustep/sizeof(u[0])), uT,
 838                 v, (int)(vstep/sizeof(v[0])), vT,
 839                 b, (int)(bstep/sizeof(b[0])), nb,
 840                 x, (int)(xstep/sizeof(x[0])),
 841                 (double*)alignPtr(buffer, sizeof(double)), DBL_EPSILON*2 );
 842 }
 843
 844 }
 845
 846 /****************************************************************************************\
 847 *                                 Determinant of the matrix                              *
 848 \****************************************************************************************/
 849
 850 #define det2(m)   ((double)m(0,0)*m(1,1) - (double)m(0,1)*m(1,0))
 851 #define det3(m)   (m(0,0)*((double)m(1,1)*m(2,2) - (double)m(1,2)*m(2,1)) -  \
 852                    m(0,1)*((double)m(1,0)*m(2,2) - (double)m(1,2)*m(2,0)) +  \
 853                    m(0,2)*((double)m(1,0)*m(2,1) - (double)m(1,1)*m(2,0)))
 854
 855 double cv::determinant( InputArray _mat )
 856 {
 857     Mat mat = _mat.getMat();
 858     double result = 0;
 859     int type = mat.type(), rows = mat.rows;
 860     size_t step = mat.step;
 861     const uchar* m = mat.data;
 862
 863     CV_Assert( mat.rows == mat.cols && (type == CV_32F || type == CV_64F));
 864
 865     #define Mf(y, x) ((float*)(m + y*step))[x]
 866     #define Md(y, x) ((double*)(m + y*step))[x]
 867
 868     if( type == CV_32F )
 869     {
 870         if( rows == 2 )
 871             result = det2(Mf);
 872         else if( rows == 3 )
 873             result = det3(Mf);
 874         else if( rows == 1 )
 875             result = Mf(0,0);
 876         else
 877         {
 878             size_t bufSize = rows*rows*sizeof(float);
 879             AutoBuffer<uchar> buffer(bufSize);
 880             Mat a(rows, rows, CV_32F, (uchar*)buffer);
 881             mat.copyTo(a);
 882
 883             result = LU((float*)a.data, a.step, rows, 0, 0, 0);
 884             if( result )
 885             {
 886                 for( int i = 0; i < rows; i++ )
 887                     result *= ((const float*)(a.data + a.step*i))[i];
 888                 result = 1./result;
 889             }
 890         }
 891     }
 892     else
 893     {
 894         if( rows == 2 )
 895             result = det2(Md);
 896         else if( rows == 3 )
 897             result = det3(Md);
 898         else if( rows == 1 )
 899             result = Md(0,0);
 900         else
 901         {
 902             size_t bufSize = rows*rows*sizeof(double);
 903             AutoBuffer<uchar> buffer(bufSize);
 904             Mat a(rows, rows, CV_64F, (uchar*)buffer);
 905             mat.copyTo(a);
 906
 907             result = LU((double*)a.data, a.step, rows, 0, 0, 0);
 908             if( result )
 909             {
 910                 for( int i = 0; i < rows; i++ )
 911                     result *= ((const double*)(a.data + a.step*i))[i];
 912                 result = 1./result;
 913             }
 914         }
 915     }
 916
 917     #undef Mf
 918     #undef Md
 919
 920     return result;
 921 }
 922
 923 /****************************************************************************************\
 924 *                          Inverse (or pseudo-inverse) of a matrix                       *
 925 \****************************************************************************************/
 926
 927 #define Sf( y, x ) ((float*)(srcdata + y*srcstep))[x]
 928 #define Sd( y, x ) ((double*)(srcdata + y*srcstep))[x]
 929 #define Df( y, x ) ((float*)(dstdata + y*dststep))[x]
 930 #define Dd( y, x ) ((double*)(dstdata + y*dststep))[x]
 931
 932 double cv::invert( InputArray _src, OutputArray _dst, int method )
 933 {
 934     bool result = false;
 935     Mat src = _src.getMat();
 936     int type = src.type();
 937
 938     CV_Assert(type == CV_32F || type == CV_64F);
 939
 940     size_t esz = CV_ELEM_SIZE(type);
 941     int m = src.rows, n = src.cols;
 942
 943     if( method == DECOMP_SVD )
 944     {
 945         int nm = std::min(m, n);
 946
 947         AutoBuffer<uchar> _buf((m*nm + nm + nm*n)*esz + sizeof(double));
 948         uchar* buf = alignPtr((uchar*)_buf, (int)esz);
 949         Mat u(m, nm, type, buf);
 950         Mat w(nm, 1, type, u.data + m*nm*esz);
 951         Mat vt(nm, n, type, w.data + nm*esz);
 952
 953         SVD::compute(src, w, u, vt);
 954         SVD::backSubst(w, u, vt, Mat(), _dst);
 955         return type == CV_32F ?
 956             (((float*)w.data)[0] >= FLT_EPSILON ?
 957              ((float*)w.data)[n-1]/((float*)w.data)[0] : 0) :
 958             (((double*)w.data)[0] >= DBL_EPSILON ?
 959              ((double*)w.data)[n-1]/((double*)w.data)[0] : 0);
 960     }
 961
 962     CV_Assert( m == n );
 963
 964     if( method == DECOMP_EIG )
 965     {
 966         AutoBuffer<uchar> _buf((n*n*2 + n)*esz + sizeof(double));
 967         uchar* buf = alignPtr((uchar*)_buf, (int)esz);
 968         Mat u(n, n, type, buf);
 969         Mat w(n, 1, type, u.data + n*n*esz);
 970         Mat vt(n, n, type, w.data + n*esz);
 971
 972         eigen(src, w, vt);
 973         transpose(vt, u);
 974         SVD::backSubst(w, u, vt, Mat(), _dst);
 975         return type == CV_32F ?
 976         (((float*)w.data)[0] >= FLT_EPSILON ?
 977          ((float*)w.data)[n-1]/((float*)w.data)[0] : 0) :
 978         (((double*)w.data)[0] >= DBL_EPSILON ?
 979          ((double*)w.data)[n-1]/((double*)w.data)[0] : 0);
 980     }
 981
 982     CV_Assert( method == DECOMP_LU || method == DECOMP_CHOLESKY );
 983
 984     _dst.create( n, n, type );
 985     Mat dst = _dst.getMat();
 986
 987     if( n <= 3 )
 988     {
 989         uchar* srcdata = src.data;
 990         uchar* dstdata = dst.data;
 991         size_t srcstep = src.step;
 992         size_t dststep = dst.step;
 993
 994         if( n == 2 )
 995         {
 996             if( type == CV_32FC1 )
 997             {
 998                 double d = det2(Sf);
 999                 if( d != 0. )
1000                 {
1001                     result = true;
1002                     d = 1./d;
1003
1004                     #if CV_SSE2
1005                         if(USE_SSE2)
1006                         {
1007                             __m128 zero = _mm_setzero_ps();
1008                             __m128 t0 = _mm_loadl_pi(zero, (const __m64*)srcdata); //t0 = sf(0,0) sf(0,1)
1009                             __m128 t1 = _mm_loadh_pi(zero, (const __m64*)(srcdata+srcstep)); //t1 = sf(1,0) sf(1,1)
1010                             __m128 s0 = _mm_or_ps(t0, t1);
1011                             __m128 det =_mm_set1_ps((float)d);
1012                             s0 =  _mm_mul_ps(s0, det);
1013                             static const uchar CV_DECL_ALIGNED(16) inv[16] = {0,0,0,0,0,0,0,0x80,0,0,0,0x80,0,0,0,0};
1014                             __m128 pattern = _mm_load_ps((const float*)inv);
1015                             s0 = _mm_xor_ps(s0, pattern);//==-1*s0
1016                             s0 = _mm_shuffle_ps(s0, s0, _MM_SHUFFLE(0,2,1,3));
1017                             _mm_storel_pi((__m64*)dstdata, s0);
1018                             _mm_storeh_pi((__m64*)((float*)(dstdata+dststep)), s0);
1019                         }
1020                         else
1021                     #endif
1022                         {
1023                         double t0, t1;
1024                         t0 = Sf(0,0)*d;
1025                         t1 = Sf(1,1)*d;
1026                         Df(1,1) = (float)t0;
1027                         Df(0,0) = (float)t1;
1028                         t0 = -Sf(0,1)*d;
1029                         t1 = -Sf(1,0)*d;
1030                         Df(0,1) = (float)t0;
1031                         Df(1,0) = (float)t1;
1032                         }
1033
1034                 }
1035             }
1036             else
1037             {
1038                 double d = det2(Sd);
1039                 if( d != 0. )
1040                 {
1041                     result = true;
1042                     d = 1./d;
1043                     #if CV_SSE2
1044                         if(USE_SSE2)
1045                         {
1046                             __m128d s0 = _mm_loadu_pd((const double*)srcdata); //s0 = sf(0,0) sf(0,1)
1047                             __m128d s1 = _mm_loadu_pd ((const double*)(srcdata+srcstep));//s1 = sf(1,0) sf(1,1)
1048                             __m128d sm = _mm_unpacklo_pd(s0, _mm_load_sd((const double*)(srcdata+srcstep)+1)); //sm = sf(0,0) sf(1,1) - main diagonal
1049                             __m128d ss = _mm_shuffle_pd(s0, s1, _MM_SHUFFLE2(0,1)); //ss = sf(0,1) sf(1,0) - secondary diagonal
1050                             __m128d det = _mm_load1_pd((const double*)&d);
1051                             sm =  _mm_mul_pd(sm, det);
1052
1053                             static const uchar CV_DECL_ALIGNED(16) inv[8] = {0,0,0,0,0,0,0,0x80};
1054                             __m128d pattern = _mm_load1_pd((double*)inv);
1055                             ss = _mm_mul_pd(ss, det);
1056                             ss = _mm_xor_pd(ss, pattern);//==-1*ss
1057
1058                             s0 = _mm_shuffle_pd(sm, ss, _MM_SHUFFLE2(0,1));
1059                             s1 = _mm_shuffle_pd(ss, sm, _MM_SHUFFLE2(0,1));
1060                             _mm_storeu_pd((double*)dstdata, s0);
1061                             _mm_storeu_pd((double*)(dstdata+dststep), s1);
1062                         }
1063                         else
1064                     #endif
1065                         {
1066                             double t0, t1;
1067                             t0 = Sd(0,0)*d;
1068                             t1 = Sd(1,1)*d;
1069                             Dd(1,1) = t0;
1070                             Dd(0,0) = t1;
1071                             t0 = -Sd(0,1)*d;
1072                             t1 = -Sd(1,0)*d;
1073                             Dd(0,1) = t0;
1074                             Dd(1,0) = t1;
1075                         }
1076                 }
1077             }
1078         }
1079         else if( n == 3 )
1080         {
1081             if( type == CV_32FC1 )
1082             {
1083                 double d = det3(Sf);
1084
1085                 if( d != 0. )
1086                 {
1087                     double t[12];
1088
1089                     result = true;
1090                     d = 1./d;
1091                     t[0] = (((double)Sf(1,1) * Sf(2,2) - (double)Sf(1,2) * Sf(2,1)) * d);
1092                     t[1] = (((double)Sf(0,2) * Sf(2,1) - (double)Sf(0,1) * Sf(2,2)) * d);
1093                     t[2] = (((double)Sf(0,1) * Sf(1,2) - (double)Sf(0,2) * Sf(1,1)) * d);
1094
1095                     t[3] = (((double)Sf(1,2) * Sf(2,0) - (double)Sf(1,0) * Sf(2,2)) * d);
1096                     t[4] = (((double)Sf(0,0) * Sf(2,2) - (double)Sf(0,2) * Sf(2,0)) * d);
1097                     t[5] = (((double)Sf(0,2) * Sf(1,0) - (double)Sf(0,0) * Sf(1,2)) * d);
1098
1099                     t[6] = (((double)Sf(1,0) * Sf(2,1) - (double)Sf(1,1) * Sf(2,0)) * d);
1100                     t[7] = (((double)Sf(0,1) * Sf(2,0) - (double)Sf(0,0) * Sf(2,1)) * d);
1101                     t[8] = (((double)Sf(0,0) * Sf(1,1) - (double)Sf(0,1) * Sf(1,0)) * d);
1102
1103                     Df(0,0) = (float)t[0]; Df(0,1) = (float)t[1]; Df(0,2) = (float)t[2];
1104                     Df(1,0) = (float)t[3]; Df(1,1) = (float)t[4]; Df(1,2) = (float)t[5];
1105                     Df(2,0) = (float)t[6]; Df(2,1) = (float)t[7]; Df(2,2) = (float)t[8];
1106                 }
1107             }
1108             else
1109             {
1110                 double d = det3(Sd);
1111                 if( d != 0. )
1112                 {
1113                     result = true;
1114                     d = 1./d;
1115                     double t[9];
1116
1117                     t[0] = (Sd(1,1) * Sd(2,2) - Sd(1,2) * Sd(2,1)) * d;
1118                     t[1] = (Sd(0,2) * Sd(2,1) - Sd(0,1) * Sd(2,2)) * d;
1119                     t[2] = (Sd(0,1) * Sd(1,2) - Sd(0,2) * Sd(1,1)) * d;
1120
1121                     t[3] = (Sd(1,2) * Sd(2,0) - Sd(1,0) * Sd(2,2)) * d;
1122                     t[4] = (Sd(0,0) * Sd(2,2) - Sd(0,2) * Sd(2,0)) * d;
1123                     t[5] = (Sd(0,2) * Sd(1,0) - Sd(0,0) * Sd(1,2)) * d;
1124
1125                     t[6] = (Sd(1,0) * Sd(2,1) - Sd(1,1) * Sd(2,0)) * d;
1126                     t[7] = (Sd(0,1) * Sd(2,0) - Sd(0,0) * Sd(2,1)) * d;
1127                     t[8] = (Sd(0,0) * Sd(1,1) - Sd(0,1) * Sd(1,0)) * d;
1128
1129                     Dd(0,0) = t[0]; Dd(0,1) = t[1]; Dd(0,2) = t[2];
1130                     Dd(1,0) = t[3]; Dd(1,1) = t[4]; Dd(1,2) = t[5];
1131                     Dd(2,0) = t[6]; Dd(2,1) = t[7]; Dd(2,2) = t[8];
1132                 }
1133             }
1134         }
1135         else
1136         {
1137             assert( n == 1 );
1138
1139             if( type == CV_32FC1 )
1140             {
1141                 double d = Sf(0,0);
1142                 if( d != 0. )
1143                 {
1144                     result = true;
1145                     Df(0,0) = (float)(1./d);
1146                 }
1147             }
1148             else
1149             {
1150                 double d = Sd(0,0);
1151                 if( d != 0. )
1152                 {
1153                     result = true;
1154                     Dd(0,0) = 1./d;
1155                 }
1156             }
1157         }
1158         if( !result )
1159             dst = Scalar(0);
1160         return result;
1161     }
1162
1163    int elem_size = CV_ELEM_SIZE(type);
1164     AutoBuffer<uchar> buf(n*n*elem_size);
1165     Mat src1(n, n, type, (uchar*)buf);
1166     src.copyTo(src1);
1167     setIdentity(dst);
1168
1169     if( method == DECOMP_LU && type == CV_32F )
1170         result = LU((float*)src1.data, src1.step, n, (float*)dst.data, dst.step, n) != 0;
1171     else if( method == DECOMP_LU && type == CV_64F )
1172         result = LU((double*)src1.data, src1.step, n, (double*)dst.data, dst.step, n) != 0;
1173     else if( method == DECOMP_CHOLESKY && type == CV_32F )
1174         result = Cholesky((float*)src1.data, src1.step, n, (float*)dst.data, dst.step, n);
1175     else
1176         result = Cholesky((double*)src1.data, src1.step, n, (double*)dst.data, dst.step, n);
1177
1178     if( !result )
1179         dst = Scalar(0);
1180
1181     return result;
1182 }
1183
1184
1185
1186 /****************************************************************************************\
1187 *                              Solving a linear system                                   *
1188 \****************************************************************************************/
1189
1190 bool cv::solve( InputArray _src, InputArray _src2arg, OutputArray _dst, int method )
1191 {
1192     bool result = true;
1193     Mat src = _src.getMat(), _src2 = _src2arg.getMat();
1194     int type = src.type();
1195     bool is_normal = (method & DECOMP_NORMAL) != 0;
1196
1197     CV_Assert( type == _src2.type() && (type == CV_32F || type == CV_64F) );
1198
1199     method &= ~DECOMP_NORMAL;
1200     CV_Assert( (method != DECOMP_LU && method != DECOMP_CHOLESKY) ||
1201         is_normal || src.rows == src.cols );
1202
1203     // check case of a single equation and small matrix
1204     if( (method == DECOMP_LU || method == DECOMP_CHOLESKY) && !is_normal &&
1205         src.rows <= 3 && src.rows == src.cols && _src2.cols == 1 )
1206     {
1207         _dst.create( src.cols, _src2.cols, src.type() );
1208         Mat dst = _dst.getMat();
1209
1210         #define bf(y) ((float*)(bdata + y*src2step))[0]
1211         #define bd(y) ((double*)(bdata + y*src2step))[0]
1212
1213         uchar* srcdata = src.data;
1214         uchar* bdata = _src2.data;
1215         uchar* dstdata = dst.data;
1216         size_t srcstep = src.step;
1217         size_t src2step = _src2.step;
1218         size_t dststep = dst.step;
1219
1220         if( src.rows == 2 )
1221         {
1222             if( type == CV_32FC1 )
1223             {
1224                 double d = det2(Sf);
1225                 if( d != 0. )
1226                 {
1227                     double t;
1228                     d = 1./d;
1229                     t = (float)(((double)bf(0)*Sf(1,1) - (double)bf(1)*Sf(0,1))*d);
1230                     Df(1,0) = (float)(((double)bf(1)*Sf(0,0) - (double)bf(0)*Sf(1,0))*d);
1231                     Df(0,0) = (float)t;
1232                 }
1233                 else
1234                     result = false;
1235             }
1236             else
1237             {
1238                 double d = det2(Sd);
1239                 if( d != 0. )
1240                 {
1241                     double t;
1242                     d = 1./d;
1243                     t = (bd(0)*Sd(1,1) - bd(1)*Sd(0,1))*d;
1244                     Dd(1,0) = (bd(1)*Sd(0,0) - bd(0)*Sd(1,0))*d;
1245                     Dd(0,0) = t;
1246                 }
1247                 else
1248                     result = false;
1249             }
1250         }
1251         else if( src.rows == 3 )
1252         {
1253             if( type == CV_32FC1 )
1254             {
1255                 double d = det3(Sf);
1256                 if( d != 0. )
1257                 {
1258                     float t[3];
1259                     d = 1./d;
1260
1261                     t[0] = (float)(d*
1262                            (bf(0)*((double)Sf(1,1)*Sf(2,2) - (double)Sf(1,2)*Sf(2,1)) -
1263                             Sf(0,1)*((double)bf(1)*Sf(2,2) - (double)Sf(1,2)*bf(2)) +
1264                             Sf(0,2)*((double)bf(1)*Sf(2,1) - (double)Sf(1,1)*bf(2))));
1265
1266                     t[1] = (float)(d*
1267                            (Sf(0,0)*(double)(bf(1)*Sf(2,2) - (double)Sf(1,2)*bf(2)) -
1268                             bf(0)*((double)Sf(1,0)*Sf(2,2) - (double)Sf(1,2)*Sf(2,0)) +
1269                             Sf(0,2)*((double)Sf(1,0)*bf(2) - (double)bf(1)*Sf(2,0))));
1270
1271                     t[2] = (float)(d*
1272                            (Sf(0,0)*((double)Sf(1,1)*bf(2) - (double)bf(1)*Sf(2,1)) -
1273                             Sf(0,1)*((double)Sf(1,0)*bf(2) - (double)bf(1)*Sf(2,0)) +
1274                             bf(0)*((double)Sf(1,0)*Sf(2,1) - (double)Sf(1,1)*Sf(2,0))));
1275
1276                     Df(0,0) = t[0];
1277                     Df(1,0) = t[1];
1278                     Df(2,0) = t[2];
1279                 }
1280                 else
1281                     result = false;
1282             }
1283             else
1284             {
1285                 double d = det3(Sd);
1286                 if( d != 0. )
1287                 {
1288                     double t[9];
1289
1290                     d = 1./d;
1291
1292                     t[0] = ((Sd(1,1) * Sd(2,2) - Sd(1,2) * Sd(2,1))*bd(0) +
1293                             (Sd(0,2) * Sd(2,1) - Sd(0,1) * Sd(2,2))*bd(1) +
1294                             (Sd(0,1) * Sd(1,2) - Sd(0,2) * Sd(1,1))*bd(2))*d;
1295
1296                     t[1] = ((Sd(1,2) * Sd(2,0) - Sd(1,0) * Sd(2,2))*bd(0) +
1297                             (Sd(0,0) * Sd(2,2) - Sd(0,2) * Sd(2,0))*bd(1) +
1298                             (Sd(0,2) * Sd(1,0) - Sd(0,0) * Sd(1,2))*bd(2))*d;
1299
1300                     t[2] = ((Sd(1,0) * Sd(2,1) - Sd(1,1) * Sd(2,0))*bd(0) +
1301                             (Sd(0,1) * Sd(2,0) - Sd(0,0) * Sd(2,1))*bd(1) +
1302                             (Sd(0,0) * Sd(1,1) - Sd(0,1) * Sd(1,0))*bd(2))*d;
1303
1304                     Dd(0,0) = t[0];
1305                     Dd(1,0) = t[1];
1306                     Dd(2,0) = t[2];
1307                 }
1308                 else
1309                     result = false;
1310             }
1311         }
1312         else
1313         {
1314             assert( src.rows == 1 );
1315
1316             if( type == CV_32FC1 )
1317             {
1318                 double d = Sf(0,0);
1319                 if( d != 0. )
1320                     Df(0,0) = (float)(bf(0)/d);
1321                 else
1322                     result = false;
1323             }
1324             else
1325             {
1326                 double d = Sd(0,0);
1327                 if( d != 0. )
1328                     Dd(0,0) = (bd(0)/d);
1329                 else
1330                     result = false;
1331             }
1332         }
1333         return result;
1334     }
1335
1336     if( method == DECOMP_QR )
1337         method = DECOMP_SVD;
1338
1339     int m = src.rows, m_ = m, n = src.cols, nb = _src2.cols;
1340     size_t esz = CV_ELEM_SIZE(type), bufsize = 0;
1341     size_t vstep = alignSize(n*esz, 16);
1342     size_t astep = method == DECOMP_SVD && !is_normal ? alignSize(m*esz, 16) : vstep;
1343     AutoBuffer<uchar> buffer;
1344
1345     Mat src2 = _src2;
1346     _dst.create( src.cols, src2.cols, src.type() );
1347     Mat dst = _dst.getMat();
1348
1349     if( m < n )
1350         CV_Error(CV_StsBadArg, "The function can not solve under-determined linear systems" );
1351
1352     if( m == n )
1353         is_normal = false;
1354     else if( is_normal )
1355     {
1356         m_ = n;
1357         if( method == DECOMP_SVD )
1358             method = DECOMP_EIG;
1359     }
1360
1361     size_t asize = astep*(method == DECOMP_SVD || is_normal ? n : m);
1362     bufsize += asize + 32;
1363
1364     if( is_normal )
1365         bufsize += n*nb*esz;
1366
1367     if( method == DECOMP_SVD || method == DECOMP_EIG )
1368         bufsize += n*5*esz + n*vstep + nb*sizeof(double) + 32;
1369
1370     buffer.allocate(bufsize);
1371     uchar* ptr = alignPtr((uchar*)buffer, 16);
1372
1373     Mat a(m_, n, type, ptr, astep);
1374
1375     if( is_normal )
1376         mulTransposed(src, a, true);
1377     else if( method != DECOMP_SVD )
1378         src.copyTo(a);
1379     else
1380     {
1381         a = Mat(n, m_, type, ptr, astep);
1382         transpose(src, a);
1383     }
1384     ptr += asize;
1385
1386     if( !is_normal )
1387     {
1388         if( method == DECOMP_LU || method == DECOMP_CHOLESKY )
1389             src2.copyTo(dst);
1390     }
1391     else
1392     {
1393         // a'*b
1394         if( method == DECOMP_LU || method == DECOMP_CHOLESKY )
1395             gemm( src, src2, 1, Mat(), 0, dst, GEMM_1_T );
1396         else
1397         {
1398             Mat tmp(n, nb, type, ptr);
1399             ptr += n*nb*esz;
1400             gemm( src, src2, 1, Mat(), 0, tmp, GEMM_1_T );
1401             src2 = tmp;
1402         }
1403     }
1404
1405     if( method == DECOMP_LU )
1406     {
1407         if( type == CV_32F )
1408             result = LU(a.ptr<float>(), a.step, n, dst.ptr<float>(), dst.step, nb) != 0;
1409         else
1410             result = LU(a.ptr<double>(), a.step, n, dst.ptr<double>(), dst.step, nb) != 0;
1411     }
1412     else if( method == DECOMP_CHOLESKY )
1413     {
1414         if( type == CV_32F )
1415             result = Cholesky(a.ptr<float>(), a.step, n, dst.ptr<float>(), dst.step, nb);
1416         else
1417             result = Cholesky(a.ptr<double>(), a.step, n, dst.ptr<double>(), dst.step, nb);
1418     }
1419     else
1420     {
1421         ptr = alignPtr(ptr, 16);
1422         Mat v(n, n, type, ptr, vstep), w(n, 1, type, ptr + vstep*n), u;
1423         ptr += n*(vstep + esz);
1424
1425         if( method == DECOMP_EIG )
1426         {
1427             if( type == CV_32F )
1428                 Jacobi(a.ptr<float>(), a.step, w.ptr<float>(), v.ptr<float>(), v.step, n, ptr);
1429             else
1430                 Jacobi(a.ptr<double>(), a.step, w.ptr<double>(), v.ptr<double>(), v.step, n, ptr);
1431             u = v;
1432         }
1433         else
1434         {
1435             if( type == CV_32F )
1436                 JacobiSVD(a.ptr<float>(), a.step, w.ptr<float>(), v.ptr<float>(), v.step, m_, n);
1437             else
1438                 JacobiSVD(a.ptr<double>(), a.step, w.ptr<double>(), v.ptr<double>(), v.step, m_, n);
1439             u = a;
1440         }
1441
1442         if( type == CV_32F )
1443         {
1444             SVBkSb(m_, n, w.ptr<float>(), 0, u.ptr<float>(), u.step, true,
1445                    v.ptr<float>(), v.step, true, src2.ptr<float>(),
1446                    src2.step, nb, dst.ptr<float>(), dst.step, ptr);
1447         }
1448         else
1449         {
1450             SVBkSb(m_, n, w.ptr<double>(), 0, u.ptr<double>(), u.step, true,
1451                    v.ptr<double>(), v.step, true, src2.ptr<double>(),
1452                    src2.step, nb, dst.ptr<double>(), dst.step, ptr);
1453         }
1454         result = true;
1455     }
1456
1457     if( !result )
1458         dst = Scalar(0);
1459
1460     return result;
1461 }
1462
1463
1464 /////////////////// finding eigenvalues and eigenvectors of a symmetric matrix ///////////////
1465
1466 bool cv::eigen( InputArray _src, bool computeEvects, OutputArray _evals, OutputArray _evects )
1467 {
1468     Mat src = _src.getMat();
1469     int type = src.type();
1470     int n = src.rows;
1471
1472     CV_Assert( src.rows == src.cols );
1473     CV_Assert (type == CV_32F || type == CV_64F);
1474
1475     Mat v;
1476     if( computeEvects )
1477     {
1478         _evects.create(n, n, type);
1479         v = _evects.getMat();
1480     }
1481
1482     size_t elemSize = src.elemSize(), astep = alignSize(n*elemSize, 16);
1483     AutoBuffer<uchar> buf(n*astep + n*5*elemSize + 32);
1484     uchar* ptr = alignPtr((uchar*)buf, 16);
1485     Mat a(n, n, type, ptr, astep), w(n, 1, type, ptr + astep*n);
1486     ptr += astep*n + elemSize*n;
1487     src.copyTo(a);
1488     bool ok = type == CV_32F ?
1489         Jacobi(a.ptr<float>(), a.step, w.ptr<float>(), v.ptr<float>(), v.step, n, ptr) :
1490         Jacobi(a.ptr<double>(), a.step, w.ptr<double>(), v.ptr<double>(), v.step, n, ptr);
1491
1492     w.copyTo(_evals);
1493     return ok;
1494 }
1495
1496 bool cv::eigen( InputArray src, OutputArray evals, int, int )
1497 {
1498     return eigen(src, false, evals, noArray());
1499 }
1500
1501 bool cv::eigen( InputArray src, OutputArray evals, OutputArray evects, int, int)
1502 {
1503     return eigen(src, true, evals, evects);
1504 }
1505
1506 namespace cv
1507 {
1508
1509 static void _SVDcompute( InputArray _aarr, OutputArray _w,
1510                          OutputArray _u, OutputArray _vt, int flags )
1511 {
1512     Mat src = _aarr.getMat();
1513     int m = src.rows, n = src.cols;
1514     int type = src.type();
1515     bool compute_uv = _u.needed() || _vt.needed();
1516     bool full_uv = (flags & SVD::FULL_UV) != 0;
1517
1518     CV_Assert( type == CV_32F || type == CV_64F );
1519
1520     if( flags & SVD::NO_UV )
1521     {
1522         _u.release();
1523         _vt.release();
1524         compute_uv = full_uv = false;
1525     }
1526
1527     bool at = false;
1528     if( m < n )
1529     {
1530         std::swap(m, n);
1531         at = true;
1532     }
1533
1534     int urows = full_uv ? m : n;
1535     size_t esz = src.elemSize(), astep = alignSize(m*esz, 16), vstep = alignSize(n*esz, 16);
1536     AutoBuffer<uchar> _buf(urows*astep + n*vstep + n*esz + 32);
1537     uchar* buf = alignPtr((uchar*)_buf, 16);
1538     Mat temp_a(n, m, type, buf, astep);
1539     Mat temp_w(n, 1, type, buf + urows*astep);
1540     Mat temp_u(urows, m, type, buf, astep), temp_v;
1541
1542     if( compute_uv )
1543         temp_v = Mat(n, n, type, alignPtr(buf + urows*astep + n*esz, 16), vstep);
1544
1545     if( urows > n )
1546         temp_u = Scalar::all(0);
1547
1548     if( !at )
1549         transpose(src, temp_a);
1550     else
1551         src.copyTo(temp_a);
1552
1553     if( type == CV_32F )
1554     {
1555         JacobiSVD(temp_a.ptr<float>(), temp_u.step, temp_w.ptr<float>(),
1556               temp_v.ptr<float>(), temp_v.step, m, n, compute_uv ? urows : 0);
1557     }
1558     else
1559     {
1560         JacobiSVD(temp_a.ptr<double>(), temp_u.step, temp_w.ptr<double>(),
1561               temp_v.ptr<double>(), temp_v.step, m, n, compute_uv ? urows : 0);
1562     }
1563     temp_w.copyTo(_w);
1564     if( compute_uv )
1565     {
1566         if( !at )
1567         {
1568             transpose(temp_u, _u);
1569             temp_v.copyTo(_vt);
1570         }
1571         else
1572         {
1573             transpose(temp_v, _u);
1574             temp_u.copyTo(_vt);
1575         }
1576     }
1577 }
1578
1579
1580 void SVD::compute( InputArray a, OutputArray w, OutputArray u, OutputArray vt, int flags )
1581 {
1582     _SVDcompute(a, w, u, vt, flags);
1583 }
1584
1585 void SVD::compute( InputArray a, OutputArray w, int flags )
1586 {
1587     _SVDcompute(a, w, noArray(), noArray(), flags);
1588 }
1589
1590 void SVD::backSubst( InputArray _w, InputArray _u, InputArray _vt,
1591                      InputArray _rhs, OutputArray _dst )
1592 {
1593     Mat w = _w.getMat(), u = _u.getMat(), vt = _vt.getMat(), rhs = _rhs.getMat();
1594     int type = w.type(), esz = (int)w.elemSize();
1595     int m = u.rows, n = vt.cols, nb = rhs.data ? rhs.cols : m, nm = std::min(m, n);
1596     size_t wstep = w.rows == 1 ? (size_t)esz : w.cols == 1 ? (size_t)w.step : (size_t)w.step + esz;
1597     AutoBuffer<uchar> buffer(nb*sizeof(double) + 16);
1598     CV_Assert( w.type() == u.type() && u.type() == vt.type() && u.data && vt.data && w.data );
1599     CV_Assert( u.cols >= nm && vt.rows >= nm &&
1600               (w.size() == Size(nm, 1) || w.size() == Size(1, nm) || w.size() == Size(vt.rows, u.cols)) );
1601     CV_Assert( rhs.data == 0 || (rhs.type() == type && rhs.rows == m) );
1602
1603     _dst.create( n, nb, type );
1604     Mat dst = _dst.getMat();
1605     if( type == CV_32F )
1606         SVBkSb(m, n, w.ptr<float>(), wstep, u.ptr<float>(), u.step, false,
1607                vt.ptr<float>(), vt.step, true, rhs.ptr<float>(), rhs.step, nb,
1608                dst.ptr<float>(), dst.step, buffer);
1609     else if( type == CV_64F )
1610         SVBkSb(m, n, w.ptr<double>(), wstep, u.ptr<double>(), u.step, false,
1611                vt.ptr<double>(), vt.step, true, rhs.ptr<double>(), rhs.step, nb,
1612                dst.ptr<double>(), dst.step, buffer);
1613     else
1614         CV_Error( CV_StsUnsupportedFormat, "" );
1615 }
1616
1617
1618 SVD& SVD::operator ()(InputArray a, int flags)
1619 {
1620     _SVDcompute(a, w, u, vt, flags);
1621     return *this;
1622 }
1623
1624
1625 void SVD::backSubst( InputArray rhs, OutputArray dst ) const
1626 {
1627     backSubst( w, u, vt, rhs, dst );
1628 }
1629
1630 }
1631
1632
1633 void cv::SVDecomp(InputArray src, OutputArray w, OutputArray u, OutputArray vt, int flags)
1634 {
1635     SVD::compute(src, w, u, vt, flags);
1636 }
1637
1638 void cv::SVBackSubst(InputArray w, InputArray u, InputArray vt, InputArray rhs, OutputArray dst)
1639 {
1640     SVD::backSubst(w, u, vt, rhs, dst);
1641 }
1642
1643
1644 CV_IMPL double
1645 cvDet( const CvArr* arr )
1646 {
1647     if( CV_IS_MAT(arr) && ((CvMat*)arr)->rows <= 3 )
1648     {
1649         CvMat* mat = (CvMat*)arr;
1650         int type = CV_MAT_TYPE(mat->type);
1651         int rows = mat->rows;
1652         uchar* m = mat->data.ptr;
1653         int step = mat->step;
1654         CV_Assert( rows == mat->cols );
1655
1656         #define Mf(y, x) ((float*)(m + y*step))[x]
1657         #define Md(y, x) ((double*)(m + y*step))[x]
1658
1659         if( type == CV_32F )
1660         {
1661             if( rows == 2 )
1662                 return det2(Mf);
1663             if( rows == 3 )
1664                 return det3(Mf);
1665         }
1666         else if( type == CV_64F )
1667         {
1668             if( rows == 2 )
1669                 return det2(Md);
1670             if( rows == 3 )
1671                 return det3(Md);
1672         }
1673         return cv::determinant(cv::Mat(mat));
1674     }
1675     return cv::determinant(cv::cvarrToMat(arr));
1676 }
1677
1678
1679 CV_IMPL double
1680 cvInvert( const CvArr* srcarr, CvArr* dstarr, int method )
1681 {
1682     cv::Mat src = cv::cvarrToMat(srcarr), dst = cv::cvarrToMat(dstarr);
1683
1684     CV_Assert( src.type() == dst.type() && src.rows == dst.cols && src.cols == dst.rows );
1685     return cv::invert( src, dst, method == CV_CHOLESKY ? cv::DECOMP_CHOLESKY :
1686                       method == CV_SVD ? cv::DECOMP_SVD :
1687                       method == CV_SVD_SYM ? cv::DECOMP_EIG : cv::DECOMP_LU );
1688 }
1689
1690
1691 CV_IMPL int
1692 cvSolve( const CvArr* Aarr, const CvArr* barr, CvArr* xarr, int method )
1693 {
1694     cv::Mat A = cv::cvarrToMat(Aarr), b = cv::cvarrToMat(barr), x = cv::cvarrToMat(xarr);
1695
1696     CV_Assert( A.type() == x.type() && A.cols == x.rows && x.cols == b.cols );
1697     bool is_normal = (method & CV_NORMAL) != 0;
1698     method &= ~CV_NORMAL;
1699     return cv::solve( A, b, x, (method == CV_CHOLESKY ? cv::DECOMP_CHOLESKY :
1700                                 method == CV_SVD ? cv::DECOMP_SVD :
1701                                 method == CV_SVD_SYM ? cv::DECOMP_EIG :
1702         A.rows > A.cols ? cv::DECOMP_QR : cv::DECOMP_LU) + (is_normal ? cv::DECOMP_NORMAL : 0) );
1703 }
1704
1705
1706 CV_IMPL void
1707 cvEigenVV( CvArr* srcarr, CvArr* evectsarr, CvArr* evalsarr, double,
1708            int lowindex, int highindex)
1709 {
1710     cv::Mat src = cv::cvarrToMat(srcarr), evals0 = cv::cvarrToMat(evalsarr), evals = evals0;
1711     if( evectsarr )
1712     {
1713         cv::Mat evects0 = cv::cvarrToMat(evectsarr), evects = evects0;
1714         eigen(src, evals, evects, lowindex, highindex);
1715         if( evects0.data != evects.data )
1716         {
1717             uchar* p = evects0.data;
1718             evects.convertTo(evects0, evects0.type());
1719             CV_Assert( p == evects0.data );
1720         }
1721     }
1722     else
1723         eigen(src, evals, lowindex, highindex);
1724     if( evals0.data != evals.data )
1725     {
1726         uchar* p = evals0.data;
1727         if( evals0.size() == evals.size() )
1728             evals.convertTo(evals0, evals0.type());
1729         else if( evals0.type() == evals.type() )
1730             cv::transpose(evals, evals0);
1731         else
1732             cv::Mat(evals.t()).convertTo(evals0, evals0.type());
1733         CV_Assert( p == evals0.data );
1734     }
1735 }
1736
1737
1738 CV_IMPL void
1739 cvSVD( CvArr* aarr, CvArr* warr, CvArr* uarr, CvArr* varr, int flags )
1740 {
1741     cv::Mat a = cv::cvarrToMat(aarr), w = cv::cvarrToMat(warr), u, v;
1742     int m = a.rows, n = a.cols, type = a.type(), mn = std::max(m, n), nm = std::min(m, n);
1743
1744     CV_Assert( w.type() == type &&
1745         (w.size() == cv::Size(nm,1) || w.size() == cv::Size(1, nm) ||
1746         w.size() == cv::Size(nm, nm) || w.size() == cv::Size(n, m)) );
1747
1748     cv::SVD svd;
1749
1750     if( w.size() == cv::Size(nm, 1) )
1751         svd.w = cv::Mat(nm, 1, type, w.data );
1752     else if( w.isContinuous() )
1753         svd.w = w;
1754
1755     if( uarr )
1756     {
1757         u = cv::cvarrToMat(uarr);
1758         CV_Assert( u.type() == type );
1759         svd.u = u;
1760     }
1761
1762     if( varr )
1763     {
1764         v = cv::cvarrToMat(varr);
1765         CV_Assert( v.type() == type );
1766         svd.vt = v;
1767     }
1768
1769     svd(a, ((flags & CV_SVD_MODIFY_A) ? cv::SVD::MODIFY_A : 0) |
1770         ((!svd.u.data && !svd.vt.data) ? cv::SVD::NO_UV : 0) |
1771         ((m != n && (svd.u.size() == cv::Size(mn, mn) ||
1772         svd.vt.size() == cv::Size(mn, mn))) ? cv::SVD::FULL_UV : 0));
1773
1774     if( u.data )
1775     {
1776         if( flags & CV_SVD_U_T )
1777             cv::transpose( svd.u, u );
1778         else if( u.data != svd.u.data )
1779         {
1780             CV_Assert( u.size() == svd.u.size() );
1781             svd.u.copyTo(u);
1782         }
1783     }
1784
1785     if( v.data )
1786     {
1787         if( !(flags & CV_SVD_V_T) )
1788             cv::transpose( svd.vt, v );
1789         else if( v.data != svd.vt.data )
1790         {
1791             CV_Assert( v.size() == svd.vt.size() );
1792             svd.vt.copyTo(v);
1793         }
1794     }
1795
1796     if( w.data != svd.w.data )
1797     {
1798         if( w.size() == svd.w.size() )
1799             svd.w.copyTo(w);
1800         else
1801         {
1802             w = cv::Scalar(0);
1803             cv::Mat wd = w.diag();
1804             svd.w.copyTo(wd);
1805         }
1806     }
1807 }
1808
1809
1810 CV_IMPL void
1811 cvSVBkSb( const CvArr* warr, const CvArr* uarr,
1812           const CvArr* varr, const CvArr* rhsarr,
1813           CvArr* dstarr, int flags )
1814 {
1815     cv::Mat w = cv::cvarrToMat(warr), u = cv::cvarrToMat(uarr),
1816         v = cv::cvarrToMat(varr), rhs,
1817         dst = cv::cvarrToMat(dstarr), dst0 = dst;
1818     if( flags & CV_SVD_U_T )
1819     {
1820         cv::Mat tmp;
1821         transpose(u, tmp);
1822         u = tmp;
1823     }
1824     if( !(flags & CV_SVD_V_T) )
1825     {
1826         cv::Mat tmp;
1827         transpose(v, tmp);
1828         v = tmp;
1829     }
1830     if( rhsarr )
1831         rhs = cv::cvarrToMat(rhsarr);
1832
1833     cv::SVD::backSubst(w, u, v, rhs, dst);
1834     CV_Assert( dst.data == dst0.data );
1835 }