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

   1 /*M///////////////////////////////////////////////////////////////////////////////////////
   2 //
   3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
   4 //
   5 //  By downloading, copying, installing or using the software you agree to this license.
   6 //  If you do not agree to this license, do not download, install,
   7 //  copy or use the software.
   8 //
   9 //
  10 //                        Intel License Agreement
  11 //                For Open Source Computer Vision Library
  12 //
  13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
  14 // Third party copyrights are property of their respective owners.
  15 //
  16 // Redistribution and use in source and binary forms, with or without modification,
  17 // are permitted provided that the following conditions are met:
  18 //
  19 //   * Redistribution's of source code must retain the above copyright notice,
  20 //     this list of conditions and the following disclaimer.
  21 //
  22 //   * Redistribution's in binary form must reproduce the above copyright notice,
  23 //     this list of conditions and the following disclaimer in the documentation
  24 //     and/or other materials provided with the distribution.
  25 //
  26 //   * The name of Intel Corporation may not be used to endorse or promote products
  27 //     derived from this software without specific prior written permission.
  28 //
  29 // This software is provided by the copyright holders and contributors "as is" and
  30 // any express or implied warranties, including, but not limited to, the implied
  31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
  32 // In no event shall the Intel Corporation or contributors be liable for any direct,
  33 // indirect, incidental, special, exemplary, or consequential damages
  34 // (including, but not limited to, procurement of substitute goods or services;
  35 // loss of use, data, or profits; or business interruption) however caused
  36 // and on any theory of liability, whether in contract, strict liability,
  37 // or tort (including negligence or otherwise) arising in any way out of
  38 // the use of this software, even if advised of the possibility of such damage.
  39 //
  40 //M*/
  41
  42 #include "_cxcore.h"
  43
  44 /*F///////////////////////////////////////////////////////////////////////////////////////
  45 //    Names:      icvJacobiEigens_32f, icvJacobiEigens_64d
  46 //    Purpose:    Eigenvalues & eigenvectors calculation of a symmetric matrix:
  47 //                A Vi  =  Ei Vi
  48 //    Context:
  49 //    Parameters: A(n, n) - source symmetric matrix (n - rows & columns number),
  50 //                V(n, n) - matrix of its eigenvectors
  51 //                          (i-th row is an eigenvector Vi),
  52 //                E(n)    - vector of its eigenvalues
  53 //                          (i-th element is an eigenvalue Ei),
  54 //                eps     - accuracy of diagonalization.
  55 //
  56 //    Returns:
  57 //    CV_NO_ERROR or error code
  58 //    Notes:
  59 //        1. The functions destroy source matrix A, so if you need it further, you
  60 //           have to copy it before the processing.
  61 //        2. Eigenvalies and eigenvectors are sorted in Ei absolute value descending.
  62 //        3. Calculation time depends on eps value. If the time isn't very important,
  63 //           we recommend to set eps = 0.
  64 //F*/
  65
  66 /*=========================== Single precision function ================================*/
  67
  68 static CvStatus CV_STDCALL
  69 icvJacobiEigens_32f(float *A, float *V, float *E, int n, float eps)
  70 {
  71     int i, j, k, ind, iters = 0;
  72     float *AA = A, *VV = V;
  73     double Amax, anorm = 0, ax;
  74
  75     if( A == NULL || V == NULL || E == NULL )
  76         return CV_NULLPTR_ERR;
  77     if( n <= 0 )
  78         return CV_BADSIZE_ERR;
  79     if( eps < DBL_EPSILON )
  80         eps = DBL_EPSILON;
  81
  82     /*-------- Prepare --------*/
  83     for( i = 0; i < n; i++, VV += n, AA += n )
  84     {
  85         for( j = 0; j < i; j++ )
  86         {
  87             double Am = AA[j];
  88
  89             anorm += Am * Am;
  90         }
  91         for( j = 0; j < n; j++ )
  92             VV[j] = 0.f;
  93         VV[i] = 1.f;
  94     }
  95
  96     anorm = sqrt( anorm + anorm );
  97     ax = anorm * eps / n;
  98     Amax = anorm;
  99
 100     while( Amax > ax && iters++ < 100 )
 101     {
 102         Amax /= n;
 103         do                      /* while (ind) */
 104         {
 105             int p, q;
 106             float *V1 = V, *A1 = A;
 107
 108             ind = 0;
 109             for( p = 0; p < n - 1; p++, A1 += n, V1 += n )
 110             {
 111                 float *A2 = A + n * (p + 1), *V2 = V + n * (p + 1);
 112
 113                 for( q = p + 1; q < n; q++, A2 += n, V2 += n )
 114                 {
 115                     double x, y, c, s, c2, s2, a;
 116                     float *A3, Apq = A1[q], App, Aqq, Aip, Aiq, Vpi, Vqi;
 117
 118                     if( fabs( Apq ) < Amax )
 119                         continue;
 120
 121                     ind = 1;
 122
 123                     /*---- Calculation of rotation angle's sine & cosine ----*/
 124                     App = A1[p];
 125                     Aqq = A2[q];
 126                     y = 5.0e-1 * (App - Aqq);
 127                     x = -Apq / sqrt( (double)Apq * Apq + (double)y * y );
 128                     if( y < 0.0 )
 129                         x = -x;
 130                     s = x / sqrt( 2.0 * (1.0 + sqrt( 1.0 - (double)x * x )));
 131                     s2 = s * s;
 132                     c = sqrt( 1.0 - s2 );
 133                     c2 = c * c;
 134                     a = 2.0 * Apq * c * s;
 135
 136                     /*---- Apq annulation ----*/
 137                     A3 = A;
 138                     for( i = 0; i < p; i++, A3 += n )
 139                     {
 140                         Aip = A3[p];
 141                         Aiq = A3[q];
 142                         Vpi = V1[i];
 143                         Vqi = V2[i];
 144                         A3[p] = (float) (Aip * c - Aiq * s);
 145                         A3[q] = (float) (Aiq * c + Aip * s);
 146                         V1[i] = (float) (Vpi * c - Vqi * s);
 147                         V2[i] = (float) (Vqi * c + Vpi * s);
 148                     }
 149                     for( ; i < q; i++, A3 += n )
 150                     {
 151                         Aip = A1[i];
 152                         Aiq = A3[q];
 153                         Vpi = V1[i];
 154                         Vqi = V2[i];
 155                         A1[i] = (float) (Aip * c - Aiq * s);
 156                         A3[q] = (float) (Aiq * c + Aip * s);
 157                         V1[i] = (float) (Vpi * c - Vqi * s);
 158                         V2[i] = (float) (Vqi * c + Vpi * s);
 159                     }
 160                     for( ; i < n; i++ )
 161                     {
 162                         Aip = A1[i];
 163                         Aiq = A2[i];
 164                         Vpi = V1[i];
 165                         Vqi = V2[i];
 166                         A1[i] = (float) (Aip * c - Aiq * s);
 167                         A2[i] = (float) (Aiq * c + Aip * s);
 168                         V1[i] = (float) (Vpi * c - Vqi * s);
 169                         V2[i] = (float) (Vqi * c + Vpi * s);
 170                     }
 171                     A1[p] = (float) (App * c2 + Aqq * s2 - a);
 172                     A2[q] = (float) (App * s2 + Aqq * c2 + a);
 173                     A1[q] = A2[p] = 0.0f;
 174                 }               /*q */
 175             }                   /*p */
 176         }
 177         while( ind );
 178         Amax /= n;
 179     }                           /* while ( Amax > ax ) */
 180
 181     for( i = 0, k = 0; i < n; i++, k += n + 1 )
 182         E[i] = A[k];
 183     /*printf(" M = %d\n", M); */
 184
 185     /* -------- ordering -------- */
 186     for( i = 0; i < n; i++ )
 187     {
 188         int m = i;
 189         float Em = (float) fabs( E[i] );
 190
 191         for( j = i + 1; j < n; j++ )
 192         {
 193             float Ej = (float) fabs( E[j] );
 194
 195             m = (Em < Ej) ? j : m;
 196             Em = (Em < Ej) ? Ej : Em;
 197         }
 198         if( m != i )
 199         {
 200             int l;
 201             float b = E[i];
 202
 203             E[i] = E[m];
 204             E[m] = b;
 205             for( j = 0, k = i * n, l = m * n; j < n; j++, k++, l++ )
 206             {
 207                 b = V[k];
 208                 V[k] = V[l];
 209                 V[l] = b;
 210             }
 211         }
 212     }
 213
 214     return CV_NO_ERR;
 215 }
 216
 217 /*=========================== Double precision function ================================*/
 218
 219 static CvStatus CV_STDCALL
 220 icvJacobiEigens_64d(double *A, double *V, double *E, int n, double eps)
 221 {
 222     int i, j, k, p, q, ind, iters = 0;
 223     double *A1 = A, *V1 = V, *A2 = A, *V2 = V;
 224     double Amax = 0.0, anorm = 0.0, ax;
 225
 226     if( A == NULL || V == NULL || E == NULL )
 227         return CV_NULLPTR_ERR;
 228     if( n <= 0 )
 229         return CV_BADSIZE_ERR;
 230     if( eps < DBL_EPSILON )
 231         eps = DBL_EPSILON;
 232
 233     /*-------- Prepare --------*/
 234     for( i = 0; i < n; i++, V1 += n, A1 += n )
 235     {
 236         for( j = 0; j < i; j++ )
 237         {
 238             double Am = A1[j];
 239
 240             anorm += Am * Am;
 241         }
 242         for( j = 0; j < n; j++ )
 243             V1[j] = 0.0;
 244         V1[i] = 1.0;
 245     }
 246
 247     anorm = sqrt( anorm + anorm );
 248     ax = anorm * eps / n;
 249     Amax = anorm;
 250
 251     while( Amax > ax && iters++ < 100 )
 252     {
 253         Amax /= n;
 254         do                      /* while (ind) */
 255         {
 256             ind = 0;
 257             A1 = A;
 258             V1 = V;
 259             for( p = 0; p < n - 1; p++, A1 += n, V1 += n )
 260             {
 261                 A2 = A + n * (p + 1);
 262                 V2 = V + n * (p + 1);
 263                 for( q = p + 1; q < n; q++, A2 += n, V2 += n )
 264                 {
 265                     double x, y, c, s, c2, s2, a;
 266                     double *A3, Apq, App, Aqq, App2, Aqq2, Aip, Aiq, Vpi, Vqi;
 267
 268                     if( fabs( A1[q] ) < Amax )
 269                         continue;
 270                     Apq = A1[q];
 271
 272                     ind = 1;
 273
 274                     /*---- Calculation of rotation angle's sine & cosine ----*/
 275                     App = A1[p];
 276                     Aqq = A2[q];
 277                     y = 5.0e-1 * (App - Aqq);
 278                     x = -Apq / sqrt( Apq * Apq + (double)y * y );
 279                     if( y < 0.0 )
 280                         x = -x;
 281                     s = x / sqrt( 2.0 * (1.0 + sqrt( 1.0 - (double)x * x )));
 282                     s2 = s * s;
 283                     c = sqrt( 1.0 - s2 );
 284                     c2 = c * c;
 285                     a = 2.0 * Apq * c * s;
 286
 287                     /*---- Apq annulation ----*/
 288                     A3 = A;
 289                     for( i = 0; i < p; i++, A3 += n )
 290                     {
 291                         Aip = A3[p];
 292                         Aiq = A3[q];
 293                         Vpi = V1[i];
 294                         Vqi = V2[i];
 295                         A3[p] = Aip * c - Aiq * s;
 296                         A3[q] = Aiq * c + Aip * s;
 297                         V1[i] = Vpi * c - Vqi * s;
 298                         V2[i] = Vqi * c + Vpi * s;
 299                     }
 300                     for( ; i < q; i++, A3 += n )
 301                     {
 302                         Aip = A1[i];
 303                         Aiq = A3[q];
 304                         Vpi = V1[i];
 305                         Vqi = V2[i];
 306                         A1[i] = Aip * c - Aiq * s;
 307                         A3[q] = Aiq * c + Aip * s;
 308                         V1[i] = Vpi * c - Vqi * s;
 309                         V2[i] = Vqi * c + Vpi * s;
 310                     }
 311                     for( ; i < n; i++ )
 312                     {
 313                         Aip = A1[i];
 314                         Aiq = A2[i];
 315                         Vpi = V1[i];
 316                         Vqi = V2[i];
 317                         A1[i] = Aip * c - Aiq * s;
 318                         A2[i] = Aiq * c + Aip * s;
 319                         V1[i] = Vpi * c - Vqi * s;
 320                         V2[i] = Vqi * c + Vpi * s;
 321                     }
 322                     App2 = App * c2 + Aqq * s2 - a;
 323                     Aqq2 = App * s2 + Aqq * c2 + a;
 324                     A1[p] = App2;
 325                     A2[q] = Aqq2;
 326                     A1[q] = A2[p] = 0.0;
 327                 }               /*q */
 328             }                   /*p */
 329         }
 330         while( ind );
 331     }                           /* while ( Amax > ax ) */
 332
 333     for( i = 0, k = 0; i < n; i++, k += n + 1 )
 334         E[i] = A[k];
 335
 336     /* -------- ordering -------- */
 337     for( i = 0; i < n; i++ )
 338     {
 339         int m = i;
 340         double Em = fabs( E[i] );
 341
 342         for( j = i + 1; j < n; j++ )
 343         {
 344             double Ej = fabs( E[j] );
 345
 346             m = (Em < Ej) ? j : m;
 347             Em = (Em < Ej) ? Ej : Em;
 348         }
 349         if( m != i )
 350         {
 351             int l;
 352             double b = E[i];
 353
 354             E[i] = E[m];
 355             E[m] = b;
 356             for( j = 0, k = i * n, l = m * n; j < n; j++, k++, l++ )
 357             {
 358                 b = V[k];
 359                 V[k] = V[l];
 360                 V[l] = b;
 361             }
 362         }
 363     }
 364
 365     return CV_NO_ERR;
 366 }
 367
 368
 369 CV_IMPL void
 370 cvEigenVV( CvArr* srcarr, CvArr* evectsarr, CvArr* evalsarr, double eps )
 371 {
 372
 373     CV_FUNCNAME( "cvEigenVV" );
 374
 375     __BEGIN__;
 376
 377     CvMat sstub, *src = (CvMat*)srcarr;
 378     CvMat estub1, *evects = (CvMat*)evectsarr;
 379     CvMat estub2, *evals = (CvMat*)evalsarr;
 380
 381     if( !CV_IS_MAT( src ))
 382         CV_CALL( src = cvGetMat( src, &sstub ));
 383
 384     if( !CV_IS_MAT( evects ))
 385         CV_CALL( evects = cvGetMat( evects, &estub1 ));
 386
 387     if( !CV_IS_MAT( evals ))
 388         CV_CALL( evals = cvGetMat( evals, &estub2 ));
 389
 390     if( src->cols != src->rows )
 391         CV_ERROR( CV_StsUnmatchedSizes, "source is not quadratic matrix" );
 392
 393     if( !CV_ARE_SIZES_EQ( src, evects) )
 394         CV_ERROR( CV_StsUnmatchedSizes, "eigenvectors matrix has inappropriate size" );
 395
 396     if( (evals->rows != src->rows || evals->cols != 1) &&
 397         (evals->cols != src->rows || evals->rows != 1))
 398         CV_ERROR( CV_StsBadSize, "eigenvalues vector has inappropriate size" );
 399
 400     if( !CV_ARE_TYPES_EQ( src, evects ) || !CV_ARE_TYPES_EQ( src, evals ))
 401         CV_ERROR( CV_StsUnmatchedFormats,
 402         "input matrix, eigenvalues and eigenvectors must have the same type" );
 403
 404     if( !CV_IS_MAT_CONT( src->type & evals->type & evects->type ))
 405         CV_ERROR( CV_BadStep, "all the matrices must be continuous" );
 406
 407     if( CV_MAT_TYPE(src->type) == CV_32FC1 )
 408     {
 409         IPPI_CALL( icvJacobiEigens_32f( src->data.fl,
 410                                         evects->data.fl,
 411                                         evals->data.fl, src->cols, (float)eps ));
 412
 413     }
 414     else if( CV_MAT_TYPE(src->type) == CV_64FC1 )
 415     {
 416         IPPI_CALL( icvJacobiEigens_64d( src->data.db,
 417                                         evects->data.db,
 418                                         evals->data.db, src->cols, eps ));
 419     }
 420     else
 421     {
 422         CV_ERROR( CV_StsUnsupportedFormat, "Only 32fC1 and 64fC1 types are supported" );
 423     }
 424
 425     CV_CHECK_NANS( evects );
 426     CV_CHECK_NANS( evals );
 427
 428     __END__;
 429 }
 430
 431 /* End of file */