flower/lib/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         y(i) /= D(i);
  20
  21     // backward subst
  22     Vector &x(rhs);             // using input as return val.
  23     for (int i=n-1; i >= 0; i--) {
  24         Real sum(0.0);
  25         for (int j=i+1; j < n; j++)
  26             sum += L(j,i)*x(j);
  27         x(i) = (y(i) - sum)/L(i,i);
  28     }
  29     return x;
  30 }
  31
  32 /*
  33   Standard matrix algorithm.
  34   */
  35
  36 Choleski_decomposition::Choleski_decomposition(Matrix P)
  37     : L(P.dim()), D(P.dim())
  38 {
  39     int n = P.dim();
  40     assert((P-P.transposed()).norm()/P.norm() < EPS);
  41
  42     L.unit();
  43     for (int k= 0; k < n; k++) {
  44         for (int j = 0; j < k; j++){
  45             Real sum(0.0);
  46             for (int l=0; l < j; l++)
  47                 sum += L(k,l)*L(j,l)*D(l);
  48             L(k,j) = (P(k,j) - sum)/D(j);
  49         }
  50         Real sum=0.0;
  51
  52         for (int l=0; l < k; l++)
  53             sum += sqr(L(k,l))*D(l);
  54         Real d = P(k,k) - sum;
  55         D(k) = d;
  56     }
  57
  58 #ifdef NDEBUG
  59     assert((original()-P).norm() / P.norm() < EPS);
  60 #endif
  61 }
  62
  63 Matrix
  64 Choleski_decomposition::original() const
  65 {
  66     Matrix T(L.dim());
  67     T.set_diag(D);
  68     return L*T*L.transposed();
  69 }
  70
  71 Matrix
  72 Choleski_decomposition::inverse() const
  73 {
  74     int n=L.dim();
  75     Matrix invm(n);
  76     Vector e_i(n);
  77     for (int i = 0; i < n; i++) {
  78         e_i.set_unit(i);
  79         Vector inv(solve(e_i));
  80         for (int j = 0 ; j<n; j++)
  81             invm(i,j) = inv(j);
  82     }
  83
  84 #ifdef NDEBUG
  85     Matrix I1(n), I2(original());
  86     I1.unit();
  87     assert((I1-original()*invm).norm()/original.norm() < EPS);
  88 #endif
  89
  90     return invm;
  91 }