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 <TooN/lapack.h>
00036
00037 #include <TooN/TooN.h>
00038
00039 namespace TooN {
00040
00041 static const double symeigen_condition_no=1e9;
00042
00043 template <int Size> struct ComputeSymEigen {
00044
00045 template<typename P, typename B>
00046 static inline void compute(const Matrix<Size,Size,P, B>& m, Matrix<Size,Size,P> & evectors, Vector<Size, P>& evalues) {
00047 evectors = m;
00048 int N = evalues.size();
00049 int lda = evalues.size();
00050 int info;
00051 int lwork=-1;
00052 P size;
00053
00054
00055 syev_((char*)"V",(char*)"U",&N,&evectors[0][0],&lda,&evalues[0], &size,&lwork,&info);
00056 lwork = int(size);
00057 Vector<Dynamic, P> WORK(lwork);
00058
00059
00060 syev_((char*)"V",(char*)"U",&N,&evectors[0][0],&lda,&evalues[0], &WORK[0],&lwork,&info);
00061
00062 if(info!=0){
00063 std::cerr << "In SymEigen<"<<Size<<">: " << info
00064 << " off-diagonal elements of an intermediate tridiagonal form did not converge to zero." << std::endl
00065 << "M = " << m << std::endl;
00066 }
00067 }
00068 };
00069
00070 template <> struct ComputeSymEigen<2> {
00071
00072 template<typename P, typename B>
00073 static inline void compute(const Matrix<2,2,P,B>& m, Matrix<2,2,P>& eig, Vector<2, P>& ev) {
00074 double trace = m[0][0] + m[1][1];
00075 double det = m[0][0]*m[1][1] - m[0][1]*m[1][0];
00076 double disc = trace*trace - 4 * det;
00077 assert(disc>=0);
00078 double root_disc = sqrt(disc);
00079 ev[0] = 0.5 * (trace - root_disc);
00080 ev[1] = 0.5 * (trace + root_disc);
00081 double a = m[0][0] - ev[0];
00082 double b = m[0][1];
00083 double magsq = a*a + b*b;
00084 if (magsq == 0) {
00085 eig[0][0] = 1.0;
00086 eig[0][1] = 0;
00087 } else {
00088 eig[0][0] = -b;
00089 eig[0][1] = a;
00090 eig[0] *= 1.0/sqrt(magsq);
00091 }
00092 eig[1][0] = -eig[0][1];
00093 eig[1][1] = eig[0][0];
00094 }
00095 };
00096
00123 template <int Size=Dynamic, typename Precision = double>
00124 class SymEigen {
00125 public:
00126 inline SymEigen(){}
00129 template<int R, int C, typename B>
00130 inline SymEigen(const Matrix<R, C, Precision, B>& m) : my_evectors(m.num_rows(), m.num_cols()), my_evalues(m.num_rows()) {
00131 compute(m);
00132 }
00133
00135 template<int R, int C, typename B>
00136 inline void compute(const Matrix<R,C,Precision,B>& m){
00137 SizeMismatch<R, C>::test(m.num_rows(), m.num_cols());
00138 SizeMismatch<R, Size>::test(m.num_rows(), my_evectors.num_rows());
00139 ComputeSymEigen<Size>::compute(m, my_evectors, my_evalues);
00140 }
00141
00146 template <int S, typename P, typename B>
00147 Vector<Size, Precision> backsub(const Vector<S,P,B>& rhs) const {
00148 return (my_evectors.T() * diagmult(get_inv_diag(symeigen_condition_no),(my_evectors * rhs)));
00149 }
00150
00155 template <int R, int C, typename P, typename B>
00156 Matrix<Size,C, Precision> backsub(const Matrix<R,C,P,B>& rhs) const {
00157 return (my_evectors.T() * diagmult(get_inv_diag(symeigen_condition_no),(my_evectors * rhs)));
00158 }
00159
00165 Matrix<Size, Size, Precision> get_pinv(const double condition=symeigen_condition_no) const {
00166 return my_evectors.T() * diagmult(get_inv_diag(condition),my_evectors);
00167 }
00168
00175 Vector<Size, Precision> get_inv_diag(const double condition) const {
00176 Precision max_diag = -my_evalues[0] > my_evalues[my_evalues.size()-1] ? -my_evalues[0]:my_evalues[my_evalues.size()-1];
00177 Vector<Size, Precision> invdiag(my_evalues.size());
00178 for(int i=0; i<my_evalues.size(); i++){
00179 if(fabs(my_evalues[i]) * condition > max_diag) {
00180 invdiag[i] = 1/my_evalues[i];
00181 } else {
00182 invdiag[i]=0;
00183 }
00184 }
00185 return invdiag;
00186 }
00187
00193 Matrix<Size,Size,Precision>& get_evectors() {return my_evectors;}
00194 const Matrix<Size,Size,Precision>& get_evectors() const {return my_evectors;}
00195
00196
00200 Vector<Size, Precision>& get_evalues() {return my_evalues;}
00201 const Vector<Size, Precision>& get_evalues() const {return my_evalues;}
00202
00204 bool is_posdef() const {
00205 for (int i = 0; i < my_evalues.size(); ++i) {
00206 if (my_evalues[i] <= 0.0)
00207 return false;
00208 }
00209 return true;
00210 }
00211
00213 bool is_negdef() const {
00214 for (int i = 0; i < my_evalues.size(); ++i) {
00215 if (my_evalues[i] >= 0.0)
00216 return false;
00217 }
00218 return true;
00219 }
00220
00222 Precision get_determinant () const {
00223 Precision det = 1.0;
00224 for (int i = 0; i < my_evalues.size(); ++i) {
00225 det *= my_evalues[i];
00226 }
00227 return det;
00228 }
00229
00230 private:
00231
00232 Matrix<Size,Size,Precision> my_evectors;
00233
00234 Vector<Size, Precision> my_evalues;
00235 };
00236
00237 }
00238
00239 #endif