core/vnl/algo/vnl_ldl_cholesky.cxx
Go to the documentation of this file.
00001 // This is core/vnl/algo/vnl_ldl_cholesky.cxx
00002 #ifdef VCL_NEEDS_PRAGMA_INTERFACE
00003 #pragma implementation
00004 #endif
00005 //:
00006 // \file
00007 // \brief Updateable Cholesky decomposition: A=LDL'
00008 // \author Tim Cootes
00009 // \date   29 Mar 2006
00010 //
00011 //-----------------------------------------------------------------------------
00012 
00013 #include "vnl_ldl_cholesky.h"
00014 #include <vcl_cmath.h> // pow()
00015 #include <vcl_cassert.h>
00016 #include <vcl_iostream.h>
00017 #include <vnl/algo/vnl_netlib.h> // dpofa_(), dposl_(), dpoco_(), dpodi_()
00018 
00019 //: Cholesky decomposition.
00020 // Make cholesky decomposition of M optionally computing
00021 // the reciprocal condition number.  If mode is estimate_condition, the
00022 // condition number and an approximate nullspace are estimated, at a cost
00023 // of a factor of (1 + 18/n).  Here's a table of 1 + 18/n:
00024 // \verbatim
00025 // n:              3      5     10     50    100    500   1000
00026 // slowdown:     7.0    4.6    2.8    1.4   1.18   1.04   1.02
00027 // \endverbatim
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     // Quick factorization
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   // L_ is currently part of plain decomposition, M=L_ * L_.transpose()
00053   // Extract diagonal and tweak L_
00054   d_.set_size(n);
00055 
00056     //: Sqrt of elements of diagonal matrix
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   // Scale column j by 1/sqrt_d_[i] and set upper triangular elements to zero
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;   // Zero upper triangle
00072   }
00073 }
00074 
00075 //: Sum of v1[i]*v2[i]  (i=0..n-1)
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 //: Sum of v1[i*s]*v2[i]  (i=0..n-1)
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 //: Solve Lx=y (in-place)
00091 //  x is overwritten with solution
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 //: Solve Mx=b, overwriting input vector with the solution.
00100 //  x points to beginning of an n-element vector containing b
00101 //  On exit, x[i] filled with solution vector.
00102 void vnl_ldl_cholesky::inplace_solve(double* x) const
00103 {
00104   unsigned n = d_.size();
00105   // Solve Ly=b for y
00106   for (unsigned i=1;i<n;++i)
00107     x[i] -= dot(L_[i],x,i);
00108 
00109   // Scale by inverse of D
00110   for (unsigned i=0;i<n;++i) x[i]/=d_[i];
00111 
00112   // Solve L'x=y for x
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 //: Efficient computation of x' * inv(M) * x
00123 //  Useful when M is a covariance matrix!
00124 //  Solves Ly=x for y, then returns sum y[i]*y[i]/d[i]
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   // Solve Ly=x for y and compute sum as we go
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 //: Efficient computation of x' * M * x
00141 //  Twice as fast as explicitly computing x' * M * x
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);  // x * i-th column
00153     sum+= xLi*xLi*d_[i];
00154   }
00155   return sum;
00156 }
00157 
00158 
00159 //: Solve least squares problem M x = b.
00160 //  The right-hand-side vcl_vector x may be b,
00161 //  which will give a fractional increase in speed.
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 //: Solve least squares problem M x = b.
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 //: Compute determinant.
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 //: Compute rank-1 update, ie the decomposition of (M+v.v')
00190 //  If the initial state is the decomposition of M, then
00191 //  L and D are updated so that on exit  LDL'=M+v.v'
00192 //
00193 //  Uses the algorithm given by Davis and Hager in
00194 //  "Multiple-Rank Modifications of a Sparse Cholesky Factorization",2001.
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;  // Workspace, modified as algorithm goes along
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 //: Multi-rank update, ie the decomposition of (M+W.W')
00218 //  If the initial state is the decomposition of M, then
00219 //  L and D are updated so that on exit  LDL'=M+W.W'
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);  // Workspace
00227   vnl_vector<double> a(r,1.0),gamma(r);  // Workspace
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 // : Compute inverse.  Not efficient.
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   // Set each row to solution of Mx=(unit)
00265   // Since result should be symmetric, this is OK
00266   for (unsigned int i=0; i<n; ++i)
00267     inplace_solve(R[i]);
00268 
00269   return R;
00270 }
00271