00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
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
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
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
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
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
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