1 //$$ cholesky.cpp cholesky decomposition
2
3 // Copyright (C) 1991,2,3,4: R B Davies
4
5 #define WANT_MATH
6 //#define WANT_STREAM
7
8 #include <cmath>
9 #include <ossim/matrix/include.h>
10
11 #include <ossim/matrix/newmat.h>
12 #include <ossim/matrix/newmatrm.h>
13
14 #ifdef use_namespace
15 namespace NEWMAT {
16 #endif
17
18 #ifdef DO_REPORT
19 #define REPORT { static ExeCounter ExeCount(__LINE__,14); ++ExeCount; }
20 #else
21 #define REPORT {}
22 #endif
23
24 /********* Cholesky decomposition of a positive definite matrix *************/
25
26 // Suppose S is symmetrix and positive definite. Then there exists a unique
27 // lower triangular matrix L such that L L.t() = S;
28
29
Cholesky(const SymmetricMatrix & S)30 ReturnMatrix Cholesky(const SymmetricMatrix& S)
31 {
32 REPORT
33 Tracer trace("Cholesky");
34 int nr = S.Nrows();
35 LowerTriangularMatrix T(nr);
36 Real* s = S.Store(); Real* t = T.Store(); Real* ti = t;
37 for (int i=0; i<nr; i++)
38 {
39 Real* tj = t; Real sum; int k;
40 for (int j=0; j<i; j++)
41 {
42 Real* tk = ti; sum = 0.0; k = j;
43 while (k--) { sum += *tj++ * *tk++; }
44 *tk = (*s++ - sum) / *tj++;
45 }
46 sum = 0.0; k = i;
47 while (k--) { sum += square(*ti++); }
48 Real d = *s++ - sum;
49 if (d<=0.0) Throw(NPDException(S));
50 *ti++ = std::sqrt(d);
51 }
52 T.Release(); return T.ForReturn();
53 }
54
Cholesky(const SymmetricBandMatrix & S)55 ReturnMatrix Cholesky(const SymmetricBandMatrix& S)
56 {
57 REPORT
58 Tracer trace("Band-Cholesky");
59 int nr = S.Nrows(); int m = S.lower;
60 LowerBandMatrix T(nr,m);
61 Real* s = S.Store(); Real* t = T.Store(); Real* ti = t;
62
63 for (int i=0; i<nr; i++)
64 {
65 Real* tj = t; Real sum; int l;
66 if (i<m) { REPORT l = m-i; s += l; ti += l; l = i; }
67 else { REPORT t += (m+1); l = m; }
68
69 for (int j=0; j<l; j++)
70 {
71 Real* tk = ti; sum = 0.0; int k = j; tj += (m-j);
72 while (k--) { sum += *tj++ * *tk++; }
73 *tk = (*s++ - sum) / *tj++;
74 }
75 sum = 0.0;
76 while (l--) { sum += square(*ti++); }
77 Real d = *s++ - sum;
78 if (d<=0.0) Throw(NPDException(S));
79 *ti++ = std::sqrt(d);
80 }
81
82 T.Release(); return T.ForReturn();
83 }
84
85
86
87
88 // Contributed by Nick Bennett of Schlumberger-Doll Research; modified by RBD
89
90 // The enclosed routines can be used to update the Cholesky decomposition of
91 // a positive definite symmetric matrix. A good reference for this routines
92 // can be found in
93 // LINPACK User's Guide, Chapter 10, Dongarra et. al., SIAM, Philadelphia, 1979
94
95 // produces the Cholesky decomposition of A + x.t() * x where A = chol.t() * chol
UpdateCholesky(UpperTriangularMatrix & chol,RowVector r1Modification)96 void UpdateCholesky(UpperTriangularMatrix &chol, RowVector r1Modification)
97 {
98 int ncols = chol.Nrows();
99 ColumnVector cGivens(ncols); cGivens = 0.0;
100 ColumnVector sGivens(ncols); sGivens = 0.0;
101
102 for(int j = 1; j <= ncols; ++j) // process the jth column of chol
103 {
104 // apply the previous Givens rotations k = 1,...,j-1 to column j
105 for(int k = 1; k < j; ++k)
106 GivensRotation(cGivens(k), sGivens(k), chol(k,j), r1Modification(j));
107
108 // determine the jth Given's rotation
109 pythag(chol(j,j), r1Modification(j), cGivens(j), sGivens(j));
110
111 // apply the jth Given's rotation
112 {
113 Real tmp0 = cGivens(j) * chol(j,j) + sGivens(j) * r1Modification(j);
114 chol(j,j) = tmp0; r1Modification(j) = 0.0;
115 }
116
117 }
118
119 }
120
121
122 // produces the Cholesky decomposition of A - x.t() * x where A = chol.t() * chol
DowndateCholesky(UpperTriangularMatrix & chol,RowVector x)123 void DowndateCholesky(UpperTriangularMatrix &chol, RowVector x)
124 {
125 int nRC = chol.Nrows();
126
127 // solve R^T a = x
128 LowerTriangularMatrix L = chol.t();
129 ColumnVector a(nRC); a = 0.0;
130 int i, j;
131
132 for (i = 1; i <= nRC; ++i)
133 {
134 // accumulate subtr sum
135 Real subtrsum = 0.0;
136 for(int k = 1; k < i; ++k) subtrsum += a(k) * L(i,k);
137
138 a(i) = (x(i) - subtrsum) / L(i,i);
139 }
140
141 // test that l2 norm of a is < 1
142 Real squareNormA = a.SumSquare();
143 if (squareNormA >= 1.0)
144 Throw(ProgramException("DowndateCholesky() fails", chol));
145
146 Real alpha = std::sqrt(1.0 - squareNormA);
147
148 // compute and apply Givens rotations to the vector a
149 ColumnVector cGivens(nRC); cGivens = 0.0;
150 ColumnVector sGivens(nRC); sGivens = 0.0;
151 for(i = nRC; i >= 1; i--)
152 alpha = pythag(alpha, a(i), cGivens(i), sGivens(i));
153
154 // apply Givens rotations to the jth column of chol
155 ColumnVector xtilde(nRC); xtilde = 0.0;
156 for(j = nRC; j >= 1; j--)
157 {
158 // only the first j rotations have an affect on chol,0
159 for(int k = j; k >= 1; k--)
160 GivensRotation(cGivens(k), -sGivens(k), chol(k,j), xtilde(j));
161 }
162 }
163
164
165
166 // produces the Cholesky decomposition of EAE where A = chol.t() * chol
167 // and E produces a RIGHT circular shift of the rows and columns from
168 // 1,...,k-1,k,k+1,...l,l+1,...,p to
169 // 1,...,k-1,l,k,k+1,...l-1,l+1,...p
RightCircularUpdateCholesky(UpperTriangularMatrix & chol,int k,int l)170 void RightCircularUpdateCholesky(UpperTriangularMatrix &chol, int k, int l)
171 {
172 int nRC = chol.Nrows();
173 int i, j;
174
175 // I. compute shift of column l to the kth position
176 Matrix cholCopy = chol;
177 // a. grab column l
178 ColumnVector columnL = cholCopy.Column(l);
179 // b. shift columns k,...l-1 to the RIGHT
180 for(j = l-1; j >= k; --j)
181 cholCopy.Column(j+1) = cholCopy.Column(j);
182 // c. copy the top k-1 elements of columnL into the kth column of cholCopy
183 cholCopy.Column(k) = 0.0;
184 for(i = 1; i < k; ++i) cholCopy(i,k) = columnL(i);
185
186 // II. determine the l-k Given's rotations
187 int nGivens = l-k;
188 ColumnVector cGivens(nGivens); cGivens = 0.0;
189 ColumnVector sGivens(nGivens); sGivens = 0.0;
190 for(i = l; i > k; i--)
191 {
192 int givensIndex = l-i+1;
193 columnL(i-1) = pythag(columnL(i-1), columnL(i),
194 cGivens(givensIndex), sGivens(givensIndex));
195 columnL(i) = 0.0;
196 }
197 // the kth entry of columnL is the new diagonal element in column k of cholCopy
198 cholCopy(k,k) = columnL(k);
199
200 // III. apply these Given's rotations to subsequent columns
201 // for columns k+1,...,l-1 we only need to apply the last nGivens-(j-k) rotations
202 for(j = k+1; j <= nRC; ++j)
203 {
204 ColumnVector columnJ = cholCopy.Column(j);
205 int imin = nGivens - (j-k) + 1; if (imin < 1) imin = 1;
206 for(int gIndex = imin; gIndex <= nGivens; ++gIndex)
207 {
208 // apply gIndex Given's rotation
209 int topRowIndex = k + nGivens - gIndex;
210 GivensRotationR(cGivens(gIndex), sGivens(gIndex),
211 columnJ(topRowIndex), columnJ(topRowIndex+1));
212 }
213 cholCopy.Column(j) = columnJ;
214 }
215
216 chol << cholCopy;
217 }
218
219
220
221 // produces the Cholesky decomposition of EAE where A = chol.t() * chol
222 // and E produces a LEFT circular shift of the rows and columns from
223 // 1,...,k-1,k,k+1,...l,l+1,...,p to
224 // 1,...,k-1,k+1,...l,k,l+1,...,p to
LeftCircularUpdateCholesky(UpperTriangularMatrix & chol,int k,int l)225 void LeftCircularUpdateCholesky(UpperTriangularMatrix &chol, int k, int l)
226 {
227 int nRC = chol.Nrows();
228 int i, j;
229
230 // I. compute shift of column k to the lth position
231 Matrix cholCopy = chol;
232 // a. grab column k
233 ColumnVector columnK = cholCopy.Column(k);
234 // b. shift columns k+1,...l to the LEFT
235 for(j = k+1; j <= l; ++j)
236 cholCopy.Column(j-1) = cholCopy.Column(j);
237 // c. copy the elements of columnK into the lth column of cholCopy
238 cholCopy.Column(l) = 0.0;
239 for(i = 1; i <= k; ++i)
240 cholCopy(i,l) = columnK(i);
241
242 // II. apply and compute Given's rotations
243 int nGivens = l-k;
244 ColumnVector cGivens(nGivens); cGivens = 0.0;
245 ColumnVector sGivens(nGivens); sGivens = 0.0;
246 for(j = k; j <= nRC; ++j)
247 {
248 ColumnVector columnJ = cholCopy.Column(j);
249
250 // apply the previous Givens rotations to columnJ
251 int imax = j - k; if (imax > nGivens) imax = nGivens;
252 for(int i = 1; i <= imax; ++i)
253 {
254 int gIndex = i;
255 int topRowIndex = k + i - 1;
256 GivensRotationR(cGivens(gIndex), sGivens(gIndex),
257 columnJ(topRowIndex), columnJ(topRowIndex+1));
258 }
259
260 // compute a new Given's rotation when j < l
261 if(j < l)
262 {
263 int gIndex = j-k+1;
264 columnJ(j) = pythag(columnJ(j), columnJ(j+1), cGivens(gIndex), sGivens(gIndex));
265 columnJ(j+1) = 0.0;
266 }
267
268 cholCopy.Column(j) = columnJ;
269 }
270
271 chol << cholCopy;
272
273 }
274
275
276
277
278 #ifdef use_namespace
279 }
280 #endif
281
282