Belle II Software  release-08-01-10
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  // imode == 0 (Armijo) asserted above
1132  if (imode == 1) { // Wolfe
1133  dphi = meritFunctionDeriv(mu, vecxnew, vece, vecdx, vecw);
1134  if (dphi < zeta * dphi0) {
1135  B2DEBUG(15, "NewFitterGSL::doLineSearch, Wolfe: dphi=" << dphi
1136  << " < " << zeta * dphi0
1137  << " at alpha=" << alpha);
1138  alphaL = alpha;
1139  // phiL = phi; //fix warning
1140  // dphiL = dphi; //fix warning
1141  } else {
1142  break;
1143  }
1144  } else { // Goldstein
1145  if (phi < phi0 + zeta * alpha * dphi0) {
1146  B2DEBUG(15, "NewFitterGSL::doLineSearch, Goldstein: phi=" << phi
1147  << " < " << phi0 + zeta * alpha * dphi0
1148  << " at alpha=" << alpha);
1149  alphaL = alpha;
1150  // phiL = phi; //fix warning
1151  } else {
1152  break;
1153  }
1154  }
1155  } while (nite < 30 && (alphaL == 0 || nite < 6));
1156  if (alphaL > 0) alpha = alphaL;
1157  return 1;
1158  }
1159 
1160 
1161  double NewFitterGSL::calcMu(const gsl_vector* vecx, const gsl_vector* vece,
1162  const gsl_vector* vecdx, const gsl_vector* vecdxscal,
1163  const gsl_vector* vecxnew,
1164  const gsl_matrix* MatM, const gsl_matrix* MatMscal,
1165  gsl_vector* vecw)
1166  {
1167  assert(vecx);
1168  assert(vecx->size == idim);
1169  assert(vece);
1170  assert(vece->size == idim);
1171  assert(vecdx);
1172  assert(vecdx->size == idim);
1173  assert(vecdxscal);
1174  assert(vecdxscal->size == idim);
1175  assert(MatM);
1176  assert(MatM->size1 == idim && MatM->size2 == idim);
1177  assert(MatMscal);
1178  assert(MatMscal->size1 == idim && MatMscal->size2 == idim);
1179  assert(vecw);
1180  assert(vecw->size == idim);
1181 
1182  double result = 0;
1183  switch (imerit) {
1184  case 1: { // l1 penalty function, Nocedal&Wright Eq. (15.24)
1185  result = 0;
1186 // for (int kglobal = npar; kglobal < npar+ncon; ++kglobal) {
1187 // double abslambda = std::fabs (gsl_vector_get (vecx, kglobal));
1188 // if (abslambda > result)
1189 // result = abslambda;
1190 // }
1191  // calculate grad f^T*p
1192  gsl_vector_set_zero(vecw);
1193  addConstraints(vecw);
1194  gsl_vector_view c(gsl_vector_subvector(vecw, npar, ncon));
1195  // ||c||_1
1196  double cnorm1 = gsl_blas_dasum(&c.vector);
1197  // scale constraint values by 1/(delta e)
1198  gsl_vector_const_view lambdaerr(gsl_vector_const_subvector(vece, npar, ncon));
1199  gsl_vector_mul(&c.vector, &lambdaerr.vector);
1200  // ||c||_1
1201  double cnorm1scal = gsl_blas_dasum(&c.vector);
1202 
1203  double rho = 0.1;
1204  double eps = 0.001;
1205 
1206  assembleChi2Der(vecw);
1207  gsl_vector_view gradf(gsl_vector_subvector(vecw, 0, npar));
1208  gsl_vector_const_view p(gsl_vector_const_subvector(vecdx, 0, npar));
1209  double gradfTp;
1210  gsl_blas_ddot(&gradf.vector, &p.vector, &gradfTp);
1211 
1212  B2DEBUG(17, "NewFitterGSL::calcMu: cnorm1scal=" << cnorm1scal
1213  << ", gradfTp=" << gradfTp);
1214 
1215  // all constraints very well fulfilled, use max(lambda+1) criterium
1216  if (cnorm1scal < ncon * eps || gradfTp <= 0) {
1217  for (int kglobal = npar; kglobal < npar + ncon; ++kglobal) {
1218  double abslambda = std::fabs(gsl_vector_get(vecxnew, kglobal));
1219  if (abslambda > result)
1220  result = abslambda;
1221  }
1222  result /= (1 - rho);
1223  } else {
1224  // calculate p^T L p
1225  double pTLp = calcpTLp(vecdx, MatM, vecw);
1226  double sigma = (pTLp > 0) ? 1 : 0;
1227  B2DEBUG(17, " pTLp = " << pTLp);
1228  // Nocedal&Wright Eq. (18.36)
1229  result = (gradfTp + 0.5 * sigma * pTLp) / ((1 - rho) * cnorm1);
1230  }
1231  B2DEBUG(17, " result = " << result);
1232  }
1233  break;
1234  case 2: // l1 penalty function, errors scaled, Nocedal&Wright Eq. (15.24)
1235  result = 0;
1236  for (int kglobal = npar; kglobal < npar + ncon; ++kglobal) {
1237  double abslambdascal = std::fabs(gsl_vector_get(vecx, kglobal) / gsl_vector_get(vece, kglobal));
1238  if (abslambdascal > result)
1239  result = abslambdascal;
1240  }
1241  break;
1242  default: assert(0);
1243  }
1244  return result;
1245 
1246  }
1247 
1248 
1249  double NewFitterGSL::meritFunction(double mu, const gsl_vector* vecx, const gsl_vector* vece)
1250  {
1251  assert(vecx);
1252  assert(vecx->size == idim);
1253  assert(vece);
1254  assert(vece->size == idim);
1255 
1256  double result = 0;
1257  switch (imerit) {
1258  case 1: // l1 penalty function, Nocedal&Wright Eq. (15.24)
1259  result = calcChi2();
1260  for (auto c : constraints) {
1261  assert(c);
1262  int kglobal = c->getGlobalNum();
1263  assert(kglobal >= 0 && kglobal < (int)idim);
1264  result += mu * std::fabs(c->getValue());
1265  }
1266  break;
1267  case 2: // l1 penalty function, errors scaled, Nocedal&Wright Eq. (15.24)
1268  result = calcChi2();
1269  for (auto c : constraints) {
1270  assert(c);
1271  int kglobal = c->getGlobalNum();
1272  assert(kglobal >= 0 && kglobal < (int)idim);
1273  // vece[kglobal] is 1/error for constraint k
1274  result += mu * std::fabs(c->getValue() * gsl_vector_get(vece, kglobal));
1275  }
1276  break;
1277  default: assert(0);
1278  }
1279  return result;
1280 
1281  }
1282 
1283 
1284  double NewFitterGSL::meritFunctionDeriv(double mu, const gsl_vector* vecx, const gsl_vector* vece,
1285  const gsl_vector* vecdx, gsl_vector* vecw)
1286  {
1287  assert(vecx);
1288  assert(vecx->size == idim);
1289  assert(vece);
1290  assert(vece->size == idim);
1291  assert(vecdx);
1292  assert(vecdx->size == idim);
1293  assert(vecw);
1294  assert(vecw->size == idim);
1295 
1296  double result = 0;
1297  switch (imerit) {
1298  case 1: // l1 penalty function, Nocedal&Wright Eq. (15.24), Eq. (18.29)
1299  assembleChi2Der(vecw);
1300  for (int i = 0; i < npar; ++i) {
1301  result += gsl_vector_get(vecdx, i) * gsl_vector_get(vecw, i);
1302  }
1303 // for (ConstraintIterator i = constraints.begin(); i != constraints.end(); ++i) {
1304 // BaseHardConstraint *c = *i;
1305 // assert (c);
1306 // int kglobal = c->getGlobalNum();
1307 // assert (kglobal >= 0 && kglobal < (int)idim);
1308 // result += c->dirDerAbs (vecdx->block->data, vecw->block->data, npar, mu);
1309 // }
1310  for (auto c : constraints) {
1311  assert(c);
1312  int kglobal = c->getGlobalNum();
1313  assert(kglobal >= 0 && kglobal < (int)idim);
1314  result -= mu * std::fabs(c->getValue());
1315  }
1316  break;
1317  case 2: // l1 penalty function, errors scaled, Nocedal&Wright Eq. (15.24), Eq. (18.29)
1318  assembleChi2Der(vecw);
1319  for (int i = 0; i < npar; ++i) {
1320  result += gsl_vector_get(vecdx, i) * gsl_vector_get(vecw, i);
1321  }
1322 // for (ConstraintIterator i = constraints.begin(); i != constraints.end(); ++i) {
1323 // BaseHardConstraint *c = *i;
1324 // assert (c);
1325 // int kglobal = c->getGlobalNum();
1326 // assert (kglobal >= 0 && kglobal < (int)idim);
1327 // result += c->dirDerAbs (vecdx->block->data, vecw->block->data, npar, mu*gsl_vector_get (vece, kglobal));
1328 // }
1329  for (auto c : constraints) {
1330  assert(c);
1331  int kglobal = c->getGlobalNum();
1332  assert(kglobal >= 0 && kglobal < (int)idim);
1333  result -= mu * std::fabs(c->getValue()) * gsl_vector_get(vece, kglobal);
1334  }
1335  break;
1336  default: assert(0);
1337  }
1338  return result;
1339  }
1340 
1341 
1342  int NewFitterGSL::invertM()
1343  {
1344  gsl_matrix_memcpy(W, M);
1345 
1346  int ifail = 0;
1347 
1348  int signum;
1349  // Calculate LU decomposition of M into W
1350  int result = gsl_linalg_LU_decomp(W, permW, &signum);
1351  B2DEBUG(11, "invertM: gsl_linalg_LU_decomp result=" << result);
1352  // Calculate inverse of M
1353  ifail = gsl_linalg_LU_invert(W, permW, M);
1354  B2DEBUG(11, "invertM: gsl_linalg_LU_invert result=" << ifail);
1355 
1356  if (ifail != 0) {
1357  B2ERROR("NewtonFitter::invert: ifail from gsl_linalg_LU_invert=" << ifail);
1358  }
1359  return ifail;
1360  }
1361 
1362  void NewFitterGSL::setDebug(int Debuglevel)
1363  {
1364  debug = Debuglevel;
1365  }
1366 
1367 
1368  int NewFitterGSL::calcCovMatrix(gsl_matrix* MatW,
1369  gsl_permutation* permw,
1370  gsl_vector* vecx)
1371  {
1372  // Set up equation system M*dadeta + dydeta = 0
1373  // here, dadeta is d a / d eta, i.e. the derivatives of the fitted
1374  // parameters a w.r.t. to the measured parameters eta,
1375  // and dydeta is the derivative of the set of equations
1376  // w.r.t eta, i.e. simply d^2 chi^2 / d a d eta.
1377  // Now, if chi2 = (a-eta)^T*Vinv((a-eta), we have simply
1378  // d^2 chi^2 / d a d eta = - d^2 chi^2 / d a d a
1379  // and can use the method addToGlobalChi2DerMatrix.
1380 
1381  gsl_matrix_set_zero(M1);
1382  gsl_matrix_set_zero(M2);
1383  // First, all terms d^2 chi^2/dx1 dx2
1384  for (auto fo : fitobjects) {
1385  assert(fo);
1386  fo->addToGlobalChi2DerMatrix(M1->block->data, M1->tda);
1387  fo->addToGlobCov(M2->block->data, M2->tda);
1388  }
1389  // multiply by -1
1390  gsl_matrix_scale(M1, -1);
1391 
1392  // JL: dy/eta are the derivatives of the "objective function" with respect to the MEASURED parameters.
1393  // Since the soft constraints do not depend on the measured, but only on the fitted (!) parameters,
1394  // dy/deta stays -1*M1 also in the presence of soft constraints!
1395  gsl_matrix_view dydeta = gsl_matrix_submatrix(M1, 0, 0, idim, npar);
1396  gsl_matrix_view Cov_eta = gsl_matrix_submatrix(M2, 0, 0, npar, npar);
1397 
1398  if (debug > 13) {
1399  B2INFO("NewFitterGSL::calcCovMatrix\n");
1400  debug_print(&dydeta.matrix, "dydeta");
1401  debug_print(&Cov_eta.matrix, "Cov_eta");
1402  }
1403 
1404  // JL: calculates d^2 chi^2 / dx1 dx2 + first (!) derivatives of hard & soft constraints, and the
1405  // second derivatives of the soft constraints times the values of the fitted parameters
1406  // - all of the with respect to the FITTED parameters, therefore with soft constraints like in the fit itself.
1407  assembleM(MatW, vecx, true);
1408 
1409  if (debug > 13) {
1410  debug_print(MatW, "MatW");
1411  }
1412 
1413  // Now, solve M*dadeta = dydeta
1414 
1415  // Calculate LU decomposition of M into M3
1416  int signum;
1417  int result = gsl_linalg_LU_decomp(MatW, permw, &signum);
1418 
1419  if (debug > 13) {
1420  B2INFO("calcCovMatrix: gsl_linalg_LU_decomp result=" << result);
1421  debug_print(MatW, "M_LU");
1422  }
1423 
1424  // Calculate inverse of M, store in M3
1425  gsl_set_error_handler_off();
1426  int ifail = gsl_linalg_LU_invert(MatW, permw, M3);
1427  if (ifail) {
1428  B2WARNING("NewFitterGSL: MatW matrix is singular!");
1429  return ifail;
1430  }
1431 
1432  if (debug > 13) {
1433  B2INFO("calcCovMatrix: gsl_linalg_LU_invert ifail=" << ifail);
1434  debug_print(M3, "Minv");
1435  }
1436 
1437  // Calculate dadeta = M3*dydeta
1438  gsl_matrix_set_zero(M4);
1439  gsl_matrix_view dadeta = gsl_matrix_submatrix(M4, 0, 0, idim, npar);
1440 
1441  if (debug > 13) {
1442  debug_print(&dadeta.matrix, "dadeta");
1443  }
1444 
1445  // dadeta = 1*M*dydeta + 0*dadeta
1446  gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, M3, &dydeta.matrix, 0, &dadeta.matrix);
1447 
1448 
1449  // Now calculate Cov_a = dadeta*Cov_eta*dadeta^T
1450 
1451  // First, calculate M3 = Cov_eta*dadeta^T as
1452  gsl_matrix_view M3part = gsl_matrix_submatrix(M3, 0, 0, npar, idim);
1453  gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1, &Cov_eta.matrix, &dadeta.matrix, 0, &M3part.matrix);
1454  // Now Cov_a = dadeta*M3part
1455  gsl_matrix_set_zero(M5);
1456  gsl_matrix_view Cov_a = gsl_matrix_submatrix(M5, 0, 0, npar, npar);
1457  gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, &dadeta.matrix, &M3part.matrix, 0, M5);
1458  gsl_matrix_memcpy(CCinv, M5);
1459 
1460  if (debug > 13) {
1461  debug_print(&Cov_a.matrix, "Cov_a");
1462  debug_print(CCinv, "full Cov from err prop");
1463  debug_print(M1, "uncorr Cov from err prop");
1464  }
1465 
1466  // Finally, copy covariance matrix
1467  if (cov && covDim != npar) {
1468  delete[] cov;
1469  cov = nullptr;
1470  }
1471  covDim = npar;
1472  if (!cov) cov = new double[covDim * covDim];
1473  for (int i = 0; i < covDim; ++i) {
1474  for (int j = 0; j < covDim; ++j) {
1475  cov[i * covDim + j] = gsl_matrix_get(&Cov_a.matrix, i, j);
1476  }
1477  }
1478  covValid = true;
1479  return 0;
1480  }
1481 
1482  int NewFitterGSL::determineLambdas(gsl_vector* vecxnew,
1483  const gsl_matrix* MatM, const gsl_vector* vecx,
1484  gsl_matrix* MatW, gsl_vector* vecw,
1485  double eps)
1486  {
1487  assert(vecxnew);
1488  assert(vecxnew->size == idim);
1489  assert(MatM);
1490  assert(MatM->size1 == idim && MatM->size2 == idim);
1491  assert(vecx);
1492  assert(vecx->size == idim);
1493  assert(MatW);
1494  assert(MatW->size1 == idim && MatW->size2 == idim);
1495  assert(vecw);
1496  assert(vecw->size == idim);
1497  assert(idim == static_cast<unsigned int>(npar + ncon));
1498 
1499  gsl_matrix_const_view A(gsl_matrix_const_submatrix(MatM, 0, npar, npar, ncon));
1500  gsl_matrix_view ATA(gsl_matrix_submatrix(MatW, npar, npar, ncon, ncon));
1501  gsl_matrix_view Acopy(gsl_matrix_submatrix(MatW, 0, npar, npar, ncon));
1502  gsl_matrix_view& V = ATA;
1503 
1504  gsl_vector_view gradf(gsl_vector_subvector(vecw, 0, npar));
1505  gsl_vector_view ATgradf(gsl_vector_subvector(vecw, npar, ncon));
1506  gsl_vector_view& s = ATgradf;
1507 
1508  gsl_vector_view lambdanew(gsl_vector_subvector(vecxnew, npar, ncon));
1509 
1510  if (debug > 15) {
1511 // gsl_vector_const_view lambda(gsl_vector_const_subvector(vecx, npar, ncon));
1512  B2INFO("lambda: ");
1513  gsl_vector_fprintf(stdout, &lambdanew.vector, "%f");
1514  }
1515 
1516  // ATA = 1*A^T*A + 0*ATA
1517  gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1, &A.matrix, &A.matrix, 0, &ATA.matrix);
1518 
1519  // put grad(f) into vecw
1520  int isfail = assembleChi2Der(vecw);
1521  if (isfail) return isfail;
1522 
1523 
1524  // ATgradf = -1*A^T*gradf + 0*ATgradf
1525  gsl_blas_dgemv(CblasTrans, -1, &A.matrix, &gradf.vector, 0, &ATgradf.vector);
1526 
1527  if (debug > 17) {
1528  B2INFO("A: ");
1529  gsl_matrix_fprintf(stdout, &A.matrix, "%f");
1530  B2INFO("ATA: ");
1531  gsl_matrix_fprintf(stdout, &ATA.matrix, "%f");
1532  B2INFO("gradf: ");
1533  gsl_vector_fprintf(stdout, &gradf.vector, "%f");
1534  B2INFO("ATgradf: ");
1535  gsl_vector_fprintf(stdout, &ATgradf.vector, "%f");
1536  }
1537 
1538  // solve ATA * lambdanew = ATgradf using the Cholsky factorization method
1539  gsl_error_handler_t* old_handler = gsl_set_error_handler_off();
1540  int cholesky_result = gsl_linalg_cholesky_decomp(&ATA.matrix);
1541  gsl_set_error_handler(old_handler);
1542  if (cholesky_result) {
1543  B2INFO("NewFitterGSL::determineLambdas: resorting to SVD");
1544  // ATA is not positive definite, i.e. A does not have full column rank
1545  // => use the SVD of A to solve A lambdanew = gradf
1546 
1547  // Acopy = A
1548  gsl_matrix_memcpy(&Acopy.matrix, &A.matrix);
1549 
1550  // SVD decomposition of Acopy
1551  gsl_linalg_SV_decomp_jacobi(&Acopy.matrix, &V.matrix, &s.vector);
1552  // set small values to zero
1553  double mins = eps * std::fabs(gsl_vector_get(&s.vector, 0));
1554  for (int i = 0; i < ncon; ++i) {
1555  if (std::fabs(gsl_vector_get(&s.vector, i)) <= mins)
1556  gsl_vector_set(&s.vector, i, 0);
1557  }
1558  gsl_linalg_SV_solve(&Acopy.matrix, &V.matrix, &s.vector, &gradf.vector, &lambdanew.vector);
1559  } else {
1560  gsl_linalg_cholesky_solve(&ATA.matrix, &ATgradf.vector, &lambdanew.vector);
1561  }
1562  if (debug > 15) {
1563  B2INFO("lambdanew: ");
1564  gsl_vector_fprintf(stdout, &lambdanew.vector, "%f");
1565  }
1566  return 0;
1567  }
1568 
1569  void NewFitterGSL::MoorePenroseInverse(gsl_matrix* Ainv, gsl_matrix* A,
1570  gsl_matrix* Wm, gsl_vector* w,
1571  double eps
1572  )
1573  {
1574  assert(Ainv);
1575  assert(A);
1576  assert(Ainv->size1 == A->size2 && Ainv->size2 == A->size1);
1577  assert(A->size1 >= A->size2);
1578  assert(Wm);
1579  assert(Wm->size1 >= A->size1 && Wm->size2 >= A->size2);
1580  assert(w);
1581  assert(w->size >= A->size2);
1582 
1583  int n = A->size1;
1584  int m = A->size2;
1585 
1586  // Original A -> A diag(w) Wm^T
1587  gsl_linalg_SV_decomp_jacobi(A, Wm, w);
1588 
1589  double mins = eps * std::fabs(gsl_vector_get(w, 0));
1590 
1591  // Compute pseudo-inverse of diag(w)
1592  for (int i = 0; i < m; ++i) {
1593  double singval = gsl_vector_get(w, i);
1594  if (std::fabs(singval) > mins)
1595  gsl_vector_set(w, i, 1 / singval);
1596  else
1597  gsl_vector_set(w, i, 0);
1598  }
1599 
1600  // Compute Ainv = Wm diag(w) A^T
1601 
1602  // first: Ainv = Wm* diag(w)
1603  for (int j = 0; j < n; ++j) {
1604  double wval = gsl_vector_get(w, j);
1605  for (int i = 0; i < m; ++i)
1606  gsl_matrix_set(Wm, i, j, wval * gsl_matrix_get(Wm, i, j));
1607  }
1608  // Ainv = 1*Wm*A^T + 0*Ainv
1609  gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1, Wm, A, 0, Ainv);
1610 
1611  }
1612 
1613  double NewFitterGSL::calcpTLp(const gsl_vector* vecdx, const gsl_matrix* MatM,
1614  gsl_vector* vecw)
1615  {
1616  assert(vecdx);
1617  assert(vecdx->size == idim);
1618  assert(MatM);
1619  assert(MatM->size1 == idim && MatM->size2 == idim);
1620  assert(vecw);
1621  assert(vecw->size == idim);
1622 
1623  gsl_vector_const_view p(gsl_vector_const_subvector(vecdx, 0, npar));
1624  gsl_vector_view Lp(gsl_vector_subvector(vecw, 0, npar));
1625  gsl_matrix_const_view L(gsl_matrix_const_submatrix(MatM, 0, 0, npar, npar));
1626  gsl_blas_dsymv(CblasUpper, 1, &L.matrix, &p.vector, 0, &Lp.vector);
1627  double result;
1628  gsl_blas_ddot(&p.vector, &Lp.vector, &result);
1629 
1630  return result;
1631  }
1632 
1633  void NewFitterGSL::calc2ndOrderCorr(gsl_vector* vecdxhat,
1634  const gsl_vector* vecxnew,
1635  const gsl_matrix* MatM,
1636  gsl_matrix* MatW,
1637  gsl_vector* vecw,
1638  double eps)
1639  {
1640  assert(vecdxhat);
1641  assert(vecdxhat->size == idim);
1642  assert(vecxnew);
1643  assert(vecxnew->size == idim);
1644  assert(MatM);
1645  assert(MatM->size1 == idim && MatM->size2 == idim);
1646  assert(MatW);
1647  assert(MatW->size1 == idim && MatW->size2 == idim);
1648  assert(vecw);
1649  assert(vecw->size == idim);
1650  assert(idim == static_cast<unsigned int>(npar + ncon));
1651 
1652 
1653  // Calculate 2nd order correction
1654  // see Nocedal&Wright (15.36)
1655  gsl_matrix_const_view AT(gsl_matrix_const_submatrix(MatM, 0, npar, npar, ncon));
1656  gsl_matrix_view AAT(gsl_matrix_submatrix(MatW, npar, npar, ncon, ncon));
1657  gsl_matrix_view ATcopy(gsl_matrix_submatrix(MatW, 0, npar, npar, ncon));
1658  gsl_matrix_view& V = AAT;
1659 
1660  gsl_vector_set_zero(vecdxhat);
1661  addConstraints(vecdxhat);
1662  gsl_vector_view c(gsl_vector_subvector(vecdxhat, npar, ncon));
1663  gsl_vector_view phat = (gsl_vector_subvector(vecdxhat, 0, npar));
1664 
1665  gsl_vector_set_zero(vecw);
1666  gsl_vector_view AATinvc(gsl_vector_subvector(vecw, npar, ncon));
1667  gsl_vector_view& s = AATinvc;
1668 
1669 
1670  // AAT = 1*A*A^T + 0*AAT
1671  gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1, &AT.matrix, &AT.matrix, 0, &AAT.matrix);
1672 
1673  // solve AAT * AATinvc = c using the Cholsky factorization method
1674  gsl_error_handler_t* old_handler = gsl_set_error_handler_off();
1675  int cholesky_result = gsl_linalg_cholesky_decomp(&AAT.matrix);
1676  gsl_set_error_handler(old_handler);
1677  if (cholesky_result) {
1678  B2INFO("NewFitterGSL::calc2ndOrderCorr: resorting to SVD");
1679  // AAT is not positive definite, i.e. A does not have full column rank
1680  // => use the SVD of AT to solve A lambdanew = gradf
1681 
1682  // ATcopy = AT
1683  gsl_matrix_memcpy(&ATcopy.matrix, &AT.matrix);
1684 
1685  // SVD decomposition of Acopy
1686  gsl_linalg_SV_decomp_jacobi(&ATcopy.matrix, &V.matrix, &s.vector);
1687  // set small values to zero
1688  double mins = eps * std::fabs(gsl_vector_get(&s.vector, 0));
1689  for (int i = 0; i < ncon; ++i) {
1690  if (std::fabs(gsl_vector_get(&s.vector, i)) <= mins)
1691  gsl_vector_set(&s.vector, i, 0);
1692  }
1693  gsl_linalg_SV_solve(&ATcopy.matrix, &V.matrix, &s.vector, &c.vector, &AATinvc.vector);
1694  } else {
1695  gsl_linalg_cholesky_solve(&AAT.matrix, &c.vector, &AATinvc.vector);
1696  }
1697 
1698  // phat = -1*A^T*AATinvc+ 0*phat
1699  gsl_blas_dgemv(CblasNoTrans, -1, &AT.matrix, &AATinvc.vector, 0, &phat.vector);
1700  gsl_vector_set_zero(&c.vector);
1701 
1702  }
1703 
1704  int NewFitterGSL::solveSystem(gsl_vector* vecdxscal,
1705  double& detW,
1706  const gsl_vector* vecyscal,
1707  const gsl_matrix* MatMscal,
1708  gsl_matrix* MatW,
1709  gsl_matrix* MatW2,
1710  gsl_vector* vecw,
1711  double epsLU,
1712  double epsSV)
1713  {
1714  assert(vecdxscal);
1715  assert(vecdxscal->size == idim);
1716  assert(vecyscal);
1717  assert(vecyscal->size == idim);
1718  assert(MatMscal);
1719  assert(MatMscal->size1 == idim && MatMscal->size2 == idim);
1720  assert(MatW);
1721  assert(MatW->size1 == idim && MatW->size2 == idim);
1722  assert(MatW2);
1723  assert(MatW2->size1 == idim && MatW2->size2 == idim);
1724  assert(vecw);
1725  assert(vecw->size == idim);
1726 
1727  int result = 0;
1728 
1729  int iLU = solveSystemLU(vecdxscal, detW, vecyscal, MatMscal, MatW, vecw, epsLU);
1730  if (iLU == 0) return result;
1731 
1732  result = 1;
1733  int iSVD = solveSystemSVD(vecdxscal, vecyscal, MatMscal, MatW, MatW2, vecw, epsSV);
1734  if (iSVD == 0) return result;
1735 
1736  return -1;
1737  }
1738 
1739  int NewFitterGSL::solveSystemLU(gsl_vector* vecdxscal,
1740  double& detW,
1741  const gsl_vector* vecyscal,
1742  const gsl_matrix* MatMscal,
1743  gsl_matrix* MatW,
1744  gsl_vector* vecw,
1745  double eps)
1746  {
1747  assert(vecdxscal);
1748  assert(vecdxscal->size == idim);
1749  assert(vecyscal);
1750  assert(vecyscal->size == idim);
1751  assert(MatMscal);
1752  assert(MatMscal->size1 == idim && MatMscal->size2 == idim);
1753  assert(MatW);
1754  assert(MatW->size1 == idim && MatW->size2 == idim);
1755  assert(vecw);
1756  assert(vecw->size == idim);
1757 
1758  gsl_matrix_memcpy(MatW, MatMscal);
1759 
1760  int ifail = 0;
1761  detW = 0;
1762 
1763  int signum;
1764  int result = gsl_linalg_LU_decomp(MatW, permW, &signum);
1765  B2DEBUG(14, "NewFitterGSL::solveSystem: gsl_linalg_LU_decomp result=" << result);
1766  if (result != 0) return 1;
1767 
1768  detW = gsl_linalg_LU_det(MatW, signum);
1769  B2DEBUG(14, "NewFitterGSL::solveSystem: determinant of W=" << detW);
1770  if (std::fabs(detW) < eps) return 2;
1771  if (!std::isfinite(detW)) {
1772  B2DEBUG(10, "NewFitterGSL::solveSystem: infinite determinant of W=" << detW);
1773  return 3;
1774  }
1775  if (debug > 15) {
1776  B2INFO("NewFitterGSL::solveSystem: after LU decomposition: \n");
1777  debug_print(MatW, "W");
1778  }
1779  // Solve W*dxscal = yscal
1780  ifail = gsl_linalg_LU_solve(MatW, permW, vecyscal, vecdxscal);
1781  B2DEBUG(14, "NewFitterGSL::solveSystem: gsl_linalg_LU_solve result=" << ifail);
1782 
1783  if (ifail != 0) {
1784  B2ERROR("NewFitterGSL::solveSystem: ifail from gsl_linalg_LU_solve=" << ifail);
1785  return 3;
1786  }
1787  return 0;
1788  }
1789 
1790 
1791 
1792 
1793  int NewFitterGSL::solveSystemSVD(gsl_vector* vecdxscal,
1794 // double& detW,
1795  const gsl_vector* vecyscal,
1796  const gsl_matrix* MatMscal,
1797  gsl_matrix* MatW,
1798  gsl_matrix* MatW2,
1799  gsl_vector* vecw,
1800  double eps)
1801  {
1802  assert(vecdxscal);
1803  assert(vecdxscal->size == idim);
1804  assert(vecyscal);
1805  assert(vecyscal->size == idim);
1806  assert(MatMscal);
1807  assert(MatMscal->size1 == idim && MatMscal->size2 == idim);
1808  assert(MatW);
1809  assert(MatW->size1 == idim && MatW->size2 == idim);
1810  assert(MatW2);
1811  assert(MatW2->size1 == idim && MatW2->size2 == idim);
1812  assert(vecw);
1813  assert(vecw->size == idim);
1814 
1815  B2DEBUG(10, "solveSystemSVD called");
1816 
1817  gsl_matrix_memcpy(MatW, MatMscal);
1818 
1819  // SVD decomposition of MatW
1820  gsl_linalg_SV_decomp_jacobi(MatW, MatW2, vecw);
1821  // set small values to zero
1822  double mins = eps * std::fabs(gsl_vector_get(vecw, 0));
1823  B2DEBUG(15, "SV 0 = " << gsl_vector_get(vecw, 0));
1824  for (unsigned int i = 0; i < idim; ++i) {
1825  if (std::fabs(gsl_vector_get(vecw, i)) <= mins) {
1826  B2DEBUG(15, "Setting SV" << i << " = " << gsl_vector_get(vecw, i) << " to zero!");
1827  gsl_vector_set(vecw, i, 0);
1828  }
1829  }
1830  gsl_linalg_SV_solve(MatW, MatW2, vecw, vecyscal, vecdxscal);
1831  return 0;
1832  }
1833 
1834  gsl_matrix_view NewFitterGSL::calcZ(int& rankA, gsl_matrix* MatW1, gsl_matrix* MatW2,
1835  gsl_vector* vecw1, gsl_vector* vecw2,
1836  gsl_permutation* permw, double eps)
1837  {
1838  assert(MatW1);
1839  assert(MatW1->size1 == idim && MatW1->size2 == idim);
1840  assert(MatW2);
1841  assert(MatW2->size1 == idim && MatW2->size2 == idim);
1842  assert(vecw1);
1843  assert(vecw1->size == idim);
1844  assert(vecw2);
1845  assert(vecw2->size == idim);
1846  assert(permw);
1847  assert(permw->size == idim);
1848 
1849  // fill A and AT
1850  assembleConstDer(MatW2);
1851 
1852  gsl_matrix_const_view AT(gsl_matrix_const_submatrix(MatW2, 0, 0, npar, npar));
1853  //gsl_matrix_view QR (gsl_matrix_submatrix (MatW2, 0, 0, npar, npar));
1854  gsl_matrix_view Q(gsl_matrix_submatrix(MatW1, 0, 0, npar, npar));
1855  gsl_matrix_view R(gsl_matrix_submatrix(MatW1, npar, 0, ncon, npar));
1856 
1857  int signum = 0;
1858  //gsl_linalg_QRPT_decomp (&QR.matrix, vecw1, permw, &signum, vecw2);
1859  //gsl_linalg_QR_unpack (&QR.matrix, vecw1, &Q.matrix, &R.matrix);
1860  gsl_linalg_QRPT_decomp2(&AT.matrix, &Q.matrix, &R.matrix, vecw1, permw, &signum, vecw2);
1861 
1862  rankA = 0;
1863  for (int i = 0; i < ncon; ++i) {
1864  if (fabs(gsl_matrix_get(&R.matrix, i, i)) > eps) rankA++;
1865  }
1866  auto result = gsl_matrix_view(gsl_matrix_submatrix(MatW1, 0, rankA, ncon - rankA, npar));
1867  return result;
1868  }
1869 
1870  gsl_matrix_view NewFitterGSL::calcReducedHessian(int& rankH, gsl_matrix* MatW1,
1871  const gsl_vector* vecx, gsl_matrix* MatW2,
1872  gsl_matrix* MatW3,
1873  gsl_vector* vecw1, gsl_vector* vecw2,
1874  gsl_permutation* permw, double eps)
1875  {
1876  assert(MatW1);
1877  assert(MatW1->size1 == idim && MatW1->size2 == idim);
1878  assert(vecx);
1879  assert(vecx->size == idim);
1880  assert(MatW2);
1881  assert(MatW2->size1 == idim && MatW2->size2 == idim);
1882  assert(MatW3);
1883  assert(MatW2->size1 == idim && MatW2->size2 == idim);
1884  assert(vecw1);
1885  assert(vecw1->size == idim);
1886  assert(vecw2);
1887  assert(vecw2->size == idim);
1888  assert(permw);
1889  assert(permw->size == idim);
1890 
1891  int rankA;
1892  // Z is a matrix view of MatW2!
1893  gsl_matrix_view Z = calcZ(rankA, MatW2, MatW1, vecw1, vecw2, permw, eps);
1894 
1895  // fill Lagrangian
1896  assembleG(MatW1, vecx);
1897 
1898  rankH = npar - rankA;
1899 
1900  gsl_matrix_view G(gsl_matrix_submatrix(MatW1, 0, 0, npar, npar));
1901  gsl_matrix_view GZ(gsl_matrix_submatrix(MatW3, 0, 0, npar, rankH));
1902  gsl_matrix_view ZGZ(gsl_matrix_submatrix(MatW1, 0, 0, rankH, rankH));
1903 
1904  // Calculate Z^T G Z
1905 
1906  // GZ = 1*G*Z + 0*GZ
1907  gsl_blas_dsymm(CblasLeft, CblasUpper, 1, &G.matrix, &Z.matrix, 0, &GZ.matrix);
1908  // ZGZ = 1*Z^T*GZ + 0*ZGZ
1909  gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1, &Z.matrix, &GZ.matrix, 0, &ZGZ.matrix);
1910 
1911  return ZGZ;
1912  }
1913 
1914  gsl_vector_view NewFitterGSL::calcReducedHessianEigenvalues(int& rankH, gsl_matrix* MatW1,
1915  const gsl_vector* vecx, gsl_matrix* MatW2,
1916  gsl_matrix* MatW3,
1917  gsl_vector* vecw1, gsl_vector* vecw2,
1918  gsl_permutation* permw,
1919  gsl_eigen_symm_workspace* eigensws,
1920  double eps)
1921  {
1922  assert(MatW1);
1923  assert(MatW1->size1 == idim && MatW1->size2 == idim);
1924  assert(vecx);
1925  assert(vecx->size == idim);
1926  assert(MatW2);
1927  assert(MatW2->size1 == idim && MatW2->size2 == idim);
1928  assert(MatW3);
1929  assert(MatW2->size1 == idim && MatW2->size2 == idim);
1930  assert(vecw1);
1931  assert(vecw1->size == idim);
1932  assert(vecw2);
1933  assert(vecw2->size == idim);
1934  assert(permw);
1935  assert(permw->size == idim);
1936  assert(eigensws);
1937 
1938  gsl_matrix_view Hred = calcReducedHessian(rankH, MatW1, vecx, MatW2, MatW3, vecw1, vecw2, permw, eps);
1939 
1940  gsl_matrix_view Hredcopy(gsl_matrix_submatrix(MatW3, 0, 0, rankH, rankH));
1941  // copy Hred -> Hredcopy
1942  gsl_matrix_memcpy(&Hredcopy.matrix, &Hred.matrix);
1943 
1944  gsl_vector_view eval(gsl_vector_subvector(vecw1, 0, rankH));
1945  gsl_eigen_symm(&Hredcopy.matrix, &eval.vector, eigensws);
1946 
1947  return eval;
1948  }
1949 
1950  }// end OrcaKinFit namespace
1952 } // end Belle2 namespace
1953 
E
R E
internal precision of FFTW codelets
Definition: DiscreteCosineTransform_31points.cc:36
R
double R
typedef autogenerated by FFTW
Definition: DiscreteCosineTransform_31points.cc:35
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:1161
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:1569
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:1739
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:1870
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:1482
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:1793
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:1704
Belle2::OrcaKinFit::NewFitterGSL::meritFunction
double meritFunction(double mu, const gsl_vector *vecx, const gsl_vector *vece)
Definition: NewFitterGSL.cc:1249
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:1914
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:1834
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:1633
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:1284
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:1362
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:1613
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