Go to the documentation of this file.00001
00002 #ifdef VCL_NEEDS_PRAGMA_INTERFACE
00003 #pragma implementation
00004 #endif
00005
00006
00007
00008
00009
00010
00011
00012
00013 #include "vnl_ldl_cholesky.h"
00014 #include <vcl_cmath.h>
00015 #include <vcl_cassert.h>
00016 #include <vcl_iostream.h>
00017 #include <vnl/algo/vnl_netlib.h>
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029 vnl_ldl_cholesky::vnl_ldl_cholesky(vnl_matrix<double> const & M, Operation mode):
00030 L_(M)
00031 {
00032 long n = M.columns();
00033 assert(n == (int)(M.rows()));
00034 num_dims_rank_def_ = -1;
00035 if (vcl_fabs(M(0,n-1) - M(n-1,0)) > 1e-8) {
00036 vcl_cerr << "vnl_ldl_cholesky: WARNING: non-symmetric: " << M << vcl_endl;
00037 }
00038
00039 if (mode != estimate_condition) {
00040
00041 v3p_netlib_dpofa_(L_.data_block(), &n, &n, &num_dims_rank_def_);
00042 if (mode == verbose && num_dims_rank_def_ != 0)
00043 vcl_cerr << "vnl_ldl_cholesky: " << num_dims_rank_def_ << " dimensions of non-posdeffness\n";
00044 }
00045 else {
00046 vnl_vector<double> nullvec(n);
00047 v3p_netlib_dpoco_(L_.data_block(), &n, &n, &rcond_, nullvec.data_block(), &num_dims_rank_def_);
00048 if (num_dims_rank_def_ != 0)
00049 vcl_cerr << "vnl_ldl_cholesky: rcond=" << rcond_ << " so " << num_dims_rank_def_ << " dimensions of non-posdeffness\n";
00050 }
00051
00052
00053
00054 d_.set_size(n);
00055
00056
00057 vnl_vector<double> sqrt_d(n);
00058
00059 for (int i=0; i<n; ++i)
00060 {
00061 sqrt_d[i]=L_(i,i);
00062 d_[i]=sqrt_d[i]*sqrt_d[i];
00063 }
00064
00065
00066 for (int i=0; i<n; ++i)
00067 {
00068 double *row = L_[i];
00069 for (int j=0; j<i; ++j) row[j]/=sqrt_d[j];
00070 row[i]=1.0;
00071 for (int j=i+1; j<n; ++j) row[j]=0.0;
00072 }
00073 }
00074
00075
00076 inline double dot(const double* v1, const double* v2, unsigned n)
00077 {
00078 double sum=0.0;
00079 for (unsigned i=0;i<n;++i) sum+= v1[i]*v2[i];
00080 return sum;
00081 }
00082
00083 inline double dot(const double* v1, unsigned s, const double* v2, unsigned n)
00084 {
00085 double sum=0.0;
00086 for (unsigned i=0;i<n;++i,v1+=s) sum+= (*v1)*v2[i];
00087 return sum;
00088 }
00089
00090
00091
00092 void vnl_ldl_cholesky::solve_lx(vnl_vector<double>& x)
00093 {
00094 unsigned n = d_.size();
00095 for (unsigned i=1;i<n;++i)
00096 x[i] -= dot(L_[i],x.data_block(),i);
00097 }
00098
00099
00100
00101
00102 void vnl_ldl_cholesky::inplace_solve(double* x) const
00103 {
00104 unsigned n = d_.size();
00105
00106 for (unsigned i=1;i<n;++i)
00107 x[i] -= dot(L_[i],x,i);
00108
00109
00110 for (unsigned i=0;i<n;++i) x[i]/=d_[i];
00111
00112
00113 const double* L_data = &L_(n-1,n-2);
00114 const double* x_data = &x[n-1];
00115 unsigned c=1;
00116 for (int i=n-2;i>=0;--i,L_data-=(n+1),--x_data,++c)
00117 {
00118 x[i] -= dot(L_data,n,x_data,c);
00119 }
00120 }
00121
00122
00123
00124
00125 double vnl_ldl_cholesky::xt_m_inv_x(const vnl_vector<double>& x) const
00126 {
00127 unsigned n=d_.size();
00128 assert(x.size()==n);
00129 vnl_vector<double> y=x;
00130
00131 double sum = y[0]*y[0]/d_[0];
00132 for (unsigned i=1;i<n;++i)
00133 {
00134 y[i] -= dot(L_[i],y.data_block(),i);
00135 sum += y[i]*y[i]/d_[i];
00136 }
00137 return sum;
00138 }
00139
00140
00141
00142 double vnl_ldl_cholesky::xt_m_x(const vnl_vector<double>& x) const
00143 {
00144 unsigned n=d_.size();
00145 assert(x.size()==n);
00146 double sum=0.0;
00147 const double* xd = x.data_block();
00148 const double* L_col = L_.data_block();
00149 unsigned c=n;
00150 for (unsigned i=0;i<n;++i,++xd,L_col+=(n+1),--c)
00151 {
00152 double xLi = dot(L_col,n,xd,c);
00153 sum+= xLi*xLi*d_[i];
00154 }
00155 return sum;
00156 }
00157
00158
00159
00160
00161
00162 void vnl_ldl_cholesky::solve(vnl_vector<double> const& b,
00163 vnl_vector<double>* xp) const
00164 {
00165 assert(b.size() == d_.size());
00166 *xp = b;
00167 inplace_solve(xp->data_block());
00168 }
00169
00170
00171 vnl_vector<double> vnl_ldl_cholesky::solve(vnl_vector<double> const& b) const
00172 {
00173 assert(b.size() == L_.columns());
00174
00175 vnl_vector<double> ret = b;
00176 solve(b,&ret);
00177 return ret;
00178 }
00179
00180
00181 double vnl_ldl_cholesky::determinant() const
00182 {
00183 unsigned n=d_.size();
00184 double det=1.0;
00185 for (unsigned i=0;i<n;++i) det*=d_[i];
00186 return det;
00187 }
00188
00189
00190
00191
00192
00193
00194
00195 void vnl_ldl_cholesky::rank1_update(const vnl_vector<double>& v)
00196 {
00197 unsigned n = d_.size();
00198 assert(v.size()==n);
00199 double a = 1.0;
00200 vnl_vector<double> w=v;
00201 for (unsigned j=0;j<n;++j)
00202 {
00203 double a2=a+w[j]*w[j]/d_[j];
00204 d_[j]*=a2;
00205 double gamma = w[j]/d_[j];
00206 d_[j]/=a;
00207 a=a2;
00208
00209 for (unsigned p=j+1;p<n;++p)
00210 {
00211 w[p]-=w[j]*L_(p,j);
00212 L_(p,j)+=gamma*w[p];
00213 }
00214 }
00215 }
00216
00217
00218
00219
00220 void vnl_ldl_cholesky::update(const vnl_matrix<double>& W0)
00221 {
00222 unsigned n = d_.size();
00223 assert(W0.rows()==n);
00224 unsigned r = W0.columns();
00225
00226 vnl_matrix<double> W(W0);
00227 vnl_vector<double> a(r,1.0),gamma(r);
00228 for (unsigned j=0;j<n;++j)
00229 {
00230 double* Wj = W[j];
00231 for (unsigned i=0;i<r;++i)
00232 {
00233 double a2=a[i]+Wj[i]*Wj[i]/d_[j];
00234 d_[j]*=a2;
00235 gamma[i]=Wj[i]/d_[j];
00236 d_[j]/=a[i];
00237 a[i]=a2;
00238 }
00239 for (unsigned p=j+1;p<n;++p)
00240 {
00241 double *Wp = W[p];
00242 double *Lp = L_[p];
00243 for (unsigned i=0;i<r;++i)
00244 {
00245 Wp[i]-=Wj[i]*Lp[j];
00246 Lp[j]+=gamma[i]*Wp[i];
00247 }
00248 }
00249 }
00250 }
00251
00252
00253 vnl_matrix<double> vnl_ldl_cholesky::inverse() const
00254 {
00255 if (num_dims_rank_def_) {
00256 vcl_cerr << "vnl_ldl_cholesky: Calling inverse() on rank-deficient matrix\n";
00257 return vnl_matrix<double>();
00258 }
00259
00260 unsigned int n = d_.size();
00261 vnl_matrix<double> R(n,n);
00262 R.set_identity();
00263
00264
00265
00266 for (unsigned int i=0; i<n; ++i)
00267 inplace_solve(R[i]);
00268
00269 return R;
00270 }
00271