Belle II Software light-2406-ragdoll
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
40using std::abs;
41
42namespace 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
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
Abstract base class for different kinds of events.
Definition: ClusterUtils.h:24