Belle II Software  release-05-02-19
BaseFitObject.cc
1 /**************************************************************************
2  * BASF2 (Belle Analysis Framework 2) *
3  * See https://github.com/tferber/OrcaKinfit, forked from *
4  * https://github.com/iLCSoft/MarlinKinfit *
5  * *
6  * Further information about the fit engine and the user interface *
7  * provided in MarlinKinfit can be found at *
8  * https://www.desy.de/~blist/kinfit/doc/html/ *
9  * and in the LCNotes LC-TOOL-2009-001 and LC-TOOL-2009-004 available *
10  * from http://www-flc.desy.de/lcnotes/ *
11  * *
12  * Adopted by: Torben Ferber (torben.ferber@desy.de) (TF) *
13  * *
14  * This software is provided "as is" without any warranty. *
15  **************************************************************************/
16 
17 #include "analysis/OrcaKinFit/BaseFitObject.h"
18 #include <framework/logging/Logger.h>
19 
20 #undef NDEBUG
21 #include <cassert>
22 
23 #include <cstring>
24 #include <iostream>
25 #include <cmath>
26 using std::isfinite;
27 
28 #include <gsl/gsl_matrix.h>
29 #include <gsl/gsl_linalg.h>
30 
31 namespace Belle2 {
36  namespace OrcaKinFit {
37 
38  BaseFitObject::BaseFitObject(): name(nullptr), par{}, mpar{}, measured{}, fixed{}, globalParNum{}, cov{}, covinv{}, covinvvalid(
39  false),
40  cachevalid(false)
41  {
42  setName("???");
43  invalidateCache();
44 
45  for (int ilocal = 0; ilocal < BaseDefs::MAXPAR; ++ilocal) {
46  globalParNum[ilocal] = -1;
47  fixed[ilocal] = false;
48  for (int jlocal = 0; jlocal < BaseDefs::MAXPAR; ++jlocal)
49  cov[ilocal][jlocal] = 0;
50  }
51 
52  }
53 
54  BaseFitObject::BaseFitObject(const BaseFitObject& rhs)
55  : name(nullptr), par{}, mpar{}, measured{}, fixed{}, globalParNum{}, cov{}, covinv{}, covinvvalid(false), cachevalid(false)
56  {
57  BaseFitObject::assign(rhs);
58  }
59 
60  BaseFitObject& BaseFitObject::operator= (const BaseFitObject& rhs)
61  {
62  if (this != &rhs) {
63  assign(rhs); // calls virtual function assign of derived class
64  }
65  return *this;
66  }
67 
68  BaseFitObject& BaseFitObject::assign(const BaseFitObject& source)
69  {
70  if (&source != this) {
71  name = nullptr;
72  setName(source.name);
73  for (int i = 0; i < BaseDefs::MAXPAR; ++i) {
74  par[i] = source.par[i];
75  mpar[i] = source.mpar[i];
76  measured[i] = source.measured[i];
77  fixed[i] = source.fixed[i];
78  globalParNum[i] = source.globalParNum[i];
79  for (int j = 0; j < BaseDefs::MAXPAR; ++j) {
80  cov[i][j] = source.cov[i][j];
81  covinv[i][j] = source.covinv[i][j];
82  }
83  }
84  covinvvalid = false;
85  cachevalid = false;
86  }
87  return *this;
88  }
89 
90 
91  BaseFitObject::~BaseFitObject()
92  {
93  delete[] name;
94  }
95 
96  const double BaseFitObject::eps2 = 0.0001; // changed to 1^-4, then sqrt(eps2) corresponds to 1%
97 
98  void BaseFitObject::setName(const char* name_)
99  {
100  if (name_ == nullptr) return;
101  size_t l = strlen(name_);
102  if (name) delete[] name;
103  name = new char[l + 1];
104  strcpy(name, name_);
105  }
106 
107  const char* BaseFitObject::getName() const
108  {
109  return name ? name : "???";
110  }
111 
112  int BaseFitObject::getNMeasured() const
113  {
114  int nmeasured = 0;
115  for (int i = 0; i < getNPar(); ++i) if (isParamMeasured(i) && !isParamFixed(i)) ++nmeasured;
116  return nmeasured;
117  }
118  int BaseFitObject::getNUnmeasured() const
119  {
120  int nunmeasrd = 0;
121  for (int i = 0; i < getNPar(); ++i) if (!isParamMeasured(i) && !isParamFixed(i)) ++nunmeasrd;
122  return nunmeasrd;
123  }
124  int BaseFitObject::getNFree() const
125  {
126  int nfree = 0;
127  for (int i = 0; i < getNPar(); ++i) if (!isParamFixed(i)) ++nfree;
128  return nfree;
129  }
130  int BaseFitObject::getNFixed() const
131  {
132  int nfixed = 0;
133  for (int i = 0; i < getNPar(); ++i) if (isParamFixed(i)) ++nfixed;
134  return nfixed;
135  }
136 
137  std::ostream& BaseFitObject::printParams(std::ostream& os) const
138  {
139  os << "(";
140  for (int i = 0; i < getNPar(); ++i) {
141  if (i > 0) os << ", ";
142  os << " " << getParam(i);
143  if (isParamFixed(i)) os << " fix";
144 // else if (getError(i)>0) os << " \261 " << getError(i);
145  else if (getError(i) > 0) os << " +- " << getError(i); // " \261 " appeared as <B1> in ASCII ... Graham
146  }
147  os << ")";
148  return os;
149  }
150 
151  std::ostream& BaseFitObject::printRhoValues(std::ostream& os) const
152  {
153  os << "{";
154  for (int i = 0; i < getNPar(); ++i) {
155  for (int j = 0; j < getNPar(); ++j) {
156  if (i > 0 && j == 0) os << ",";
157  os << " " << getRho(i, j);
158  }
159  }
160  os << "}" << std::endl;
161  return os;
162  }
163 
164  std::ostream& BaseFitObject::print1stDerivatives(std::ostream& os) const
165  {
166  int metaSet = 0;
167  os << "#";
168  for (int i = 0; i < BaseDefs::nMetaVars[metaSet]; ++i) {
169  for (int j = 0; j < getNPar(); ++j) {
170  if (i > 0 && j == 0) os << ",";
171  os << " " << getFirstDerivative_Meta_Local(i, j, metaSet);
172  }
173  }
174  os << "#" << std::endl;
175  return os;
176  }
177 
178  std::ostream& BaseFitObject::print2ndDerivatives(std::ostream& os) const
179  {
180  int metaSet = 0;
181  os << "##";
182  for (int i = 0; i < BaseDefs::nMetaVars[metaSet]; ++i) {
183  if (i > 0) os << std::endl << " ";
184  for (int j = 0; j < getNPar(); ++j) {
185  for (int k = 0; k < getNPar(); ++k) {
186 // if (i>0 && k==0) os << ",";
187  os << " " << getSecondDerivative_Meta_Local(i, j, k, metaSet);
188  if (k == getNPar() - 1) os << ",";
189  }
190  }
191  }
192  os << "##" << std::endl;
193  return os;
194  }
195 
196  bool BaseFitObject::updateParams(double p[], int idim)
197  {
198  bool result = false;
199  invalidateCache();
200  for (int ilocal = 0; ilocal < getNPar(); ++ilocal) {
201  if (!isParamFixed(ilocal)) { // daniel added this
202  int iglobal = getGlobalParNum(ilocal);
203  assert(iglobal >= 0 && iglobal < idim);
204  // result = result || setParam (ilocal, p[iglobal]); // daniel thinks this is a BUG !!! if first param is successfully updated, no further ones are
205  // result = setParam (ilocal, p[iglobal]) || result; // daniel thinks this would be OK
206  // but to makes it clearer, does the following:
207  bool thisresult = setParam(ilocal, p[iglobal]);
208  result = result || thisresult;
209  // if illegal value: read back legal value
210  if (!thisresult) // daniel added
211  p[iglobal] = getParam(ilocal);
212  }
213  }
214  return result;
215  }
216 
217  void BaseFitObject::addToGlobCov(double* globCov, int idim) const
218  {
219  for (int ilocal = 0; ilocal < getNPar(); ++ilocal) {
220  if (!isParamFixed(ilocal) && isParamMeasured(ilocal)) {
221  int iglobal = getGlobalParNum(ilocal);
222  assert(iglobal >= 0 && iglobal < idim);
223  int ioffs = idim * iglobal;
224  for (int jlocal = 0; jlocal < getNPar(); ++jlocal) {
225  if (!isParamFixed(jlocal) && isParamMeasured(jlocal)) {
226  int jglobal = getGlobalParNum(jlocal);
227  assert(jglobal >= 0 && jglobal < idim);
228  globCov[ioffs + jglobal] += getCov(ilocal, jlocal);
229  }
230  }
231  }
232  }
233  }
234 
235  bool BaseFitObject::calculateCovInv() const
236  {
237 
238  // DANIEL added
239 
240  int n = getNPar();
241 
242  gsl_matrix* covm = gsl_matrix_alloc(n, n);
243  gsl_matrix_set_identity(covm);
244 
245  for (int i = 0; i < n; ++i) {
246  if (isParamMeasured(i)) {
247  for (int j = 0; j < n; ++j) {
248  if (isParamMeasured(j)) {
249  gsl_matrix_set(covm, i, j, cov[i][j]);
250  }
251  }
252  }
253  }
254  gsl_error_handler_t* e = gsl_set_error_handler_off();
255  int result = gsl_linalg_cholesky_decomp(covm);
256  if (result == 0) result = gsl_linalg_cholesky_invert(covm);
257  gsl_set_error_handler(e);
258 
259  for (int i = 0; i < n; ++i) {
260  for (int j = 0; j < i; ++j) {
261  covinv[i][j] = covinv[j][i] = gsl_matrix_get(covm, i, j);
262  }
263  covinv[i][i] = gsl_matrix_get(covm, i, i);
264  }
265 
266  gsl_matrix_free(covm);
267  covinvvalid = (result == 0);
268 
269  if (!covinvvalid) {
270  B2WARNING("ERROR, COULD NOT INVERT COV MATR!");
271 
272  B2INFO("COV ");
273  for (int i = 0; i < n; ++i) {
274  if (isParamMeasured(i)) {
275  for (int j = 0; j < n; ++j) {
276  if (isParamMeasured(j)) {
277  B2INFO(cov[i][j] << " ");
278  }
279  }
280  }
281  }
282 
283  B2INFO("CORREL ");
284  for (int i = 0; i < n; ++i) {
285  if (isParamMeasured(i)) {
286  for (int j = 0; j < n; ++j) {
287  if (isParamMeasured(j)) {
288  B2INFO(cov[i][j] / sqrt(cov[i][i]*cov[j][j]) << " ");
289  }
290  }
291  }
292  }
293 
294  }
295 
296  return covinvvalid;
297  }
298 
299  bool BaseFitObject::setParam(int ilocal, double par_,
300  bool measured_, bool fixed_)
301  {
302  assert(ilocal >= 0 && ilocal < getNPar());
303  if (measured[ilocal] != measured_ || fixed[ilocal] != fixed_) invalidateCache();
304  measured[ilocal] = measured_;
305  fixed[ilocal] = fixed_;
306  return setParam(ilocal, par_);
307  }
308 
309  bool BaseFitObject::setParam(int ilocal, double par_)
310  {
311  if (!isfinite(par_)) return true;
312  assert(ilocal >= 0 && ilocal < getNPar());
313  if (par[ilocal] == par_) return false;
314  invalidateCache();
315  bool result = pow((par_ - par[ilocal]) , 2) > eps2 * cov[ilocal][ilocal];
316  par[ilocal] = par_;
317  return result;
318  }
319 
320  bool BaseFitObject::setMParam(int ilocal, double mpar_)
321  {
322  if (!isfinite(mpar_)) return false;
323  assert(ilocal >= 0 && ilocal < getNPar());
324  if (mpar[ilocal] == mpar_) return true;
325  invalidateCache();
326  mpar[ilocal] = mpar_;
327  return true;
328  }
329 
330  bool BaseFitObject::setError(int ilocal, double err_)
331  {
332  if (!isfinite(err_)) return false;
333  assert(ilocal >= 0 && ilocal < getNPar());
334  invalidateCache();
335  covinvvalid = false;
336  cov[ilocal][ilocal] = err_ * err_;
337  return true;
338  }
339 
340  bool BaseFitObject::setCov(int ilocal, int jlocal, double cov_)
341  {
342  if (!isfinite(cov_)) return false;
343  assert(ilocal >= 0 && ilocal < getNPar());
344  assert(jlocal >= 0 && jlocal < getNPar());
345  invalidateCache();
346  covinvvalid = false;
347  cov[ilocal][jlocal] = cov[jlocal][ilocal] = cov_;
348  return true;
349  }
350 
351 
352  bool BaseFitObject::fixParam(int ilocal, bool fix)
353  {
354  assert(ilocal >= 0 && ilocal < getNPar());
355  return fixed [ilocal] = fix;
356  }
357 
358  bool BaseFitObject::setGlobalParNum(int ilocal, int iglobal)
359  {
360  if (ilocal < 0 || ilocal >= getNPar()) return false;
361  globalParNum[ilocal] = iglobal;
362  return true;
363  }
364 
365  int BaseFitObject::getGlobalParNum(int ilocal) const
366  {
367  if (ilocal < 0 || ilocal >= getNPar()) return -1;
368  return globalParNum[ilocal];
369  }
370 
371  double BaseFitObject::getParam(int ilocal) const
372  {
373  assert(ilocal >= 0 && ilocal < getNPar());
374  return par[ilocal];
375  }
376 
377  double BaseFitObject::getMParam(int ilocal) const
378  {
379  assert(ilocal >= 0 && ilocal < getNPar());
380  return mpar[ilocal];
381  }
382 
383  double BaseFitObject::getError(int ilocal) const
384  {
385  assert(ilocal >= 0 && ilocal < getNPar());
386  return std::sqrt(cov[ilocal][ilocal]);
387  }
388 
389  double BaseFitObject::getCov(int ilocal, int jlocal) const
390  {
391  assert(ilocal >= 0 && ilocal < getNPar());
392  assert(jlocal >= 0 && jlocal < getNPar());
393  return cov[ilocal][jlocal];
394  }
395 
396  double BaseFitObject::getRho(int ilocal, int jlocal) const
397  {
398  assert(ilocal >= 0 && ilocal < getNPar());
399  assert(jlocal >= 0 && jlocal < getNPar());
400  return cov[ilocal][jlocal] / std::sqrt(cov[ilocal][ilocal] * cov[jlocal][jlocal]);
401  }
402  bool BaseFitObject::isParamMeasured(int ilocal) const
403  {
404  assert(ilocal >= 0 && ilocal < getNPar());
405  return measured[ilocal];
406  }
407 
408  bool BaseFitObject::isParamFixed(int ilocal) const
409  {
410  assert(ilocal >= 0 && ilocal < getNPar());
411  return fixed[ilocal];
412  }
413 
414  double BaseFitObject::getChi2() const
415  {
416  if (not covinvvalid and not calculateCovInv()) return -1;
417  double chi2 = 0;
418  static double resid[BaseDefs::MAXPAR];
419  static bool chi2contr[BaseDefs::MAXPAR];
420  for (int i = 0; i < getNPar(); ++i) {
421  resid[i] = par[i] - mpar[i];
422 
423  B2INFO(" BaseFitObject::getChi2() " << i << " " << resid[i] << " " << covinv[i][i]);
424 
425  if ((chi2contr[i] = (isParamMeasured(i) && !isParamFixed(i)))) {
426  chi2 += resid[i] * covinv[i][i] * resid[i];
427  for (int j = 0; j < getNPar(); ++j) {
428  if (j < i && chi2contr[j]) chi2 += 2 * resid[i] * covinv[i][j] * resid[j];
429  }
430  }
431  }
432  return chi2;
433  }
434 
435  double BaseFitObject::getDChi2DParam(int ilocal) const
436  {
437  assert(ilocal >= 0 && ilocal < getNPar());
438  if (isParamFixed(ilocal) || !isParamMeasured(ilocal)) return 0;
439 
440  if (not covinvvalid and not calculateCovInv()) return 0;
441 
442  double result = 0;
443  for (int jlocal = 0; jlocal < getNPar(); jlocal++)
444  if (!isParamFixed(jlocal) && isParamMeasured(jlocal))
445  result += covinv[ilocal][jlocal] * (par[jlocal] - mpar[jlocal]);
446  return 2 * result;
447  }
448 
449  double BaseFitObject::getD2Chi2DParam2(int ilocal, int jlocal) const
450  {
451  assert(ilocal >= 0 && ilocal < getNPar());
452  assert(jlocal >= 0 && jlocal < getNPar());
453  if (isParamFixed(ilocal) || !isParamMeasured(ilocal) ||
454  isParamFixed(jlocal) || !isParamMeasured(jlocal))
455  return 0;
456  if (not covinvvalid and not calculateCovInv()) return 0;
457  return 2 * covinv[ilocal][jlocal]; // JL: ok in absence of soft constraints
458  }
459 
460 
461 
462  void BaseFitObject::addToGlobalChi2DerMatrix(double* M, int idim) const
463  {
464  if (!covinvvalid) calculateCovInv();
465  for (int ilocal = 0; ilocal < getNPar(); ++ilocal) {
466  if (!isParamFixed(ilocal) && isParamMeasured(ilocal)) {
467  int iglobal = getGlobalParNum(ilocal);
468  assert(iglobal >= 0 && iglobal < idim);
469  int ioffs = idim * iglobal;
470  for (int jlocal = 0; jlocal < getNPar(); ++jlocal) {
471  if (!isParamFixed(jlocal) && isParamMeasured(jlocal)) {
472  int jglobal = getGlobalParNum(jlocal);
473  assert(jglobal >= 0 && jglobal < idim);
474  M[ioffs + jglobal] += getD2Chi2DParam2(ilocal, jlocal);
475  }
476  }
477  }
478  }
479  }
480 
481 
482  int BaseFitObject::addToGlobalChi2DerVector(double* y, int idim) const
483  {
484  // This adds the dChi2/dpar piece
485  assert(getNPar() <= BaseDefs::MAXPAR);
486  if (not covinvvalid and not calculateCovInv()) return 0;
487  for (int ilocal = 0; ilocal < getNPar(); ++ilocal) {
488  if (!isParamFixed(ilocal) && isParamMeasured(ilocal)) {
489  int iglobal = getGlobalParNum(ilocal);
490  assert(iglobal >= 0 && iglobal < idim);
491  y[iglobal] += getDChi2DParam(ilocal);
492  }
493  }
494  return 0;
495  }
496 
497 
498  void BaseFitObject::addToGlobalChi2DerVector(double* y, int idim,
499  double lambda, double der[], int metaSet) const
500  {
501  // This adds the lambda * dConst/dpar piece
502  if (!cachevalid) updateCache();
503  for (int ilocal = 0; ilocal < getNPar(); ilocal++) {
504  int iglobal = globalParNum[ilocal];
505  if (iglobal >= 0) {
506  assert(iglobal < idim);
507  for (int j = 0; j < BaseDefs::nMetaVars[metaSet]; j++) {
508  y[iglobal] += lambda * der[j] * getFirstDerivative_Meta_Local(j , ilocal , metaSet);
509  }
510  }
511  }
512  }
513 
514 
515  void BaseFitObject::addTo1stDerivatives(double M[], int idim,
516  double der[], int kglobal, int metaSet) const
517  {
518  if (!cachevalid) updateCache();
519  for (int ilocal = 0; ilocal < getNPar(); ilocal++) {
520  int iglobal = globalParNum[ilocal];
521  if (iglobal >= 0) {
522  for (int j = 0; j < BaseDefs::nMetaVars[metaSet]; j++) {
523  double x = der[j] * getFirstDerivative_Meta_Local(j, ilocal , metaSet);
524  M[idim * kglobal + iglobal] += x;
525  M[idim * iglobal + kglobal] += x;
526  }
527  }
528  }
529  return;
530  }
531 
532 
533 
534  void BaseFitObject::addTo2ndDerivatives(double der2[], int idim,
535  double factor[], int metaSet
536  //double efact, double pxfact,
537  //double pyfact, double pzfact
538  ) const
539  {
540  if (!cachevalid) updateCache();
541  for (int ilocal = 0; ilocal < getNPar(); ilocal++) {
542  int iglobal = getGlobalParNum(ilocal);
543  if (iglobal < 0) continue;
544  for (int jlocal = ilocal; jlocal < getNPar(); jlocal++) {
545  int jglobal = getGlobalParNum(jlocal);
546  if (jglobal < 0) continue;
547  double sum(0);
548  for (int imeta = 0; imeta < BaseDefs::nMetaVars[metaSet]; imeta++) {
549  sum += factor[imeta] * getSecondDerivative_Meta_Local(imeta, ilocal , jlocal , metaSet);
550  }
551  der2[idim * iglobal + jglobal] += sum;
552  if (iglobal != jglobal) der2[idim * jglobal + iglobal] += sum;
553  }
554  }
555  return;
556  }
557 
558 
559  void BaseFitObject::addTo2ndDerivatives(double M[], int idim, double lambda, double der[], int metaSet) const
560  {
561  double factor[BaseDefs::MAXINTERVARS];
562  for (int i = 0; i < BaseDefs::nMetaVars[metaSet]; i++) factor[i] = lambda * der[i];
563  addTo2ndDerivatives(M, idim, factor, metaSet);
564  return;
565  }
566 
567 // seems not used
568 // void BaseFitObject::addToDerivatives (double der[], int idim,
569 // double factor[], int metaSet
570 // ) const {
571 // // DANIEL moved to BaseFitObject
572 // if (!cachevalid) updateCache();
573 // for (int ilocal=0; ilocal<getNPar(); ilocal++) {
574 // int iglobal = globalParNum[ilocal];
575 // if ( iglobal >= 0 ) {
576 // double der_sum(0);
577 // for ( int iInter=0; iInter<BaseDefs::nMetaVars[metaSet]; iInter++) {
578 // der_sum += factor[iInter]*getFirstDerivative( iInter , ilocal , metaSet );
579 // }
580 // der[iglobal] += der_sum;
581 // }
582 // }
583 // return;
584 // }
585 
586 
587  void BaseFitObject::initCov()
588  {
589  for (int i = 0; i < getNPar(); ++i) {
590  for (int j = 0; j < getNPar(); ++j) {
591  cov[i][j] = static_cast<double>(i == j);
592  }
593  }
594  }
595 
596 
597  double BaseFitObject::getError2(double der[], int metaSet) const
598  {
599  if (!cachevalid) updateCache();
600  double totError(0);
601  for (int i = 0; i < BaseDefs::nMetaVars[metaSet]; i++) {
602  for (int j = 0; j < BaseDefs::nMetaVars[metaSet]; j++) {
603  double cov_i_j = 0; // this will become the covariance of intermediate variables i and j
604  for (int k = 0; k < getNPar(); k++) {
605  for (int l = 0; l < getNPar(); l++) {
606  cov_i_j += getFirstDerivative_Meta_Local(i , k , metaSet) * cov[k][l] * getFirstDerivative_Meta_Local(j , l , metaSet);
607  }
608  }
609  totError += der[i] * der[j] * cov_i_j;
610  }
611  }
612  return totError;
613  }
614 
615  }// end OrcaKinFit namespace
617 } // end Belle2 namespace
prepareAsicCrosstalkSimDB.e
e
aux.
Definition: prepareAsicCrosstalkSimDB.py:53
Belle2
Abstract base class for different kinds of events.
Definition: MillepedeAlgorithm.h:19