00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #ifndef __SYMEIGEN_H
00031 #define __SYMEIGEN_H
00032
00033 #include <iostream>
00034 #include <cassert>
00035 #include <cmath>
00036 #include <complex>
00037 #include <TooN/lapack.h>
00038
00039 #include <TooN/TooN.h>
00040
00041 namespace TooN {
00042 static const double root3=1.73205080756887729352744634150587236694280525381038062805580;
00043
00044 namespace Internal{
00045
00046 using std::swap;
00047
00048
00049 static const double symeigen_condition_no=1e9;
00050
00051
00052
00053
00054
00055
00056 template <int Size> struct ComputeSymEigen {
00057
00058
00059
00060
00061
00062
00063 template<int Rows, int Cols, typename P, typename B>
00064 static inline void compute(const Matrix<Rows,Cols,P, B>& m, Matrix<Size,Size,P> & evectors, Vector<Size, P>& evalues) {
00065
00066 SizeMismatch<Rows, Cols>::test(m.num_rows(), m.num_cols());
00067 SizeMismatch<Size, Rows>::test(m.num_rows(), evalues.size());
00068
00069
00070 evectors = m;
00071 int N = evalues.size();
00072 int lda = evalues.size();
00073 int info;
00074 int lwork=-1;
00075 P size;
00076
00077
00078 syev_((char*)"V",(char*)"U",&N,&evectors[0][0],&lda,&evalues[0], &size,&lwork,&info);
00079 lwork = int(size);
00080 Vector<Dynamic, P> WORK(lwork);
00081
00082
00083 syev_((char*)"V",(char*)"U",&N,&evectors[0][0],&lda,&evalues[0], &WORK[0],&lwork,&info);
00084
00085 if(info!=0){
00086 std::cerr << "In SymEigen<"<<Size<<">: " << info
00087 << " off-diagonal elements of an intermediate tridiagonal form did not converge to zero." << std::endl
00088 << "M = " << m << std::endl;
00089 }
00090 }
00091 };
00092
00093
00094
00095
00096
00097 template <> struct ComputeSymEigen<2> {
00098
00099
00100
00101
00102
00103
00104 template<typename P, typename B>
00105 static inline void compute(const Matrix<2,2,P,B>& m, Matrix<2,2,P>& eig, Vector<2, P>& ev) {
00106 double trace = m[0][0] + m[1][1];
00107 double det = m[0][0]*m[1][1] - m[0][1]*m[1][0];
00108 double disc = trace*trace - 4 * det;
00109 assert(disc>=0);
00110 using std::sqrt;
00111 double root_disc = sqrt(disc);
00112 ev[0] = 0.5 * (trace - root_disc);
00113 ev[1] = 0.5 * (trace + root_disc);
00114 double a = m[0][0] - ev[0];
00115 double b = m[0][1];
00116 double magsq = a*a + b*b;
00117 if (magsq == 0) {
00118 eig[0][0] = 1.0;
00119 eig[0][1] = 0;
00120 } else {
00121 eig[0][0] = -b;
00122 eig[0][1] = a;
00123 eig[0] *= 1.0/sqrt(magsq);
00124 }
00125 eig[1][0] = -eig[0][1];
00126 eig[1][1] = eig[0][0];
00127 }
00128 };
00129
00130
00131
00132
00133
00134 template <> struct ComputeSymEigen<3> {
00135
00136
00137
00138
00139
00140
00141 template<typename P, typename B>
00142 static inline void compute(const Matrix<3,3,P,B>& m, Matrix<3,3,P>& eig, Vector<3, P>& ev) {
00143
00144
00145
00146
00147
00148 const double& a11 = m[0][0];
00149 const double& a12 = m[0][1];
00150 const double& a13 = m[0][2];
00151
00152 const double& a22 = m[1][1];
00153 const double& a23 = m[1][2];
00154
00155 const double& a33 = m[2][2];
00156
00157
00158 double a = -a11-a22-a33;
00159 double b = a11*a22+a11*a33+a22*a33-a12*a12-a13*a13-a23*a23;
00160 double c = a11*(a23*a23)+(a13*a13)*a22+(a12*a12)*a33-a12*a13*a23*2.0-a11*a22*a33;
00161
00162
00163 double p = b - a*a/3;
00164 double q = c + (2*a*a*a - 9*a*b)/27;
00165
00166 double alpha = -q/2;
00167 double beta_descriminant = q*q/4 + p*p*p/27;
00168
00169
00170 double beta = sqrt(-beta_descriminant);
00171 double r2 = alpha*alpha - beta_descriminant;
00172
00173
00174
00175
00176
00177 double cuberoot_r = pow(r2, 1./6);
00178 double theta3 = atan2(beta, alpha)/3;
00179
00180 double A_plus_B = 2*cuberoot_r*cos(theta3);
00181 double A_minus_B = -2*cuberoot_r*sin(theta3);
00182
00183
00184 ev = makeVector(A_plus_B, -A_plus_B/2 + A_minus_B * sqrt(3)/2, -A_plus_B/2 - A_minus_B * sqrt(3)/2) - Ones * a/3;
00185
00186 if(ev[0] > ev[1])
00187 swap(ev[0], ev[1]);
00188 if(ev[1] > ev[2])
00189 swap(ev[1], ev[2]);
00190 if(ev[0] > ev[1])
00191 swap(ev[0], ev[1]);
00192
00193
00194 eig[0][0]=a12 * a23 - a13 * (a22 - ev[0]);
00195 eig[0][1]=a12 * a13 - a23 * (a11 - ev[0]);
00196 eig[0][2]=(a11-ev[0])*(a22-ev[0]) - a12*a12;
00197 normalize(eig[0]);
00198 eig[1][0]=a12 * a23 - a13 * (a22 - ev[1]);
00199 eig[1][1]=a12 * a13 - a23 * (a11 - ev[1]);
00200 eig[1][2]=(a11-ev[1])*(a22-ev[1]) - a12*a12;
00201 normalize(eig[1]);
00202 eig[2][0]=a12 * a23 - a13 * (a22 - ev[2]);
00203 eig[2][1]=a12 * a13 - a23 * (a11 - ev[2]);
00204 eig[2][2]=(a11-ev[2])*(a22-ev[2]) - a12*a12;
00205 normalize(eig[2]);
00206 }
00207 };
00208
00209 };
00210
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221
00222
00223
00224
00225
00226
00227
00228
00229
00230
00231
00232
00233
00234
00235
00236
00237
00238
00239
00240
00241
00242
00243
00244
00245
00246
00247
00248
00249
00250
00251
00252
00253
00254
00255
00256
00257
00258
00259 template <int Size=Dynamic, typename Precision = double>
00260 class SymEigen {
00261 public:
00262 inline SymEigen(){}
00263
00264
00265
00266
00267
00268 inline SymEigen(int m) : my_evectors(m,m), my_evalues(m) {}
00269
00270
00271
00272 template<int R, int C, typename B>
00273 inline SymEigen(const Matrix<R, C, Precision, B>& m) : my_evectors(m.num_rows(), m.num_cols()), my_evalues(m.num_rows()) {
00274 compute(m);
00275 }
00276
00277
00278 template<int R, int C, typename B>
00279 inline void compute(const Matrix<R,C,Precision,B>& m){
00280 SizeMismatch<R, C>::test(m.num_rows(), m.num_cols());
00281 SizeMismatch<R, Size>::test(m.num_rows(), my_evectors.num_rows());
00282 Internal::ComputeSymEigen<Size>::compute(m, my_evectors, my_evalues);
00283 }
00284
00285
00286
00287
00288
00289 template <int S, typename P, typename B>
00290 Vector<Size, Precision> backsub(const Vector<S,P,B>& rhs) const {
00291 return (my_evectors.T() * diagmult(get_inv_diag(Internal::symeigen_condition_no),(my_evectors * rhs)));
00292 }
00293
00294
00295
00296
00297
00298 template <int R, int C, typename P, typename B>
00299 Matrix<Size,C, Precision> backsub(const Matrix<R,C,P,B>& rhs) const {
00300 return (my_evectors.T() * diagmult(get_inv_diag(Internal::symeigen_condition_no),(my_evectors * rhs)));
00301 }
00302
00303
00304
00305
00306
00307
00308 Matrix<Size, Size, Precision> get_pinv(const double condition=Internal::symeigen_condition_no) const {
00309 return my_evectors.T() * diagmult(get_inv_diag(condition),my_evectors);
00310 }
00311
00312
00313
00314
00315
00316
00317
00318 Vector<Size, Precision> get_inv_diag(const double condition) const {
00319 Precision max_diag = -my_evalues[0] > my_evalues[my_evalues.size()-1] ? -my_evalues[0]:my_evalues[my_evalues.size()-1];
00320 Vector<Size, Precision> invdiag(my_evalues.size());
00321 for(int i=0; i<my_evalues.size(); i++){
00322 if(fabs(my_evalues[i]) * condition > max_diag) {
00323 invdiag[i] = 1/my_evalues[i];
00324 } else {
00325 invdiag[i]=0;
00326 }
00327 }
00328 return invdiag;
00329 }
00330
00331
00332
00333
00334
00335
00336 Matrix<Size,Size,Precision>& get_evectors() {return my_evectors;}
00337
00338
00339
00340 const Matrix<Size,Size,Precision>& get_evectors() const {return my_evectors;}
00341
00342
00343
00344
00345
00346 Vector<Size, Precision>& get_evalues() {return my_evalues;}
00347
00348
00349 const Vector<Size, Precision>& get_evalues() const {return my_evalues;}
00350
00351
00352 bool is_posdef() const {
00353 for (int i = 0; i < my_evalues.size(); ++i) {
00354 if (my_evalues[i] <= 0.0)
00355 return false;
00356 }
00357 return true;
00358 }
00359
00360
00361 bool is_negdef() const {
00362 for (int i = 0; i < my_evalues.size(); ++i) {
00363 if (my_evalues[i] >= 0.0)
00364 return false;
00365 }
00366 return true;
00367 }
00368
00369
00370 Precision get_determinant () const {
00371 Precision det = 1.0;
00372 for (int i = 0; i < my_evalues.size(); ++i) {
00373 det *= my_evalues[i];
00374 }
00375 return det;
00376 }
00377
00378
00379
00380 Matrix<Size, Size, Precision> get_sqrtm () const {
00381 Vector<Size, Precision> diag_sqrt(my_evalues.size());
00382
00383
00384 for (int i = 0; i < my_evalues.size(); ++i) {
00385 diag_sqrt[i] = std::sqrt(my_evalues[i]);
00386 }
00387 return my_evectors.T() * diagmult(diag_sqrt, my_evectors);
00388 }
00389
00390
00391
00392
00393
00394
00395
00396 Matrix<Size, Size, Precision> get_isqrtm (const double condition=Internal::symeigen_condition_no) const {
00397 Vector<Size, Precision> diag_isqrt(my_evalues.size());
00398
00399
00400 Precision max_diag = -my_evalues[0] > my_evalues[my_evalues.size()-1] ? (-std::sqrt(my_evalues[0])):(std::sqrt(my_evalues[my_evalues.size()-1]));
00401
00402
00403 for (int i = 0; i < my_evalues.size(); ++i) {
00404 diag_isqrt[i] = std::sqrt(my_evalues[i]);
00405 if(fabs(diag_isqrt[i]) * condition > max_diag) {
00406 diag_isqrt[i] = 1/diag_isqrt[i];
00407 } else {
00408 diag_isqrt[i] = 0;
00409 }
00410 }
00411 return my_evectors.T() * diagmult(diag_isqrt, my_evectors);
00412 }
00413
00414 private:
00415
00416 Matrix<Size,Size,Precision> my_evectors;
00417
00418 Vector<Size, Precision> my_evalues;
00419 };
00420
00421 }
00422
00423 #endif
00424