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