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