Cholesky.h

00001 // -*- c++ -*-
00002 
00003 //     Copyright (C) 2009 Tom Drummond (twd20@cam.ac.uk)
00004 //
00005 // This file is part of the TooN Library.  This library is free
00006 // software; you can redistribute it and/or modify it under the
00007 // terms of the GNU General Public License as published by the
00008 // Free Software Foundation; either version 2, or (at your option)
00009 // any later version.
00010 
00011 // This library is distributed in the hope that it will be useful,
00012 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00013 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014 // GNU General Public License for more details.
00015 
00016 // You should have received a copy of the GNU General Public License along
00017 // with this library; see the file COPYING.  If not, write to the Free
00018 // Software Foundation, 59 Temple Place - Suite 330, Boston, MA 02111-1307,
00019 // USA.
00020 
00021 // As a special exception, you may use this file as part of a free software
00022 // library without restriction.  Specifically, if other files instantiate
00023 // templates or use macros or inline functions from this file, or you compile
00024 // this file and link it with other files to produce an executable, this
00025 // file does not by itself cause the resulting executable to be covered by
00026 // the GNU General Public License.  This exception does not however
00027 // invalidate any other reasons why the executable file might be covered by
00028 // the GNU General Public License.
00029 
00030 
00031 #ifndef TOON_INCLUDE_CHOLESKY_H
00032 #define TOON_INCLUDE_CHOLESKY_H
00033 
00034 #include <TooN/TooN.h>
00035 
00036 namespace TooN {
00037 
00038 
00067 template <int Size=Dynamic, class Precision=DefaultPrecision>
00068 class Cholesky {
00069 public:
00070     Cholesky(){}
00071 
00075     template<class P2, class B2>
00076     Cholesky(const Matrix<Size, Size, P2, B2>& m)
00077         : my_cholesky(m) {
00078         SizeMismatch<Size,Size>::test(m.num_rows(), m.num_cols());
00079         do_compute();
00080     }
00081     
00083     Cholesky(int size) : my_cholesky(size,size) {}
00084 
00085 
00088     template<class P2, class B2> void compute(const Matrix<Size, Size, P2, B2>& m){
00089         SizeMismatch<Size,Size>::test(m.num_rows(), m.num_cols());
00090         SizeMismatch<Size,Size>::test(m.num_rows(), my_cholesky.num_rows());
00091         my_cholesky=m;
00092         do_compute();
00093     }
00094 
00095     void do_compute() {
00096         int size=my_cholesky.num_rows();
00097         for(int col=0; col<size; col++){
00098             Precision inv_diag = 1;
00099             for(int row=col; row < size; row++){
00100                 // correct for the parts of cholesky already computed
00101                 Precision val = my_cholesky(row,col);
00102                 for(int col2=0; col2<col; col2++){
00103                     // val-=my_cholesky(col,col2)*my_cholesky(row,col2)*my_cholesky(col2,col2);
00104                     val-=my_cholesky(col2,col)*my_cholesky(row,col2);
00105                 }
00106                 if(row==col){
00107                     // this is the diagonal element so don't divide
00108                     my_cholesky(row,col)=val;
00109                     inv_diag=1/val;
00110                 } else {
00111                     // cache the value without division in the upper half
00112                     my_cholesky(col,row)=val;
00113                     // divide my the diagonal element for all others
00114                     my_cholesky(row,col)=val*inv_diag;
00115                 }
00116             }
00117         }
00118     }
00119 
00122     template<int Size2, class P2, class B2>
00123     Vector<Size, Precision> backsub (const Vector<Size2, P2, B2>& v) {
00124         int size=my_cholesky.num_rows();
00125         SizeMismatch<Size,Size2>::test(size, v.size());
00126 
00127         // first backsub through L
00128         Vector<Size, Precision> y(size);
00129         for(int i=0; i<size; i++){
00130             Precision val = v[i];
00131             for(int j=0; j<i; j++){
00132                 val -= my_cholesky(i,j)*y[j];
00133             }
00134             y[i]=val;
00135         }
00136         
00137         // backsub through diagonal
00138         for(int i=0; i<size; i++){
00139             y[i]/=my_cholesky(i,i);
00140         }
00141 
00142         // backsub through L.T()
00143         Vector<Size,Precision> result(size);
00144         for(int i=size-1; i>=0; i--){
00145             Precision val = y[i];
00146             for(int j=i+1; j<size; j++){
00147                 val -= my_cholesky(j,i)*result[j];
00148             }
00149             result[i]=val;
00150         }
00151 
00152         return result;
00153     }
00154 
00156     template<int Size2, int C2, class P2, class B2>
00157     Matrix<Size, C2, Precision> backsub (const Matrix<Size2, C2, P2, B2>& m) {
00158         int size=my_cholesky.num_rows();
00159         SizeMismatch<Size,Size2>::test(size, m.num_rows());
00160 
00161         // first backsub through L
00162         Matrix<Size, C2, Precision> y(size, m.num_cols());
00163         for(int i=0; i<size; i++){
00164             Vector<C2, Precision> val = m[i];
00165             for(int j=0; j<i; j++){
00166                 val -= my_cholesky(i,j)*y[j];
00167             }
00168             y[i]=val;
00169         }
00170         
00171         // backsub through diagonal
00172         for(int i=0; i<size; i++){
00173             y[i]*=(1/my_cholesky(i,i));
00174         }
00175 
00176         // backsub through L.T()
00177         Matrix<Size,C2,Precision> result(size, m.num_cols());
00178         for(int i=size-1; i>=0; i--){
00179             Vector<C2,Precision> val = y[i];
00180             for(int j=i+1; j<size; j++){
00181                 val -= my_cholesky(j,i)*result[j];
00182             }
00183             result[i]=val;
00184         }
00185         return result;
00186     }
00187 
00188 
00191     // easy way to get inverse - could be made more efficient
00192     Matrix<Size,Size,Precision> get_inverse(){
00193         Matrix<Size,Size,Precision>I(Identity(my_cholesky.num_rows()));
00194         return backsub(I);
00195     }
00196 
00197     Precision determinant(){
00198         Precision answer=my_cholesky(0,0);
00199         for(int i=1; i<my_cholesky.num_rows(); i++){
00200             answer*=my_cholesky(i,i);
00201         }
00202         return answer;
00203     }
00204 
00205 private:
00206     Matrix<Size,Size,Precision> my_cholesky;
00207 };
00208 
00209 
00210 }
00211 
00212 #endif

Generated on Thu May 7 20:28:40 2009 for TooN by  doxygen 1.5.3