Analysis Software
Documentation for sPHENIX simulation software
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
Tools.cc
Go to the documentation of this file. Or view the newest version in sPHENIX GitHub for file Tools.cc
1 /* Copyright 2008-2010, Technische Universitaet Muenchen,
2  Authors: Christian Hoeppner & Sebastian Neubert & Johannes Rauch
3 
4  This file is part of GENFIT.
5 
6  GENFIT is free software: you can redistribute it and/or modify
7  it under the terms of the GNU Lesser General Public License as published
8  by the Free Software Foundation, either version 3 of the License, or
9  (at your option) any later version.
10 
11  GENFIT is distributed in the hope that it will be useful,
12  but WITHOUT ANY WARRANTY; without even the implied warranty of
13  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  GNU Lesser General Public License for more details.
15 
16  You should have received a copy of the GNU Lesser General Public License
17  along with GENFIT. If not, see <http://www.gnu.org/licenses/>.
18 */
19 
20 #include "Tools.h"
21 
22 #include <cmath>
23 #include <memory>
24 #include <typeinfo>
25 #include <cassert>
26 
27 #include <TDecompChol.h>
28 #include <TMatrixDSymEigen.h>
29 #include <TMatrixTSymCramerInv.h>
30 #include <TMath.h>
31 
32 #include "AbsHMatrix.h"
33 #include "Exception.h"
34 
35 
36 namespace genfit {
37 
38 void tools::invertMatrix(const TMatrixDSym& mat, TMatrixDSym& inv, double* determinant){
39  inv.ResizeTo(mat);
40 
41  // check if numerical limits are reached (i.e at least one entry < 1E-100 and/or at least one entry > 1E100)
42  if (!(mat<1.E100) || !(mat>-1.E100)){
43  Exception e("Tools::invertMatrix() - cannot invert matrix, entries too big (>1e100)",
44  __LINE__,__FILE__);
45  e.setFatal();
46  throw e;
47  }
48  // do the trivial inversions for 1x1 and 2x2 matrices manually
49  if (mat.GetNrows() == 1){
50  if (determinant != nullptr) *determinant = mat(0,0);
51  inv(0,0) = 1./mat(0,0);
52  return;
53  }
54 
55  if (mat.GetNrows() == 2){
56  double det = mat(0,0)*mat(1,1) - mat(1,0)*mat(1,0);
57  if (determinant != nullptr) *determinant = det;
58  if(fabs(det) < 1E-50){
59  Exception e("Tools::invertMatrix() - cannot invert matrix , determinant = 0",
60  __LINE__,__FILE__);
61  e.setFatal();
62  throw e;
63  }
64  det = 1./det;
65  inv(0,0) = det * mat(1,1);
66  inv(0,1) = inv(1,0) = -det * mat(1,0);
67  inv(1,1) = det * mat(0,0);
68  return;
69  }
70 
71  // else use TDecompChol
72  bool status = 0;
73  TDecompChol invertAlgo(mat, 1E-50);
74 
75  status = invertAlgo.Invert(inv);
76  if(status == 0){
77  Exception e("Tools::invertMatrix() - cannot invert matrix, status = 0",
78  __LINE__,__FILE__);
79  e.setFatal();
80  throw e;
81  }
82 
83  if (determinant != nullptr) {
84  double d1, d2;
85  invertAlgo.Det(d1, d2);
86  *determinant = ldexp(d1, d2);
87  }
88 }
89 
90 void tools::invertMatrix(TMatrixDSym& mat, double* determinant){
91  // check if numerical limits are reached (i.e at least one entry < 1E-100 and/or at least one entry > 1E100)
92  if (!(mat<1.E100) || !(mat>-1.E100)){
93  Exception e("Tools::invertMatrix() - cannot invert matrix, entries too big (>1e100)",
94  __LINE__,__FILE__);
95  e.setFatal();
96  throw e;
97  }
98  // do the trivial inversions for 1x1 and 2x2 matrices manually
99  if (mat.GetNrows() == 1){
100  if (determinant != nullptr) *determinant = mat(0,0);
101  mat(0,0) = 1./mat(0,0);
102  return;
103  }
104 
105  if (mat.GetNrows() == 2){
106  double *arr = mat.GetMatrixArray();
107  double det = arr[0]*arr[3] - arr[1]*arr[1];
108  if (determinant != nullptr) *determinant = det;
109  if(fabs(det) < 1E-50){
110  Exception e("Tools::invertMatrix() - cannot invert matrix, determinant = 0",
111  __LINE__,__FILE__);
112  e.setFatal();
113  throw e;
114  }
115  det = 1./det;
116  double temp[3];
117  temp[0] = det * arr[3];
118  temp[1] = -det * arr[1];
119  temp[2] = det * arr[0];
120  //double *arr = mat.GetMatrixArray();
121  arr[0] = temp[0];
122  arr[1] = arr[2] = temp[1];
123  arr[3] = temp[2];
124  return;
125  }
126 
127  // else use TDecompChol
128  bool status = 0;
129  TDecompChol invertAlgo(mat, 1E-50);
130 
131  status = invertAlgo.Invert(mat);
132  if(status == 0){
133  Exception e("Tools::invertMatrix() - cannot invert matrix, status = 0",
134  __LINE__,__FILE__);
135  e.setFatal();
136  throw e;
137  }
138 
139  if (determinant != nullptr) {
140  double d1, d2;
141  invertAlgo.Det(d1, d2);
142  *determinant = ldexp(d1, d2);
143  }
144 }
145 
146 
147 // Solves R^T x = b, replaces b with the result x. R is assumed
148 // to be upper-diagonal. This is forward substitution, but with
149 // indices flipped.
150 bool tools::transposedForwardSubstitution(const TMatrixD& R, TVectorD& b)
151 {
152  size_t n = R.GetNrows();
153  double *const bk = b.GetMatrixArray();
154  const double *const Rk = R.GetMatrixArray();
155  for (unsigned int i = 0; i < n; ++i) {
156  double sum = bk[i];
157  for (unsigned int j = 0; j < i; ++j) {
158  sum -= bk[j]*Rk[j*n + i]; // already replaced previous elements in b.
159  }
160  if (Rk[i*n+i] == 0)
161  return false;
162  bk[i] = sum / Rk[i*n + i];
163  }
164  return true;
165 }
166 
167 
168 // Same, but for one column of the matrix b. Used by transposedInvert below
169 // assumes b(i,j) == (i == j)
170 bool tools::transposedForwardSubstitution(const TMatrixD& R, TMatrixD& b, int nCol)
171 {
172  size_t n = R.GetNrows();
173  double *const bk = b.GetMatrixArray() + nCol;
174  const double *const Rk = R.GetMatrixArray();
175  for (unsigned int i = nCol; i < n; ++i) {
176  double sum = (i == (size_t)nCol);
177  for (unsigned int j = 0; j < i; ++j) {
178  sum -= bk[j*n]*Rk[j*n + i]; // already replaced previous elements in b.
179  }
180  if (Rk[i*n+i] == 0)
181  return false;
182  bk[i*n] = sum / Rk[i*n + i];
183  }
184  return true;
185 }
186 
187 
188 // inv will be the inverse of the transposed of the upper-right matrix R
189 bool tools::transposedInvert(const TMatrixD& R, TMatrixD& inv)
190 {
191  bool result = true;
192 
193  inv.ResizeTo(R);
194  double *const invk = inv.GetMatrixArray();
195  int nRows = inv.GetNrows();
196  for (int i = 0; i < nRows; ++i)
197  for (int j = 0; j < nRows; ++j)
198  invk[i*nRows + j] = (i == j);
199 
200  for (int i = 0; i < inv.GetNcols(); ++i)
201  result = result && transposedForwardSubstitution(R, inv, i);
202 
203  return result;
204 }
205 
206 // This replaces A with an upper right matrix connected to A by a
207 // orthogonal transformation. I.e., it computes the R from a QR
208 // decomposition of A replacing A.
209 void tools::QR(TMatrixD& A)
210 {
211  int nCols = A.GetNcols();
212  int nRows = A.GetNrows();
213  assert(nRows >= nCols);
214  // This uses Businger and Golub's algorithm from Handbook for
215  // Automatical Computation, Vol. 2, Chapter 8, but without
216  // pivoting. I.e., we stop at the middle of page 112. We don't
217  // explicitly calculate the orthogonal matrix.
218 
219  double *const ak = A.GetMatrixArray();
220  // No variable-length arrays in C++, alloca does the exact same thing ...
221  double *const u = (double *)alloca(sizeof(double)*nRows);
222 
223  // Main loop over matrix columns.
224  for (int k = 0; k < nCols; ++k) {
225  double akk = ak[k*nCols + k];
226 
227  double sum = akk*akk;
228  // Put together a housholder transformation.
229  for (int i = k + 1; i < nRows; ++i) {
230  sum += ak[i*nCols + k]*ak[i*nCols + k];
231  u[i] = ak[i*nCols + k];
232  }
233  double sigma = sqrt(sum);
234  double beta = 1/(sum + sigma*fabs(akk));
235  // The algorithm uses only the uk[i] for i >= k.
236  u[k] = copysign(sigma + fabs(akk), akk);
237 
238  // Calculate y (again taking into account zero entries). This
239  // encodes how the (sub)matrix changes by the householder transformation.
240  for (int i = k; i < nCols; ++i) {
241  double y = 0;
242  for (int j = k; j < nRows; ++j)
243  y += u[j]*ak[j*nCols + i];
244  y *= beta;
245  // ... and apply the changes.
246  for (int j = k; j < nRows; ++j)
247  ak[j*nCols + i] -= u[j]*y; //y[j];
248  }
249  }
250 
251  // Zero below diagonal
252  for (int i = 1; i < nCols; ++i)
253  for (int j = 0; j < i; ++j)
254  ak[i*nCols + j] = 0.;
255  for (int i = nCols; i < nRows; ++i)
256  for (int j = 0; j < nCols; ++j)
257  ak[i*nCols + j] = 0.;
258 }
259 
260 // This replaces A with an upper right matrix connected to A by a
261 // orthogonal transformation. I.e., it computes the R from a QR
262 // decomposition of A replacing A. Simultaneously it transforms b by
263 // the inverse orthogonal transformation.
264 //
265 // The purpose is this: the least-squared problem
266 // ||Ax - b|| = min
267 // is equivalent to
268 // ||QRx - b|| = ||Rx - Q'b|| = min
269 // where Q' denotes the transposed (i.e. inverse).
270 void tools::QR(TMatrixD& A, TVectorD& b)
271 {
272  int nCols = A.GetNcols();
273  int nRows = A.GetNrows();
274  assert(nRows >= nCols);
275  assert(b.GetNrows() == nRows);
276  // This uses Businger and Golub's algorithm from Handbook for
277  // Automatic Computation, Vol. 2, Chapter 8, but without pivoting.
278  // I.e., we stop at the middle of page 112. We don't explicitly
279  // calculate the orthogonal matrix, but Q'b which is not done
280  // explicitly in Businger et al.
281  // Also in Numer. Math. 7, 269-276 (1965)
282 
283  double *const ak = A.GetMatrixArray();
284  double *const bk = b.GetMatrixArray();
285  // No variable-length arrays in C++, alloca does the exact same thing ...
286  //double * u = (double *)alloca(sizeof(double)*nRows);
287  double u[500];
288 
289  // Main loop over matrix columns.
290  for (int k = 0; k < nCols; ++k) {
291  double akk = ak[k*nCols + k];
292 
293  double sum = akk*akk;
294  // Put together a housholder transformation.
295  for (int i = k + 1; i < nRows; ++i) {
296  sum += ak[i*nCols + k]*ak[i*nCols + k];
297  u[i] = ak[i*nCols + k];
298  }
299  double sigma = sqrt(sum);
300  double beta = 1/(sum + sigma*fabs(akk));
301  // The algorithm uses only the uk[i] for i >= k.
302  u[k] = copysign(sigma + fabs(akk), akk);
303 
304  // Calculate b (again taking into account zero entries). This
305  // encodes how the (sub)vector changes by the householder transformation.
306  double yb = 0;
307  for (int j = k; j < nRows; ++j)
308  yb += u[j]*bk[j];
309  yb *= beta;
310  // ... and apply the changes.
311  for (int j = k; j < nRows; ++j)
312  bk[j] -= u[j]*yb;
313 
314  // Calculate y (again taking into account zero entries). This
315  // encodes how the (sub)matrix changes by the householder transformation.
316  for (int i = k; i < nCols; ++i) {
317  double y = 0;
318  for (int j = k; j < nRows; ++j)
319  y += u[j]*ak[j*nCols + i];
320  y *= beta;
321  // ... and apply the changes.
322  for (int j = k; j < nRows; ++j)
323  ak[j*nCols + i] -= u[j]*y;
324  }
325  }
326 
327  // Zero below diagonal
328  for (int i = 1; i < nCols; ++i)
329  for (int j = 0; j < i; ++j)
330  ak[i*nCols + j] = 0.;
331  for (int i = nCols; i < nRows; ++i)
332  for (int j = 0; j < nCols; ++j)
333  ak[i*nCols + j] = 0.;
334 }
335 
336 
337 void
338 tools::noiseMatrixSqrt(const TMatrixDSym& noise,
339  TMatrixD& noiseSqrt)
340 {
341  // This is the slowest part of the whole Sqrt Kalman. Using an LDLt
342  // transform is probably the easiest way of remedying this.
343  TMatrixDSymEigen eig(noise);
344  noiseSqrt.ResizeTo(noise);
345  noiseSqrt = eig.GetEigenVectors();
346  double* pNoiseSqrt = noiseSqrt.GetMatrixArray();
347  const TVectorD& evs(eig.GetEigenValues());
348  const double* pEvs = evs.GetMatrixArray();
349  // GetEigenVectors is such that noise = noiseSqrt * evs * noiseSqrt'
350  // We're evaluating the first product with the eigenvalues replaced
351  // by their square roots, so we're multiplying with a diagonal
352  // matrix from the right.
353  int iCol = 0;
354  for (; iCol < noiseSqrt.GetNrows(); ++iCol) {
355  double ev = pEvs[iCol] > 0 ? sqrt(pEvs[iCol]) : 0;
356  // if (ev == 0)
357  // break;
358  for (int j = 0; j < noiseSqrt.GetNrows(); ++j) {
359  pNoiseSqrt[j*noiseSqrt.GetNcols() + iCol] *= ev;
360  }
361  }
362  if (iCol < noiseSqrt.GetNcols()) {
363  // Hit zero eigenvalue, resize matrix
364  noiseSqrt.ResizeTo(noiseSqrt.GetNrows(), iCol);
365  }
366 
367  // noiseSqrt * noiseSqrt' = noise
368 }
369 
370 
371 // Transports the square root of the covariance matrix using a
372 // square-root formalism
373 //
374 // With covariance square root S, transport matrix F and noise matrix
375 // square root Q.
376 void
378  const TMatrixD& F, const TMatrixD& Q,
379  TMatrixD& Snew)
380 {
381  Snew.ResizeTo(S.GetNrows() + Q.GetNcols(),
382  S.GetNcols());
383 
384  // This overwrites all elements, no precautions necessary
385  Snew.SetSub(0, 0, TMatrixD(S, TMatrixD::kMultTranspose, F));
386  if (Q.GetNcols() != 0)
387  Snew.SetSub(S.GetNrows(), 0, TMatrixD(TMatrixD::kTransposed, Q));
388 
389  tools::QR(Snew);
390 
391  // The result is in the upper right corner of the matrix.
392  Snew.ResizeTo(S.GetNrows(), S.GetNrows());
393 }
394 
395 
396 // Kalman measurement update (no transport)
397 // x, S : state prediction, covariance square root
398 // res, R, H : residual, measurement covariance square root, H matrix of the measurement
399 // gives the update (new state = x + update) and the updated covariance square root.
400 // S and Snew are allowed to refer to the same object.
401 void
402 tools::kalmanUpdateSqrt(const TMatrixD& S,
403  const TVectorD& res, const TMatrixD& R,
404  const AbsHMatrix* H,
405  TVectorD& update, TMatrixD& SNew)
406 {
407  TMatrixD pre(S.GetNrows() + R.GetNrows(),
408  S.GetNcols() + R.GetNcols());
409  pre.SetSub(0, 0, R); /* Zeros in upper right block */
410  pre.SetSub(R.GetNrows(), 0, H->MHt(S)); pre.SetSub(R.GetNrows(), R.GetNcols(), S);
411 
412  tools::QR(pre);
413  const TMatrixD& r = pre;
414 
415  const TMatrixD& a(r.GetSub(0, R.GetNrows()-1,
416  0, R.GetNcols()-1));
417  TMatrixD K(TMatrixD::kTransposed, r.GetSub(0, R.GetNrows()-1, R.GetNcols(), pre.GetNcols()-1));
418  SNew = r.GetSub(R.GetNrows(), pre.GetNrows()-1, R.GetNcols(), pre.GetNcols()-1);
419 
420  update.ResizeTo(res);
421  update = res;
423  update *= K;
424 }
425 
426 } /* End of namespace genfit */