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
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
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
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
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
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
virtual bool updateParams(double p[], int idim)
Read values from global vector, readjust vector; return: significant change.
virtual int getNPar() const =0
Get total number of parameters of this FitObject.
virtual void addToGlobalChi2DerMatrix(double *M, int idim) const
Add 2nd derivatives of chi squared to global derivative matrix.
virtual double getError(int ilocal) const
Get error of parameter ilocal.
virtual int getGlobalParNum(int ilocal) const
Get global parameter number of parameter ilocal.
virtual const char * getName() const
Get object's name.
virtual bool isParamFixed(int ilocal) const
Returns whether parameter is fixed.
virtual double getParam(int ilocal) const
Get current value of parameter ilocal.
virtual int addToGlobalChi2DerVector(double *y, int idim) const
Add derivatives of chi squared to global derivative vector.
virtual void addToGlobCov(double *glcov, int idim) const
Add covariance matrix elements to global covariance matrix of size idim x idim.
double * cov
global covariance matrix of last fit problem
Definition: BaseFitter.h:104
bool covValid
Flag whether global covariance is valid.
Definition: BaseFitter.h:105
int covDim
dimension of global covariance matrix
Definition: BaseFitter.h:103
Abstract base class for constraints of kinematic fits.
virtual void step(BaseFitter &fitter)
Called at the end of each step.
Definition: BaseTracer.cc:35
virtual void initialize(BaseFitter &fitter)
Called at the start of a new fit (during initialization)
Definition: BaseTracer.cc:30
virtual void finish(BaseFitter &fitter)
Called at the end of a fit.
Definition: BaseTracer.cc:45
virtual void substep(BaseFitter &fitter, int flag)
Called at intermediate points during a step.
Definition: BaseTracer.cc:40
int ncon
total number of hard constraints
Definition: NewFitterGSL.h:255
int npar
total number of parameters
Definition: NewFitterGSL.h:254
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)
virtual double calcChi2()
Calculate the chi2.
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.
virtual int getNsoft() const
Get the number of soft constraints of the last fit.
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
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.
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.
virtual double fit() override
The fit method, returns the fit probability.
virtual bool initialize() override
Initialize the fitter.
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
virtual int getNcon() const
Get the number of hard constraints of the last fit.
virtual ~NewFitterGSL()
Virtual destructor.
Definition: NewFitterGSL.cc:78
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)
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)
double fitprob
fit probability
Definition: NewFitterGSL.h:261
int nsoft
total number of soft constraints
Definition: NewFitterGSL.h:256
virtual int getError() const override
Get the error code of the last fit: 0=OK, 1=failed.
int nunm
total number of unmeasured parameters
Definition: NewFitterGSL.h:257
int nit
Number of iterations.
Definition: NewFitterGSL.h:259
virtual int getNunm() const
Get the number of unmeasured parameters of the last fit.
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
double meritFunction(double mu, const gsl_vector *vecx, const gsl_vector *vece)
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)
virtual int getIterations() const override
Get the number of iterations of the last fit.
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...
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.
double meritFunctionDeriv(double mu, const gsl_vector *vecx, const gsl_vector *vece, const gsl_vector *vecdx, gsl_vector *vecw)
virtual double getProbability() const override
Get the fit probability of the last fit.
virtual void setDebug(int debuglevel)
Set the Debug Level.
virtual int getDoF() const override
Get the number of degrees of freedom of the last fit.
virtual double getChi2() const override
Get the chi**2 of the last fit.
virtual int getNpar() const
Get the number of all parameters of the last fit.
double calcpTLp(const gsl_vector *vecdx, const gsl_matrix *MatM, gsl_vector *vecw)
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)
Abstract base class for different kinds of events.
Definition: ClusterUtils.h:24