Belle II Software  release-06-02-00
NewFitterGSL.cc
1 /**************************************************************************
2  * basf2 (Belle II Analysis Software Framework) *
3  * Author: The Belle II Collaboration *
4  * *
5  * Forked from https://github.com/iLCSoft/MarlinKinfit *
6  * *
7  * Further information about the fit engine and the user interface *
8  * provided in MarlinKinfit can be found at *
9  * https://www.desy.de/~blist/kinfit/doc/html/ *
10  * and in the LCNotes LC-TOOL-2009-001 and LC-TOOL-2009-004 available *
11  * from http://www-flc.desy.de/lcnotes/ *
12  * *
13  * See git log for contributors and copyright holders. *
14  * This file is licensed under LGPL-3.0, see LICENSE.md. *
15  **************************************************************************/
16 
17 #undef NDEBUG
18 
19 #include "analysis/OrcaKinFit/NewFitterGSL.h"
20 #include <framework/logging/Logger.h>
21 
22 #include<iostream>
23 #include<cmath>
24 #include<cassert>
25 #include<limits>
26 
27 #include "analysis/OrcaKinFit/BaseFitObject.h"
28 #include "analysis/OrcaKinFit/BaseHardConstraint.h"
29 #include "analysis/OrcaKinFit/BaseSoftConstraint.h"
30 #include "analysis/OrcaKinFit/BaseTracer.h"
31 
32 #include <gsl/gsl_block.h>
33 #include <gsl/gsl_vector.h>
34 #include <gsl/gsl_matrix.h>
35 #include <gsl/gsl_permutation.h>
36 #include <gsl/gsl_linalg.h>
37 #include <gsl/gsl_blas.h>
38 #include <gsl/gsl_cdf.h>
39 
40 using std::abs;
41 
42 namespace Belle2 {
47  namespace OrcaKinFit {
48 
49  static int debuglevel = 0;
50  static int nitdebug = 0;
51 // static int nitcalc = 0;
52 // static int nitsvd = 0;
53 
54 // constructor
55  NewFitterGSL::NewFitterGSL()
56  : npar(0), ncon(0), nsoft(0), nunm(0), ierr(0), nit(0),
57  fitprob(0), chi2(0), idim(0), x(nullptr), xold(nullptr), xnew(nullptr),
58  // xbest(0),
59  dx(nullptr), dxscal(nullptr),
60  //grad(0),
61  y(nullptr), yscal(nullptr),
62  perr(nullptr), v1(nullptr), v2(nullptr),
63  //Meval (0),
64  M(nullptr), Mscal(nullptr), W(nullptr), W2(nullptr), W3(nullptr),
65  M1(nullptr), M2(nullptr), M3(nullptr), M4(nullptr), M5(nullptr),
66  //Mevec (0),
67  CC(nullptr), CC1(nullptr), CCinv(nullptr),
68  permW(nullptr), eigenws(nullptr), eigenwsdim(0),
69  chi2best(0), chi2new(0), chi2old(0), fvalbest(0),
70  scale(0), scalebest(0), stepsize(0), stepbest(0),
71  scalevals{0}, fvals{0} ,
72  imerit(1),
73  try2ndOrderCorr(true),
74  debug(debuglevel)
75  {}
76 
77 // destructor
78  NewFitterGSL::~NewFitterGSL()
79  {
80 
81  if (x) gsl_vector_free(x);
82  if (xold) gsl_vector_free(xold);
83  if (xnew) gsl_vector_free(xnew);
84 // if (xbest) gsl_vector_free (xbest);
85  if (dx) gsl_vector_free(dx);
86  if (dxscal) gsl_vector_free(dxscal);
87 // if (grad) gsl_vector_free (grad);
88  if (y) gsl_vector_free(y);
89  if (yscal) gsl_vector_free(yscal);
90  if (perr) gsl_vector_free(perr);
91  if (v1) gsl_vector_free(v1);
92  if (v2) gsl_vector_free(v2);
93 // if (Meval) gsl_vector_free (Meval);
94  if (M) gsl_matrix_free(M);
95  if (Mscal) gsl_matrix_free(Mscal);
96  if (W) gsl_matrix_free(W);
97  if (W2) gsl_matrix_free(W2);
98  if (W3) gsl_matrix_free(W3);
99  if (M1) gsl_matrix_free(M1);
100  if (M2) gsl_matrix_free(M2);
101  if (M3) gsl_matrix_free(M3);
102  if (M4) gsl_matrix_free(M4);
103  if (M5) gsl_matrix_free(M5);
104 // if (Mevec) gsl_matrix_free (Mevec);
105  if (CC) gsl_matrix_free(CC);
106  if (CC1) gsl_matrix_free(CC1);
107  if (CCinv) gsl_matrix_free(CCinv);
108  if (permW) gsl_permutation_free(permW);
109  if (eigenws) gsl_eigen_symm_free(eigenws);
110  x = nullptr;
111  xold = nullptr;
112  xnew = nullptr;
113 // xbest=0;
114  dx = nullptr;
115  dxscal = nullptr;
116 // grad=0;
117  y = nullptr;
118  yscal = nullptr;
119  perr = nullptr;
120  v1 = nullptr;
121  v2 = nullptr;
122 // Meval=0;
123  M = nullptr;
124  Mscal = nullptr;
125  W = nullptr;
126  W2 = nullptr;
127  W3 = nullptr;
128  M1 = nullptr;
129  M2 = nullptr;
130  M3 = nullptr;
131  M4 = nullptr;
132  M5 = nullptr;
133 // Mevec=0;
134  CC = nullptr;
135  CC1 = nullptr;
136  CCinv = nullptr;
137  permW = nullptr;
138  eigenws = nullptr; eigenwsdim = 0;
139  }
140 
141 
142 
143  double NewFitterGSL::fit()
144  {
145 
146  // order parameters etc
147  initialize();
148 
149  // initialize eta, etasv, y
150  assert(x && (x->size == idim));
151  assert(xold && (xold->size == idim));
152  assert(xnew && (xnew->size == idim));
153  assert(dx && (dx->size == idim));
154  assert(y && (y->size == idim));
155  assert(perr && (perr->size == idim));
156  assert(v1 && (v1->size == idim));
157  assert(v2 && (v2->size == idim));
158 // assert (Meval && Meval->size == idim);
159  assert(M && (M->size1 == idim && M->size2 == idim));
160  assert(W && (W->size1 == idim && W->size2 == idim));
161  assert(W2 && (W2->size1 == idim && W2->size2 == idim));
162  assert(M1 && (M1->size1 == idim && M1->size2 == idim));
163 // assert (Mevec && Mevec->size1 == idim && Mevec->size2 == idim);
164  assert(permW && (permW->size == idim));
165 
166  gsl_vector_set_zero(x);
167  gsl_vector_set_zero(y);
168  gsl_vector_set_all(perr, 1);
169 
170  // Store initial x values in x
171  fillx(x);
172  if (debug > 14) {
173  B2INFO("fit: Start values: \n");
174  debug_print(x, "x");
175  }
176  // make sure parameters are consistent
177  updateParams(x);
178  fillx(x);
179 
180  assembleConstDer(M);
181  int ifailL = determineLambdas(x, M, x, W, v1);
182  if (ifailL) return -1;
183 
184  // Get starting values into x
185 // gsl_vector_memcpy (x, xold);
186 
187 
188  // LET THE GAMES BEGIN
189 
190 #ifndef FIT_TRACEOFF
191  calcChi2();
192  traceValues["alpha"] = 0;
193  traceValues["phi"] = 0;
194  traceValues["mu"] = 0;
195  traceValues["detW"] = 0;
196  if (tracer) tracer->initialize(*this);
197 #endif
198 
199  bool converged = false;
200  ierr = 0;
201 
202  chi2new = calcChi2();
203  nit = 0;
204 
205  do {
206 #ifndef FIT_TRACEOFF
207  if (tracer) tracer->step(*this);
208 #endif
209 
210  chi2old = chi2new;
211  // Store old x values in xold
212  gsl_blas_dcopy(x, xold);
213  // Fill errors into perr
214  fillperr(perr);
215 
216  // Now, calculate the result vector y with the values of the derivatives
217  // d chi^2/d x
218  int ifail = calcNewtonDx(dx, dxscal, x, perr, M, Mscal, y, yscal, W, W2, permW, v1);
219 
220  if (ifail) {
221  ierr = 99;
222  B2DEBUG(10, "NewFitterGSL::fit: calcNewtonDx error " << ifail);
223 
224  break;
225  }
226 
227  if (debug > 15) {
228  B2INFO("Before test convergence, calcNewtonDe: Result: \n");
229  debug_print(dx, "dx");
230  debug_print(dxscal, "dxscal");
231  }
232 
233  // test convergence:
234  if (gsl_blas_dasum(dxscal) < 1E-6 * idim) {
235  converged = true;
236  break;
237  }
238 
239  double alpha = 1;
240  double mu = 0;
241  int imode = 2;
242 
243  calcLimitedDx(alpha, mu, xnew, imode, x, v2, dx, dxscal, perr, M, Mscal, W, v1);
244 
245  gsl_blas_dcopy(xnew, x);
246 
247  chi2new = calcChi2();
248 
249 // *-- Convergence criteria
250 
251  ++nit;
252  if (nit > 200) ierr = 1;
253 
254  converged = (abs(chi2new - chi2old) < 0.0001);
255 
256 // if (abs (chi2new - chi2old) >= 0.001)
257 // B2INFO("abs (chi2new - chi2old)=" << abs (chi2new - chi2old) << " -> try again\n");
258 // if (fvalbest >= 1E-3)
259 // B2INFO("fvalbest=" << fvalbest << " -> try again\n");
260 // if (fvalbest >= 1E-6 && abs(fvals[0]-fvalbest) >= 0.2*fvalbest )
261 // B2INFO("fvalbest=" << fvalbest
262 // << ", abs(fvals[0]-fvalbest)=" << abs(fvals[0]-fvalbest)<< " -> try again\n");
263 // if (stepbest >= 1E-3)
264 // B2INFO("stepbest=" << stepbest << " -> try again\n");
265 // B2INFO("converged=" << converged );
266  if (converged) {
267  B2DEBUG(12, "abs (chi2new - chi2old)=" << abs(chi2new - chi2old) << "\n"
268  << "fvalbest=" << fvalbest << "\n"
269  << "abs(fvals[0]-fvalbest)=" << abs(fvals[0] - fvalbest) << "\n");
270  }
271 
272  } while (!(converged || ierr));
273 
274 
275 #ifndef FIT_TRACEOFF
276  if (tracer) tracer->step(*this);
277 #endif
278 
279 //*-- End of iterations - calculate errors.
280 
281 // ERROR CALCULATION
282 
283  if (!ierr) {
284 
285  int ifailw = calcCovMatrix(W, permW, x);
286 
287  if (!ifailw) {
288  // update errors in fitobjects
289  for (auto& fitobject : fitobjects) {
290  for (int ilocal = 0; ilocal < fitobject->getNPar(); ++ilocal) {
291  int iglobal = fitobject->getGlobalParNum(ilocal);
292  for (int jlocal = ilocal; jlocal < fitobject->getNPar(); ++jlocal) {
293  int jglobal = fitobject->getGlobalParNum(jlocal);
294  if (iglobal >= 0 && jglobal >= 0)
295  fitobject->setCov(ilocal, jlocal, gsl_matrix_get(CCinv, iglobal, jglobal));
296  }
297  }
298  }
299  } else ierr = 999;
300  }
301 
302 
303  if (debug > 11) {
304  B2INFO("========= END =========\n");
305  B2INFO("Fit objects:\n");
306  for (auto fo : fitobjects) {
307  assert(fo);
308  B2INFO(fo->getName() << ": " << *fo << ", chi2=" << fo->getChi2());
309  }
310  B2INFO("constraints:\n");
311  for (auto i = constraints.begin(); i != constraints.end(); ++i) {
312  BaseHardConstraint* c = *i;
313  assert(c);
314  B2INFO(i - constraints.begin() << " " << c->getName() << ": " << c->getValue() << "+-" << c->getError());
315  }
316  B2INFO("=============================================\n");
317  }
318 
319 // *-- Turn chisq into probability.
320  fitprob = (chi2new >= 0 && ncon + nsoft - nunm > 0) ? gsl_cdf_chisq_Q(chi2new, ncon + nsoft - nunm) : -1;
321 
322 #ifndef FIT_TRACEOFF
323  if (tracer) tracer->finish(*this);
324 #endif
325 
326  B2DEBUG(10, "NewFitterGSL::fit: converged=" << converged
327  << ", nit=" << nit << ", fitprob=" << fitprob);
328 
329  if (ierr > 0) fitprob = -1;
330 
331  return fitprob;
332 
333  }
334 
335  bool NewFitterGSL::initialize()
336  {
337  covValid = false;
338 // bool debug = true;
339 
340  // tell fitobjects the global ordering of their parameters:
341  npar = 0;
342  nunm = 0;
343  //
344  for (auto& fitobject : fitobjects) {
345  for (int ilocal = 0; ilocal < fitobject->getNPar(); ++ilocal) {
346  if (!fitobject->isParamFixed(ilocal)) {
347  fitobject->setGlobalParNum(ilocal, npar);
348  ++npar;
349  if (!fitobject->isParamMeasured(ilocal)) ++nunm;
350  }
351  }
352  }
353 
354  // set number of constraints
355  ncon = constraints.size();
356  // Tell the constraints their numbers
357  for (unsigned int icon = 0; icon < constraints.size(); ++icon) {
358  BaseHardConstraint* c = constraints[icon];
359  assert(c);
360  c->setGlobalNum(npar + icon);
361  }
362 
363  // JL: should soft constraints have numbers assigned as well?
364  nsoft = softconstraints.size();
365 
366  if (nunm > ncon + nsoft) {
367  B2ERROR("NewFitterGSL::initialize: nunm=" << nunm << " > ncon+nsoft="
368  << ncon << "+" << nsoft);
369  }
370 
371  // dimension of "big M" matrix
372  idim = npar + ncon;
373 
374  ini_gsl_vector(x, idim);
375  ini_gsl_vector(xold, idim);
376  ini_gsl_vector(xnew, idim);
377 // ini_gsl_vector (xbest, idim);
378  ini_gsl_vector(dx, idim);
379  ini_gsl_vector(dxscal, idim);
380 // ini_gsl_vector (grad, idim);
381  ini_gsl_vector(y, idim);
382  ini_gsl_vector(yscal, idim);
383  ini_gsl_vector(perr, idim);
384  ini_gsl_vector(v1, idim);
385  ini_gsl_vector(v2, idim);
386 // ini_gsl_vector (Meval, idim);
387 
388  ini_gsl_matrix(M, idim, idim);
389  ini_gsl_matrix(Mscal, idim, idim);
390  ini_gsl_matrix(W, idim, idim);
391  ini_gsl_matrix(W2, idim, idim);
392  ini_gsl_matrix(W3, idim, idim);
393  ini_gsl_matrix(M1, idim, idim);
394  ini_gsl_matrix(M2, idim, idim);
395  ini_gsl_matrix(M3, idim, idim);
396  ini_gsl_matrix(M4, idim, idim);
397  ini_gsl_matrix(M5, idim, idim);
398 // ini_gsl_matrix (Mevec, idim, idim);
399  ini_gsl_matrix(CC, idim, idim);
400  ini_gsl_matrix(CC1, idim, idim);
401  ini_gsl_matrix(CCinv, idim, idim);
402 
403  ini_gsl_permutation(permW, idim);
404 
405  if (eigenws && eigenwsdim != idim) {
406  gsl_eigen_symm_free(eigenws);
407  eigenws = nullptr;
408  }
409  if (eigenws == nullptr) eigenws = gsl_eigen_symm_alloc(idim);
410  eigenwsdim = idim;
411 
412  return true;
413 
414  }
415 
416  double NewFitterGSL::calcChi2()
417  {
418  chi2 = 0;
419  for (auto fo : fitobjects) {
420  assert(fo);
421  chi2 += fo->getChi2();
422  }
423  for (auto bsc : softconstraints) {
424  assert(bsc);
425  chi2 += bsc->getChi2();
426  }
427  return chi2;
428  }
429 
430  int NewFitterGSL::getError() const {return ierr;}
431  double NewFitterGSL::getProbability() const {return fitprob;}
432  double NewFitterGSL::getChi2() const {return chi2;}
433  int NewFitterGSL::getDoF() const {return ncon + nsoft - nunm;}
434  int NewFitterGSL::getIterations() const {return nit;}
435 
436  void NewFitterGSL::ini_gsl_permutation(gsl_permutation*& p, unsigned int size)
437  {
438  if (p) {
439  if (p->size != size) {
440  gsl_permutation_free(p);
441  if (size > 0) p = gsl_permutation_alloc(size);
442  else p = nullptr;
443  }
444  } else if (size > 0) p = gsl_permutation_alloc(size);
445  }
446 
447  void NewFitterGSL::ini_gsl_vector(gsl_vector*& v, unsigned int size)
448  {
449 
450  if (v) {
451  if (v->size != size) {
452  gsl_vector_free(v);
453  if (size > 0) v = gsl_vector_alloc(size);
454  else v = nullptr;
455  }
456  } else if (size > 0) v = gsl_vector_alloc(size);
457  }
458 
459  void NewFitterGSL::ini_gsl_matrix(gsl_matrix*& m, unsigned int size1, unsigned int size2)
460  {
461  if (m) {
462  if (m->size1 != size1 || m->size2 != size2) {
463  gsl_matrix_free(m);
464  if (size1 * size2 > 0) m = gsl_matrix_alloc(size1, size2);
465  else m = nullptr;
466  }
467  } else if (size1 * size2 > 0) m = gsl_matrix_alloc(size1, size2);
468  }
469 
470  void NewFitterGSL::debug_print(const gsl_matrix* m, const char* name)
471  {
472  for (unsigned int i = 0; i < m->size1; ++i)
473  for (unsigned int j = 0; j < m->size2; ++j)
474  if (gsl_matrix_get(m, i, j) != 0)
475  B2INFO(name << "[" << i << "][" << j << "]=" << gsl_matrix_get(m, i, j));
476  }
477 
478  void NewFitterGSL::debug_print(const gsl_vector* v, const char* name)
479  {
480  for (unsigned int i = 0; i < v->size; ++i)
481  if (gsl_vector_get(v, i) != 0)
482  B2INFO(name << "[" << i << "]=" << gsl_vector_get(v, i));
483  }
484 
485  void NewFitterGSL::add(gsl_vector* vecz, const gsl_vector* vecx, double a, const gsl_vector* vecy)
486  {
487  assert(vecx);
488  assert(vecy);
489  assert(vecz);
490  assert(vecx->size == vecy->size);
491  assert(vecx->size == vecz->size);
492 
493  gsl_blas_dcopy(vecx, vecz);
494  gsl_blas_daxpy(a, vecy, vecz);
495  }
496 
497  int NewFitterGSL::getNcon() const {return ncon;}
498  int NewFitterGSL::getNsoft() const {return nsoft;}
499  int NewFitterGSL::getNunm() const {return nunm;}
500  int NewFitterGSL::getNpar() const {return npar;}
501 
502  bool NewFitterGSL::updateParams(gsl_vector* vecx)
503  {
504  assert(vecx);
505  assert(vecx->size == idim);
506  bool significant = false;
507  for (auto i = fitobjects.begin(); i != fitobjects.end(); ++i) {
508  BaseFitObject* fo = *i;
509  assert(fo);
510  bool s = fo->updateParams(vecx->block->data, vecx->size);
511  significant |= s;
512  if (nit < nitdebug && s) {
513  B2DEBUG(15, "Significant update for FO " << i - fitobjects.begin() << " ("
514  << fo->getName() << ")\n");
515  }
516  }
517  return significant;
518  }
519 
520  bool NewFitterGSL::isfinite(const gsl_vector* vec)
521  {
522  if (vec == nullptr) return true;
523  for (size_t i = 0; i < vec->size; ++i)
524  if (!std::isfinite(gsl_vector_get(vec, i))) return false;
525  return true;
526  }
527 
528  bool NewFitterGSL::isfinite(const gsl_matrix* mat)
529  {
530  if (mat == nullptr) return true;
531  for (size_t i = 0; i < mat->size1; ++i)
532  for (size_t j = 0; j < mat->size2; ++j)
533  if (!std::isfinite(gsl_matrix_get(mat, i, j))) return false;
534  return true;
535  }
536 
537 
538  void NewFitterGSL::fillx(gsl_vector* vecx)
539  {
540  assert(vecx);
541  assert(vecx->size == idim);
542 
543  gsl_vector_set_zero(vecx);
544  for (auto fo : fitobjects) {
545  assert(fo);
546  for (int ilocal = 0; ilocal < fo->getNPar(); ++ilocal) {
547  if (!fo->isParamFixed(ilocal)) {
548  int iglobal = fo->getGlobalParNum(ilocal);
549  assert(iglobal >= 0 && iglobal < npar);
550  gsl_vector_set(vecx, iglobal, fo->getParam(ilocal));
551  }
552  }
553  }
554  }
555 
556  void NewFitterGSL::fillperr(gsl_vector* vece)
557  {
558  assert(vece);
559  assert(vece->size == idim);
560  gsl_vector_set_all(vece, 1);
561  for (auto fo : fitobjects) {
562  assert(fo);
563  for (int ilocal = 0; ilocal < fo->getNPar(); ++ilocal) {
564  if (!fo->isParamFixed(ilocal)) {
565  int iglobal = fo->getGlobalParNum(ilocal);
566  assert(iglobal >= 0 && iglobal < npar);
567  double e = std::abs(fo->getError(ilocal));
568  gsl_vector_set(vece, iglobal, e ? e : 1);
569  }
570  }
571  }
572  for (auto c : constraints) {
573  assert(c);
574  int iglobal = c->getGlobalNum();
575  assert(iglobal >= 0 && iglobal < (int)idim);
576  double e = c->getError();
577  gsl_vector_set(vece, iglobal, e ? 1 / e : 1);
578  }
579  }
580 
581  void NewFitterGSL::assembleM(gsl_matrix* MatM, const gsl_vector* vecx, bool errorpropagation)
582  {
583  assert(MatM);
584  assert(MatM->size1 == idim && MatM->size2 == idim);
585  assert(vecx);
586  assert(vecx->size == idim);
587 
588  gsl_matrix_set_zero(MatM);
589 
590  // First, all terms d^2 chi^2/dx1 dx2
591  for (auto fo : fitobjects) {
592  assert(fo);
593  fo->addToGlobalChi2DerMatrix(MatM->block->data, MatM->tda);
594  if (!isfinite(MatM)) {
595  B2DEBUG(10, "NewFitterGSL::assembleM: illegal elements in MatM after adding fo " << *fo << ":\n");
596  if (debug > 15) debug_print(MatM, "M");
597  }
598  }
599  if (debug > 13) {
600  B2INFO("After adding covariances from fit objects:\n");
601  //printMy ((double*) M, (double*) y, (int) idim);
602  debug_print(MatM, "MatM");
603  }
604 
605  // Second, all terms d^2 chi^2/dlambda dx,
606  // i.e. the first derivatives of the constraints,
607  // plus the second derivatives times the lambda values
608  for (auto c : constraints) {
609  assert(c);
610  int kglobal = c->getGlobalNum();
611  assert(kglobal >= 0 && kglobal < (int)idim);
612  c->add1stDerivativesToMatrix(MatM->block->data, MatM->tda);
613  if (!isfinite(MatM)) {
614  B2DEBUG(10, "NewFitterGSL::assembleM: illegal elements in MatM after adding 1st derivatives of constraint " << *c << ":\n");
615  if (debug > 13) debug_print(MatM, "M");
616  }
617  if (debug > 13) {
618  B2INFO("After adding first derivatives of constraint " << c->getName());
619  //printMy ((double*) M, (double*) y, (int) idim);
620  debug_print(MatM, "MatM");
621  B2INFO("errorpropagation = " << errorpropagation);
622  }
623  // for error propagation after fit,
624  //2nd derivatives of constraints times lambda should _not_ be included!
625  if (!errorpropagation) c->add2ndDerivativesToMatrix(MatM->block->data, MatM->tda, gsl_vector_get(vecx, kglobal));
626  if (!isfinite(MatM)) {
627  B2DEBUG(10, "NewFitterGSL::assembleM: illegal elements in MatM after adding 2nd derivatives of constraint " << *c << ":\n");
628  if (debug > 13) debug_print(MatM, "MatM");
629  }
630  }
631  if (debug > 13) {
632  B2INFO("After adding derivatives of constraints::\n");
633  //printMy ((double*) M, (double*) y, (int) idim);
634  debug_print(M, "M");
635  B2INFO("===========================================::\n");
636  }
637 
638  // Finally, treat the soft constraints
639 
640  for (auto bsc : softconstraints) {
641  assert(bsc);
642  bsc->add2ndDerivativesToMatrix(MatM->block->data, MatM->tda);
643  if (!isfinite(MatM)) {
644  B2DEBUG(10, "NewFitterGSL::assembleM: illegal elements in MatM after adding soft constraint " << *bsc << ":\n");
645  if (debug > 13) debug_print(MatM, "M");
646  }
647  }
648  if (debug > 13) {
649  B2INFO("After adding soft constraints::\n");
650  //printMy ((double*) M, (double*) y, (int) idim);
651  debug_print(M, "M");
652  B2INFO("===========================================::\n");
653  }
654 
655  }
656 
657  void NewFitterGSL::assembleG(gsl_matrix* MatM, const gsl_vector* vecx)
658  {
659  assert(MatM);
660  assert(MatM->size1 == idim && MatM->size2 == idim);
661  assert(vecx);
662  assert(vecx->size == idim);
663 
664  gsl_matrix_set_zero(MatM);
665  // First, all terms d^2 chi^2/dx1 dx2
666  for (auto fo : fitobjects) {
667  assert(fo);
668  fo->addToGlobalChi2DerMatrix(MatM->block->data, MatM->tda);
669  }
670 
671  // Second, the second derivatives times the lambda values
672  for (auto c : constraints) {
673  assert(c);
674  int kglobal = c->getGlobalNum();
675  assert(kglobal >= 0 && kglobal < (int)idim);
676  c->add2ndDerivativesToMatrix(MatM->block->data, MatM->tda, gsl_vector_get(vecx, kglobal));
677  }
678 
679  // Finally, treat the soft constraints
680 
681  for (auto bsc : softconstraints) {
682  assert(bsc);
683  bsc->add2ndDerivativesToMatrix(MatM->block->data, MatM->tda);
684  }
685  }
686 
687  void NewFitterGSL::scaleM(gsl_matrix* MatMscal, const gsl_matrix* MatM, const gsl_vector* vece)
688  {
689  assert(MatMscal);
690  assert(MatMscal->size1 == idim && MatMscal->size2 == idim);
691  assert(MatM);
692  assert(MatM->size1 == idim && MatM->size2 == idim);
693  assert(vece);
694  assert(vece->size == idim);
695 
696  // Rescale columns and rows by perr
697  for (unsigned int i = 0; i < idim; ++i)
698  for (unsigned int j = 0; j < idim; ++j)
699  gsl_matrix_set(MatMscal, i, j,
700  gsl_vector_get(vece, i)*gsl_vector_get(vece, j)*gsl_matrix_get(MatM, i, j));
701  }
702 
703 
704 
705  void NewFitterGSL::assembley(gsl_vector* vecy, const gsl_vector* vecx)
706  {
707  assert(vecy);
708  assert(vecy->size == idim);
709  assert(vecx);
710  assert(vecx->size == idim);
711  gsl_vector_set_zero(vecy);
712  // First, for the parameters
713  for (auto fo : fitobjects) {
714  assert(fo);
715 // B2INFO("In New assembley FitObject: "<< fo->getName());
716  fo->addToGlobalChi2DerVector(vecy->block->data, vecy->size);
717  }
718 
719  // Now add lambda*derivatives of constraints,
720  // And finally, the derivatives w.r.t. to the constraints, i.e. the constraints themselves
721  for (auto c : constraints) {
722  assert(c);
723  int kglobal = c->getGlobalNum();
724  assert(kglobal >= 0 && kglobal < (int)idim);
725  c->addToGlobalChi2DerVector(vecy->block->data, vecy->size, gsl_vector_get(vecx, kglobal));
726  gsl_vector_set(vecy, kglobal, c->getValue());
727  }
728 
729  // Finally, treat the soft constraints
730 
731  for (auto bsc : softconstraints) {
732  assert(bsc);
733  bsc->addToGlobalChi2DerVector(vecy->block->data, vecy->size);
734  }
735  }
736 
737  void NewFitterGSL::scaley(gsl_vector* vecyscal, const gsl_vector* vecy, const gsl_vector* vece)
738  {
739  assert(vecyscal);
740  assert(vecyscal->size == idim);
741  assert(vecy);
742  assert(vecy->size == idim);
743  assert(vece);
744  assert(vece->size == idim);
745 
746  gsl_vector_memcpy(vecyscal, vecy);
747  gsl_vector_mul(vecyscal, vece);
748  }
749 
750  int NewFitterGSL::assembleChi2Der(gsl_vector* vecy)
751  {
752  assert(vecy);
753  assert(vecy->size == idim);
754 
755  gsl_vector_set_zero(vecy);
756  // First, for the parameters
757  for (auto fo : fitobjects) {
758  assert(fo);
759  // B2INFO("In New assembleChi2Der FitObject: "<< fo->getName());
760  int ifail = fo->addToGlobalChi2DerVector(vecy->block->data, vecy->size);
761  if (ifail) return ifail;
762  }
763 
764  // Treat the soft constraints
765 
766  for (auto bsc : softconstraints) {
767  assert(bsc);
768  bsc->addToGlobalChi2DerVector(vecy->block->data, vecy->size);
769  }
770  return 0;
771  }
772 
773  void NewFitterGSL::addConstraints(gsl_vector* vecy)
774  {
775  assert(vecy);
776  assert(vecy->size == idim);
777 
778  // Now add the derivatives w.r.t. to the constraints, i.e. the constraints themselves
779  for (auto c : constraints) {
780  assert(c);
781  int kglobal = c->getGlobalNum();
782  gsl_vector_set(vecy, kglobal, c->getValue());
783  }
784  }
785 
786  void NewFitterGSL::assembleConstDer(gsl_matrix* MatM)
787  {
788  assert(MatM);
789  assert(MatM->size1 == idim && MatM->size2 == idim);
790 
791  gsl_matrix_set_zero(MatM);
792 
793  // The first derivatives of the constraints,
794  for (auto c : constraints) {
795  assert(c);
796  int kglobal = c->getGlobalNum();
797  assert(kglobal >= 0 && kglobal < (int)idim);
798  c->add1stDerivativesToMatrix(MatM->block->data, MatM->tda);
799  }
800  }
801 
802  int NewFitterGSL::calcNewtonDx(gsl_vector* vecdx, gsl_vector* vecdxscal,
803  gsl_vector* vecx, const gsl_vector* vece,
804  gsl_matrix* MatM, gsl_matrix* MatMscal,
805  gsl_vector* vecy, gsl_vector* vecyscal,
806  gsl_matrix* MatW, gsl_matrix* MatW2,
807  gsl_permutation* permw,
808  gsl_vector* vecw
809  )
810  {
811  assert(vecdx);
812  assert(vecdx->size == idim);
813  assert(vecdxscal);
814  assert(vecdxscal->size == idim);
815  assert(vecx);
816  assert(vecx->size == idim);
817  assert(vece);
818  assert(vece->size == idim);
819  assert(MatM);
820  assert(MatM->size1 == idim && MatM->size2 == idim);
821  assert(MatMscal);
822  assert(MatMscal->size1 == idim && MatMscal->size2 == idim);
823  assert(vecy);
824  assert(vecy->size == idim);
825  assert(vecyscal);
826  assert(vecyscal->size == idim);
827  assert(MatW);
828  assert(MatW->size1 == idim && MatW->size2 == idim);
829  assert(MatW2);
830  assert(MatW2->size1 == idim && MatW2->size2 == idim);
831  assert(permw);
832  assert(permw->size == idim);
833  assert(vecw);
834  assert(vecw->size == idim);
835 
836  int ncalc = 0;
837  double ptLp = 0;
838 
839  do {
840  if (ncalc == 1) {
841  // try to recalculate lambdas
842  assembleConstDer(MatM);
843  determineLambdas(vecx, MatM, vecx, MatW, vecw);
844  B2DEBUG(12, "NewFitterGSL::calcNewtonDx: ptLp=" << ptLp << " with lambdas from last iteration");
845  } else if (ncalc == 2) {
846  // try to set lambdas to zero
847  gsl_vector_view lambda(gsl_vector_subvector(vecx, npar, ncon));
848  gsl_vector_set_zero(&lambda.vector);
849  B2DEBUG(12, "NewFitterGSL::calcNewtonDx: ptLp=" << ptLp << " with recalculated lambdas");
850  } else if (ncalc >= 3) {
851  B2DEBUG(12, "NewFitterGSL::calcNewtonDx: ptLp=" << ptLp << " with zero lambdas");
852  break;
853  }
854 
855  if (debug > 15) {
856  B2INFO("calcNewtonDx: before setting up equations: \n");
857  debug_print(vecx, "x");
858  }
859 
860  assembleM(MatM, vecx);
861  if (!isfinite(MatM)) return 1;
862 
863  scaleM(MatMscal, MatM, vece);
864  assembley(vecy, vecx);
865  if (!isfinite(vecy)) return 2;
866  scaley(vecyscal, vecy, vece);
867 
868  if (debug > 15) {
869  B2INFO("calcNewtonDx: After setting up equations: \n");
870  debug_print(MatM, "M");
871  debug_print(MatMscal, "Mscal");
872  debug_print(vecy, "y");
873  debug_print(vecyscal, "yscal");
874  debug_print(vece, "perr");
875  debug_print(vecx, "x");
876  debug_print(xnew, "xnew");
877  }
878 
879  // from x_(n+1) = x_n - y/y' = x_n - M^(-1)*y we have M*(x_n-x_(n+1)) = y,
880  // which we solve for dx = x_n-x_(n+1) and hence x_(n+1) = x_n-dx
881 
882  double epsLU = 1E-12;
883  double epsSV = 1E-3;
884  double detW;
885  solveSystem(vecdxscal, detW, vecyscal, MatMscal, MatW, MatW2, vecw, epsLU, epsSV);
886 
887 
888 #ifndef FIT_TRACEOFF
889  traceValues["detW"] = detW;
890 #endif
891 
892  // step is - computed vector
893  gsl_blas_dscal(-1, dxscal);
894 
895  // dx = dxscal*e (component wise)
896  gsl_vector_memcpy(vecdx, vecdxscal);
897  gsl_vector_mul(vecdx, vece);
898 
899  if (debug > 15) {
900  B2INFO("calcNewtonDx: Result: \n");
901  debug_print(vecdx, "dx");
902  debug_print(vecdxscal, "dxscal");
903  }
904 
905 
906  ptLp = calcpTLp(dx, M, v1);
907  ++ncalc;
908  } while (ptLp < 0);
909 
910  return 0;
911  }
912 
913  int NewFitterGSL::calcLimitedDx(double& alpha, double& mu, gsl_vector* vecxnew,
914  int imode,
915  gsl_vector* vecx, gsl_vector* vecdxhat,
916  const gsl_vector* vecdx, const gsl_vector* vecdxscal,
917  const gsl_vector* vece,
918  const gsl_matrix* MatM, const gsl_matrix* MatMscal,
919  gsl_matrix* MatW, gsl_vector* vecw
920  )
921  {
922  assert(vecxnew);
923  assert(vecxnew->size == idim);
924  assert(vecx);
925  assert(vecx->size == idim);
926  assert(vecdxhat);
927  assert(vecdxhat->size == idim);
928  assert(vecdx);
929  assert(vecdx->size == idim);
930  assert(vecdxscal);
931  assert(vecdxscal->size == idim);
932  assert(vece);
933  assert(vece->size == idim);
934  assert(MatM);
935  assert(MatM->size1 == idim && MatM->size2 == idim);
936  assert(MatMscal);
937  assert(MatMscal->size1 == idim && MatMscal->size2 == idim);
938  assert(MatW);
939  assert(MatW->size1 == idim && MatW->size2 == idim);
940  assert(vecw);
941  assert(vecw->size == idim);
942 
943  alpha = 1;
944  double eta = 0.1;
945 
946  add(vecxnew, vecx, alpha, vecdx);
947 
948  if (debug > 15) {
949  B2INFO("calcLimitedDx: After solving equations: \n");
950  debug_print(vecx, "x");
951  gsl_blas_dcopy(vecx, vecw);
952  gsl_vector_div(vecw, vece);
953  debug_print(vecw, "xscal");
954  debug_print(vecxnew, "xnew");
955  gsl_blas_dcopy(vecxnew, vecw);
956  gsl_vector_div(vecw, vece);
957  debug_print(vecw, "xnewscal");
958  }
959 
960  mu = calcMu(vecx, vece, vecdx, vecdxscal, vecxnew, MatM, MatMscal, vecw);
961 
962  updateParams(vecx);
963 
964  double phi0 = meritFunction(mu, vecx, vece);
965  double dphi0 = meritFunctionDeriv(mu, vecx, vece, vecdx, vecw);
966 
967  if (debug > 12) {
968  double eps = 1E-6;
969  add(vecw, vecx, eps, vecdx);
970  updateParams(vecw);
971  double dphinum = (meritFunction(mu, vecw, vece) - phi0) / eps;
972  updateParams(vecx);
973 
974  B2INFO("analytic der: " << dphi0 << ", num=" << dphinum);
975  }
976 
977 #ifndef FIT_TRACEOFF
978  traceValues["alpha"] = 0;
979  traceValues["phi"] = phi0;
980  traceValues["mu"] = mu;
981  if (tracer) tracer->substep(*this, 0);
982 #endif
983 
984  updateParams(vecxnew);
985 
986  double phiR = meritFunction(mu, vecxnew, vece);
987 
988  B2DEBUG(12, "calcLimitedDx: phi0=" << phi0 << ", dphi0=" << dphi0
989  << ", phiR = " << phiR << ", mu=" << mu
990  << ", threshold = " << phi0 + eta * dphi0
991  << " => test=" << (phiR > phi0 + eta * dphi0));
992 
993 #ifndef FIT_TRACEOFF
994  traceValues["alpha"] = 1;
995  traceValues["phi"] = phiR;
996  if (tracer) tracer->substep(*this, 0);
997 #endif
998 
999 
1000  // Try Armijo's rule for alpha=1 first, do linesearch only if it fails
1001  if (phiR > phi0 + eta * alpha * dphi0) {
1002 
1003  // try second order correction first
1004  if (try2ndOrderCorr) {
1005  calc2ndOrderCorr(vecdxhat, vecxnew, MatM, MatW, vecw);
1006  gsl_blas_dcopy(vecxnew, vecw);
1007  add(vecxnew, vecxnew, 1, vecdxhat);
1008  updateParams(vecxnew);
1009  double phi2ndOrder = meritFunction(mu, vecxnew, vece);
1010 
1011 #ifndef FIT_TRACEOFF
1012  traceValues["alpha"] = 1.5;
1013  traceValues["phi"] = phi2ndOrder;
1014  if (tracer) tracer->substep(*this, 2);
1015 #endif
1016 
1017  B2DEBUG(12, "calcLimitedDx: tried 2nd order correction, phi2ndOrder = "
1018  << phi2ndOrder << ", threshold = " << phi0 + eta * dphi0);
1019  if (debug > 15) {
1020  debug_print(vecdxhat, "dxhat");
1021  debug_print(xnew, "xnew");
1022  }
1023  if (phi2ndOrder <= phi0 + eta * alpha * dphi0) {
1024  B2DEBUG(12, " -> 2nd order correction successful!");
1025  return 1;
1026  }
1027  B2DEBUG(12, " -> 2nd order correction failed, do linesearch!");
1028  gsl_blas_dcopy(vecw, vecxnew);
1029  updateParams(vecxnew);
1030 #ifndef FIT_TRACEOFF
1031  calcChi2();
1032  traceValues["alpha"] = 1;
1033  traceValues["phi"] = phiR;
1034  if (tracer) tracer->substep(*this, 2);
1035 #endif
1036 
1037  }
1038 
1039  double zeta = 0.5;
1040  doLineSearch(alpha, vecxnew, imode, phi0, dphi0, eta, zeta, mu,
1041  vecx, vecdx, vece, vecw);
1042  }
1043 
1044  return 0;
1045  }
1046 
1047  int NewFitterGSL::doLineSearch(double& alpha, gsl_vector* vecxnew,
1048  int imode,
1049  double phi0, double dphi0,
1050  double eta, double zeta,
1051  double mu,
1052  const gsl_vector* vecx, const gsl_vector* vecdx,
1053  const gsl_vector* vece, gsl_vector* vecw)
1054  {
1055  assert(vecxnew);
1056  assert(vecxnew->size == idim);
1057  assert(vecx);
1058  assert(vecx->size == idim);
1059  assert(vecdx);
1060  assert(vecdx->size == idim);
1061  assert(vece);
1062  assert(vece->size == idim);
1063  assert(vecw);
1064  assert(vecw->size == idim);
1065 
1066  // don't do anything for imode = -1
1067  if (imode < 0) return -1;
1068 
1069  assert((imode == 0) || eta < zeta);
1070 
1071  if (dphi0 >= 0) {
1072  // Difficult situation: merit function will increase,
1073  // thus every step makes it worse
1074  // => choose the minimum step and return
1075  B2DEBUG(12, "NewFitterGSL::doLineSearch: dphi0 > 0!");
1076  // Choose new alpha
1077  alpha = 0.001;
1078 
1079  add(vecxnew, vecx, alpha, vecdx);
1080  updateParams(vecxnew);
1081 
1082  double phi = meritFunction(mu, vecxnew, vece);
1083 
1084 #ifndef FIT_TRACEOFF
1085  traceValues["alpha"] = alpha;
1086  traceValues["phi"] = phi;
1087  if (tracer) tracer->substep(*this, 1);
1088 #endif
1089  return 2;
1090  }
1091 
1092  // alpha=1 already tried
1093  double alphaR = alpha;
1094  // vecxnew = vecx + alpha*vecdx
1095  add(vecxnew, vecx, alpha, vecdx);
1096  updateParams(vecxnew);
1097 
1098  double alphaL = 0;
1099  // double phiL = phi0; //fix warning
1100  // double dphiL = dphi0; //fix warning
1101 
1102  double dphi;
1103  int nite = 0;
1104 
1105  do {
1106  nite++;
1107  // Choose new alpha
1108  alpha = 0.5 * (alphaL + alphaR);
1109 
1110  add(vecxnew, vecx, alpha, vecdx);
1111  updateParams(vecxnew);
1112 
1113  double phi = meritFunction(mu, vecxnew, vece);
1114 
1115 #ifndef FIT_TRACEOFF
1116  traceValues["alpha"] = alpha;
1117  traceValues["phi"] = phi;
1118  if (tracer) tracer->substep(*this, 1);
1119 #endif
1120 
1121  // Armijo's rule always holds
1122  if (phi >= phi0 + eta * alpha * dphi0) {
1123  B2DEBUG(15, "NewFitterGSL::doLineSearch, Armijo: phi=" << phi
1124  << " >= " << phi0 + eta * alpha * dphi0
1125  << " at alpha=" << alpha);
1126  alphaR = alpha;
1127  // phiR = phi; //fix warning
1128  continue;
1129  }
1130 
1131  if (imode == 0) { // Armijo
1132  break;
1133  } else if (imode == 1) { // Wolfe
1134  dphi = meritFunctionDeriv(mu, vecxnew, vece, vecdx, vecw);
1135  if (dphi < zeta * dphi0) {
1136  B2DEBUG(15, "NewFitterGSL::doLineSearch, Wolfe: dphi=" << dphi
1137  << " < " << zeta * dphi0
1138  << " at alpha=" << alpha);
1139  alphaL = alpha;
1140  // phiL = phi; //fix warning
1141  // dphiL = dphi; //fix warning
1142  } else {
1143  break;
1144  }
1145  } else { // Goldstein
1146  if (phi < phi0 + zeta * alpha * dphi0) {
1147  B2DEBUG(15, "NewFitterGSL::doLineSearch, Goldstein: phi=" << phi
1148  << " < " << phi0 + zeta * alpha * dphi0
1149  << " at alpha=" << alpha);
1150  alphaL = alpha;
1151  // phiL = phi; //fix warning
1152  } else {
1153  break;
1154  }
1155  }
1156  } while (nite < 30 && (alphaL == 0 || nite < 6));
1157  if (alphaL > 0) alpha = alphaL;
1158  return 1;
1159  }
1160 
1161 
1162  double NewFitterGSL::calcMu(const gsl_vector* vecx, const gsl_vector* vece,
1163  const gsl_vector* vecdx, const gsl_vector* vecdxscal,
1164  const gsl_vector* vecxnew,
1165  const gsl_matrix* MatM, const gsl_matrix* MatMscal,
1166  gsl_vector* vecw)
1167  {
1168  assert(vecx);
1169  assert(vecx->size == idim);
1170  assert(vece);
1171  assert(vece->size == idim);
1172  assert(vecdx);
1173  assert(vecdx->size == idim);
1174  assert(vecdxscal);
1175  assert(vecdxscal->size == idim);
1176  assert(MatM);
1177  assert(MatM->size1 == idim && MatM->size2 == idim);
1178  assert(MatMscal);
1179  assert(MatMscal->size1 == idim && MatMscal->size2 == idim);
1180  assert(vecw);
1181  assert(vecw->size == idim);
1182 
1183  double result = 0;
1184  switch (imerit) {
1185  case 1: { // l1 penalty function, Nocedal&Wright Eq. (15.24)
1186  result = 0;
1187 // for (int kglobal = npar; kglobal < npar+ncon; ++kglobal) {
1188 // double abslambda = std::fabs (gsl_vector_get (vecx, kglobal));
1189 // if (abslambda > result)
1190 // result = abslambda;
1191 // }
1192  // calculate grad f^T*p
1193  gsl_vector_set_zero(vecw);
1194  addConstraints(vecw);
1195  gsl_vector_view c(gsl_vector_subvector(vecw, npar, ncon));
1196  // ||c||_1
1197  double cnorm1 = gsl_blas_dasum(&c.vector);
1198  // scale constraint values by 1/(delta e)
1199  gsl_vector_const_view lambdaerr(gsl_vector_const_subvector(vece, npar, ncon));
1200  gsl_vector_mul(&c.vector, &lambdaerr.vector);
1201  // ||c||_1
1202  double cnorm1scal = gsl_blas_dasum(&c.vector);
1203 
1204  double rho = 0.1;
1205  double eps = 0.001;
1206 
1207  assembleChi2Der(vecw);
1208  gsl_vector_view gradf(gsl_vector_subvector(vecw, 0, npar));
1209  gsl_vector_const_view p(gsl_vector_const_subvector(vecdx, 0, npar));
1210  double gradfTp;
1211  gsl_blas_ddot(&gradf.vector, &p.vector, &gradfTp);
1212 
1213  B2DEBUG(17, "NewFitterGSL::calcMu: cnorm1scal=" << cnorm1scal
1214  << ", gradfTp=" << gradfTp);
1215 
1216  // all constraints very well fulfilled, use max(lambda+1) criterium
1217  if (cnorm1scal < ncon * eps || gradfTp <= 0) {
1218  for (int kglobal = npar; kglobal < npar + ncon; ++kglobal) {
1219  double abslambda = std::fabs(gsl_vector_get(vecxnew, kglobal));
1220  if (abslambda > result)
1221  result = abslambda;
1222  }
1223  result /= (1 - rho);
1224  } else {
1225  // calculate p^T L p
1226  double pTLp = calcpTLp(vecdx, MatM, vecw);
1227  double sigma = (pTLp > 0) ? 1 : 0;
1228  B2DEBUG(17, " pTLp = " << pTLp);
1229  // Nocedal&Wright Eq. (18.36)
1230  result = (gradfTp + 0.5 * sigma * pTLp) / ((1 - rho) * cnorm1);
1231  }
1232  B2DEBUG(17, " result = " << result);
1233  }
1234  break;
1235  case 2: // l1 penalty function, errors scaled, Nocedal&Wright Eq. (15.24)
1236  result = 0;
1237  for (int kglobal = npar; kglobal < npar + ncon; ++kglobal) {
1238  double abslambdascal = std::fabs(gsl_vector_get(vecx, kglobal) / gsl_vector_get(vece, kglobal));
1239  if (abslambdascal > result)
1240  result = abslambdascal;
1241  }
1242  break;
1243  default: assert(0);
1244  }
1245  return result;
1246 
1247  }
1248 
1249 
1250  double NewFitterGSL::meritFunction(double mu, const gsl_vector* vecx, const gsl_vector* vece)
1251  {
1252  assert(vecx);
1253  assert(vecx->size == idim);
1254  assert(vece);
1255  assert(vece->size == idim);
1256 
1257  double result = 0;
1258  switch (imerit) {
1259  case 1: // l1 penalty function, Nocedal&Wright Eq. (15.24)
1260  result = calcChi2();
1261  for (auto c : constraints) {
1262  assert(c);
1263  int kglobal = c->getGlobalNum();
1264  assert(kglobal >= 0 && kglobal < (int)idim);
1265  result += mu * std::fabs(c->getValue());
1266  }
1267  break;
1268  case 2: // l1 penalty function, errors scaled, Nocedal&Wright Eq. (15.24)
1269  result = calcChi2();
1270  for (auto c : constraints) {
1271  assert(c);
1272  int kglobal = c->getGlobalNum();
1273  assert(kglobal >= 0 && kglobal < (int)idim);
1274  // vece[kglobal] is 1/error for constraint k
1275  result += mu * std::fabs(c->getValue() * gsl_vector_get(vece, kglobal));
1276  }
1277  break;
1278  default: assert(0);
1279  }
1280  return result;
1281 
1282  }
1283 
1284 
1285  double NewFitterGSL::meritFunctionDeriv(double mu, const gsl_vector* vecx, const gsl_vector* vece,
1286  const gsl_vector* vecdx, gsl_vector* vecw)
1287  {
1288  assert(vecx);
1289  assert(vecx->size == idim);
1290  assert(vece);
1291  assert(vece->size == idim);
1292  assert(vecdx);
1293  assert(vecdx->size == idim);
1294  assert(vecw);
1295  assert(vecw->size == idim);
1296 
1297  double result = 0;
1298  switch (imerit) {
1299  case 1: // l1 penalty function, Nocedal&Wright Eq. (15.24), Eq. (18.29)
1300  assembleChi2Der(vecw);
1301  for (int i = 0; i < npar; ++i) {
1302  result += gsl_vector_get(vecdx, i) * gsl_vector_get(vecw, i);
1303  }
1304 // for (ConstraintIterator i = constraints.begin(); i != constraints.end(); ++i) {
1305 // BaseHardConstraint *c = *i;
1306 // assert (c);
1307 // int kglobal = c->getGlobalNum();
1308 // assert (kglobal >= 0 && kglobal < (int)idim);
1309 // result += c->dirDerAbs (vecdx->block->data, vecw->block->data, npar, mu);
1310 // }
1311  for (auto c : constraints) {
1312  assert(c);
1313  int kglobal = c->getGlobalNum();
1314  assert(kglobal >= 0 && kglobal < (int)idim);
1315  result -= mu * std::fabs(c->getValue());
1316  }
1317  break;
1318  case 2: // l1 penalty function, errors scaled, Nocedal&Wright Eq. (15.24), Eq. (18.29)
1319  assembleChi2Der(vecw);
1320  for (int i = 0; i < npar; ++i) {
1321  result += gsl_vector_get(vecdx, i) * gsl_vector_get(vecw, i);
1322  }
1323 // for (ConstraintIterator i = constraints.begin(); i != constraints.end(); ++i) {
1324 // BaseHardConstraint *c = *i;
1325 // assert (c);
1326 // int kglobal = c->getGlobalNum();
1327 // assert (kglobal >= 0 && kglobal < (int)idim);
1328 // result += c->dirDerAbs (vecdx->block->data, vecw->block->data, npar, mu*gsl_vector_get (vece, kglobal));
1329 // }
1330  for (auto c : constraints) {
1331  assert(c);
1332  int kglobal = c->getGlobalNum();
1333  assert(kglobal >= 0 && kglobal < (int)idim);
1334  result -= mu * std::fabs(c->getValue()) * gsl_vector_get(vece, kglobal);
1335  }
1336  break;
1337  default: assert(0);
1338  }
1339  return result;
1340  }
1341 
1342 
1343  int NewFitterGSL::invertM()
1344  {
1345  gsl_matrix_memcpy(W, M);
1346 
1347  int ifail = 0;
1348 
1349  int signum;
1350  // Calculate LU decomposition of M into W
1351  int result = gsl_linalg_LU_decomp(W, permW, &signum);
1352  B2DEBUG(11, "invertM: gsl_linalg_LU_decomp result=" << result);
1353  // Calculate inverse of M
1354  ifail = gsl_linalg_LU_invert(W, permW, M);
1355  B2DEBUG(11, "invertM: gsl_linalg_LU_invert result=" << ifail);
1356 
1357  if (ifail != 0) {
1358  B2ERROR("NewtonFitter::invert: ifail from gsl_linalg_LU_invert=" << ifail);
1359  }
1360  return ifail;
1361  }
1362 
1363  void NewFitterGSL::setDebug(int Debuglevel)
1364  {
1365  debug = Debuglevel;
1366  }
1367 
1368 
1369  int NewFitterGSL::calcCovMatrix(gsl_matrix* MatW,
1370  gsl_permutation* permw,
1371  gsl_vector* vecx)
1372  {
1373  // Set up equation system M*dadeta + dydeta = 0
1374  // here, dadeta is d a / d eta, i.e. the derivatives of the fitted
1375  // parameters a w.r.t. to the measured parameters eta,
1376  // and dydeta is the derivative of the set of equations
1377  // w.r.t eta, i.e. simply d^2 chi^2 / d a d eta.
1378  // Now, if chi2 = (a-eta)^T*Vinv((a-eta), we have simply
1379  // d^2 chi^2 / d a d eta = - d^2 chi^2 / d a d a
1380  // and can use the method addToGlobalChi2DerMatrix.
1381 
1382  gsl_matrix_set_zero(M1);
1383  gsl_matrix_set_zero(M2);
1384  // First, all terms d^2 chi^2/dx1 dx2
1385  for (auto fo : fitobjects) {
1386  assert(fo);
1387  fo->addToGlobalChi2DerMatrix(M1->block->data, M1->tda);
1388  fo->addToGlobCov(M2->block->data, M2->tda);
1389  }
1390  // multiply by -1
1391  gsl_matrix_scale(M1, -1);
1392 
1393  // JL: dy/eta are the derivatives of the "objective function" with respect to the MEASURED parameters.
1394  // Since the soft constraints do not depend on the measured, but only on the fitted (!) parameters,
1395  // dy/deta stays -1*M1 also in the presence of soft constraints!
1396  gsl_matrix_view dydeta = gsl_matrix_submatrix(M1, 0, 0, idim, npar);
1397  gsl_matrix_view Cov_eta = gsl_matrix_submatrix(M2, 0, 0, npar, npar);
1398 
1399  if (debug > 13) {
1400  B2INFO("NewFitterGSL::calcCovMatrix\n");
1401  debug_print(&dydeta.matrix, "dydeta");
1402  debug_print(&Cov_eta.matrix, "Cov_eta");
1403  }
1404 
1405  // JL: calculates d^2 chi^2 / dx1 dx2 + first (!) derivatives of hard & soft constraints, and the
1406  // second derivatives of the soft constraints times the values of the fitted parameters
1407  // - all of the with respect to the FITTED parameters, therefore with soft constraints like in the fit itself.
1408  assembleM(MatW, vecx, true);
1409 
1410  if (debug > 13) {
1411  debug_print(MatW, "MatW");
1412  }
1413 
1414  // Now, solve M*dadeta = dydeta
1415 
1416  // Calculate LU decomposition of M into M3
1417  int signum;
1418  int result = gsl_linalg_LU_decomp(MatW, permw, &signum);
1419 
1420  if (debug > 13) {
1421  B2INFO("calcCovMatrix: gsl_linalg_LU_decomp result=" << result);
1422  debug_print(MatW, "M_LU");
1423  }
1424 
1425  // Calculate inverse of M, store in M3
1426  gsl_set_error_handler_off();
1427  int ifail = gsl_linalg_LU_invert(MatW, permw, M3);
1428  if (ifail) {
1429  B2WARNING("NewFitterGSL: MatW matrix is singular!");
1430  return ifail;
1431  }
1432 
1433  if (debug > 13) {
1434  B2INFO("calcCovMatrix: gsl_linalg_LU_invert ifail=" << ifail);
1435  debug_print(M3, "Minv");
1436  }
1437 
1438  // Calculate dadeta = M3*dydeta
1439  gsl_matrix_set_zero(M4);
1440  gsl_matrix_view dadeta = gsl_matrix_submatrix(M4, 0, 0, idim, npar);
1441 
1442  if (debug > 13) {
1443  debug_print(&dadeta.matrix, "dadeta");
1444  }
1445 
1446  // dadeta = 1*M*dydeta + 0*dadeta
1447  gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, M3, &dydeta.matrix, 0, &dadeta.matrix);
1448 
1449 
1450  // Now calculate Cov_a = dadeta*Cov_eta*dadeta^T
1451 
1452  // First, calculate M3 = Cov_eta*dadeta^T as
1453  gsl_matrix_view M3part = gsl_matrix_submatrix(M3, 0, 0, npar, idim);
1454  gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1, &Cov_eta.matrix, &dadeta.matrix, 0, &M3part.matrix);
1455  // Now Cov_a = dadeta*M3part
1456  gsl_matrix_set_zero(M5);
1457  gsl_matrix_view Cov_a = gsl_matrix_submatrix(M5, 0, 0, npar, npar);
1458  gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, &dadeta.matrix, &M3part.matrix, 0, M5);
1459  gsl_matrix_memcpy(CCinv, M5);
1460 
1461  if (debug > 13) {
1462  debug_print(&Cov_a.matrix, "Cov_a");
1463  debug_print(CCinv, "full Cov from err prop");
1464  debug_print(M1, "uncorr Cov from err prop");
1465  }
1466 
1467  // Finally, copy covariance matrix
1468  if (cov && covDim != npar) {
1469  delete[] cov;
1470  cov = nullptr;
1471  }
1472  covDim = npar;
1473  if (!cov) cov = new double[covDim * covDim];
1474  for (int i = 0; i < covDim; ++i) {
1475  for (int j = 0; j < covDim; ++j) {
1476  cov[i * covDim + j] = gsl_matrix_get(&Cov_a.matrix, i, j);
1477  }
1478  }
1479  covValid = true;
1480  return 0;
1481  }
1482 
1483  int NewFitterGSL::determineLambdas(gsl_vector* vecxnew,
1484  const gsl_matrix* MatM, const gsl_vector* vecx,
1485  gsl_matrix* MatW, gsl_vector* vecw,
1486  double eps)
1487  {
1488  assert(vecxnew);
1489  assert(vecxnew->size == idim);
1490  assert(MatM);
1491  assert(MatM->size1 == idim && MatM->size2 == idim);
1492  assert(vecx);
1493  assert(vecx->size == idim);
1494  assert(MatW);
1495  assert(MatW->size1 == idim && MatW->size2 == idim);
1496  assert(vecw);
1497  assert(vecw->size == idim);
1498  assert(idim == static_cast<unsigned int>(npar + ncon));
1499 
1500  gsl_matrix_const_view A(gsl_matrix_const_submatrix(MatM, 0, npar, npar, ncon));
1501  gsl_matrix_view ATA(gsl_matrix_submatrix(MatW, npar, npar, ncon, ncon));
1502  gsl_matrix_view Acopy(gsl_matrix_submatrix(MatW, 0, npar, npar, ncon));
1503  gsl_matrix_view& V = ATA;
1504 
1505  gsl_vector_view gradf(gsl_vector_subvector(vecw, 0, npar));
1506  gsl_vector_view ATgradf(gsl_vector_subvector(vecw, npar, ncon));
1507  gsl_vector_view& s = ATgradf;
1508 
1509  gsl_vector_view lambdanew(gsl_vector_subvector(vecxnew, npar, ncon));
1510 
1511  if (debug > 15) {
1512 // gsl_vector_const_view lambda(gsl_vector_const_subvector(vecx, npar, ncon));
1513  B2INFO("lambda: ");
1514  gsl_vector_fprintf(stdout, &lambdanew.vector, "%f");
1515  }
1516 
1517  // ATA = 1*A^T*A + 0*ATA
1518  gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1, &A.matrix, &A.matrix, 0, &ATA.matrix);
1519 
1520  // put grad(f) into vecw
1521  int isfail = assembleChi2Der(vecw);
1522  if (isfail) return isfail;
1523 
1524 
1525  // ATgradf = -1*A^T*gradf + 0*ATgradf
1526  gsl_blas_dgemv(CblasTrans, -1, &A.matrix, &gradf.vector, 0, &ATgradf.vector);
1527 
1528  if (debug > 17) {
1529  B2INFO("A: ");
1530  gsl_matrix_fprintf(stdout, &A.matrix, "%f");
1531  B2INFO("ATA: ");
1532  gsl_matrix_fprintf(stdout, &ATA.matrix, "%f");
1533  B2INFO("gradf: ");
1534  gsl_vector_fprintf(stdout, &gradf.vector, "%f");
1535  B2INFO("ATgradf: ");
1536  gsl_vector_fprintf(stdout, &ATgradf.vector, "%f");
1537  }
1538 
1539  // solve ATA * lambdanew = ATgradf using the Cholsky factorization method
1540  gsl_error_handler_t* old_handler = gsl_set_error_handler_off();
1541  int cholesky_result = gsl_linalg_cholesky_decomp(&ATA.matrix);
1542  gsl_set_error_handler(old_handler);
1543  if (cholesky_result) {
1544  B2INFO("NewFitterGSL::determineLambdas: resorting to SVD");
1545  // ATA is not positive definite, i.e. A does not have full column rank
1546  // => use the SVD of A to solve A lambdanew = gradf
1547 
1548  // Acopy = A
1549  gsl_matrix_memcpy(&Acopy.matrix, &A.matrix);
1550 
1551  // SVD decomposition of Acopy
1552  gsl_linalg_SV_decomp_jacobi(&Acopy.matrix, &V.matrix, &s.vector);
1553  // set small values to zero
1554  double mins = eps * std::fabs(gsl_vector_get(&s.vector, 0));
1555  for (int i = 0; i < ncon; ++i) {
1556  if (std::fabs(gsl_vector_get(&s.vector, i)) <= mins)
1557  gsl_vector_set(&s.vector, i, 0);
1558  }
1559  gsl_linalg_SV_solve(&Acopy.matrix, &V.matrix, &s.vector, &gradf.vector, &lambdanew.vector);
1560  } else {
1561  gsl_linalg_cholesky_solve(&ATA.matrix, &ATgradf.vector, &lambdanew.vector);
1562  }
1563  if (debug > 15) {
1564  B2INFO("lambdanew: ");
1565  gsl_vector_fprintf(stdout, &lambdanew.vector, "%f");
1566  }
1567  return 0;
1568  }
1569 
1570  void NewFitterGSL::MoorePenroseInverse(gsl_matrix* Ainv, gsl_matrix* A,
1571  gsl_matrix* Wm, gsl_vector* w,
1572  double eps
1573  )
1574  {
1575  assert(Ainv);
1576  assert(A);
1577  assert(Ainv->size1 == A->size2 && Ainv->size2 == A->size1);
1578  assert(A->size1 >= A->size2);
1579  assert(Wm);
1580  assert(Wm->size1 >= A->size1 && Wm->size2 >= A->size2);
1581  assert(w);
1582  assert(w->size >= A->size2);
1583 
1584  int n = A->size1;
1585  int m = A->size2;
1586 
1587  // Original A -> A diag(w) Wm^T
1588  gsl_linalg_SV_decomp_jacobi(A, Wm, w);
1589 
1590  double mins = eps * std::fabs(gsl_vector_get(w, 0));
1591 
1592  // Compute pseudo-inverse of diag(w)
1593  for (int i = 0; i < m; ++i) {
1594  double singval = gsl_vector_get(w, i);
1595  if (std::fabs(singval) > mins)
1596  gsl_vector_set(w, i, 1 / singval);
1597  else
1598  gsl_vector_set(w, i, 0);
1599  }
1600 
1601  // Compute Ainv = Wm diag(w) A^T
1602 
1603  // first: Ainv = Wm* diag(w)
1604  for (int j = 0; j < n; ++j) {
1605  double wval = gsl_vector_get(w, j);
1606  for (int i = 0; i < m; ++i)
1607  gsl_matrix_set(Wm, i, j, wval * gsl_matrix_get(Wm, i, j));
1608  }
1609  // Ainv = 1*Wm*A^T + 0*Ainv
1610  gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1, Wm, A, 0, Ainv);
1611 
1612  }
1613 
1614  double NewFitterGSL::calcpTLp(const gsl_vector* vecdx, const gsl_matrix* MatM,
1615  gsl_vector* vecw)
1616  {
1617  assert(vecdx);
1618  assert(vecdx->size == idim);
1619  assert(MatM);
1620  assert(MatM->size1 == idim && MatM->size2 == idim);
1621  assert(vecw);
1622  assert(vecw->size == idim);
1623 
1624  gsl_vector_const_view p(gsl_vector_const_subvector(vecdx, 0, npar));
1625  gsl_vector_view Lp(gsl_vector_subvector(vecw, 0, npar));
1626  gsl_matrix_const_view L(gsl_matrix_const_submatrix(MatM, 0, 0, npar, npar));
1627  gsl_blas_dsymv(CblasUpper, 1, &L.matrix, &p.vector, 0, &Lp.vector);
1628  double result;
1629  gsl_blas_ddot(&p.vector, &Lp.vector, &result);
1630 
1631  return result;
1632  }
1633 
1634  void NewFitterGSL::calc2ndOrderCorr(gsl_vector* vecdxhat,
1635  const gsl_vector* vecxnew,
1636  const gsl_matrix* MatM,
1637  gsl_matrix* MatW,
1638  gsl_vector* vecw,
1639  double eps)
1640  {
1641  assert(vecdxhat);
1642  assert(vecdxhat->size == idim);
1643  assert(vecxnew);
1644  assert(vecxnew->size == idim);
1645  assert(MatM);
1646  assert(MatM->size1 == idim && MatM->size2 == idim);
1647  assert(MatW);
1648  assert(MatW->size1 == idim && MatW->size2 == idim);
1649  assert(vecw);
1650  assert(vecw->size == idim);
1651  assert(idim == static_cast<unsigned int>(npar + ncon));
1652 
1653 
1654  // Calculate 2nd order correction
1655  // see Nocedal&Wright (15.36)
1656  gsl_matrix_const_view AT(gsl_matrix_const_submatrix(MatM, 0, npar, npar, ncon));
1657  gsl_matrix_view AAT(gsl_matrix_submatrix(MatW, npar, npar, ncon, ncon));
1658  gsl_matrix_view ATcopy(gsl_matrix_submatrix(MatW, 0, npar, npar, ncon));
1659  gsl_matrix_view& V = AAT;
1660 
1661  gsl_vector_set_zero(vecdxhat);
1662  addConstraints(vecdxhat);
1663  gsl_vector_view c(gsl_vector_subvector(vecdxhat, npar, ncon));
1664  gsl_vector_view phat = (gsl_vector_subvector(vecdxhat, 0, npar));
1665 
1666  gsl_vector_set_zero(vecw);
1667  gsl_vector_view AATinvc(gsl_vector_subvector(vecw, npar, ncon));
1668  gsl_vector_view& s = AATinvc;
1669 
1670 
1671  // AAT = 1*A*A^T + 0*AAT
1672  gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1, &AT.matrix, &AT.matrix, 0, &AAT.matrix);
1673 
1674  // solve AAT * AATinvc = c using the Cholsky factorization method
1675  gsl_error_handler_t* old_handler = gsl_set_error_handler_off();
1676  int cholesky_result = gsl_linalg_cholesky_decomp(&AAT.matrix);
1677  gsl_set_error_handler(old_handler);
1678  if (cholesky_result) {
1679  B2INFO("NewFitterGSL::calc2ndOrderCorr: resorting to SVD");
1680  // AAT is not positive definite, i.e. A does not have full column rank
1681  // => use the SVD of AT to solve A lambdanew = gradf
1682 
1683  // ATcopy = AT
1684  gsl_matrix_memcpy(&ATcopy.matrix, &AT.matrix);
1685 
1686  // SVD decomposition of Acopy
1687  gsl_linalg_SV_decomp_jacobi(&ATcopy.matrix, &V.matrix, &s.vector);
1688  // set small values to zero
1689  double mins = eps * std::fabs(gsl_vector_get(&s.vector, 0));
1690  for (int i = 0; i < ncon; ++i) {
1691  if (std::fabs(gsl_vector_get(&s.vector, i)) <= mins)
1692  gsl_vector_set(&s.vector, i, 0);
1693  }
1694  gsl_linalg_SV_solve(&ATcopy.matrix, &V.matrix, &s.vector, &c.vector, &AATinvc.vector);
1695  } else {
1696  gsl_linalg_cholesky_solve(&AAT.matrix, &c.vector, &AATinvc.vector);
1697  }
1698 
1699  // phat = -1*A^T*AATinvc+ 0*phat
1700  gsl_blas_dgemv(CblasNoTrans, -1, &AT.matrix, &AATinvc.vector, 0, &phat.vector);
1701  gsl_vector_set_zero(&c.vector);
1702 
1703  }
1704 
1705  int NewFitterGSL::solveSystem(gsl_vector* vecdxscal,
1706  double& detW,
1707  const gsl_vector* vecyscal,
1708  const gsl_matrix* MatMscal,
1709  gsl_matrix* MatW,
1710  gsl_matrix* MatW2,
1711  gsl_vector* vecw,
1712  double epsLU,
1713  double epsSV)
1714  {
1715  assert(vecdxscal);
1716  assert(vecdxscal->size == idim);
1717  assert(vecyscal);
1718  assert(vecyscal->size == idim);
1719  assert(MatMscal);
1720  assert(MatMscal->size1 == idim && MatMscal->size2 == idim);
1721  assert(MatW);
1722  assert(MatW->size1 == idim && MatW->size2 == idim);
1723  assert(MatW2);
1724  assert(MatW2->size1 == idim && MatW2->size2 == idim);
1725  assert(vecw);
1726  assert(vecw->size == idim);
1727 
1728  int result = 0;
1729 
1730  int iLU = solveSystemLU(vecdxscal, detW, vecyscal, MatMscal, MatW, vecw, epsLU);
1731  if (iLU == 0) return result;
1732 
1733  result = 1;
1734  int iSVD = solveSystemSVD(vecdxscal, vecyscal, MatMscal, MatW, MatW2, vecw, epsSV);
1735  if (iSVD == 0) return result;
1736 
1737  return -1;
1738  }
1739 
1740  int NewFitterGSL::solveSystemLU(gsl_vector* vecdxscal,
1741  double& detW,
1742  const gsl_vector* vecyscal,
1743  const gsl_matrix* MatMscal,
1744  gsl_matrix* MatW,
1745  gsl_vector* vecw,
1746  double eps)
1747  {
1748  assert(vecdxscal);
1749  assert(vecdxscal->size == idim);
1750  assert(vecyscal);
1751  assert(vecyscal->size == idim);
1752  assert(MatMscal);
1753  assert(MatMscal->size1 == idim && MatMscal->size2 == idim);
1754  assert(MatW);
1755  assert(MatW->size1 == idim && MatW->size2 == idim);
1756  assert(vecw);
1757  assert(vecw->size == idim);
1758 
1759  gsl_matrix_memcpy(MatW, MatMscal);
1760 
1761  int ifail = 0;
1762  detW = 0;
1763 
1764  int signum;
1765  int result = gsl_linalg_LU_decomp(MatW, permW, &signum);
1766  B2DEBUG(14, "NewFitterGSL::solveSystem: gsl_linalg_LU_decomp result=" << result);
1767  if (result != 0) return 1;
1768 
1769  detW = gsl_linalg_LU_det(MatW, signum);
1770  B2DEBUG(14, "NewFitterGSL::solveSystem: determinant of W=" << detW);
1771  if (std::fabs(detW) < eps) return 2;
1772  if (!std::isfinite(detW)) {
1773  B2DEBUG(10, "NewFitterGSL::solveSystem: infinite determinant of W=" << detW);
1774  return 3;
1775  }
1776  if (debug > 15) {
1777  B2INFO("NewFitterGSL::solveSystem: after LU decomposition: \n");
1778  debug_print(MatW, "W");
1779  }
1780  // Solve W*dxscal = yscal
1781  ifail = gsl_linalg_LU_solve(MatW, permW, vecyscal, vecdxscal);
1782  B2DEBUG(14, "NewFitterGSL::solveSystem: gsl_linalg_LU_solve result=" << ifail);
1783 
1784  if (ifail != 0) {
1785  B2ERROR("NewFitterGSL::solveSystem: ifail from gsl_linalg_LU_solve=" << ifail);
1786  return 3;
1787  }
1788  return 0;
1789  }
1790 
1791 
1792 
1793 
1794  int NewFitterGSL::solveSystemSVD(gsl_vector* vecdxscal,
1795 // double& detW,
1796  const gsl_vector* vecyscal,
1797  const gsl_matrix* MatMscal,
1798  gsl_matrix* MatW,
1799  gsl_matrix* MatW2,
1800  gsl_vector* vecw,
1801  double eps)
1802  {
1803  assert(vecdxscal);
1804  assert(vecdxscal->size == idim);
1805  assert(vecyscal);
1806  assert(vecyscal->size == idim);
1807  assert(MatMscal);
1808  assert(MatMscal->size1 == idim && MatMscal->size2 == idim);
1809  assert(MatW);
1810  assert(MatW->size1 == idim && MatW->size2 == idim);
1811  assert(MatW2);
1812  assert(MatW2->size1 == idim && MatW2->size2 == idim);
1813  assert(vecw);
1814  assert(vecw->size == idim);
1815 
1816  B2DEBUG(10, "solveSystemSVD called");
1817 
1818  gsl_matrix_memcpy(MatW, MatMscal);
1819 
1820  // SVD decomposition of MatW
1821  gsl_linalg_SV_decomp_jacobi(MatW, MatW2, vecw);
1822  // set small values to zero
1823  double mins = eps * std::fabs(gsl_vector_get(vecw, 0));
1824  B2DEBUG(15, "SV 0 = " << gsl_vector_get(vecw, 0));
1825  for (unsigned int i = 0; i < idim; ++i) {
1826  if (std::fabs(gsl_vector_get(vecw, i)) <= mins) {
1827  B2DEBUG(15, "Setting SV" << i << " = " << gsl_vector_get(vecw, i) << " to zero!");
1828  gsl_vector_set(vecw, i, 0);
1829  }
1830  }
1831  gsl_linalg_SV_solve(MatW, MatW2, vecw, vecyscal, vecdxscal);
1832  return 0;
1833  }
1834 
1835  gsl_matrix_view NewFitterGSL::calcZ(int& rankA, gsl_matrix* MatW1, gsl_matrix* MatW2,
1836  gsl_vector* vecw1, gsl_vector* vecw2,
1837  gsl_permutation* permw, double eps)
1838  {
1839  assert(MatW1);
1840  assert(MatW1->size1 == idim && MatW1->size2 == idim);
1841  assert(MatW2);
1842  assert(MatW2->size1 == idim && MatW2->size2 == idim);
1843  assert(vecw1);
1844  assert(vecw1->size == idim);
1845  assert(vecw2);
1846  assert(vecw2->size == idim);
1847  assert(permw);
1848  assert(permw->size == idim);
1849 
1850  // fill A and AT
1851  assembleConstDer(MatW2);
1852 
1853  gsl_matrix_const_view AT(gsl_matrix_const_submatrix(MatW2, 0, 0, npar, npar));
1854  //gsl_matrix_view QR (gsl_matrix_submatrix (MatW2, 0, 0, npar, npar));
1855  gsl_matrix_view Q(gsl_matrix_submatrix(MatW1, 0, 0, npar, npar));
1856  gsl_matrix_view R(gsl_matrix_submatrix(MatW1, npar, 0, ncon, npar));
1857 
1858  int signum = 0;
1859  //gsl_linalg_QRPT_decomp (&QR.matrix, vecw1, permw, &signum, vecw2);
1860  //gsl_linalg_QR_unpack (&QR.matrix, vecw1, &Q.matrix, &R.matrix);
1861  gsl_linalg_QRPT_decomp2(&AT.matrix, &Q.matrix, &R.matrix, vecw1, permw, &signum, vecw2);
1862 
1863  rankA = 0;
1864  for (int i = 0; i < ncon; ++i) {
1865  if (fabs(gsl_matrix_get(&R.matrix, i, i)) > eps) rankA++;
1866  }
1867  auto result = gsl_matrix_view(gsl_matrix_submatrix(MatW1, 0, rankA, ncon - rankA, npar));
1868  return result;
1869  }
1870 
1871  gsl_matrix_view NewFitterGSL::calcReducedHessian(int& rankH, gsl_matrix* MatW1,
1872  const gsl_vector* vecx, gsl_matrix* MatW2,
1873  gsl_matrix* MatW3,
1874  gsl_vector* vecw1, gsl_vector* vecw2,
1875  gsl_permutation* permw, double eps)
1876  {
1877  assert(MatW1);
1878  assert(MatW1->size1 == idim && MatW1->size2 == idim);
1879  assert(vecx);
1880  assert(vecx->size == idim);
1881  assert(MatW2);
1882  assert(MatW2->size1 == idim && MatW2->size2 == idim);
1883  assert(MatW3);
1884  assert(MatW2->size1 == idim && MatW2->size2 == idim);
1885  assert(vecw1);
1886  assert(vecw1->size == idim);
1887  assert(vecw2);
1888  assert(vecw2->size == idim);
1889  assert(permw);
1890  assert(permw->size == idim);
1891 
1892  int rankA;
1893  // Z is a matrix view of MatW2!
1894  gsl_matrix_view Z = calcZ(rankA, MatW2, MatW1, vecw1, vecw2, permw, eps);
1895 
1896  // fill Lagrangian
1897  assembleG(MatW1, vecx);
1898 
1899  rankH = npar - rankA;
1900 
1901  gsl_matrix_view G(gsl_matrix_submatrix(MatW1, 0, 0, npar, npar));
1902  gsl_matrix_view GZ(gsl_matrix_submatrix(MatW3, 0, 0, npar, rankH));
1903  gsl_matrix_view ZGZ(gsl_matrix_submatrix(MatW1, 0, 0, rankH, rankH));
1904 
1905  // Calculate Z^T G Z
1906 
1907  // GZ = 1*G*Z + 0*GZ
1908  gsl_blas_dsymm(CblasLeft, CblasUpper, 1, &G.matrix, &Z.matrix, 0, &GZ.matrix);
1909  // ZGZ = 1*Z^T*GZ + 0*ZGZ
1910  gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1, &Z.matrix, &GZ.matrix, 0, &ZGZ.matrix);
1911 
1912  return ZGZ;
1913  }
1914 
1915  gsl_vector_view NewFitterGSL::calcReducedHessianEigenvalues(int& rankH, gsl_matrix* MatW1,
1916  const gsl_vector* vecx, gsl_matrix* MatW2,
1917  gsl_matrix* MatW3,
1918  gsl_vector* vecw1, gsl_vector* vecw2,
1919  gsl_permutation* permw,
1920  gsl_eigen_symm_workspace* eigensws,
1921  double eps)
1922  {
1923  assert(MatW1);
1924  assert(MatW1->size1 == idim && MatW1->size2 == idim);
1925  assert(vecx);
1926  assert(vecx->size == idim);
1927  assert(MatW2);
1928  assert(MatW2->size1 == idim && MatW2->size2 == idim);
1929  assert(MatW3);
1930  assert(MatW2->size1 == idim && MatW2->size2 == idim);
1931  assert(vecw1);
1932  assert(vecw1->size == idim);
1933  assert(vecw2);
1934  assert(vecw2->size == idim);
1935  assert(permw);
1936  assert(permw->size == idim);
1937  assert(eigensws);
1938 
1939  gsl_matrix_view Hred = calcReducedHessian(rankH, MatW1, vecx, MatW2, MatW3, vecw1, vecw2, permw, eps);
1940 
1941  gsl_matrix_view Hredcopy(gsl_matrix_submatrix(MatW3, 0, 0, rankH, rankH));
1942  // copy Hred -> Hredcopy
1943  gsl_matrix_memcpy(&Hredcopy.matrix, &Hred.matrix);
1944 
1945  gsl_vector_view eval(gsl_vector_subvector(vecw1, 0, rankH));
1946  gsl_eigen_symm(&Hredcopy.matrix, &eval.vector, eigensws);
1947 
1948  return eval;
1949  }
1950 
1951  }// end OrcaKinFit namespace
1953 } // end Belle2 namespace
1954 
Belle2::OrcaKinFit::BaseFitObject::updateParams
virtual bool updateParams(double p[], int idim)
Read values from global vector, readjust vector; return: significant change.
Definition: BaseFitObject.cc:196
Belle2::OrcaKinFit::BaseFitObject::getNPar
virtual int getNPar() const =0
Get total number of parameters of this FitObject.
Belle2::OrcaKinFit::BaseFitObject::addToGlobalChi2DerMatrix
virtual void addToGlobalChi2DerMatrix(double *M, int idim) const
Add 2nd derivatives of chi squared to global derivative matrix.
Definition: BaseFitObject.cc:462
Belle2::OrcaKinFit::BaseFitObject::getError
virtual double getError(int ilocal) const
Get error of parameter ilocal.
Definition: BaseFitObject.cc:383
Belle2::OrcaKinFit::BaseFitObject::getGlobalParNum
virtual int getGlobalParNum(int ilocal) const
Get global parameter number of parameter ilocal.
Definition: BaseFitObject.cc:365
Belle2::OrcaKinFit::BaseFitObject::getName
virtual const char * getName() const
Get object's name.
Definition: BaseFitObject.cc:107
Belle2::OrcaKinFit::BaseFitObject::isParamFixed
virtual bool isParamFixed(int ilocal) const
Returns whether parameter is fixed.
Definition: BaseFitObject.cc:408
Belle2::OrcaKinFit::BaseFitObject::getParam
virtual double getParam(int ilocal) const
Get current value of parameter ilocal.
Definition: BaseFitObject.cc:371
Belle2::OrcaKinFit::BaseFitObject::addToGlobalChi2DerVector
virtual int addToGlobalChi2DerVector(double *y, int idim) const
Add derivatives of chi squared to global derivative vector.
Definition: BaseFitObject.cc:482
Belle2::OrcaKinFit::BaseFitObject::addToGlobCov
virtual void addToGlobCov(double *glcov, int idim) const
Add covariance matrix elements to global covariance matrix of size idim x idim.
Definition: BaseFitObject.cc:217
Belle2::OrcaKinFit::BaseFitter::cov
double * cov
global covariance matrix of last fit problem
Definition: BaseFitter.h:104
Belle2::OrcaKinFit::BaseFitter::covValid
bool covValid
Flag whether global covariance is valid.
Definition: BaseFitter.h:105
Belle2::OrcaKinFit::BaseFitter::covDim
int covDim
dimension of global covariance matrix
Definition: BaseFitter.h:103
Belle2::OrcaKinFit::BaseHardConstraint
Abstract base class for constraints of kinematic fits.
Definition: BaseHardConstraint.h:75
Belle2::OrcaKinFit::BaseTracer::step
virtual void step(BaseFitter &fitter)
Called at the end of each step.
Definition: BaseTracer.cc:35
Belle2::OrcaKinFit::BaseTracer::initialize
virtual void initialize(BaseFitter &fitter)
Called at the start of a new fit (during initialization)
Definition: BaseTracer.cc:30
Belle2::OrcaKinFit::BaseTracer::finish
virtual void finish(BaseFitter &fitter)
Called at the end of a fit.
Definition: BaseTracer.cc:45
Belle2::OrcaKinFit::BaseTracer::substep
virtual void substep(BaseFitter &fitter, int flag)
Called at intermediate points during a step.
Definition: BaseTracer.cc:40
Belle2::OrcaKinFit::NewFitterGSL::ncon
int ncon
total number of hard constraints
Definition: NewFitterGSL.h:255
Belle2::OrcaKinFit::NewFitterGSL::npar
int npar
total number of parameters
Definition: NewFitterGSL.h:254
Belle2::OrcaKinFit::NewFitterGSL::calcMu
double calcMu(const gsl_vector *vecx, const gsl_vector *vece, const gsl_vector *vecdx, const gsl_vector *vecdxscal, const gsl_vector *xnew, const gsl_matrix *MatM, const gsl_matrix *MatMscal, gsl_vector *vecw)
Definition: NewFitterGSL.cc:1162
Belle2::OrcaKinFit::NewFitterGSL::calcChi2
virtual double calcChi2()
Calculate the chi2.
Definition: NewFitterGSL.cc:416
Belle2::OrcaKinFit::NewFitterGSL::MoorePenroseInverse
static void MoorePenroseInverse(gsl_matrix *Ainv, gsl_matrix *A, gsl_matrix *W, gsl_vector *w, double eps=0)
Compute the Moore-Penrose pseudo-inverse A+ of A, using SVD.
Definition: NewFitterGSL.cc:1570
Belle2::OrcaKinFit::NewFitterGSL::getNsoft
virtual int getNsoft() const
Get the number of soft constraints of the last fit.
Definition: NewFitterGSL.cc:498
Belle2::OrcaKinFit::NewFitterGSL::solveSystemLU
int solveSystemLU(gsl_vector *vecdxscal, double &detW, const gsl_vector *vecyscal, const gsl_matrix *MatMscal, gsl_matrix *MatW, gsl_vector *vecw, double eps)
solve system of equations Mscal*dxscal = yscal using LU decomposition
Definition: NewFitterGSL.cc:1740
Belle2::OrcaKinFit::NewFitterGSL::calcReducedHessian
virtual gsl_matrix_view calcReducedHessian(int &rankH, gsl_matrix *MatW1, const gsl_vector *vecx, gsl_matrix *MatW2, gsl_matrix *MatW3, gsl_vector *vecw1, gsl_vector *vecw2, gsl_permutation *permW, double eps=0)
Calculate reduced Hessian; return value is a matrix view of MatW1 giving the reduced Hessian.
Definition: NewFitterGSL.cc:1871
Belle2::OrcaKinFit::NewFitterGSL::determineLambdas
virtual int determineLambdas(gsl_vector *vecxnew, const gsl_matrix *MatM, const gsl_vector *vecx, gsl_matrix *MatW, gsl_vector *vecw, double eps=0)
Determine best lambda values.
Definition: NewFitterGSL.cc:1483
Belle2::OrcaKinFit::NewFitterGSL::fit
virtual double fit() override
The fit method, returns the fit probability.
Definition: NewFitterGSL.cc:143
Belle2::OrcaKinFit::NewFitterGSL::initialize
virtual bool initialize() override
Initialize the fitter.
Definition: NewFitterGSL.cc:335
Belle2::OrcaKinFit::NewFitterGSL::solveSystemSVD
int solveSystemSVD(gsl_vector *vecdxscal, const gsl_vector *vecyscal, const gsl_matrix *MatMscal, gsl_matrix *MatW, gsl_matrix *MatW2, gsl_vector *vecw, double eps)
solve system of equations Mscal*dxscal = yscal using SVD decomposition
Definition: NewFitterGSL.cc:1794
Belle2::OrcaKinFit::NewFitterGSL::getNcon
virtual int getNcon() const
Get the number of hard constraints of the last fit.
Definition: NewFitterGSL.cc:497
Belle2::OrcaKinFit::NewFitterGSL::~NewFitterGSL
virtual ~NewFitterGSL()
Virtual destructor.
Definition: NewFitterGSL.cc:78
Belle2::OrcaKinFit::NewFitterGSL::doLineSearch
int doLineSearch(double &alpha, gsl_vector *vecxnew, int imode, double phi0, double dphi0, double eta, double zeta, double mu, const gsl_vector *vecx, const gsl_vector *vecdx, const gsl_vector *vece, gsl_vector *vecw)
Definition: NewFitterGSL.cc:1047
Belle2::OrcaKinFit::NewFitterGSL::calcNewtonDx
int calcNewtonDx(gsl_vector *vecdx, gsl_vector *vecdxscal, gsl_vector *vecx, const gsl_vector *vece, gsl_matrix *MatM, gsl_matrix *MatMscal, gsl_vector *vecy, gsl_vector *vecyscal, gsl_matrix *MatW, gsl_matrix *MatW2, gsl_permutation *permW, gsl_vector *vecw)
Definition: NewFitterGSL.cc:802
Belle2::OrcaKinFit::NewFitterGSL::fitprob
double fitprob
fit probability
Definition: NewFitterGSL.h:261
Belle2::OrcaKinFit::NewFitterGSL::nsoft
int nsoft
total number of soft constraints
Definition: NewFitterGSL.h:256
Belle2::OrcaKinFit::NewFitterGSL::getError
virtual int getError() const override
Get the error code of the last fit: 0=OK, 1=failed.
Definition: NewFitterGSL.cc:430
Belle2::OrcaKinFit::NewFitterGSL::nunm
int nunm
total number of unmeasured parameters
Definition: NewFitterGSL.h:257
Belle2::OrcaKinFit::NewFitterGSL::nit
int nit
Number of iterations.
Definition: NewFitterGSL.h:259
Belle2::OrcaKinFit::NewFitterGSL::getNunm
virtual int getNunm() const
Get the number of unmeasured parameters of the last fit.
Definition: NewFitterGSL.cc:499
Belle2::OrcaKinFit::NewFitterGSL::solveSystem
int solveSystem(gsl_vector *vecdxscal, double &detW, const gsl_vector *vecyscal, const gsl_matrix *MatMscal, gsl_matrix *MatW, gsl_matrix *MatW2, gsl_vector *vecw, double epsLU, double epsSV)
solve system of equations Mscal*dxscal = yscal
Definition: NewFitterGSL.cc:1705
Belle2::OrcaKinFit::NewFitterGSL::meritFunction
double meritFunction(double mu, const gsl_vector *vecx, const gsl_vector *vece)
Definition: NewFitterGSL.cc:1250
Belle2::OrcaKinFit::NewFitterGSL::calcReducedHessianEigenvalues
virtual gsl_vector_view calcReducedHessianEigenvalues(int &rankH, gsl_matrix *MatW1, const gsl_vector *vecx, gsl_matrix *MatW2, gsl_matrix *MatW3, gsl_vector *vecw1, gsl_vector *vecw2, gsl_permutation *permW, gsl_eigen_symm_workspace *eigenws, double eps=0)
Definition: NewFitterGSL.cc:1915
Belle2::OrcaKinFit::NewFitterGSL::getIterations
virtual int getIterations() const override
Get the number of iterations of the last fit.
Definition: NewFitterGSL.cc:434
Belle2::OrcaKinFit::NewFitterGSL::calcZ
virtual gsl_matrix_view calcZ(int &rankA, gsl_matrix *MatW1, gsl_matrix *MatW2, gsl_vector *vecw1, gsl_vector *vecw2, gsl_permutation *permW, double eps=0)
Calculate null space of constraints; return value is a matrix view of MatW1, with column vectors span...
Definition: NewFitterGSL.cc:1835
Belle2::OrcaKinFit::NewFitterGSL::calc2ndOrderCorr
virtual void calc2ndOrderCorr(gsl_vector *vecdxhat, const gsl_vector *vecxnew, const gsl_matrix *MatM, gsl_matrix *MatW, gsl_vector *vecw, double eps=0)
Calculate 2nd order correction step.
Definition: NewFitterGSL.cc:1634
Belle2::OrcaKinFit::NewFitterGSL::meritFunctionDeriv
double meritFunctionDeriv(double mu, const gsl_vector *vecx, const gsl_vector *vece, const gsl_vector *vecdx, gsl_vector *vecw)
Definition: NewFitterGSL.cc:1285
Belle2::OrcaKinFit::NewFitterGSL::getProbability
virtual double getProbability() const override
Get the fit probability of the last fit.
Definition: NewFitterGSL.cc:431
Belle2::OrcaKinFit::NewFitterGSL::setDebug
virtual void setDebug(int debuglevel)
Set the Debug Level.
Definition: NewFitterGSL.cc:1363
Belle2::OrcaKinFit::NewFitterGSL::getDoF
virtual int getDoF() const override
Get the number of degrees of freedom of the last fit.
Definition: NewFitterGSL.cc:433
Belle2::OrcaKinFit::NewFitterGSL::getChi2
virtual double getChi2() const override
Get the chi**2 of the last fit.
Definition: NewFitterGSL.cc:432
Belle2::OrcaKinFit::NewFitterGSL::getNpar
virtual int getNpar() const
Get the number of all parameters of the last fit.
Definition: NewFitterGSL.cc:500
Belle2::OrcaKinFit::NewFitterGSL::calcpTLp
double calcpTLp(const gsl_vector *vecdx, const gsl_matrix *MatM, gsl_vector *vecw)
Definition: NewFitterGSL.cc:1614
Belle2::OrcaKinFit::NewFitterGSL::calcLimitedDx
int calcLimitedDx(double &alpha, double &mu, gsl_vector *vecxnew, int imode, gsl_vector *vecx, gsl_vector *vecdxhat, const gsl_vector *vecdx, const gsl_vector *vecdxscal, const gsl_vector *vece, const gsl_matrix *MatM, const gsl_matrix *MatMscal, gsl_matrix *MatW, gsl_vector *vecw)
Definition: NewFitterGSL.cc:913
Belle2::eval
double eval(const std::vector< double > &spl, const std::vector< double > &vals, double x)
Evaluate spline (zero order or first order) in point x.
Definition: tools.h:115
Belle2
Abstract base class for different kinds of events.
Definition: MillepedeAlgorithm.h:17