examples/homography/TooN/SymEigen.h

00001 
00002 /*                       
00003          Copyright (C) 2005 Tom Drummond
00004 
00005      This library is free software; you can redistribute it and/or
00006      modify it under the terms of the GNU Lesser General Public
00007      License as published by the Free Software Foundation; either
00008      version 2.1 of the License, or (at your option) any later version.
00009 
00010      This library is distributed in the hope that it will be useful,
00011      but WITHOUT ANY WARRANTY; without even the implied warranty of
00012      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00013      Lesser General Public License for more details.
00014 
00015      You should have received a copy of the GNU Lesser General Public
00016      License along with this library; if not, write to the Free Software
00017      Foundation, Inc.
00018      51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
00019 */
00020 #ifndef __SYMEIGEN_H
00021 #define __SYMEIGEN_H
00022 
00023 #include <iostream>
00024 #include <TooN/lapack.h>
00025 
00026 #include <TooN/TooN.h>
00027 
00028 #ifndef TOON_NO_NAMESPACE
00029 namespace TooN {
00030 #endif 
00031 
00032 static const double symeigen_condition_no=1e9;
00033 
00034 template <int Size=-1>
00035 class SymEigen {
00036 public:
00037   inline SymEigen(){}
00038 
00039 
00040   template<class Accessor>
00041   inline SymEigen(const FixedMatrix<Size,Size,Accessor>& m){
00042     compute(m);
00043   }
00044 
00045   template<class Accessor>
00046   inline void compute(const FixedMatrix<Size,Size,Accessor>& m){
00047     my_evectors = m;
00048     int N = Size;
00049     int lda = Size;
00050     int info;
00051     int lwork=-1;
00052     double size;
00053 
00054     // find out how much space fortran needs
00055     dsyev_("V","U",&N,my_evectors.get_data_ptr(),&lda,my_evalues.get_data_ptr(),
00056            &size,&lwork,&info);
00057     lwork = int(size);
00058     double* WORK = new double[lwork];
00059 
00060     // now compute the decomposition
00061     dsyev_("V","U",&N,my_evectors.get_data_ptr(),&lda,my_evalues.get_data_ptr(),
00062            WORK,&lwork,&info);
00063     delete [] WORK;
00064     if(info!=0){
00065       std::cerr << "In SymEigen<"<<Size<<">: " << info 
00066                 << " off-diagonal elements of an intermediate tridiagonal form did not converge to zero." << std::endl
00067                 << "M = " << m << std::endl;
00068     }
00069   }
00070   
00071   template <class Accessor>
00072   Vector<Size> backsub(const FixedVector<Size,Accessor>& rhs){
00073     Vector<Size> invdiag;
00074     get_inv_diag(invdiag,symeigen_condition_no);
00075     return (my_evectors.T() * diagmult(invdiag,(my_evectors * rhs)));
00076   }
00077 
00078   template <class Accessor>
00079   Vector<> backsub(const DynamicVector<Accessor>& rhs){
00080     Vector<Size> invdiag;
00081     get_inv_diag(invdiag,symeigen_condition_no);
00082     return (my_evectors.T() * diagmult(invdiag,(my_evectors * rhs)));
00083   }
00084 
00085 
00086   template <int NRHS, class Accessor>
00087   Matrix<Size,NRHS> backsub(const FixedMatrix<Size,NRHS,Accessor>& rhs){
00088     Vector<Size> invdiag;
00089     get_inv_diag(invdiag,symeigen_condition_no);
00090     return (my_evectors.T() * diagmult(invdiag,(my_evectors * rhs)));
00091   }
00092 
00093   template <class Accessor>
00094   Matrix<> backsub(const DynamicMatrix<Accessor>& rhs){
00095     Vector<Size> invdiag;
00096     get_inv_diag(invdiag,symeigen_condition_no);
00097     return (my_evectors.T() * diagmult(invdiag,(my_evectors * rhs)));
00098   }
00099 
00100 
00101   Matrix<Size> get_pinv(const double condition=symeigen_condition_no){
00102     Vector<Size> invdiag;
00103     get_inv_diag(invdiag,condition);
00104     return my_evectors.T() * diagmult(invdiag,my_evectors);
00105   }
00106 
00107 
00108   void get_inv_diag(Vector<Size>& invdiag, double condition){
00109     double max_diag = -my_evalues[0] > my_evalues[Size-1] ? -my_evalues[0]:my_evalues[Size-1];
00110     for(int i=0; i<Size; i++){
00111       if(fabs(my_evalues[i]) * condition > max_diag) {
00112         invdiag[i] = 1/my_evalues[i];
00113       } else {
00114         invdiag[i]=0;
00115       }
00116     }
00117   }
00118   
00119   inline Matrix<Size,Size,RowMajor>& get_evectors() {return my_evectors;}
00120   inline const Matrix<Size,Size,RowMajor>& get_evectors() const {return my_evectors;}
00121   inline Vector<Size>& get_evalues() {return my_evalues;}
00122   inline const Vector<Size>& get_evalues() const {return my_evalues;}
00123   
00124   bool is_posdef() const {
00125       for (int i = 0; i < Size; ++i) {
00126           if (my_evalues[i] <= 0.0)
00127               return false;
00128       }
00129       return true;
00130   }
00131   
00132   bool is_negdef() const {
00133       for (int i = 0; i < Size; ++i) {
00134           if (my_evalues[i] >= 0.0)
00135               return false;
00136       }
00137       return true;
00138   }
00139 
00140   double get_determinant () const {
00141           double det = 1.0;
00142           for (int i = 0; i < Size; ++i) {
00143                   det *= my_evalues[i];
00144           }
00145           return det;
00146   }
00147 
00148 private:
00149   // eigen vectors laid out row-wise so evectors[i] is the ith evector
00150   Matrix<Size,Size,RowMajor> my_evectors;
00151 
00152   Vector<Size> my_evalues;
00153 };
00154 
00155 
00156 template <>
00157 class SymEigen<> {
00158 public:
00159 
00160   inline SymEigen(int size) : 
00161     my_evectors(size,size),
00162     my_evalues(size) {}
00163 
00164   template<class Accessor>
00165   inline SymEigen(const DynamicMatrix<Accessor>& m) : 
00166     my_evectors(m.num_rows(), m.num_cols()),
00167     my_evalues(m.num_rows())
00168   {
00169     assert(m.num_rows() == m.num_cols());
00170     compute(m);
00171   }
00172 
00173   template<class Accessor>
00174   inline void compute(const DynamicMatrix<Accessor>& m){
00175     my_evectors = m;
00176     int N = m.num_cols();
00177     int lda = m.num_cols();
00178     int info;
00179     int lwork=-1;
00180     double size;
00181 
00182     // find out how much space fortran needs
00183     dsyev_("V","U",&N,my_evectors.get_data_ptr(),&lda,my_evalues.get_data_ptr(),
00184            &size,&lwork,&info);
00185     lwork = int(size);
00186     double* WORK = new double[lwork];
00187 
00188     // now compute the decomposition
00189     dsyev_("V","U",&N,my_evectors.get_data_ptr(),&lda,my_evalues.get_data_ptr(),
00190            WORK,&lwork,&info);
00191     delete [] WORK;
00192     if(info!=0){
00193       std::cerr << "info was not zero in SymEigen - it was " << info << std::endl;
00194     }
00195   }
00196   
00197   template <int Size, class Accessor>
00198   Vector<Size> backsub(const FixedVector<Size,Accessor>& rhs){
00199     Vector<> invdiag(my_evalues.size());
00200     get_inv_diag(invdiag,symeigen_condition_no);
00201     return (my_evectors.T() * diagmult(invdiag,(my_evectors * rhs)));
00202   }
00203 
00204   template <class Accessor>
00205   Vector<> backsub(const DynamicVector<Accessor>& rhs){
00206     Vector<> invdiag(my_evalues.size());
00207     get_inv_diag(invdiag,symeigen_condition_no);
00208     return (my_evectors.T() * diagmult(invdiag,(my_evectors * rhs)));
00209   }
00210 
00211 
00212   template <int NRHS, class Accessor, int Size>
00213   Matrix<Size,NRHS> backsub(const FixedMatrix<Size,NRHS,Accessor>& rhs){
00214     Vector<> invdiag(my_evalues.size());
00215     get_inv_diag(invdiag,symeigen_condition_no);
00216     return (my_evectors.T() * diagmult(invdiag,(my_evectors * rhs)));
00217   }
00218 
00219   template <class Accessor>
00220   Matrix<> backsub(const DynamicMatrix<Accessor>& rhs){
00221     Vector<> invdiag(my_evalues.size());
00222     get_inv_diag(invdiag,symeigen_condition_no);
00223     return (my_evectors.T() * diagmult(invdiag,(my_evectors * rhs)));
00224   }
00225 
00226 
00227 
00228 
00229   Matrix<> get_pinv(const double condition=symeigen_condition_no){
00230     Vector<> invdiag(my_evalues.size());
00231     get_inv_diag(invdiag,condition);
00232     return my_evectors.T() * diagmult(invdiag,my_evectors);
00233   }
00234 
00235 
00236   void get_inv_diag(Vector<>& invdiag, double condition){
00237     double max_diag = -my_evalues[0] > my_evalues[invdiag.size()-1] ? -my_evalues[0]:my_evalues[invdiag.size()-1];
00238     for(int i=0; i<invdiag.size(); i++){
00239       if(fabs(my_evalues[i]) * condition > max_diag) {
00240         invdiag[i] = 1/my_evalues[i];
00241       } else {
00242         invdiag[i]=0;
00243       }
00244     }
00245   }
00246   
00247 
00248   
00249   inline Matrix<-1,-1,RowMajor>& get_evectors() {return my_evectors;}
00250   inline const Matrix<-1,-1,RowMajor>& get_evectors() const {return my_evectors;}
00251   inline Vector<>& get_evalues() {return my_evalues;}
00252   inline const Vector<>& get_evalues() const {return my_evalues;}
00253 
00254 private:
00255   // eigen vectors laid out row-wise so evectors[i] is the ith evector
00256   Matrix<-1,-1,RowMajor> my_evectors;
00257   Vector<> my_evalues;
00258 };
00259 
00260 
00261 #ifndef TOON_NO_NAMESPACE
00262 }
00263 #endif 
00264 
00265 
00266 
00267 #endif

Generated on Fri Feb 22 18:26:55 2008 for QVision by  doxygen 1.5.3