Belle II Software  release-08-01-10
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
377 tools::kalmanPredictionCovSqrt(const TMatrixD& S,
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;
422  tools::transposedForwardSubstitution(a, update);
423  update *= K;
424 }
425 
426 } /* End of namespace genfit */
R E
internal precision of FFTW codelets
double R
typedef autogenerated by FFTW
#define K(x)
macro autogenerated by FFTW
double sqrt(double a)
sqrt for double
Definition: beamHelpers.h:28
Defines for I/O streams used for error and debug printing.