flower/choleski.cc

   1 #include "choleski.hh"
   2 const Real EPS = 1e-7;          // so sue me. Hard coded
   3
   4 Vector
   5 Choleski_decomposition::solve(Vector rhs)const
   6 {
   7     int n= rhs.dim();
   8     assert(n == L.dim());
   9     Vector y(n);
  10
  11     // forward substitution
  12     for (int i=0; i < n; i++) {
  13         Real sum(0.0);
  14         for (int j=0; j < i; j++)
  15             sum += y(j) * L(i,j);
  16         y(i) = (rhs(i) - sum)/L(i,i);
  17     }
  18     for (int i=0; i < n; i++) {
  19         assert(D(i));
  20         y(i) /= D(i);
  21     }
  22
  23     // backward subst
  24     Vector x(n);
  25     for (int i=n-1; i >= 0; i--) {
  26         Real sum(0.0);
  27         for (int j=i+1; j < n; j++)
  28             sum += L(j,i)*x(j);
  29         x(i) = (y(i) - sum)/L(i,i);
  30     }
  31     return x;
  32 }
  33
  34 /*
  35   Standard matrix algorithm.
  36   */
  37
  38 Choleski_decomposition::Choleski_decomposition(Matrix P)
  39     : L(P.dim()), D(P.dim())
  40 {
  41     int n = P.dim();
  42     assert((P-P.transposed()).norm()/P.norm() < EPS);
  43
  44     L.unit();
  45     for (int k= 0; k < n; k++) {
  46         for (int j = 0; j < k; j++){
  47             Real sum(0.0);
  48             for (int l=0; l < j; l++)
  49                 sum += L(k,l)*L(j,l)*D(l);
  50             L(k,j) = (P(k,j) - sum)/D(j);
  51         }
  52         Real sum=0.0;
  53
  54         for (int l=0; l < k; l++)
  55             sum += sqr(L(k,l))*D(l);
  56         Real d = P(k,k) - sum;
  57         D(k) = d;
  58     }
  59
  60 #ifdef NDEBUG
  61     assert((original()-P).norm() / P.norm() < EPS);
  62 #endif
  63 }
  64
  65 Matrix
  66 Choleski_decomposition::original() const
  67 {
  68     Matrix T(L.dim());
  69     T.set_diag(D);
  70     return L*T*L.transposed();
  71 }
  72
  73 Matrix
  74 Choleski_decomposition::inverse() const
  75 {
  76     int n=L.dim();
  77     Matrix invm(n);
  78     Vector e_i(n);
  79     for (int i = 0; i < n; i++) {
  80         e_i.set_unit(i);
  81         Vector inv(solve(e_i));
  82         for (int j = 0 ; j<n; j++)
  83             invm(i,j) = inv(j);
  84     }
  85
  86 #ifdef NDEBUG
  87     Matrix I1(n), I2(original());
  88     I1.unit();
  89     assert((I1-original()*invm).norm()/original.norm() < EPS);
  90 #endif
  91
  92     return invm;
  93 }
  94
  95
  96
  97