vcs.maemo.org Git - opencv/blob - src/cxcore/cxlapack.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 "_cxcore.h"
  44
  45 #ifdef HAVE_VECLIB
  46   #include <vecLib/clapack.h>
  47
  48   typedef __CLPK_integer    integer;
  49   typedef __CLPK_real       real;
  50 #else
  51   #include "clapack.h"
  52 #endif
  53
  54 #undef abs
  55 #undef max
  56 #undef min
  57
  58 namespace cv
  59 {
  60
  61 /****************************************************************************************\
  62 *                                 Determinant of the matrix                              *
  63 \****************************************************************************************/
  64
  65 #define det2(m)   (m(0,0)*m(1,1) - m(0,1)*m(1,0))
  66 #define det3(m)   (m(0,0)*(m(1,1)*m(2,2) - m(1,2)*m(2,1)) -  \
  67                    m(0,1)*(m(1,0)*m(2,2) - m(1,2)*m(2,0)) +  \
  68                    m(0,2)*(m(1,0)*m(2,1) - m(1,1)*m(2,0)))
  69
  70 double determinant( const Mat& mat )
  71 {
  72     double result = 0;
  73     int type = mat.type(), rows = mat.rows;
  74     size_t step = mat.step;
  75     const uchar* m = mat.data;
  76
  77     CV_Assert( mat.rows == mat.cols );
  78
  79     #define Mf(y, x) ((float*)(m + y*step))[x]
  80     #define Md(y, x) ((double*)(m + y*step))[x]
  81
  82     if( type == CV_32F )
  83     {
  84         if( rows == 2 )
  85             result = det2(Mf);
  86         else if( rows == 3 )
  87             result = det3(Mf);
  88         else if( rows == 1 )
  89             result = Mf(0,0);
  90         else
  91         {
  92             integer i, n = rows, *ipiv, info=0;
  93             int bufSize = n*n*sizeof(float) + (n+1)*sizeof(ipiv[0]), sign=0;
  94             AutoBuffer<uchar> buffer(bufSize);
  95
  96             Mat a(n, n, CV_32F, (uchar*)buffer);
  97             mat.copyTo(a);
  98
  99             ipiv = (integer*)cvAlignPtr(a.data + a.step*a.rows, sizeof(integer));
 100             sgetrf_(&n, &n, (float*)a.data, &n, ipiv, &info);
 101             assert(info >= 0);
 102
 103             if( info == 0 )
 104             {
 105                 result = 1;
 106                 for( i = 0; i < n; i++ )
 107                 {
 108                     result *= ((float*)a.data)[i*(n+1)];
 109                     sign ^= ipiv[i] != i+1;
 110                 }
 111                 result *= sign ? -1 : 1;
 112             }
 113         }
 114     }
 115     else if( type == CV_64F )
 116     {
 117         if( rows == 2 )
 118             result = det2(Md);
 119         else if( rows == 3 )
 120             result = det3(Md);
 121         else if( rows == 1 )
 122             result = Md(0,0);
 123         else
 124         {
 125             integer i, n = rows, *ipiv, info=0;
 126             int bufSize = n*n*sizeof(double) + (n+1)*sizeof(ipiv[0]), sign=0;
 127             AutoBuffer<uchar> buffer(bufSize);
 128
 129             Mat a(n, n, CV_64F, (uchar*)buffer);
 130             mat.copyTo(a);
 131             ipiv = (integer*)cvAlignPtr(a.data + a.step*a.rows, sizeof(integer));
 132
 133             dgetrf_(&n, &n, (double*)a.data, &n, ipiv, &info);
 134             assert(info >= 0);
 135
 136             if( info == 0 )
 137             {
 138                 result = 1;
 139                 for( i = 0; i < n; i++ )
 140                 {
 141                     result *= ((double*)a.data)[i*(n+1)];
 142                     sign ^= ipiv[i] != i+1;
 143                 }
 144                 result *= sign ? -1 : 1;
 145             }
 146         }
 147     }
 148     else
 149         CV_Error( CV_StsUnsupportedFormat, "" );
 150
 151     #undef Mf
 152     #undef Md
 153
 154     return result;
 155 }
 156
 157 /****************************************************************************************\
 158 *                          Inverse (or pseudo-inverse) of a matrix                       *
 159 \****************************************************************************************/
 160
 161 #define Sf( y, x ) ((float*)(srcdata + y*srcstep))[x]
 162 #define Sd( y, x ) ((double*)(srcdata + y*srcstep))[x]
 163 #define Df( y, x ) ((float*)(dstdata + y*dststep))[x]
 164 #define Dd( y, x ) ((double*)(dstdata + y*dststep))[x]
 165
 166 double invert( const Mat& src, Mat& dst, int method )
 167 {
 168     double result = 0;
 169     int type = src.type();
 170
 171     CV_Assert( method == DECOMP_LU || method == DECOMP_CHOLESKY || method == DECOMP_SVD );
 172
 173     if( method == DECOMP_SVD )
 174     {
 175         int n = std::min(src.rows, src.cols);
 176         SVD svd(src);
 177         svd.backSubst(Mat(), dst);
 178
 179         return type == CV_32F ?
 180             (((float*)svd.w.data)[0] >= FLT_EPSILON ?
 181             ((float*)svd.w.data)[n-1]/((float*)svd.w.data)[0] : 0) :
 182             (((double*)svd.w.data)[0] >= DBL_EPSILON ?
 183             ((double*)svd.w.data)[n-1]/((double*)svd.w.data)[0] : 0);
 184     }
 185
 186     CV_Assert( src.rows == src.cols && (type == CV_32F || type == CV_64F));
 187     dst.create( src.rows, src.cols, type );
 188
 189     if( method == DECOMP_LU || method == DECOMP_CHOLESKY )
 190     {
 191         if( src.rows <= 3 )
 192         {
 193             uchar* srcdata = src.data;
 194             uchar* dstdata = dst.data;
 195             size_t srcstep = src.step;
 196             size_t dststep = dst.step;
 197
 198             if( src.rows == 2 )
 199             {
 200                 if( type == CV_32FC1 )
 201                 {
 202                     double d = det2(Sf);
 203                     if( d != 0. )
 204                     {
 205                         double t0, t1;
 206                         result = d;
 207                         d = 1./d;
 208                         t0 = Sf(0,0)*d;
 209                         t1 = Sf(1,1)*d;
 210                         Df(1,1) = (float)t0;
 211                         Df(0,0) = (float)t1;
 212                         t0 = -Sf(0,1)*d;
 213                         t1 = -Sf(1,0)*d;
 214                         Df(0,1) = (float)t0;
 215                         Df(1,0) = (float)t1;
 216                     }
 217                 }
 218                 else
 219                 {
 220                     double d = det2(Sd);
 221                     if( d != 0. )
 222                     {
 223                         double t0, t1;
 224                         result = d;
 225                         d = 1./d;
 226                         t0 = Sd(0,0)*d;
 227                         t1 = Sd(1,1)*d;
 228                         Dd(1,1) = t0;
 229                         Dd(0,0) = t1;
 230                         t0 = -Sd(0,1)*d;
 231                         t1 = -Sd(1,0)*d;
 232                         Dd(0,1) = t0;
 233                         Dd(1,0) = t1;
 234                     }
 235                 }
 236             }
 237             else if( src.rows == 3 )
 238             {
 239                 if( type == CV_32FC1 )
 240                 {
 241                     double d = det3(Sf);
 242                     if( d != 0. )
 243                     {
 244                         float t[9];
 245                         result = d;
 246                         d = 1./d;
 247
 248                         t[0] = (float)((Sf(1,1) * Sf(2,2) - Sf(1,2) * Sf(2,1)) * d);
 249                         t[1] = (float)((Sf(0,2) * Sf(2,1) - Sf(0,1) * Sf(2,2)) * d);
 250                         t[2] = (float)((Sf(0,1) * Sf(1,2) - Sf(0,2) * Sf(1,1)) * d);
 251
 252                         t[3] = (float)((Sf(1,2) * Sf(2,0) - Sf(1,0) * Sf(2,2)) * d);
 253                         t[4] = (float)((Sf(0,0) * Sf(2,2) - Sf(0,2) * Sf(2,0)) * d);
 254                         t[5] = (float)((Sf(0,2) * Sf(1,0) - Sf(0,0) * Sf(1,2)) * d);
 255
 256                         t[6] = (float)((Sf(1,0) * Sf(2,1) - Sf(1,1) * Sf(2,0)) * d);
 257                         t[7] = (float)((Sf(0,1) * Sf(2,0) - Sf(0,0) * Sf(2,1)) * d);
 258                         t[8] = (float)((Sf(0,0) * Sf(1,1) - Sf(0,1) * Sf(1,0)) * d);
 259
 260                         Df(0,0) = t[0]; Df(0,1) = t[1]; Df(0,2) = t[2];
 261                         Df(1,0) = t[3]; Df(1,1) = t[4]; Df(1,2) = t[5];
 262                         Df(2,0) = t[6]; Df(2,1) = t[7]; Df(2,2) = t[8];
 263                     }
 264                 }
 265                 else
 266                 {
 267                     double d = det3(Sd);
 268                     if( d != 0. )
 269                     {
 270                         double t[9];
 271                         result = d;
 272                         d = 1./d;
 273
 274                         t[0] = (Sd(1,1) * Sd(2,2) - Sd(1,2) * Sd(2,1)) * d;
 275                         t[1] = (Sd(0,2) * Sd(2,1) - Sd(0,1) * Sd(2,2)) * d;
 276                         t[2] = (Sd(0,1) * Sd(1,2) - Sd(0,2) * Sd(1,1)) * d;
 277
 278                         t[3] = (Sd(1,2) * Sd(2,0) - Sd(1,0) * Sd(2,2)) * d;
 279                         t[4] = (Sd(0,0) * Sd(2,2) - Sd(0,2) * Sd(2,0)) * d;
 280                         t[5] = (Sd(0,2) * Sd(1,0) - Sd(0,0) * Sd(1,2)) * d;
 281
 282                         t[6] = (Sd(1,0) * Sd(2,1) - Sd(1,1) * Sd(2,0)) * d;
 283                         t[7] = (Sd(0,1) * Sd(2,0) - Sd(0,0) * Sd(2,1)) * d;
 284                         t[8] = (Sd(0,0) * Sd(1,1) - Sd(0,1) * Sd(1,0)) * d;
 285
 286                         Dd(0,0) = t[0]; Dd(0,1) = t[1]; Dd(0,2) = t[2];
 287                         Dd(1,0) = t[3]; Dd(1,1) = t[4]; Dd(1,2) = t[5];
 288                         Dd(2,0) = t[6]; Dd(2,1) = t[7]; Dd(2,2) = t[8];
 289                     }
 290                 }
 291             }
 292             else
 293             {
 294                 assert( src.rows == 1 );
 295
 296                 if( type == CV_32FC1 )
 297                 {
 298                     double d = Sf(0,0);
 299                     if( d != 0. )
 300                     {
 301                         result = d;
 302                         Df(0,0) = (float)(1./d);
 303                     }
 304                 }
 305                 else
 306                 {
 307                     double d = Sd(0,0);
 308                     if( d != 0. )
 309                     {
 310                         result = d;
 311                         Dd(0,0) = 1./d;
 312                     }
 313                 }
 314             }
 315             return result;
 316         }
 317
 318         {
 319         integer n = dst.cols, lwork=-1, elem_size = CV_ELEM_SIZE(type),
 320             lda = (int)(dst.step/elem_size), piv1=0, info=0;
 321
 322         if( dst.data == src.data )
 323         {
 324             dst.release();
 325             dst.create( src.rows, src.cols, type );
 326         }
 327         src.copyTo(dst);
 328         if( method == DECOMP_LU )
 329         {
 330             int buf_size = (int)(n*sizeof(integer));
 331             AutoBuffer<uchar> buf;
 332             uchar* buffer;
 333
 334             if( type == CV_32F )
 335             {
 336                 real work1 = 0;
 337                 sgetri_(&n, (float*)dst.data, &lda, &piv1, &work1, &lwork, &info);
 338                 lwork = cvRound(work1);
 339             }
 340             else
 341             {
 342                 double work1 = 0;
 343                 dgetri_(&n, (double*)dst.data, &lda, &piv1, &work1, &lwork, &info);
 344                 lwork = cvRound(work1);
 345             }
 346
 347             buf_size += (int)((lwork + 1)*elem_size);
 348             buf.allocate(buf_size);
 349             buffer = (uchar*)buf;
 350
 351             if( type == CV_32F )
 352             {
 353                 sgetrf_(&n, &n, (float*)dst.data, &lda, (integer*)buffer, &info);
 354                 if(info==0)
 355                     sgetri_(&n, (float*)dst.data, &lda, (integer*)buffer,
 356                         (float*)(buffer + n*sizeof(integer)), &lwork, &info);
 357             }
 358             else
 359             {
 360                 dgetrf_(&n, &n, (double*)dst.data, &lda, (integer*)buffer, &info);
 361                 if(info==0)
 362                     dgetri_(&n, (double*)dst.data, &lda, (integer*)buffer,
 363                         (double*)cvAlignPtr(buffer + n*sizeof(integer), elem_size), &lwork, &info);
 364             }
 365         }
 366         else if( method == CV_CHOLESKY )
 367         {
 368             char L[] = {'L', '\0'};
 369             if( type == CV_32F )
 370             {
 371                 spotrf_(L, &n, (float*)dst.data, &lda, &info);
 372                 if(info==0)
 373                     spotri_(L, &n, (float*)dst.data, &lda, &info);
 374             }
 375             else
 376             {
 377                 dpotrf_(L, &n, (double*)dst.data, &lda, &info);
 378                 if(info==0)
 379                     dpotri_(L, &n, (double*)dst.data, &lda, &info);
 380             }
 381             completeSymm(dst);
 382         }
 383         result = info == 0;
 384         }
 385     }
 386
 387     if( !result )
 388         dst = Scalar(0);
 389
 390     return result;
 391 }
 392
 393 /****************************************************************************************\
 394 *                              Solving a linear system                                   *
 395 \****************************************************************************************/
 396
 397 bool solve( const Mat& src, const Mat& src2, Mat& dst, int method )
 398 {
 399     bool result = true;
 400     int type = src.type();
 401     bool is_normal = (method & DECOMP_NORMAL) != 0;
 402
 403     CV_Assert( type == src2.type() && (type == CV_32F || type == CV_64F) );
 404
 405     method &= ~DECOMP_NORMAL;
 406     CV_Assert( (method != DECOMP_LU && method != DECOMP_CHOLESKY) ||
 407         is_normal || src.rows == src.cols );
 408
 409     dst.create( src.cols, src2.cols, src.type() );
 410
 411     // check case of a single equation and small matrix
 412     if( (method == DECOMP_LU || method == DECOMP_CHOLESKY) &&
 413         src.rows <= 3 && src.rows == src.cols && src2.cols == 1 )
 414     {
 415         #define bf(y) ((float*)(bdata + y*src2step))[0]
 416         #define bd(y) ((double*)(bdata + y*src2step))[0]
 417
 418         uchar* srcdata = src.data;
 419         uchar* bdata = src2.data;
 420         uchar* dstdata = dst.data;
 421         size_t srcstep = src.step;
 422         size_t src2step = src2.step;
 423         size_t dststep = dst.step;
 424
 425         if( src.rows == 2 )
 426         {
 427             if( type == CV_32FC1 )
 428             {
 429                 double d = det2(Sf);
 430                 if( d != 0. )
 431                 {
 432                     float t;
 433                     d = 1./d;
 434                     t = (float)((bf(0)*Sf(1,1) - bf(1)*Sf(0,1))*d);
 435                     Df(1,0) = (float)((bf(1)*Sf(0,0) - bf(0)*Sf(1,0))*d);
 436                     Df(0,0) = t;
 437                 }
 438                 else
 439                     result = false;
 440             }
 441             else
 442             {
 443                 double d = det2(Sd);
 444                 if( d != 0. )
 445                 {
 446                     double t;
 447                     d = 1./d;
 448                     t = (bd(0)*Sd(1,1) - bd(1)*Sd(0,1))*d;
 449                     Dd(1,0) = (bd(1)*Sd(0,0) - bd(0)*Sd(1,0))*d;
 450                     Dd(0,0) = t;
 451                 }
 452                 else
 453                     result = false;
 454             }
 455         }
 456         else if( src.rows == 3 )
 457         {
 458             if( type == CV_32FC1 )
 459             {
 460                 double d = det3(Sf);
 461                 if( d != 0. )
 462                 {
 463                     float t[3];
 464                     d = 1./d;
 465
 466                     t[0] = (float)(d*
 467                            (bf(0)*(Sf(1,1)*Sf(2,2) - Sf(1,2)*Sf(2,1)) -
 468                             Sf(0,1)*(bf(1)*Sf(2,2) - Sf(1,2)*bf(2)) +
 469                             Sf(0,2)*(bf(1)*Sf(2,1) - Sf(1,1)*bf(2))));
 470
 471                     t[1] = (float)(d*
 472                            (Sf(0,0)*(bf(1)*Sf(2,2) - Sf(1,2)*bf(2)) -
 473                             bf(0)*(Sf(1,0)*Sf(2,2) - Sf(1,2)*Sf(2,0)) +
 474                             Sf(0,2)*(Sf(1,0)*bf(2) - bf(1)*Sf(2,0))));
 475
 476                     t[2] = (float)(d*
 477                            (Sf(0,0)*(Sf(1,1)*bf(2) - bf(1)*Sf(2,1)) -
 478                             Sf(0,1)*(Sf(1,0)*bf(2) - bf(1)*Sf(2,0)) +
 479                             bf(0)*(Sf(1,0)*Sf(2,1) - Sf(1,1)*Sf(2,0))));
 480
 481                     Df(0,0) = t[0];
 482                     Df(1,0) = t[1];
 483                     Df(2,0) = t[2];
 484                 }
 485                 else
 486                     result = false;
 487             }
 488             else
 489             {
 490                 double d = det3(Sd);
 491                 if( d != 0. )
 492                 {
 493                     double t[9];
 494
 495                     d = 1./d;
 496
 497                     t[0] = ((Sd(1,1) * Sd(2,2) - Sd(1,2) * Sd(2,1))*bd(0) +
 498                             (Sd(0,2) * Sd(2,1) - Sd(0,1) * Sd(2,2))*bd(1) +
 499                             (Sd(0,1) * Sd(1,2) - Sd(0,2) * Sd(1,1))*bd(2))*d;
 500
 501                     t[1] = ((Sd(1,2) * Sd(2,0) - Sd(1,0) * Sd(2,2))*bd(0) +
 502                             (Sd(0,0) * Sd(2,2) - Sd(0,2) * Sd(2,0))*bd(1) +
 503                             (Sd(0,2) * Sd(1,0) - Sd(0,0) * Sd(1,2))*bd(2))*d;
 504
 505                     t[2] = ((Sd(1,0) * Sd(2,1) - Sd(1,1) * Sd(2,0))*bd(0) +
 506                             (Sd(0,1) * Sd(2,0) - Sd(0,0) * Sd(2,1))*bd(1) +
 507                             (Sd(0,0) * Sd(1,1) - Sd(0,1) * Sd(1,0))*bd(2))*d;
 508
 509                     Dd(0,0) = t[0];
 510                     Dd(1,0) = t[1];
 511                     Dd(2,0) = t[2];
 512                 }
 513                 else
 514                     result = false;
 515             }
 516         }
 517         else
 518         {
 519             assert( src.rows == 1 );
 520
 521             if( type == CV_32FC1 )
 522             {
 523                 double d = Sf(0,0);
 524                 if( d != 0. )
 525                     Df(0,0) = (float)(bf(0)/d);
 526                 else
 527                     result = false;
 528             }
 529             else
 530             {
 531                 double d = Sd(0,0);
 532                 if( d != 0. )
 533                     Dd(0,0) = (bd(0)/d);
 534                 else
 535                     result = false;
 536             }
 537         }
 538     }
 539
 540     {
 541     double rcond=-1, s1=0, work1=0, *work=0, *s=0;
 542     float frcond=-1, fs1=0, fwork1=0, *fwork=0, *fs=0;
 543     integer m = src.rows, m_ = m, n = src.cols, mn = std::max(m,n),
 544         nm = std::min(m, n), nb = src2.cols, lwork=-1, liwork=0, iwork1=0,
 545         lda = m, ldx = mn, info=0, rank=0, *iwork=0;
 546     int elem_size = CV_ELEM_SIZE(type);
 547     bool copy_rhs=false;
 548     int buf_size=0;
 549     AutoBuffer<uchar> buffer;
 550     uchar* ptr;
 551     char N[] = {'N', '\0'}, L[] = {'L', '\0'};
 552
 553     if( m <= n )
 554         is_normal = false;
 555     else if( is_normal )
 556         m_ = n;
 557
 558     buf_size += (is_normal ? n*n : m*n)*elem_size;
 559
 560     if( m_ != n || nb > 1 || !dst.isContinuous() )
 561     {
 562         copy_rhs = true;
 563         if( is_normal )
 564             buf_size += n*nb*elem_size;
 565         else
 566             buf_size += mn*nb*elem_size;
 567     }
 568
 569     if( method == DECOMP_SVD || method == DECOMP_EIG )
 570     {
 571         integer nlvl = cvRound(std::log(std::max(std::min(m_,n)/25., 1.))/CV_LOG2) + 1;
 572         liwork = std::min(m_,n)*(3*std::max(nlvl,(integer)0) + 11);
 573
 574         if( type == CV_32F )
 575             sgelsd_(&m_, &n, &nb, (float*)src.data, &lda, (float*)dst.data, &ldx,
 576                 &fs1, &frcond, &rank, &fwork1, &lwork, &iwork1, &info);
 577         else
 578             dgelsd_(&m_, &n, &nb, (double*)src.data, &lda, (double*)dst.data, &ldx,
 579                 &s1, &rcond, &rank, &work1, &lwork, &iwork1, &info );
 580         buf_size += nm*elem_size + (liwork + 1)*sizeof(integer);
 581     }
 582     else if( method == DECOMP_QR )
 583     {
 584         if( type == CV_32F )
 585             sgels_(N, &m_, &n, &nb, (float*)src.data, &lda,
 586                 (float*)dst.data, &ldx, &fwork1, &lwork, &info );
 587         else
 588             dgels_(N, &m_, &n, &nb, (double*)src.data, &lda,
 589                 (double*)dst.data, &ldx, &work1, &lwork, &info );
 590     }
 591     else if( method == DECOMP_LU )
 592     {
 593         buf_size += (n+1)*sizeof(integer);
 594     }
 595     else if( method == DECOMP_CHOLESKY )
 596         ;
 597     else
 598         CV_Error( CV_StsBadArg, "Unknown method" );
 599     assert(info == 0);
 600
 601     lwork = cvRound(type == CV_32F ? (double)fwork1 : work1);
 602     buf_size += lwork*elem_size;
 603     buffer.allocate(buf_size);
 604     ptr = (uchar*)buffer;
 605
 606     Mat at(n, m_, type, ptr);
 607     ptr += n*m_*elem_size;
 608
 609     if( method == DECOMP_CHOLESKY || method == DECOMP_EIG )
 610         src.copyTo(at);
 611     else if( !is_normal )
 612         transpose(src, at);
 613     else
 614         mulTransposed(src, at, true);
 615
 616     Mat xt;
 617     if( !is_normal )
 618     {
 619         if( copy_rhs )
 620         {
 621             Mat temp(nb, mn, type, ptr);
 622             ptr += nb*mn*elem_size;
 623             Mat bt = temp.colRange(0, m);
 624             xt = temp.colRange(0, n);
 625             transpose(src2, bt);
 626         }
 627         else
 628         {
 629             src2.copyTo(dst);
 630             xt = Mat(1, n, type, dst.data);
 631         }
 632     }
 633     else
 634     {
 635         if( copy_rhs )
 636         {
 637             xt = Mat(nb, n, type, ptr);
 638             ptr += nb*n*elem_size;
 639         }
 640         else
 641             xt = Mat(1, n, type, dst.data);
 642         // (a'*b)' = b'*a
 643         gemm( src2, src, 1, Mat(), 0, xt, GEMM_1_T );
 644     }
 645
 646     lda = (int)(at.step ? at.step/elem_size : at.cols);
 647     ldx = (int)(xt.step ? xt.step/elem_size : (!is_normal && copy_rhs ? mn : n));
 648
 649     if( method == DECOMP_SVD || method == DECOMP_EIG )
 650     {
 651         if( type == CV_32F )
 652         {
 653             fs = (float*)ptr;
 654             ptr += nm*elem_size;
 655             fwork = (float*)ptr;
 656             ptr += lwork*elem_size;
 657             iwork = (integer*)cvAlignPtr(ptr, sizeof(integer));
 658
 659             sgelsd_(&m_, &n, &nb, (float*)at.data, &lda, (float*)xt.data, &ldx,
 660                 fs, &frcond, &rank, fwork, &lwork, iwork, &info);
 661         }
 662         else
 663         {
 664             s = (double*)ptr;
 665             ptr += nm*elem_size;
 666             work = (double*)ptr;
 667             ptr += lwork*elem_size;
 668             iwork = (integer*)cvAlignPtr(ptr, sizeof(integer));
 669
 670             dgelsd_(&m_, &n, &nb, (double*)at.data, &lda, (double*)xt.data, &ldx,
 671                 s, &rcond, &rank, work, &lwork, iwork, &info);
 672         }
 673     }
 674     else if( method == CV_QR )
 675     {
 676         if( type == CV_32F )
 677         {
 678             fwork = (float*)ptr;
 679             sgels_(N, &m_, &n, &nb, (float*)at.data, &lda,
 680                 (float*)xt.data, &ldx, fwork, &lwork, &info);
 681         }
 682         else
 683         {
 684             work = (double*)ptr;
 685             dgels_(N, &m_, &n, &nb, (double*)at.data, &lda,
 686                 (double*)xt.data, &ldx, work, &lwork, &info);
 687         }
 688     }
 689     else if( method == CV_CHOLESKY || (method == CV_LU && is_normal) )
 690     {
 691         if( type == CV_32F )
 692         {
 693             spotrf_(L, &n, (float*)at.data, &lda, &info);
 694             if(info==0)
 695                 spotrs_(L, &n, &nb, (float*)at.data, &lda, (float*)xt.data, &ldx, &info);
 696         }
 697         else
 698         {
 699             dpotrf_(L, &n, (double*)at.data, &lda, &info);
 700             if(info==0)
 701                 dpotrs_(L, &n, &nb, (double*)at.data, &lda, (double*)xt.data, &ldx, &info);
 702         }
 703     }
 704     else if( method == CV_LU )
 705     {
 706         iwork = (integer*)cvAlignPtr(ptr, sizeof(integer));
 707         if( type == CV_32F )
 708             sgesv_(&n, &nb, (float*)at.data, &lda, iwork, (float*)xt.data, &ldx, &info );
 709         else
 710             dgesv_(&n, &nb, (double*)at.data, &lda, iwork, (double*)xt.data, &ldx, &info );
 711     }
 712     else
 713         assert(0);
 714     result = info == 0;
 715
 716     if( !result )
 717         dst = Scalar(0);
 718     else if( xt.data != dst.data )
 719         transpose( xt, dst );
 720
 721     return result;
 722     }
 723 }
 724
 725
 726 /////////////////// finding eigenvalues and eigenvectors of a symmetric matrix ///////////////
 727
 728 template<typename Real> static inline Real hypot(Real a, Real b)
 729 {
 730     a = std::abs(a);
 731     b = std::abs(b);
 732     Real f;
 733     if( a > b )
 734     {
 735         f = b/a;
 736         return a*std::sqrt(1 + f*f);
 737     }
 738     f = a/b;
 739     return b*std::sqrt(1 + f*f);
 740 }
 741
 742
 743 template<typename Real> bool jacobi(const Mat& _S0, Mat& _e, Mat& _E, bool computeEvects, Real eps)
 744 {
 745     int n = _S0.cols, i, j, k, m;
 746
 747     if( computeEvects )
 748         _E = Mat::eye(n, n, _S0.type());
 749
 750     int iters, maxIters = n*n*30;
 751
 752     AutoBuffer<uchar> buf(n*2*sizeof(int) + (n*n+n*2+1)*sizeof(Real));
 753     Real* S = alignPtr((Real*)(uchar*)buf, sizeof(Real));
 754     Real* maxSR = S + n*n;
 755     Real* maxSC = maxSR + n;
 756     int* indR = (int*)(maxSC + n);
 757     int* indC = indR + n;
 758
 759     Mat _S(_S0.size(), _S0.type(), S);
 760     _S0.copyTo(_S);
 761
 762     Real mv;
 763     Real* E = (Real*)_E.data;
 764     Real* e = (Real*)_e.data;
 765     int Sstep = _S.step/sizeof(Real);
 766     int estep = _e.cols == 1 ? 1 : _e.step/sizeof(Real);
 767     int Estep = _E.step/sizeof(Real);
 768
 769     for( k = 0; k < n; k++ )
 770     {
 771         e[k*estep] = S[(Sstep + 1)*k];
 772         if( k < n - 1 )
 773         {
 774             for( m = k+1, mv = std::abs(S[Sstep*k + m]), i = k+2; i < n; i++ )
 775             {
 776                 Real v = std::abs(S[Sstep*k+i]);
 777                 if( mv < v )
 778                     mv = v, m = i;
 779             }
 780             maxSR[k] = mv;
 781             indR[k] = m;
 782         }
 783         if( k > 0 )
 784         {
 785             for( m = 0, mv = std::abs(S[k]), i = 1; i < k; i++ )
 786             {
 787                 Real v = std::abs(S[Sstep*i+k]);
 788                 if( mv < v )
 789                     mv = v, m = i;
 790             }
 791             maxSC[k] = mv;
 792             indC[k] = m;
 793         }
 794     }
 795
 796     for( iters = 0; iters < maxIters; iters++ )
 797     {
 798         // find index (k,l) of pivot p
 799         for( k = 0, mv = maxSR[0], i = 1; i < n-1; i++ )
 800         {
 801             Real v = maxSR[i];
 802             if( mv < v )
 803                 mv = v, k = i;
 804         }
 805         int l = indR[k];
 806         for( i = 1; i < n; i++ )
 807         {
 808             Real v = maxSC[i];
 809             if( mv < v )
 810                 mv = v, k = indC[i], l = i;
 811         }
 812
 813         Real p = S[Sstep*k + l];
 814         if( std::abs(p) <= eps )
 815             break;
 816         Real y = Real((e[estep*l] - e[estep*k])*0.5);
 817         Real t = std::abs(y) + hypot(p, y);
 818         Real s = hypot(p, t);
 819         Real c = t/s;
 820         s = p/s; t = (p/t)*p;
 821         if( y < 0 )
 822             s = -s, t = -t;
 823         S[Sstep*k + l] = 0;
 824
 825         e[estep*k] -= t;
 826         e[estep*l] += t;
 827
 828         Real a0, b0;
 829
 830 #undef rotate
 831 #define rotate(v0, v1) a0 = v0, b0 = v1, v0 = a0*c - b0*s, v1 = a0*s + b0*c
 832
 833         // rotate rows and columns k and l
 834         for( i = 0; i < k; i++ )
 835             rotate(S[Sstep*i+k], S[Sstep*i+l]);
 836         for( i = k+1; i < l; i++ )
 837             rotate(S[Sstep*k+i], S[Sstep*i+l]);
 838         for( i = l+1; i < n; i++ )
 839             rotate(S[Sstep*k+i], S[Sstep*l+i]);
 840
 841         // rotate eigenvectors
 842         if( computeEvects )
 843             for( i = 0; i < n; i++ )
 844                 rotate(E[Estep*k+i], E[Estep*l+i]);
 845
 846 #undef rotate
 847
 848         for( j = 0; j < 2; j++ )
 849         {
 850             int idx = j == 0 ? k : l;
 851             if( idx < n - 1 )
 852             {
 853                 for( m = idx+1, mv = std::abs(S[Sstep*idx + m]), i = idx+2; i < n; i++ )
 854                 {
 855                     Real v = std::abs(S[Sstep*idx+i]);
 856                     if( mv < v )
 857                         mv = v, m = i;
 858                 }
 859                 maxSR[idx] = mv;
 860                 indR[idx] = m;
 861             }
 862             if( idx > 0 )
 863             {
 864                 for( m = 0, mv = std::abs(S[idx]), i = 1; i < idx; i++ )
 865                 {
 866                     Real v = std::abs(S[Sstep*i+idx]);
 867                     if( mv < v )
 868                         mv = v, m = i;
 869                 }
 870                 maxSC[idx] = mv;
 871                 indC[idx] = m;
 872             }
 873         }
 874     }
 875
 876     // sort eigenvalues & eigenvectors
 877     for( k = 0; k < n-1; k++ )
 878     {
 879         m = k;
 880         for( i = k+1; i < n; i++ )
 881         {
 882             if( e[estep*m] < e[estep*i] )
 883                 m = i;
 884         }
 885         if( k != m )
 886         {
 887             std::swap(e[estep*m], e[estep*k]);
 888             if( computeEvects )
 889                 for( i = 0; i < n; i++ )
 890                     std::swap(E[Estep*m + i], E[Estep*k + i]);
 891         }
 892     }
 893
 894     return true;
 895 }
 896
 897
 898 static bool eigen( const Mat& src, Mat& evals, Mat& evects, bool computeEvects,
 899                    int lowindex, int highindex )
 900 {
 901     int type = src.type();
 902     integer n = src.rows;
 903
 904     // If a range is selected both limits are needed.
 905     CV_Assert( ( lowindex >= 0 && highindex >= 0 ) ||
 906                ( lowindex < 0 && highindex < 0 ) );
 907
 908     // lapack sorts from lowest to highest so we flip
 909     integer il = n - highindex;
 910     integer iu = n - lowindex;
 911
 912     CV_Assert( src.rows == src.cols );
 913     CV_Assert (type == CV_32F || type == CV_64F);
 914
 915     // allow for 1xn eigenvalue matrix too
 916     if( !(evals.rows == 1 && evals.cols == n && evals.type() == type) )
 917         evals.create(n, 1, type);
 918
 919     if( n <= 20 )
 920     {
 921         if( type == CV_32F )
 922             return jacobi<float>(src, evals, evects, computeEvects, FLT_EPSILON);
 923         else
 924             return jacobi<double>(src, evals, evects, computeEvects, DBL_EPSILON);
 925     }
 926
 927     bool result;
 928     integer m=0, lda, ldv=n, lwork=-1, iwork1=0, liwork=-1, idummy=0, info=0;
 929     integer *isupport, *iwork;
 930     char job[] = { computeEvects ? 'V' : 'N', '\0' };
 931     char range[2] = "I";
 932     range[0] = (il < n + 1) ? 'I' : 'A';
 933
 934     char L[] = {'L', '\0'};
 935     uchar* work;
 936
 937     AutoBuffer<uchar> buf;
 938
 939     int elem_size = (int)src.elemSize();
 940     lda = (int)(src.step/elem_size);
 941
 942     if( computeEvects )
 943     {
 944         evects.create(n, n, type);
 945         ldv = (int)(evects.step/elem_size);
 946     }
 947
 948     bool copy_evals = !evals.isContinuous();
 949
 950     if( type == CV_32FC1 )
 951     {
 952         float work1 = 0, dummy = 0, abstol = 0, *s;
 953
 954         ssyevr_(job, range, L, &n, (float*)src.data, &lda, &dummy, &dummy, &il, &iu,
 955             &abstol, &m, (float*)evals.data, (float*)evects.data, &ldv,
 956             &idummy, &work1, &lwork, &iwork1, &liwork, &info );
 957         assert( info == 0 );
 958
 959         lwork = cvRound(work1);
 960         liwork = iwork1;
 961         buf.allocate((lwork + n*n + (copy_evals ? n : 0))*elem_size +
 962                      (liwork+2*n+1)*sizeof(integer));
 963         Mat a(n, n, type, (uchar*)buf);
 964         work = a.data + n*n*elem_size;
 965         if( copy_evals )
 966             s = (float*)(work + lwork*elem_size);
 967         else
 968             s = (float*)evals.data;
 969
 970         iwork = (integer*)cvAlignPtr(work + (lwork + (copy_evals ? n : 0))*elem_size, sizeof(integer));
 971         isupport = iwork + liwork;
 972
 973         ssyevr_(job, range, L, &n, (float*)src.data, &lda, &dummy, &dummy,
 974             &il, &iu, &abstol, &m, s, (float*)evects.data,
 975             &ldv, isupport, (float*)work, &lwork, iwork, &liwork, &info );
 976         result = info == 0;
 977     }
 978     else
 979     {
 980         double work1 = 0, dummy = 0, abstol = 0, *s;
 981
 982         dsyevr_(job, range, L, &n, (double*)src.data, &lda, &dummy, &dummy, &il, &iu,
 983             &abstol, &m, (double*)evals.data, (double*)evects.data, &ldv,
 984             &idummy, &work1, &lwork, &iwork1, &liwork, &info );
 985         assert( info == 0 );
 986
 987         lwork = cvRound(work1);
 988         liwork = iwork1;
 989         buf.allocate((lwork + n*n + (copy_evals ? n : 0))*elem_size +
 990                      (liwork+2*n+1)*sizeof(integer));
 991         Mat a(n, n, type, (uchar*)buf);
 992         work = a.data + n*n*elem_size;
 993
 994         if( copy_evals )
 995             s = (double*)(work + lwork*elem_size);
 996         else
 997             s = (double*)evals.data;
 998
 999         iwork = (integer*)cvAlignPtr(work + (lwork + (copy_evals ? n : 0))*elem_size, sizeof(integer));
1000         isupport = iwork + liwork;
1001
1002         dsyevr_(job, range, L, &n, (double*)src.data, &lda, &dummy, &dummy,
1003             &il, &iu, &abstol, &m, s, (double*)evects.data,
1004             &ldv, isupport, (double*)work, &lwork, iwork, &liwork, &info );
1005         result = info == 0;
1006     }
1007
1008     if( copy_evals )
1009         Mat(evals.rows, evals.cols, type, work + lwork*elem_size).copyTo(evals);
1010
1011     if( il < n + 1 && n > 20 ) {
1012         int nVV = iu - il + 1;
1013         if( computeEvects ) {
1014             Mat flipme = evects.rowRange(0, nVV);
1015             flip(flipme, flipme, 0);
1016             flipme = evals.rowRange(0, nVV);
1017             flip(flipme, flipme, 0);
1018         }
1019     } else {
1020         flip(evals, evals, 0);
1021         if( computeEvects )
1022             flip(evects, evects, 0);
1023     }
1024
1025     return result;
1026 }
1027
1028 bool eigen( const Mat& src, Mat& evals, int lowindex, int highindex )
1029 {
1030     Mat evects;
1031     return eigen(src, evals, evects, false, lowindex, highindex);
1032 }
1033
1034 bool eigen( const Mat& src, Mat& evals, Mat& evects, int lowindex,
1035             int highindex )
1036 {
1037     return eigen(src, evals, evects, true, lowindex, highindex);
1038 }
1039
1040
1041
1042 /* y[0:m,0:n] += diag(a[0:1,0:m]) * x[0:m,0:n] */
1043 template<typename T1, typename T2, typename T3> static void
1044 MatrAXPY( int m, int n, const T1* x, int dx,
1045           const T2* a, int inca, T3* y, int dy )
1046 {
1047     int i, j;
1048     for( i = 0; i < m; i++, x += dx, y += dy )
1049     {
1050         T2 s = a[i*inca];
1051         for( j = 0; j <= n - 4; j += 4 )
1052         {
1053             T3 t0 = (T3)(y[j]   + s*x[j]);
1054             T3 t1 = (T3)(y[j+1] + s*x[j+1]);
1055             y[j]   = t0;
1056             y[j+1] = t1;
1057             t0 = (T3)(y[j+2] + s*x[j+2]);
1058             t1 = (T3)(y[j+3] + s*x[j+3]);
1059             y[j+2] = t0;
1060             y[j+3] = t1;
1061         }
1062
1063         for( ; j < n; j++ )
1064             y[j] = (T3)(y[j] + s*x[j]);
1065     }
1066 }
1067
1068 template<typename T> static void
1069 SVBkSb( int m, int n, const T* w, int incw,
1070         const T* u, int ldu, int uT,
1071         const T* v, int ldv, int vT,
1072         const T* b, int ldb, int nb,
1073         T* x, int ldx, double* buffer, T eps )
1074 {
1075     double threshold = 0;
1076     int udelta0 = uT ? ldu : 1, udelta1 = uT ? 1 : ldu;
1077     int vdelta0 = vT ? ldv : 1, vdelta1 = vT ? 1 : ldv;
1078     int i, j, nm = std::min(m, n);
1079
1080     if( !b )
1081         nb = m;
1082
1083     for( i = 0; i < n; i++ )
1084         for( j = 0; j < nb; j++ )
1085             x[i*ldx + j] = 0;
1086
1087     for( i = 0; i < nm; i++ )
1088         threshold += w[i*incw];
1089     threshold *= eps;
1090
1091     // v * inv(w) * uT * b
1092     for( i = 0; i < nm; i++, u += udelta0, v += vdelta0 )
1093     {
1094         double wi = w[i*incw];
1095         if( wi <= threshold )
1096             continue;
1097         wi = 1/wi;
1098
1099         if( nb == 1 )
1100         {
1101             double s = 0;
1102             if( b )
1103                 for( j = 0; j < m; j++ )
1104                     s += u[j*udelta1]*b[j*ldb];
1105             else
1106                 s = u[0];
1107             s *= wi;
1108
1109             for( j = 0; j < n; j++ )
1110                 x[j*ldx] = (T)(x[j*ldx] + s*v[j*vdelta1]);
1111         }
1112         else
1113         {
1114             if( b )
1115             {
1116                 for( j = 0; j < nb; j++ )
1117                     buffer[j] = 0;
1118                 MatrAXPY( m, nb, b, ldb, u, udelta1, buffer, 0 );
1119                 for( j = 0; j < nb; j++ )
1120                     buffer[j] *= wi;
1121             }
1122             else
1123             {
1124                 for( j = 0; j < nb; j++ )
1125                     buffer[j] = u[j*udelta1]*wi;
1126             }
1127             MatrAXPY( n, nb, buffer, 0, v, vdelta1, x, ldx );
1128         }
1129     }
1130 }
1131
1132
1133 SVD& SVD::operator ()(const Mat& a, int flags)
1134 {
1135     integer m = a.rows, n = a.cols, mn = std::max(m, n), nm = std::min(m, n);
1136     int type = a.type(), elem_size = (int)a.elemSize();
1137
1138     if( flags & NO_UV )
1139     {
1140         u.release();
1141         vt.release();
1142     }
1143     else
1144     {
1145         u.create( (int)m, (int)((flags & FULL_UV) ? m : nm), type );
1146         vt.create( (int)((flags & FULL_UV) ? n : nm), n, type );
1147     }
1148
1149     w.create(nm, 1, type);
1150
1151     Mat _a = a;
1152     int a_ofs = 0, work_ofs=0, iwork_ofs=0, buf_size = 0;
1153     bool temp_a = false;
1154     double u1=0, v1=0, work1=0;
1155     float uf1=0, vf1=0, workf1=0;
1156     integer lda, ldu, ldv, lwork=-1, iwork1=0, info=0, *iwork=0;
1157     char mode[] = {u.data || vt.data ? 'S' : 'N', '\0'};
1158
1159     if( m != n && !(flags & NO_UV) && (flags & FULL_UV) )
1160         mode[0] = 'A';
1161
1162     if( !(flags & MODIFY_A) )
1163     {
1164         if( mode[0] == 'N' || mode[0] == 'A' )
1165             temp_a = true;
1166         else if( ((vt.data && a.size() == vt.size()) || (u.data && a.size() == u.size())) &&
1167                   mode[0] == 'S' )
1168             mode[0] = 'O';
1169     }
1170
1171     lda = a.cols;
1172     ldv = ldu = mn;
1173
1174     if( type == CV_32F )
1175     {
1176         sgesdd_(mode, &n, &m, (float*)a.data, &lda, (float*)w.data,
1177             &vf1, &ldv, &uf1, &ldu, &workf1, &lwork, &iwork1, &info );
1178         lwork = cvRound(workf1);
1179     }
1180     else
1181     {
1182         dgesdd_(mode, &n, &m, (double*)a.data, &lda, (double*)w.data,
1183             &v1, &ldv, &u1, &ldu, &work1, &lwork, &iwork1, &info );
1184         lwork = cvRound(work1);
1185     }
1186
1187     assert(info == 0);
1188     if( temp_a )
1189     {
1190         a_ofs = buf_size;
1191         buf_size += n*m*elem_size;
1192     }
1193     work_ofs = buf_size;
1194     buf_size += lwork*elem_size;
1195     buf_size = cvAlign(buf_size, sizeof(iwork[0]));
1196     iwork_ofs = buf_size;
1197     buf_size += 8*nm*sizeof(integer);
1198
1199     AutoBuffer<uchar> buf(buf_size);
1200     uchar* buffer = (uchar*)buf;
1201
1202     if( temp_a )
1203     {
1204         _a = Mat(a.rows, a.cols, type, buffer );
1205         a.copyTo(_a);
1206     }
1207
1208     if( !(flags & MODIFY_A) && !temp_a )
1209     {
1210         if( vt.data && a.size() == vt.size() )
1211         {
1212             a.copyTo(vt);
1213             _a = vt;
1214         }
1215         else if( u.data && a.size() == u.size() )
1216         {
1217             a.copyTo(u);
1218             _a = u;
1219         }
1220     }
1221
1222     if( mode[0] != 'N' )
1223     {
1224         ldv = (int)(vt.step ? vt.step/elem_size : vt.cols);
1225         ldu = (int)(u.step ? u.step/elem_size : u.cols);
1226     }
1227
1228     lda = (int)(_a.step ? _a.step/elem_size : _a.cols);
1229     if( type == CV_32F )
1230     {
1231         sgesdd_(mode, &n, &m, (float*)_a.data, &lda, (float*)w.data,
1232             (float*)vt.data, &ldv, (float*)u.data, &ldu,
1233             (float*)(buffer + work_ofs), &lwork, (integer*)(buffer + iwork_ofs), &info );
1234     }
1235     else
1236     {
1237         dgesdd_(mode, &n, &m, (double*)_a.data, &lda, (double*)w.data,
1238             (double*)vt.data, &ldv, (double*)u.data, &ldu,
1239             (double*)(buffer + work_ofs), &lwork, (integer*)(buffer + iwork_ofs), &info );
1240     }
1241     CV_Assert(info >= 0);
1242     if(info != 0)
1243     {
1244         u = Scalar(0.);
1245         vt = Scalar(0.);
1246         w = Scalar(0.);
1247     }
1248     return *this;
1249 }
1250
1251
1252 void SVD::backSubst( const Mat& rhs, Mat& dst ) const
1253 {
1254     int type = w.type(), esz = (int)w.elemSize();
1255     int m = u.rows, n = vt.cols, nb = rhs.data ? rhs.cols : m;
1256     AutoBuffer<double> buffer(nb);
1257     CV_Assert( u.data && vt.data && w.data );
1258
1259     if( rhs.data )
1260         CV_Assert( rhs.type() == type && rhs.rows == m );
1261
1262     dst.create( n, nb, type );
1263     if( type == CV_32F )
1264         SVBkSb(m, n, (float*)w.data, 1, (float*)u.data, (int)(u.step/esz), false,
1265             (float*)vt.data, (int)(vt.step/esz), true, (float*)rhs.data, (int)(rhs.step/esz),
1266             nb, (float*)dst.data, (int)(dst.step/esz), buffer, 10*FLT_EPSILON );
1267     else if( type == CV_64F )
1268         SVBkSb(m, n, (double*)w.data, 1, (double*)u.data, (int)(u.step/esz), false,
1269             (double*)vt.data, (int)(vt.step/esz), true, (double*)rhs.data, (int)(rhs.step/esz),
1270             nb, (double*)dst.data, (int)(dst.step/esz), buffer, 2*DBL_EPSILON );
1271     else
1272         CV_Error( CV_StsUnsupportedFormat, "" );
1273 }
1274
1275 }
1276
1277
1278 CV_IMPL double
1279 cvDet( const CvArr* arr )
1280 {
1281     return determinant(cv::cvarrToMat(arr));
1282 }
1283
1284
1285 CV_IMPL double
1286 cvInvert( const CvArr* srcarr, CvArr* dstarr, int method )
1287 {
1288     cv::Mat src = cv::cvarrToMat(srcarr), dst = cv::cvarrToMat(dstarr);
1289
1290     CV_Assert( src.type() == dst.type() && src.rows == dst.cols && src.cols == dst.rows );
1291     return cv::invert( src, dst, method == CV_CHOLESKY ? cv::DECOMP_CHOLESKY :
1292         method == CV_SVD || method == CV_SVD_SYM ? cv::DECOMP_SVD : cv::DECOMP_LU );
1293 }
1294
1295
1296 CV_IMPL int
1297 cvSolve( const CvArr* Aarr, const CvArr* barr, CvArr* xarr, int method )
1298 {
1299     cv::Mat A = cv::cvarrToMat(Aarr), b = cv::cvarrToMat(barr), x = cv::cvarrToMat(xarr);
1300
1301     CV_Assert( A.type() == x.type() && A.cols == x.rows && x.cols == b.cols );
1302     return cv::solve( A, b, x, method == CV_CHOLESKY ? cv::DECOMP_CHOLESKY :
1303         method == CV_SVD || method == CV_SVD_SYM ? cv::DECOMP_SVD :
1304         A.rows > A.cols ? cv::DECOMP_QR : cv::DECOMP_LU );
1305 }
1306
1307
1308 CV_IMPL void
1309 cvEigenVV( CvArr* srcarr, CvArr* evectsarr, CvArr* evalsarr, double,
1310            int lowindex, int highindex)
1311 {
1312     cv::Mat src = cv::cvarrToMat(srcarr), evals = cv::cvarrToMat(evalsarr);
1313     if( evectsarr )
1314     {
1315         cv::Mat evects = cv::cvarrToMat(evectsarr);
1316         eigen(src, evals, evects, lowindex, highindex);
1317     }
1318     else
1319         eigen(src, evals, lowindex, highindex);
1320 }
1321
1322
1323 CV_IMPL void
1324 cvSVD( CvArr* aarr, CvArr* warr, CvArr* uarr, CvArr* varr, int flags )
1325 {
1326     cv::Mat a = cv::cvarrToMat(aarr), w = cv::cvarrToMat(warr), u, v;
1327     int m = a.rows, n = a.cols, type = a.type(), mn = std::max(m, n), nm = std::min(m, n);
1328
1329     CV_Assert( w.type() == type &&
1330         (w.size() == cv::Size(nm,1) || w.size() == cv::Size(1, nm) ||
1331         w.size() == cv::Size(nm, nm) || w.size() == cv::Size(n, m)) );
1332
1333     cv::SVD svd;
1334
1335     if( w.size() == cv::Size(nm, 1) )
1336         svd.w = cv::Mat(nm, 1, type, w.data );
1337     else if( w.isContinuous() )
1338         svd.w = w;
1339
1340     if( uarr )
1341     {
1342         u = cv::cvarrToMat(uarr);
1343         CV_Assert( u.type() == type );
1344         svd.u = u;
1345     }
1346
1347     if( varr )
1348     {
1349         v = cv::cvarrToMat(varr);
1350         CV_Assert( v.type() == type );
1351         svd.vt = v;
1352     }
1353
1354     svd(a, ((flags & CV_SVD_MODIFY_A) ? cv::SVD::MODIFY_A : 0) |
1355         ((!svd.u.data && !svd.vt.data) ? cv::SVD::NO_UV : 0) |
1356         ((m != n && (svd.u.size() == cv::Size(mn, mn) ||
1357         svd.vt.size() == cv::Size(mn, mn))) ? cv::SVD::FULL_UV : 0));
1358
1359     if( u.data )
1360     {
1361         if( flags & CV_SVD_U_T )
1362             cv::transpose( svd.u, u );
1363         else if( u.data != svd.u.data )
1364         {
1365             CV_Assert( u.size() == svd.u.size() );
1366             svd.u.copyTo(u);
1367         }
1368     }
1369
1370     if( v.data )
1371     {
1372         if( !(flags & CV_SVD_V_T) )
1373             cv::transpose( svd.vt, v );
1374         else if( v.data != svd.vt.data )
1375         {
1376             CV_Assert( v.size() == svd.vt.size() );
1377             svd.vt.copyTo(v);
1378         }
1379     }
1380
1381     if( w.data != svd.w.data )
1382     {
1383         if( w.size() == svd.w.size() )
1384             svd.w.copyTo(w);
1385         else
1386         {
1387             w = cv::Scalar(0);
1388             cv::Mat wd = w.diag();
1389             svd.w.copyTo(wd);
1390         }
1391     }
1392 }
1393
1394
1395 CV_IMPL void
1396 cvSVBkSb( const CvArr* warr, const CvArr* uarr,
1397           const CvArr* varr, const CvArr* rhsarr,
1398           CvArr* dstarr, int flags )
1399 {
1400     cv::Mat w = cv::cvarrToMat(warr), u = cv::cvarrToMat(uarr),
1401         v = cv::cvarrToMat(varr), rhs, dst = cv::cvarrToMat(dstarr);
1402     int type = w.type();
1403     bool uT = (flags & CV_SVD_U_T) != 0, vT = (flags & CV_SVD_V_T) != 0;
1404     int m = !uT ? u.rows : u.cols;
1405     int n = vT ? v.cols : v.rows;
1406     int nm = std::min(n, m), nb;
1407     int esz = (int)w.elemSize();
1408     int incw = w.size() == cv::Size(nm, 1) ? 1 : (int)(w.step/esz) + (w.cols > 1 && w.rows > 1);
1409
1410     CV_Assert( type == u.type() && type == v.type() &&
1411         type == dst.type() && dst.rows == n &&
1412         (!uT ? u.cols : u.rows) >= nm && (vT ? v.rows : v.cols) >= nm &&
1413         (w.size() == cv::Size(nm, 1) || w.size() == cv::Size(1, nm) ||
1414         w.size() == cv::Size(nm, nm) || w.size() == cv::Size(n, m)));
1415
1416     if( rhsarr )
1417     {
1418         rhs = cv::cvarrToMat(rhsarr);
1419         nb = rhs.cols;
1420         CV_Assert( type == rhs.type() );
1421     }
1422     else
1423         nb = m;
1424
1425     CV_Assert( dst.cols == nb );
1426     cv::AutoBuffer<double> buffer(nb);
1427
1428     if( type == CV_32F )
1429         cv::SVBkSb(m, n, (float*)w.data, incw, (float*)u.data, (int)(u.step/esz), uT,
1430             (float*)v.data, (int)(v.step/esz), vT, (float*)rhs.data, (int)(rhs.step/esz),
1431             nb, (float*)dst.data, (int)(dst.step/esz), buffer, 2*FLT_EPSILON );
1432     else
1433         cv::SVBkSb(m, n, (double*)w.data, incw, (double*)u.data, (int)(u.step/esz), uT,
1434             (double*)v.data, (int)(v.step/esz), vT, (double*)rhs.data, (int)(rhs.step/esz),
1435             nb, (double*)dst.data, (int)(dst.step/esz), buffer, 2*DBL_EPSILON );
1436 }