1 // This is core/vnl/algo/vnl_ldl_cholesky.cxx
2 //:
3 // \file
4 // \brief Updateable Cholesky decomposition: A=LDL'
5 // \author Tim Cootes
6 // \date   29 Mar 2006
7 //
8 //-----------------------------------------------------------------------------
9 
10 #include <cmath>
11 #include <cassert>
12 #include <iostream>
13 #include "vnl_ldl_cholesky.h"
14 #include <vnl/algo/vnl_netlib.h> // dpofa_(), dposl_(), dpoco_(), dpodi_()
15 
16 //: Cholesky decomposition.
17 // Make cholesky decomposition of M optionally computing
18 // the reciprocal condition number.  If mode is estimate_condition, the
19 // condition number and an approximate nullspace are estimated, at a cost
20 // of a factor of (1 + 18/n).  Here's a table of 1 + 18/n:
21 // \verbatim
22 // n:              3      5     10     50    100    500   1000
23 // slowdown:     7.0    4.6    2.8    1.4   1.18   1.04   1.02
24 // \endverbatim
25 
vnl_ldl_cholesky(vnl_matrix<double> const & M,Operation mode)26 vnl_ldl_cholesky::vnl_ldl_cholesky(vnl_matrix<double> const & M, Operation mode):
27   L_(M)
28 {
29   long n = M.columns();
30   assert(n == (int)(M.rows()));
31   num_dims_rank_def_ = -1;
32   if (std::fabs(M(0,n-1) - M(n-1,0)) > 1e-8) {
33     std::cerr << "vnl_ldl_cholesky: WARNING: non-symmetric: " << M << std::endl;
34   }
35 
36   if (mode != estimate_condition) {
37     // Quick factorization
38     v3p_netlib_dpofa_(L_.data_block(), &n, &n, &num_dims_rank_def_);
39     if (mode == verbose && num_dims_rank_def_ != 0)
40       std::cerr << "vnl_ldl_cholesky: " << num_dims_rank_def_ << " dimensions of non-posdeffness\n";
41   }
42   else {
43     vnl_vector<double> nullvec(n);
44     v3p_netlib_dpoco_(L_.data_block(), &n, &n, &rcond_, nullvec.data_block(), &num_dims_rank_def_);
45     if (num_dims_rank_def_ != 0)
46       std::cerr << "vnl_ldl_cholesky: rcond=" << rcond_ << " so " << num_dims_rank_def_ << " dimensions of non-posdeffness\n";
47   }
48 
49   // L_ is currently part of plain decomposition, M=L_ * L_.transpose()
50   // Extract diagonal and tweak L_
51   d_.set_size(n);
52 
53     //: Sqrt of elements of diagonal matrix
54   vnl_vector<double> sqrt_d(n);
55 
56   for (int i=0; i<n; ++i)
57   {
58     sqrt_d[i]=L_(i,i);
59     d_[i]=sqrt_d[i]*sqrt_d[i];
60   }
61 
62   // Scale column j by 1/sqrt_d_[i] and set upper triangular elements to zero
63   for (int i=0; i<n; ++i)
64   {
65     double *row = L_[i];
66     for (int j=0; j<i; ++j) row[j]/=sqrt_d[j];
67     row[i]=1.0;
68     for (int j=i+1; j<n; ++j) row[j]=0.0;   // Zero upper triangle
69   }
70 }
71 
72 //: Sum of v1[i]*v2[i]  (i=0..n-1)
dot(const double * v1,const double * v2,unsigned n)73 inline double dot(const double* v1, const double* v2, unsigned n)
74 {
75   double sum=0.0;
76   for (unsigned i=0;i<n;++i) sum+= v1[i]*v2[i];
77   return sum;
78 }
79 //: Sum of v1[i*s]*v2[i]  (i=0..n-1)
dot(const double * v1,unsigned s,const double * v2,unsigned n)80 inline double dot(const double* v1, unsigned s, const double* v2, unsigned n)
81 {
82   double sum=0.0;
83   for (unsigned i=0;i<n;++i,v1+=s) sum+= (*v1)*v2[i];
84   return sum;
85 }
86 
87 //: Solve Lx=y (in-place)
88 //  x is overwritten with solution
solve_lx(vnl_vector<double> & x)89 void vnl_ldl_cholesky::solve_lx(vnl_vector<double>& x)
90 {
91   unsigned n = d_.size();
92   for (unsigned i=1;i<n;++i)
93     x[i] -= dot(L_[i],x.data_block(),i);
94 }
95 
96 //: Solve Mx=b, overwriting input vector with the solution.
97 //  x points to beginning of an n-element vector containing b
98 //  On exit, x[i] filled with solution vector.
inplace_solve(double * x) const99 void vnl_ldl_cholesky::inplace_solve(double* x) const
100 {
101   unsigned n = d_.size();
102   // Solve Ly=b for y
103   for (unsigned i=1;i<n;++i)
104     x[i] -= dot(L_[i],x,i);
105 
106   // Scale by inverse of D
107   for (unsigned i=0;i<n;++i) x[i]/=d_[i];
108 
109   // Solve L'x=y for x
110   const double* L_data = &L_(n-1,n-2);
111   const double* x_data = &x[n-1];
112   unsigned c=1;
113   for (int i=n-2;i>=0;--i,L_data-=(n+1),--x_data,++c)
114   {
115     x[i] -= dot(L_data,n,x_data,c);
116   }
117 }
118 
119 //: Efficient computation of x' * inv(M) * x
120 //  Useful when M is a covariance matrix!
121 //  Solves Ly=x for y, then returns sum y[i]*y[i]/d[i]
xt_m_inv_x(const vnl_vector<double> & x) const122 double vnl_ldl_cholesky::xt_m_inv_x(const vnl_vector<double>& x) const
123 {
124   unsigned n=d_.size();
125   assert(x.size()==n);
126   vnl_vector<double> y=x;
127   // Solve Ly=x for y and compute sum as we go
128   double sum = y[0]*y[0]/d_[0];
129   for (unsigned i=1;i<n;++i)
130   {
131     y[i] -= dot(L_[i],y.data_block(),i);
132     sum += y[i]*y[i]/d_[i];
133   }
134   return sum;
135 }
136 
137 //: Efficient computation of x' * M * x
138 //  Twice as fast as explicitly computing x' * M * x
xt_m_x(const vnl_vector<double> & x) const139 double vnl_ldl_cholesky::xt_m_x(const vnl_vector<double>& x) const
140 {
141   unsigned n=d_.size();
142   assert(x.size()==n);
143   double sum=0.0;
144   const double* xd = x.data_block();
145   const double* L_col = L_.data_block();
146   unsigned c=n;
147   for (unsigned i=0;i<n;++i,++xd,L_col+=(n+1),--c)
148   {
149     double xLi = dot(L_col,n,xd,c);  // x * i-th column
150     sum+= xLi*xLi*d_[i];
151   }
152   return sum;
153 }
154 
155 
156 //: Solve least squares problem M x = b.
157 //  The right-hand-side std::vector x may be b,
158 //  which will give a fractional increase in speed.
solve(vnl_vector<double> const & b,vnl_vector<double> * xp) const159 void vnl_ldl_cholesky::solve(vnl_vector<double> const& b,
160                              vnl_vector<double>* xp) const
161 {
162   assert(b.size() == d_.size());
163   *xp = b;
164   inplace_solve(xp->data_block());
165 }
166 
167 //: Solve least squares problem M x = b.
solve(vnl_vector<double> const & b) const168 vnl_vector<double> vnl_ldl_cholesky::solve(vnl_vector<double> const& b) const
169 {
170   assert(b.size() == L_.columns());
171 
172   vnl_vector<double> ret = b;
173   solve(b,&ret);
174   return ret;
175 }
176 
177 //: Compute determinant.
determinant() const178 double vnl_ldl_cholesky::determinant() const
179 {
180   unsigned n=d_.size();
181   double det=1.0;
182   for (unsigned i=0;i<n;++i) det*=d_[i];
183   return det;
184 }
185 
186 //: Compute rank-1 update, ie the decomposition of (M+v.v')
187 //  If the initial state is the decomposition of M, then
188 //  L and D are updated so that on exit  LDL'=M+v.v'
189 //
190 //  Uses the algorithm given by Davis and Hager in
191 //  "Multiple-Rank Modifications of a Sparse Cholesky Factorization",2001.
rank1_update(const vnl_vector<double> & v)192 void vnl_ldl_cholesky::rank1_update(const vnl_vector<double>& v)
193 {
194   unsigned n = d_.size();
195   assert(v.size()==n);
196   double a = 1.0;
197   vnl_vector<double> w=v;  // Workspace, modified as algorithm goes along
198   for (unsigned j=0;j<n;++j)
199   {
200     double a2=a+w[j]*w[j]/d_[j];
201     d_[j]*=a2;
202     double gamma = w[j]/d_[j];
203     d_[j]/=a;
204     a=a2;
205 
206     for (unsigned p=j+1;p<n;++p)
207     {
208       w[p]-=w[j]*L_(p,j);
209       L_(p,j)+=gamma*w[p];
210     }
211   }
212 }
213 
214 //: Multi-rank update, ie the decomposition of (M+W.W')
215 //  If the initial state is the decomposition of M, then
216 //  L and D are updated so that on exit  LDL'=M+W.W'
update(const vnl_matrix<double> & W0)217 void vnl_ldl_cholesky::update(const vnl_matrix<double>& W0)
218 {
219   unsigned n = d_.size();
220   assert(W0.rows()==n);
221   unsigned r = W0.columns();
222 
223   vnl_matrix<double> W(W0);  // Workspace
224   vnl_vector<double> a(r,1.0),gamma(r);  // Workspace
225   for (unsigned j=0;j<n;++j)
226   {
227     double* Wj = W[j];
228     for (unsigned i=0;i<r;++i)
229     {
230       double a2=a[i]+Wj[i]*Wj[i]/d_[j];
231       d_[j]*=a2;
232       gamma[i]=Wj[i]/d_[j];
233       d_[j]/=a[i];
234       a[i]=a2;
235     }
236     for (unsigned p=j+1;p<n;++p)
237     {
238       double *Wp = W[p];
239       double *Lp = L_[p];
240       for (unsigned i=0;i<r;++i)
241       {
242         Wp[i]-=Wj[i]*Lp[j];
243         Lp[j]+=gamma[i]*Wp[i];
244       }
245     }
246   }
247 }
248 
249 // : Compute inverse.  Not efficient.
inverse() const250 vnl_matrix<double> vnl_ldl_cholesky::inverse() const
251 {
252   if (num_dims_rank_def_) {
253     std::cerr << "vnl_ldl_cholesky: Calling inverse() on rank-deficient matrix\n";
254     return vnl_matrix<double>();
255   }
256 
257   unsigned int n = d_.size();
258   vnl_matrix<double> R(n,n);
259   R.set_identity();
260 
261   // Set each row to solution of Mx=(unit)
262   // Since result should be symmetric, this is OK
263   for (unsigned int i=0; i<n; ++i)
264     inplace_solve(R[i]);
265 
266   return R;
267 }
268