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 68static CvStatus CV_STDCALL 69icvJacobiEigens_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 219static CvStatus CV_STDCALL 220icvJacobiEigens_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 369CV_IMPL void 370cvEigenVV( 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 */ 432