examples/homography/TooN/Cholesky.h

00001 // -*- c++ -*-
00002 
00003 //     Copyright (C) 2002 Tom Drummond (twd20@eng.cam.ac.uk)
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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00018 
00019 
00020 #ifndef CHOLESKY_H
00021 #define CHOLESKY_H
00022 
00023 #include <iostream>
00024 
00025 #include <TooN/lapack.h>
00026 
00027 #include <TooN/TooN.h>
00028 #include <TooN/helpers.h>
00029 #include <limits>
00030 
00031 #ifndef TOON_NO_NAMESPACE
00032 namespace TooN {
00033 #endif 
00034     namespace util {
00035         
00036         template <int B, int E> struct Dot3 {
00037             template <class V1, class V2, class V3> static inline double eval(const V1& a, const V2& b, const V3& c) { return a[B]*b[B]*c[B] + Dot3<B+1,E>::eval(a,b,c); }
00038         };
00039         template <int B> struct Dot3<B,B> {
00040             template <class V1, class V2, class V3> static inline double eval(const V1& a, const V2& b, const V3& c) { return a[B]*b[B]*c[B]; }
00041         };
00042 
00043         //
00044         // Forward Sub
00045         //
00046         template <int N, int I=0> struct Forwardsub_L {
00047             template <class A1, class A2, class A3> static inline void eval(const FixedMatrix<N,N,A1>& L, const FixedVector<N,A2>& v, FixedVector<N,A3>& x) {
00048                 x[I] = v[I] - Dot<0,I-1>::eval(L[I], x);
00049                 Forwardsub_L<N,I+1>::eval(L, v, x);
00050             }
00051             template <class A1, class A2, class Vec> static inline void eval(const FixedMatrix<N,N,A1>& L, const DynamicVector<A2>& v, Vec & x) {
00052                 x[I] = v[I] - Dot<0,I-1>::eval(L[I], x);
00053                 Forwardsub_L<N,I+1>::eval(L, v, x);
00054             }
00055         };
00056         template <int N> struct Forwardsub_L<N,0> {
00057             template <class A1, class A2, class A3> static inline void eval(const FixedMatrix<N,N,A1>& L, const FixedVector<N,A2>& v, FixedVector<N,A3>& x) {
00058                 x[0] = v[0];
00059                 Forwardsub_L<N,1>::eval(L, v, x);
00060             }
00061             template <class A1, class A2, class Vec> static inline void eval(const FixedMatrix<N,N,A1>& L, const DynamicVector<A2>& v, Vec & x) {
00062                 assert(v.size() == N && x.size() == N);
00063                 x[0] = v[0];
00064                 Forwardsub_L<N,1>::eval(L, v, x);
00065             }
00066         };
00067         template <int N> struct Forwardsub_L<N,N> {
00068             template <class A1, class A2, class A3> static inline void eval(const FixedMatrix<N,N,A1>& L, const FixedVector<N,A2>& v, FixedVector<N,A3>& x) {}
00069             template <class A1, class A2, class Vec> static inline void eval(const FixedMatrix<N,N,A1>& L, const DynamicVector<A2>& v, Vec & x) {}
00070         };
00071         
00072         //
00073         // Back Sub
00074         //
00075         template <int N, int I=N> struct Backsub_LT {
00076             template <class A1, class A2, class A3, class A4> static inline void eval(const FixedMatrix<N,N,A1>& L, const FixedVector<N,A2>& v, 
00077                                                                                       const FixedVector<N,A3>& invdiag, FixedVector<N,A4>& x) {
00078                 x[I-1] = v[I-1]*invdiag[I-1] - Dot<I,N-1>::eval(L.T()[I-1], x);
00079                 Backsub_LT<N,I-1>::eval(L, v, invdiag, x);
00080             }
00081         };
00082         template <int N> struct Backsub_LT<N,N> {
00083             template <class A1, class A2, class A3, class A4> static inline void eval(const FixedMatrix<N,N,A1>& L, const FixedVector<N,A2>& v, 
00084                                                                                       const FixedVector<N,A3>& invdiag, FixedVector<N,A4>& x) {
00085                 x[N-1] = v[N-1]*invdiag[N-1];
00086                 Backsub_LT<N,N-1>::eval(L, v, invdiag, x);
00087             }
00088         };
00089         template <int N> struct Backsub_LT<N,0> {
00090             template <class A1, class A2, class A3, class A4> static inline void eval(const FixedMatrix<N,N,A1>& L, const FixedVector<N,A2>& v, 
00091                                                                                       const FixedVector<N,A3>& invdiag, FixedVector<N,A4>& x) {}
00092         };
00093 
00094         template <int N, class A1, class A2, class A3, class A4>
00095         void cholesky_solve(const FixedMatrix<N,N,A1>& L, const FixedVector<N,A2>& invdiag, const FixedVector<N,A3>& v, FixedVector<N,A4>& x) 
00096         {
00097             Vector<N> t;
00098             Forwardsub_L<N>::eval(L, v, t);
00099             Backsub_LT<N>::eval(L, t, invdiag, x);
00100         }
00101 
00102         //
00103         // Compute cholesky
00104         //
00105         template <int N, int I, int J=I+1> struct CholeskyInner {
00106             template <class A1, class A2, class A3> static inline void eval(const FixedMatrix<N,N,A1>& M, FixedMatrix<N,N,A2>& L, const FixedVector<N,A3>& invdiag) {
00107                 const double a = M[I][J] - Dot<0,I-1>::eval(L[I], L.T()[J]);
00108                 L[I][J] = a;
00109                 L[J][I] = a * invdiag[I];
00110                 CholeskyInner<N, I, J+1>::eval(M, L, invdiag);
00111             }
00112         };
00113 
00114         template <int N, int I> struct CholeskyInner<N,I,N> {
00115             template <class A1, class A2, class A3> static inline void eval(const FixedMatrix<N,N,A1>& M, FixedMatrix<N,N,A2>& L, const FixedVector<N,A3>& invdiag) {}
00116         };
00117         template <int N, int I=0> struct CholeskyOuter {
00118             template <class A1, class A2, class A3> static inline void eval(const FixedMatrix<N,N,A1>& M, FixedMatrix<N,N,A2>& L, FixedVector<N,A3>& invdiag, int& rank) {
00119                 const double a = M[I][I] - Dot<0,I-1>::eval(L[I], L.T()[I]);
00120                 if (0 < a) {
00121                     L[I][I] = a;
00122                     invdiag[I] = 1.0/a;
00123                     ++rank;
00124                 } else {
00125                     L[I][I] = invdiag[I] = 0;
00126                     return;
00127                 }
00128                 CholeskyInner<N,I>::eval(M,L,invdiag);
00129                 CholeskyOuter<N,I+1>::eval(M,L,invdiag,rank);
00130             }
00131         };
00132         template <int N> struct CholeskyOuter<N,N> {
00133             template <class A1, class A2, class A3> static inline void eval(const FixedMatrix<N,N,A1>& M, FixedMatrix<N,N,A2>& L, FixedVector<N,A3>& invdiag, int& rank) {}
00134         };
00135 
00136 
00137         template <int N, class A1, class A2, class A3> int cholesky_compute(const FixedMatrix<N,N,A1>& M, FixedMatrix<N,N,A2>& L, FixedVector<N,A3>& invdiag)
00138         {
00139             int rank=0;
00140             CholeskyOuter<N>::eval(M, L, invdiag, rank);
00141             return rank;
00142         }
00143 
00144         //
00145         // Compute inverse
00146         //
00147         template <int N, int Col, int I=Col+1> struct InverseForward {
00148             template <class A1, class A2> static inline void eval(const FixedMatrix<N,N,A1>& L, FixedVector<N,A2>& t) {
00149                 t[I] = -(L[I][Col] + Dot<Col+1,I-1>::eval(L[I], t));
00150                 InverseForward<N,Col,I+1>::eval(L,t);
00151             }
00152         };
00153         template <int N, int Col> struct InverseForward<N,Col,N> { 
00154             template <class A1, class A2> static inline void eval(const FixedMatrix<N,N,A1>& L, FixedVector<N,A2>& t) {} 
00155         };
00156 
00157         template <int N, int Col, int I=N-1> struct InverseBack {
00158             template <class A1, class A2, class A3, class A4> static inline void eval(const FixedMatrix<N,N,A1>& L, const FixedVector<N,A2>& t, 
00159                                                                                       const FixedVector<N,A3>& invdiag, FixedMatrix<N,N,A4>& Inv) {
00160                 Inv[I][Col] = Inv[Col][I] = invdiag[I]*t[I] - Dot<I+1,N-1>::eval(L.T()[I], Inv[Col]);
00161                 InverseBack<N,Col,I-1>::eval(L, t, invdiag, Inv);
00162             }
00163         };
00164         template <int N, int Col> struct InverseBack<N,Col,Col> {
00165             template <class A1, class A2, class A3, class A4> static inline void eval(const FixedMatrix<N,N,A1>& L, const FixedVector<N,A2>& t, 
00166                                                                                       const FixedVector<N,A3>& invdiag, FixedMatrix<N,N,A4>& Inv) {
00167                 Inv[Col][Col] = invdiag[Col] - Dot<Col+1,N-1>::eval(L.T()[Col], Inv[Col]);
00168             }
00169         };
00170         
00171         template <int N, int Col=0> struct InverseOuter {
00172             template <class A1, class A2, class A3> static inline void eval(const FixedMatrix<N,N,A1>& L, const FixedVector<N,A2>& invdiag, FixedMatrix<N,N,A3>& Inv) {
00173                 Vector<N> t;
00174                 InverseForward<N,Col>::eval(L, t);
00175                 InverseBack<N,Col>::eval(L, t, invdiag, Inv);
00176                 InverseOuter<N,Col+1>::eval(L,invdiag,Inv);
00177             }
00178         };
00179         template <int N> struct InverseOuter<N,N> {
00180             template <class A1, class A2, class A3> static inline void eval(const FixedMatrix<N,N,A1>& L, const FixedVector<N,A2>& invdiag, FixedMatrix<N,N,A3>& Inv) {}
00181         };
00182 
00183         template <int S, class A1, class A2, class A3> void cholesky_inverse(const FixedMatrix<S,S,A1>& L, const FixedVector<S,A2>& invdiag, FixedMatrix<S,S,A3>& I)
00184         {
00185             InverseOuter<S>::eval(L, invdiag, I);
00186         }
00187 
00188         //
00189         // Cholesky update
00190         //
00191         template <int N, int I, int J=I-1> struct UpdateInner {
00192             template <class LT, class PT, class FT, class ST> static inline void eval(LT& L, const PT& p, const FT& F, const ST sigma) {
00193                 const double old = L[I][J];
00194                 L[I][J] = old + (sigma * p[J]) * F[J];
00195                 UpdateInner<N,I,J-1>::eval(L, p, F, sigma + old * p[J]);
00196             }
00197         };
00198 
00199         template <int N, int I> struct UpdateInner<N,I,-1> {
00200             template <class LT, class PT, class FT, class ST> static inline void eval(LT& L, const PT& p, const FT& F, const ST sigma) {}
00201         };
00202             
00203         template <int N, int I=1> struct UpdateOuter {
00204             template <class LT, class PT, class FT> static inline void eval(LT& L, const PT& p, const FT& F) {
00205                 UpdateInner<N,I>::eval(L, p, F, p[I]);
00206                 UpdateOuter<N,I+1>::eval(L, p ,F);
00207             }
00208         };
00209 
00210         template <int N> struct UpdateOuter<N,N> {
00211             template <class LT, class PT, class FT> static inline void eval(LT& L, const PT& p, const FT& F) {}
00212         };
00213             
00214         template <class P, int N, class A1, class A2> void cholesky_update(TooN::FixedMatrix<N,N,A1>& L, TooN::FixedVector<N,A2>& invdiag, const P& p)
00215         {
00216             double F[N-1];
00217             double alpha = 1;
00218             for (int i=0; i<N-1; ++i) {
00219                 const double p2 = p[i]*p[i];
00220                 const double di = L[i][i];
00221                 L[i][i] += alpha * p2;
00222                 invdiag[i] = 1.0/L[i][i];
00223                 const double a_over_d = alpha*invdiag[i];
00224                 F[i] = a_over_d;
00225                 alpha = di * a_over_d;
00226             }
00227             L[N-1][N-1] += alpha * (p[N-1]*p[N-1]);
00228             invdiag[N-1] = 1.0/L[N-1][N-1];
00229                 
00230             UpdateOuter<N>::eval(L, p, F);
00231         }
00232     }
00233 
00234     template <int N=-1>
00235     class Cholesky {
00236     public:
00237 
00238         Cholesky() {}
00239 
00240         template<class Accessor>
00241         Cholesky(const FixedMatrix<N,N,Accessor>& m){
00242             compute(m);
00243         }
00244     
00245         template<class Accessor>
00246         void compute(const FixedMatrix<N,N,Accessor>& m){
00247             rank = util::cholesky_compute(m,L,invdiag);
00248         }
00249         int get_rank() const { return rank; }
00250 
00251         double get_determinant() const {
00252             double det = L[0][0];
00253             for (int i=1; i<N; i++)
00254                 det *= L[i][i];
00255             return det;
00256         }
00257 
00258         const Matrix<N>& get_L_D() const { return L; }
00259 
00260         template <class A1, class A2> static void sqrt(const FixedMatrix<N,N,A1>& A, FixedMatrix<N,N,A2>& L) {
00261             for (int i=0; i<N; ++i) {
00262                 double a = A[i][i];
00263                 for (int k=0; k<i; ++k)
00264                     a -= L[i][k]*L[i][k];
00265                 if (0<a) {
00266                     L[i][i] = ::sqrt(a);
00267                 } else {
00268                     Zero(L.slice(i,i,N-i,N-i));
00269                     return;
00270                 }
00271                 const double id = 1.0/L[i][i];
00272                 for (int j=i+1; j<N; ++j) {
00273                     L[i][j] = 0;
00274                     double a = A[i][j];
00275                     for (int k=0; k<i; ++k)
00276                         a -= L[i][k]*L[j][k];
00277                     L[j][i] = a * id;
00278                 }
00279             }
00280         }
00281 
00282         template <class A1> static Matrix<N> sqrt(const FixedMatrix<N,N,A1>& A) { 
00283             Matrix<N> L;
00284             Cholesky<N>::sqrt(A,L);
00285             return L;
00286         }
00287 
00288         template <class A> void get_sqrt(FixedMatrix<N,N,A>& M) const {
00289             for (int i=0; i<N; ++i) {
00290                 const double root_d = ::sqrt(L[i][i]);
00291                 M[i][i] = root_d;
00292                 for (int j=i+1; j<N; ++j) {
00293                     M[j][i] = L[j][i]*root_d;
00294                     M[i][j] = 0;
00295                 }
00296             }
00297         }
00298 
00299         Matrix<N> get_sqrt() const {
00300             Matrix<N> S;
00301             get_sqrt(S);
00302             return S;
00303         }
00304         
00305         Matrix<N> get_L() const __attribute__ ((deprecated)) { return get_sqrt(); }
00306         
00307         template <class A> void get_inv_sqrt(FixedMatrix<N,N,A>& M) const {
00308             Vector<N> root;
00309             for (int i=0; i<N; ++i)
00310                 root[i] = ::sqrt(invdiag[i]);
00311             for (int j=0; j<N; ++j) {
00312                 for (int i=j+1; i<N; ++i) {
00313                     double sum = L[i][j];
00314                     for (int k=j+1; k<i; ++k)
00315                         sum += L[i][k]*M[j][k];
00316                     M[j][i] = -sum;
00317                     M[i][j] = 0;
00318                 }
00319                 M[j][j] = root[j];
00320                 for (int i=j+1; i<N; ++i)
00321                     M[j][i] *= root[i];
00322             }
00323         }
00324         
00325         template <class A> double mahalanobis(const FixedVector<N,A>& v) const {
00326             Vector<N> L_inv_v;
00327             util::Forwardsub_L<N>::eval(L, v, L_inv_v);
00328             return util::Dot3<0,N-1>::eval(L_inv_v, L_inv_v, invdiag);
00329         }
00330 
00331         template <class F, int M, class A1, class A2> void transform_inverse(const FixedMatrix<M,N,A1>& J, FixedMatrix<M,M,A2>& T) const {
00332             Matrix<M,N> L_inv_JT;
00333             for (int i=0; i<M; ++i)
00334                 util::Forwardsub_L<N>::eval(L, J[i], L_inv_JT[i]);
00335             for (int i=0; i<M; ++i) {           
00336                 F::eval(T[i][i],util::Dot3<0,N-1>::eval(L_inv_JT[i], L_inv_JT[i], invdiag));
00337                 for (int j=i+1; j<M; ++j) {
00338                     const double x = util::Dot3<0,N-1>::eval(L_inv_JT[i], L_inv_JT[j], invdiag);
00339                     F::eval(T[i][j],x);
00340                     F::eval(T[j][i],x);
00341                 }
00342             }
00343         }
00344 
00345         template <class F, class A1, class M2> void transform_inverse(const DynamicMatrix<A1>& J, M2 & T) const {
00346             const int M = J.num_rows();
00347             assert( T.num_rows() == M && T.num_cols() == M);
00348             Matrix<> L_inv_JT(M,N);
00349             for (int i=0; i<M; ++i)
00350                 util::Forwardsub_L<N>::eval(L, J[i], L_inv_JT[i]);
00351             for (int i=0; i<M; ++i) {
00352                 F::eval(T[i][i],util::Dot3<0,N-1>::eval(L_inv_JT[i], L_inv_JT[i], invdiag));
00353                 for (int j=i+1; j<M; ++j) {
00354                     const double x = util::Dot3<0,N-1>::eval(L_inv_JT[i], L_inv_JT[j], invdiag);
00355                     F::eval(T[i][j],x);
00356                     F::eval(T[j][i],x);
00357                 }
00358             }
00359         }
00360 
00361         template <int M, class A1, class A2> void transform_inverse(const FixedMatrix<M,N,A1>& J, FixedMatrix<M,M,A2>& T) const {
00362             transform_inverse<util::Assign>(J,T);
00363         }
00364 
00365         template <class A1, class M2> void transform_inverse(const DynamicMatrix<A1>& J, M2 & T) const {
00366             transform_inverse<util::Assign>(J,T);
00367         }
00368 
00369         template <int M, class A> Matrix<M> transform_inverse(const FixedMatrix<M,N,A>& J) const {
00370             Matrix<M> T;
00371             transform_inverse(J,T);
00372             return T;
00373         }
00374 
00375         template <class A> Matrix<> transform_inverse(const DynamicMatrix<A>& J) const {
00376             Matrix<> T(J.num_rows(), J.num_rows());
00377             transform_inverse(J,T);
00378             return T;
00379         }
00380 
00381         template <class A1, class A2> inline 
00382         void inverse_times(const FixedVector<N,A1>& v, FixedVector<N,A2>& x) const
00383         {
00384             util::cholesky_solve(L, invdiag, v, x);
00385         }
00386 
00387         template <class Accessor> inline 
00388         Vector<N> inverse_times(const FixedVector<N,Accessor>& v) const
00389         {
00390             Vector<N> x;
00391             inverse_times(v, x);
00392             return x;
00393         }
00394 
00395         template <class Accessor> inline 
00396         Vector<N> backsub(const FixedVector<N,Accessor>& v) const { return inverse_times(v); }
00397       
00398         template <class A, int M> inline Matrix<N,M> inverse_times(const FixedMatrix<N,M,A>& m)
00399         {
00400             Matrix<N,M> result;
00401             for (int i=0; i<M; i++)
00402                 inverse_times(m.T()[i], result.T()[i]);
00403             return result;
00404         }
00405 
00406         template <class A> void get_inverse(FixedMatrix<N,N,A>& M) const {
00407             util::cholesky_inverse(L, invdiag, M);
00408         }
00409 
00410         Matrix<N> get_inverse() const {
00411             Matrix<N> M;
00412             get_inverse(M);
00413             return M;
00414         }
00415         
00416         template <int M, class A>  void update(const FixedMatrix<N,M,A>& V) {
00417             for (int i=0; i<M; ++i) {
00418                 Vector<N> p;
00419                 util::Forwardsub_L<N>::eval(L, V.T()[i], p);
00420                 util::cholesky_update(L, invdiag, p);
00421             }
00422         }
00423 
00424         template <class A>  void update(const FixedVector<N,A>& v) {
00425             Vector<N> p;
00426             util::Forwardsub_L<N>::eval(L, v, p);
00427             util::cholesky_update(L, invdiag, p);
00428         }
00429         
00430     private:
00431         Matrix<N> L;
00432         Vector<N> invdiag;
00433         int rank;
00434     };
00435   
00436     template <>
00437     class Cholesky<-1> {
00438     public:
00439 
00440     Cholesky(){}
00441 
00442         template<class Accessor>
00443         Cholesky(const DynamicMatrix<Accessor>& m) {
00444             compute(m);
00445         }
00446 
00447         template<class Accessor>
00448         void compute(const DynamicMatrix<Accessor>& m){
00449             assert(m.num_rows() == m.num_cols());
00450             L.assign(m);
00451             int N = L.num_rows();
00452             int info;
00453             dpotrf_("L", &N, L.get_data_ptr(), &N, &info);
00454             assert(info >= 0);
00455             if (info > 0)
00456                 rank = info-1;
00457         }
00458         int get_rank() const { return rank; }
00459 
00460         template <class V> inline
00461         Vector<> inverse_times(const V& v) const { return backsub(v); }
00462 
00463         template <class V> inline
00464         Vector<> backsub(const V& v) const
00465         {
00466             assert(v.size() == L.num_rows());
00467             Vector<> x = v;
00468             int N=L.num_rows();
00469             int NRHS=1;
00470             int info;
00471             dpotrs_("L", &N, &NRHS, L.get_data_ptr(), &N, x.get_data_ptr(), &N, &info);     
00472             assert(info==0);
00473             return x;
00474         }
00475 
00476         template <class V> double mahalanobis(const V& v) const {
00477             return v * backsub(v);
00478         }
00479 
00480         const Matrix<>& get_L() const {
00481             return L;
00482         }
00483 
00484         double get_determinant() const {
00485             double det = L[0][0];
00486             for (int i=1; i<L.num_rows(); i++)
00487                 det *= L[i][i];
00488             return det*det;
00489         }
00490 
00491         template <class Mat> void get_inverse(Mat& M) const {
00492             M = L;
00493             int N = L.num_rows();
00494             int info;
00495             dpotri_("L", &N, M.get_data_ptr(), &N, &info);
00496             assert(info == 0);
00497             TooN::Symmetrize(M);
00498         }
00499 
00500         Matrix<> get_inverse() const {
00501             Matrix<> M(L.num_rows(), L.num_rows());
00502             get_inverse(M);
00503             return M;
00504         }
00505 
00506     private:
00507         Matrix<> L;     
00508         int rank;
00509     };
00510 
00511 #ifndef TOON_NO_NAMESPACE
00512 }
00513 #endif 
00514 
00515 
00516 
00517 
00518 
00519 #endif

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