00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
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
00101 Precision val = my_cholesky(row,col);
00102 for(int col2=0; col2<col; col2++){
00103
00104 val-=my_cholesky(col2,col)*my_cholesky(row,col2);
00105 }
00106 if(row==col){
00107
00108 my_cholesky(row,col)=val;
00109 inv_diag=1/val;
00110 } else {
00111
00112 my_cholesky(col,row)=val;
00113
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
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
00138 for(int i=0; i<size; i++){
00139 y[i]/=my_cholesky(i,i);
00140 }
00141
00142
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
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
00172 for(int i=0; i<size; i++){
00173 y[i]*=(1/my_cholesky(i,i));
00174 }
00175
00176
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
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