Belle II Software light-2406-ragdoll
NewtonFitterGSL.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/NewtonFitterGSL.h"
20
21#include<iostream>
22#include<cmath>
23#include<cassert>
24
25#include "analysis/OrcaKinFit/BaseFitObject.h"
26#include "analysis/OrcaKinFit/BaseHardConstraint.h"
27#include "analysis/OrcaKinFit/BaseSoftConstraint.h"
28#include "analysis/OrcaKinFit/BaseTracer.h"
29#include <framework/logging/Logger.h>
30
31#include <gsl/gsl_block.h>
32#include <gsl/gsl_vector.h>
33#include <gsl/gsl_matrix.h>
34#include <gsl/gsl_permutation.h>
35#include <gsl/gsl_linalg.h>
36#include <gsl/gsl_blas.h>
37#include <gsl/gsl_cdf.h>
38
39using std::abs;
40
41static int nitdebug = 100;
42static int nitcalc = 0;
43static int nitsvd = 0;
44
45namespace Belle2 {
50 namespace OrcaKinFit {
51
52// constructor
54 : npar(0), ncon(0), nsoft(0), nunm(0), ierr(0), nit(0), fitprob(0), chi2(0),
55 idim(0),
56 x(nullptr), xold(nullptr), xbest(nullptr), dx(nullptr), dxscal(nullptr), grad(nullptr), y(nullptr), yscal(nullptr),
57 perr(nullptr), v1(nullptr), v2(nullptr), Meval(nullptr),
58 M(nullptr), Mscal(nullptr), M1(nullptr), M2(nullptr), M3(nullptr), M4(nullptr), M5(nullptr), Mevec(nullptr),
59 CC(nullptr), CC1(nullptr), CCinv(nullptr), permM(nullptr), ws(nullptr),
60 wsdim(0), chi2best(0),
61 chi2new(0),
62 chi2old(0),
63 fvalbest(0), scale(0), scalebest(0), stepsize(0), stepbest(0),
64 scalevals{}, fvals{},
65 imerit(1),
66 debug(0)
67 {}
68
69// destructor
71 {
72
73 if (x) gsl_vector_free(x);
74 if (xold) gsl_vector_free(xold);
75 if (xbest) gsl_vector_free(xbest);
76 if (dx) gsl_vector_free(dx);
77 if (dxscal) gsl_vector_free(dxscal);
78 if (grad) gsl_vector_free(grad);
79 if (y) gsl_vector_free(y);
80 if (yscal) gsl_vector_free(yscal);
81 if (perr) gsl_vector_free(perr);
82 if (v1) gsl_vector_free(v1);
83 if (v2) gsl_vector_free(v2);
84 if (Meval) gsl_vector_free(Meval);
85 if (M) gsl_matrix_free(M);
86 if (Mscal) gsl_matrix_free(Mscal);
87 if (M1) gsl_matrix_free(M1);
88 if (M2) gsl_matrix_free(M2);
89 if (M3) gsl_matrix_free(M3);
90 if (M4) gsl_matrix_free(M4);
91 if (M5) gsl_matrix_free(M5);
92 if (Mevec) gsl_matrix_free(Mevec);
93 if (CC) gsl_matrix_free(CC);
94 if (CC1) gsl_matrix_free(CC1);
95 if (CCinv) gsl_matrix_free(CCinv);
96 if (permM) gsl_permutation_free(permM);
97 if (ws) gsl_eigen_symmv_free(ws);
98
99 x = nullptr;
100 xold = nullptr;
101 xbest = nullptr;
102 dx = nullptr;
103 dxscal = nullptr;
104 grad = nullptr;
105 y = nullptr;
106 yscal = nullptr;
107 perr = nullptr;
108 v1 = nullptr;
109 v2 = nullptr;
110 Meval = nullptr;
111 M = nullptr;
112 Mscal = nullptr;
113 M1 = nullptr;
114 M2 = nullptr;
115 M3 = nullptr;
116 M4 = nullptr;
117 M5 = nullptr;
118 Mevec = nullptr;
119 CC = nullptr;
120 CC1 = nullptr;
121 CCinv = nullptr;
122 permM = nullptr;
123 ws = nullptr; wsdim = 0;
124 }
125
126
127
129 {
130
131 // order parameters etc
132 initialize();
133
134 // initialize eta, etasv, y
135 assert(x && (unsigned int)x->size == idim);
136 assert(dx && (unsigned int)dx->size == idim);
137 assert(y && (unsigned int)y->size == idim);
138 assert(perr && (unsigned int)perr->size == idim);
139 assert(v1 && (unsigned int)v1->size == idim);
140 assert(v2 && (unsigned int)v2->size == idim);
141 assert(Meval && (unsigned int)Meval->size == idim);
142 assert(M && (unsigned int)M->size1 == idim);
143 assert(M1 && (unsigned int)M1->size1 == idim);
144 assert(Mevec && (unsigned int)Mevec->size1 == idim);
145 assert(permM && (unsigned int)permM->size == idim);
146
147 gsl_vector_set_zero(x);
148 gsl_vector_set_zero(y);
149 gsl_vector_set_all(perr, 1);
150
151 // Store old x values in xold
152 fillxold();
153 // make sure parameters are consistent
154 updateParams(xold);
155 fillxold();
156 // Get starting values into x
157 gsl_vector_memcpy(x, xold);
158
159
160 // LET THE GAMES BEGIN
161
162#ifndef FIT_TRACEOFF
163 if (tracer) tracer->initialize(*this);
164#endif
165
166 bool converged = false;
167 ierr = 0;
168
169 chi2new = calcChi2();
170 nit = 0;
171 if (debug > 1) {
172 B2INFO("Fit objects:\n");
173 for (auto fo : fitobjects) {
174 assert(fo);
175 B2INFO(fo->getName() << ": " << *fo << ", chi2=" << fo->getChi2());
176 for (int ilocal = 0; ilocal < fo->getNPar(); ++ilocal) {
177 if (!fo->isParamFixed(ilocal)) {
178 int iglobal = fo->getGlobalParNum(ilocal);
179 B2INFO(" par " << fo->getParamName(ilocal) << ": local: " << ilocal << ": global: " << iglobal
180 << " value=" << fo->getParam(ilocal) << " +- " << fo->getError(ilocal));
181 if (fo->isParamMeasured(ilocal))
182 B2INFO(" measured: " << fo->getMParam(ilocal));
183 } else {
184 B2INFO(" par " << fo->getParamName(ilocal) << ": local: " << ilocal << " -- fixed -- "
185 << " value=" << fo->getParam(ilocal) << " +- " << fo->getError(ilocal));
186
187 }
188 }
189 }
190 B2INFO("constraints:\n");
191 for (auto i = constraints.begin(); i != constraints.end(); ++i) {
192 BaseHardConstraint* c = *i;
193 assert(c);
194 B2INFO(i - constraints.begin() << " " << c->getName() << ": " << c->getValue() << "+-" << c->getError());
195 int kglobal = c->getGlobalNum();
196 B2INFO(" global number: " << kglobal);
197 }
198 B2INFO("soft constraints:\n");
199 for (auto i = softconstraints.begin(); i != softconstraints.end(); ++i) {
200 BaseSoftConstraint* c = *i;
201 assert(c);
202 B2INFO(i - softconstraints.begin() << " " << c->getName() << ": " << c->getValue() << "+-" << c->getError()
203 << ", chi2: " << c->getChi2());
204 }
205 }
206
207 do {
208
209 chi2old = chi2new;
210
211 if (nit == 0 || nit < nitdebug) {
212 B2DEBUG(11, "===================\nStarting iteration " << nit);
213 std::string printout = "\n Fit objects:\n";
214 for (auto fo : fitobjects) {
215 assert(fo);
216 printout += fo->getName();
217 printout += ", chi2=" ;
218 printout += fo->getChi2();
219 printout += "\n";
220 }
221 printout += "constraints:\n";
222 for (auto c : constraints) {
223 assert(c);
224 printout += c->getName() ;
225 printout += ": ";
226 printout += c->getValue();
227 printout += "+-" ;
228 printout += c->getError() ;
229 printout += "\n";
230 }
231 printout += "soft constraints:\n";
232 for (auto c : softconstraints) {
233 assert(c);
234 printout += c->getName() ;
235 printout += ": ";
236 printout += c->getValue() ;
237 printout += "+-" ;
238 printout += c->getError();
239 printout += ", chi2: ";
240 printout += c->getChi2() ;
241 printout += "\n";
242 }
243 B2DEBUG(12, printout);
244 }
245
246 // Store old x values in xold
247 fillxold();
248 // Fill errors into perr
249 fillperr();
250
251 // Compose M:
252 calcM();
253
254 // Now, calculate the result vector y with the values of the derivatives
255 // d chi^2/d x
256 calcy();
257
258 if (nit == 0 || nit < nitdebug) {
259 B2DEBUG(13, "After setting up equations: \n");
260 debug_print(M, "M");
261 debug_print(Mscal, "Mscal");
262 debug_print(y, "y");
263 debug_print(yscal, "yscal");
264 debug_print(perr, "perr");
265 debug_print(x, "x");
266 debug_print(xold, "xold");
267 }
268
269 scalevals[0] = 0;
270 fvals[0] = 0.5 * pow(gsl_blas_dnrm2(yscal), 2);
271 fvalbest = fvals[0];
272 stepsize = 0;
273 scalebest = 0;
274 stepbest = 0;
275
276 int ifail = calcDx();
277 if (ifail != 0) {
278 B2INFO("NewtonFitterGSL::fit: calcDx: ifail=" << ifail);
279 ierr = 2;
280 break;
281 }
282 // Update values in Fitobjects
283 updateParams(xbest);
284
285 if (debug > 1) {
286 debug_print(xbest, "new parameters");
287 }
288
289 calcy();
290 B2DEBUG(13, "New fval: " << 0.5 * pow(gsl_blas_dnrm2(yscal), 2));
291 chi2new = calcChi2();
292 B2DEBUG(13, "chi2: " << chi2old << " -> " << chi2new);
293
294 if (nit == 0 || nit < nitdebug) {
295 B2DEBUG(13, "After solving equations: \n");
296 debug_print(xbest, "xbest");
297 }
298
299
300// *-- Convergence criteria
301
302 if (nit < nitdebug) {
303 B2DEBUG(11, "old chi2: " << chi2old << ", new chi2: " << chi2new << ", diff=" << chi2old - chi2new);
304 }
305 ++nit;
306 if (nit > 200) ierr = 1;
307
308 converged = (abs(chi2new - chi2old) < 0.001 && fvalbest < 1E-3 &&
309 (fvalbest < 1E-6 || abs(fvals[0] - fvalbest) < 0.2 * fvalbest));
310
311// if (abs (chi2new - chi2old) >= 0.001)
312// B2INFO( "abs (chi2new - chi2old)=" << abs (chi2new - chi2old) << " -> try again\n");
313// if (fvalbest >= 1E-3)
314// B2INFO("fvalbest=" << fvalbest << " -> try again\n");
315// if (fvalbest >= 1E-6 && abs(fvals[0]-fvalbest) >= 0.2*fvalbest )
316// B2INFO("fvalbest=" << fvalbest
317// << ", abs(fvals[0]-fvalbest)=" << abs(fvals[0]-fvalbest)<< " -> try again\n");
318// if (stepbest >= 1E-3)
319// B2INFO("stepbest=" << stepbest << " -> try again\n");
320// B2INFO( "converged=" << converged);
321 if (converged) {
322 B2DEBUG(10, "abs (chi2new - chi2old)=" << abs(chi2new - chi2old) << "\n"
323 << "fvalbest=" << fvalbest << "\n"
324 << "abs(fvals[0]-fvalbest)=" << abs(fvals[0] - fvalbest) << "\n");
325 }
326
327#ifndef FIT_TRACEOFF
328 if (tracer) tracer->step(*this);
329#endif
330
331 } while (!(converged || ierr));
332
333// *-- End of iterations - calculate errors.
334
335// ERROR CALCULATION
336
337 if (!ierr) {
338
339 calcCovMatrix();
340
341 // update errors in fitobjects
342 for (auto& fitobject : fitobjects) {
343 for (int ilocal = 0; ilocal < fitobject->getNPar(); ++ilocal) {
344 int iglobal = fitobject->getGlobalParNum(ilocal);
345 for (int jlocal = ilocal; jlocal < fitobject->getNPar(); ++jlocal) {
346 int jglobal = fitobject->getGlobalParNum(jlocal);
347 if (iglobal >= 0 && jglobal >= 0)
348 fitobject->setCov(ilocal, jlocal, gsl_matrix_get(CCinv, iglobal, jglobal));
349 }//CCinv
350 }
351 }
352 }
353
354
355 std::string printout = "\n ========= END =========\n";
356 printout += "Fit objects:\n";
357 for (auto fo : fitobjects) {
358 assert(fo);
359 printout += fo->getName() ;
360 printout += ": ";
361 printout += ", chi2=" ;
362 printout += fo->getChi2() ;
363 printout += "\n";
364 }
365 printout += "constraints:\n";
366 for (auto c : constraints) {
367 assert(c);
368 printout += c->getName();
369 printout += ": ";
370 printout += c->getValue();
371 printout += "+-" ;
372 printout += c->getError();
373 printout += "\n";
374 }
375 printout += "=============================================\n";
376 B2DEBUG(11, printout);
377
378// *-- Turn chisq into probability.
379 fitprob = (chi2new >= 0 && ncon + nsoft - nunm > 0) ? gsl_cdf_chisq_Q(chi2new, ncon + nsoft - nunm) : -1;
380
381#ifndef FIT_TRACEOFF
382 if (tracer) tracer->finish(*this);
383#endif
384
385 B2DEBUG(10, "NewtonFitterGSL::fit: converged=" << converged
386 << ", nit=" << nit << ", fitprob=" << fitprob);
387
388 if (ierr > 0) fitprob = -1;
389
390 return fitprob;
391
392 }
393
395 {
396 covValid = false;
397
398 // tell fitobjects the global ordering of their parameters:
399 npar = 0;
400 nunm = 0;
401
402 for (auto& fitobject : fitobjects) {
403 for (int ilocal = 0; ilocal < fitobject->getNPar(); ++ilocal) {
404 if (!fitobject->isParamFixed(ilocal)) {
405 B2DEBUG(13, "NewtonFitterGSL::initialize: parameter " << ilocal
406 << " of fitobject " << fitobject->getName()
407 << " gets global number " << npar);
408 fitobject->setGlobalParNum(ilocal, npar);
409 ++npar;
410 if (!fitobject->isParamMeasured(ilocal)) ++nunm;
411 }
412 }
413 }
414
415 // set number of constraints
416 ncon = constraints.size();
417 // Tell the constraints their numbers
418 for (int icon = 0; icon < ncon; ++icon) {
419 BaseHardConstraint* c = constraints[icon];
420 assert(c);
421 B2DEBUG(13, "NewtonFitterGSL::initialize: constraint " << c->getName()
422 << " gets global number " << npar + icon);
423 c->setGlobalNum(npar + icon);
424 B2DEBUG(14, "Constraint " << icon << " -> global " << c->getGlobalNum());
425 }
426
427 nsoft = softconstraints.size();
428
429 if (nunm > ncon + nsoft) {
430 B2ERROR("NewtonFitterGSL::initialize: nunm=" << nunm << " > ncon+nsoft="
431 << ncon << "+" << nsoft);
432 }
433
434 idim = npar + ncon;
435
436 ini_gsl_vector(x, idim);
437 ini_gsl_vector(xold, idim);
438 ini_gsl_vector(xbest, idim);
439 ini_gsl_vector(dx, idim);
440 ini_gsl_vector(dxscal, idim);
441 ini_gsl_vector(grad, idim);
442 ini_gsl_vector(y, idim);
443 ini_gsl_vector(yscal, idim);
444 ini_gsl_vector(perr, idim);
445 ini_gsl_vector(v1, idim);
446 ini_gsl_vector(v2, idim);
447 ini_gsl_vector(Meval, idim);
448
449 ini_gsl_matrix(M, idim, idim);
450 ini_gsl_matrix(Mscal, idim, idim);
451 ini_gsl_matrix(M1, idim, idim);
452 ini_gsl_matrix(M2, idim, idim);
453 ini_gsl_matrix(M3, idim, idim);
454 ini_gsl_matrix(M4, idim, idim);
455 ini_gsl_matrix(M5, idim, idim);
456 ini_gsl_matrix(Mevec, idim, idim);
457 ini_gsl_matrix(CC, idim, idim);
458 ini_gsl_matrix(CC1, idim, idim);
459 ini_gsl_matrix(CCinv, idim, idim);
460
461 ini_gsl_permutation(permM, idim);
462
463 if (ws && wsdim != idim) {
464 gsl_eigen_symmv_free(ws);
465 ws = nullptr;
466 }
467 if (ws == nullptr) ws = gsl_eigen_symmv_alloc(idim);
468 wsdim = idim;
469
470 return true;
471
472 }
473
475 {
476 chi2 = 0;
477 for (auto fo : fitobjects) {
478 assert(fo);
479 chi2 += fo->getChi2();
480 }
481 for (auto bsc : softconstraints) {
482 assert(bsc);
483 chi2 += bsc->getChi2();
484 }
485 return chi2;
486 }
487
488 void NewtonFitterGSL::printMy(const double Ma[], const double yo[], int idime)
489 {
490 for (int i = 0; i < idime; ++i) {
491 B2INFO(i << " [ " << Ma[idime * i + 0]);
492 for (int j = 1; j < idime; ++j) B2INFO(", " << Ma[idime * i + j]);
493 B2INFO("] [" << yo[i] << "]\n");
494 }
495 }
496
497 int NewtonFitterGSL::getError() const {return ierr;}
499 double NewtonFitterGSL::getChi2() const {return chi2;}
500 int NewtonFitterGSL::getDoF() const {return ncon + nsoft - nunm;}
502
504 {
505 B2DEBUG(11, "entering calcDx");
506 nitcalc++;
507 // from x_(n+1) = x_n - y/y' = x_n - M^(-1)*y we have M*(x_n-x_(n+1)) = y,
508 // which we solve for dx = x_n-x_(n+1) and hence x_(n+1) = x_n-dx
509
510 gsl_matrix_memcpy(M1, Mscal);
511
512
513 int ifail = 0;
514
515 int signum;
516 int result = gsl_linalg_LU_decomp(M1, permM, &signum);
517 B2DEBUG(11, "calcDx: gsl_linalg_LU_decomp result=" << result);
518 // Solve M1*dx = y
519 ifail = gsl_linalg_LU_solve(M1, permM, yscal, dxscal);
520 B2DEBUG(11, "calcDx: gsl_linalg_LU_solve result=" << ifail);
521
522 if (ifail != 0) {
523 B2ERROR("NewtonFitter::calcDx: ifail from gsl_linalg_LU_solve=" << ifail);
524// return calcDxSVD();
525 return -1;
526 }
527 stepsize = std::abs(gsl_vector_get(dxscal, gsl_blas_idamax(dxscal)));
528
529 // dx = dxscal*perr (component wise)
530 gsl_vector_memcpy(dx, dxscal);
531 gsl_vector_mul(dx, perr);
532
533 // optimize scale
534
535 gsl_vector_memcpy(xbest, xold);
536 chi2best = chi2old;
537
538 optimizeScale();
539
540 if (scalebest < 0.01) {
541 B2DEBUG(11, "NewtonFitter::calcDx: reverting to calcDxSVD\n");
542 return calcDxSVD();
543 }
544
545 return 0;
546 }
547
549 {
550
551 nitsvd++;
552 // from x_(n+1) = x_n - y/y' = x_n - M^(-1)*y we have M*(x_n-x_(n+1)) = y,
553 // which we solve for dx = x_n-x_(n+1) and hence x_(n+1) = x_n-dx
554
555 for (unsigned int i = 0; i < idim; ++i) assert(gsl_vector_get(perr, i) > 0);
556
557 // Get eigenvalues and eigenvectors of Mscal
558 int ierrc = 0;
559 gsl_matrix_memcpy(M1, Mscal);
560 B2DEBUG(13, "NewtonFitterGSL::calcDxSVD: Calling gsl_eigen_symmv");
561 ierrc = gsl_eigen_symmv(M1, Meval, Mevec, ws);
562 B2DEBUG(13, "NewtonFitterGSL::calcDxSVD: result of gsl_eigen_symmv: ");
563 if (ierrc != 0) {
564 B2ERROR("NewtonFitter::calcDxSVD: ierr=" << ierrc << "from gsl_eigen_symmv!\n");
565 }
566 // Sort the eigenvalues and eigenvectors in descending order in magnitude
567 ierrc = gsl_eigen_symmv_sort(Meval, Mevec, GSL_EIGEN_SORT_ABS_DESC);
568 if (ierrc != 0) {
569 B2ERROR("NewtonFitter::calcDxSVD: ierr=" << ierrc << "from gsl_eigen_symmv_sort!\n");
570 }
571
572
573 // The eigenvectors are stored in the columns of Mevec;
574 // the eigenvectors are orthonormal, therefore Mevec^T = Mevec^-1
575 // Therefore, Mscal = Mevec * diag(Meval)* Mevec^T, and
576 // Mscal^-1 = (Mevec^T)^-1 * diag(Meval)^-1 * Mevec^-1
577 // = Mevec * diag(Meval)^-1 * Mevec^T
578 // So, the solution of M*dx = y is given by
579 // dx = M^-1 * y = Mevec * diag(Meval)^-1 * Mevec^-1 *y
580 // = Mevec * v2
581 // For the pseudoinverse, the last elements of Meveal^-1 are set
582 // to 0, therefore the last elements of v2 will be 0,
583 // therefore we can restrict the calculation of Mevec * v2
584 // to the first ndim rows.
585 // So, we calculate v2 only once, with only the inverse of zero eigenvalues
586 // set to 0, and then calculate Mevec * v2 for fewer and fewer rows
587
588
589 // Now M = U * s * V^T
590 // We want to solve M*dx = y, hence dx = V * s^-1 * U^T * y
591 // Calculate UTy first; we need only the first ndim entries
592 unsigned int ndim = 0;
593 while (ndim < idim && gsl_vector_get(Meval, ndim) != 0) ++ndim;
594
595 if (ndim < idim) {
596 B2INFO("calcDxSVD: idim = " << idim << " > ndim = " << ndim);
597 }
598
599
600 // Calculate v2 = 1*Mevec^T*y + 0*v2
601 gsl_blas_dgemv(CblasTrans, 1, Mevec, yscal, 0, v2);
602
603 // Divide by nonzero eigenvalues
604 for (unsigned int i = 0; i < idim; ++i) {
605 if (double e = gsl_vector_get(Meval, i)) gsl_vector_set(v2, i, gsl_vector_get(v2, i) / e);
606 else gsl_vector_set(v2, i, 0);
607 }
608
609 stepsize = 0;
610
611 do {
612 gsl_vector_view v2part = gsl_vector_subvector(v2, 0, ndim);
613 gsl_matrix_view Mevecpart = gsl_matrix_submatrix(Mevec, 0, 0, idim, ndim);
614
615 // Calculate dx = 1*Mevecpart^T*v2 + 0*dx
616 gsl_blas_dgemv(CblasNoTrans, 1, &Mevecpart.matrix, &v2part.vector, 0, dxscal);
617 // get maximum element
618// for (unsigned int i = 0; i < idim; ++i) {
619// if(std::abs(gsl_vector_get (dxscal, i))>stepsize)
620// stepsize=std::abs(gsl_vector_get (dxscal, i));
621// }
622 stepsize = std::abs(gsl_vector_get(dxscal, gsl_blas_idamax(dxscal)));
623
624 // dx = dxscal*perr (component wise)
625 gsl_vector_memcpy(dx, dxscal);
626 gsl_vector_mul(dx, perr);
627
628 if (debug > 1) {
629 B2INFO("calcDxSVD: Optimizing scale for ndim=" << ndim);
630 debug_print(dxscal, "dxscal");
631 }
632
633 optimizeScale();
634
635 --ndim;
636
637 if (scalebest < 0.01 || ndim < idim - 1) {
638 B2DEBUG(11, "ndim=" << ndim << ", scalebest=" << scalebest);
639 }
640 } while (ndim > 0 && scalebest < 0.01);
641
642
643 return 0;
644 }
645
646 void NewtonFitterGSL::ini_gsl_permutation(gsl_permutation*& p, unsigned int size)
647 {
648 if (p) {
649 if (p->size != size) {
650 gsl_permutation_free(p);
651 if (size > 0) p = gsl_permutation_alloc(size);
652 else p = nullptr;
653 }
654 } else if (size > 0) p = gsl_permutation_alloc(size);
655 }
656
657 void NewtonFitterGSL::ini_gsl_vector(gsl_vector*& v, unsigned int size)
658 {
659
660 if (v) {
661 if (v->size != size) {
662 gsl_vector_free(v);
663 if (size > 0) v = gsl_vector_alloc(size);
664 else v = nullptr;
665 }
666 } else if (size > 0) v = gsl_vector_alloc(size);
667 }
668
669 void NewtonFitterGSL::ini_gsl_matrix(gsl_matrix*& m, unsigned int size1, unsigned int size2)
670 {
671 if (m) {
672 if (m->size1 != size1 || m->size2 != size2) {
673 gsl_matrix_free(m);
674 if (size1 * size2 > 0) m = gsl_matrix_alloc(size1, size2);
675 else m = nullptr;
676 }
677 } else if (size1 * size2 > 0) m = gsl_matrix_alloc(size1, size2);
678 }
679
680 void NewtonFitterGSL::debug_print(gsl_matrix* m, const char* name)
681 {
682 for (unsigned int i = 0; i < m->size1; ++i)
683 for (unsigned int j = 0; j < m->size2; ++j)
684 if (gsl_matrix_get(m, i, j) != 0)
685 B2INFO(name << "[" << i << "][" << j << "]=" << gsl_matrix_get(m, i, j));
686 }
687
688 void NewtonFitterGSL::debug_print(gsl_vector* v, const char* name)
689 {
690 for (unsigned int i = 0; i < v->size; ++i)
691 if (gsl_vector_get(v, i) != 0)
692 B2INFO(name << "[" << i << "]=" << gsl_vector_get(v, i));
693 }
694
695 int NewtonFitterGSL::getNcon() const {return ncon;}
696 int NewtonFitterGSL::getNsoft() const {return nsoft;}
697 int NewtonFitterGSL::getNunm() const {return nunm;}
698 int NewtonFitterGSL::getNpar() const {return npar;}
699
700 bool NewtonFitterGSL::updateParams(gsl_vector* xnew)
701 {
702 assert(xnew);
703 assert(xnew->size == idim);
704 bool significant = false;
705 for (auto i = fitobjects.begin(); i != fitobjects.end(); ++i) {
706 BaseFitObject* fo = *i;
707 assert(fo);
708 bool s = fo->updateParams(xnew->block->data, xnew->size);
709 significant |= s;
710 if (nit < nitdebug && s) {
711 B2DEBUG(11, "Significant update for FO " << i - fitobjects.begin() << " ("
712 << fo->getName() << ")\n");
713 }
714 }
715 return significant;
716 }
717
718
719 void NewtonFitterGSL::fillxold()
720 {
721 assert(xold);
722 assert(xold->size == idim);
723 for (auto fo : fitobjects) {
724 assert(fo);
725 for (int ilocal = 0; ilocal < fo->getNPar(); ++ilocal) {
726 if (!fo->isParamFixed(ilocal)) {
727 int iglobal = fo->getGlobalParNum(ilocal);
728 assert(iglobal >= 0 && iglobal < npar);
729 gsl_vector_set(xold, iglobal, fo->getParam(ilocal));
730 }
731 }
732 }
733 for (auto c : constraints) {
734 assert(c);
735 int iglobal = c->getGlobalNum();
736 assert(iglobal >= 0 && iglobal < (int)idim);
737 gsl_vector_set(xold, iglobal, gsl_vector_get(x, iglobal));
738 }
739 }
740
741 void NewtonFitterGSL::fillperr()
742 {
743 assert(perr);
744 assert(perr->size == idim);
745 gsl_vector_set_all(perr, 1);
746 for (auto fo : fitobjects) {
747 assert(fo);
748 for (int ilocal = 0; ilocal < fo->getNPar(); ++ilocal) {
749 if (!fo->isParamFixed(ilocal)) {
750 int iglobal = fo->getGlobalParNum(ilocal);
751 assert(iglobal >= 0 && iglobal < npar);
752 double e = std::abs(fo->getError(ilocal));
753 gsl_vector_set(perr, iglobal, e ? e : 1);
754 }
755 }
756 }
757 for (auto c : constraints) {
758 assert(c);
759 int iglobal = c->getGlobalNum();
760 assert(iglobal >= 0 && iglobal < (int)idim);
761 double e = c->getError();
762 gsl_vector_set(perr, iglobal, e ? 1 / e : 1);
763 }
764 }
765
766 int NewtonFitterGSL::calcM(bool errorpropagation)
767 {
768 assert(M);
769 assert(M->size1 == idim && M->size2 == idim);
770
771 gsl_matrix_set_zero(M);
772
773 // First, all terms d^2 chi^2/dx1 dx2
774 for (auto fo : fitobjects) {
775 assert(fo);
776 fo->addToGlobalChi2DerMatrix(M->block->data, M->tda);
777 }
778 if (debug > 3) {
779 B2INFO("After adding covariances from fit objects:\n");
780 debug_print(M, "M");
781 }
782
783 // Second, all terms d^2 chi^2/dlambda dx,
784 // i.e. the first derivatives of the constraints,
785 // plus the second derivatives times the lambda values
786 for (auto c : constraints) {
787 assert(c);
788 int kglobal = c->getGlobalNum();
789 assert(kglobal >= 0 && kglobal < (int)idim);
790 c->add1stDerivativesToMatrix(M->block->data, M->tda);
791 if (debug > 3) {
792 B2INFO("After adding first derivatives of constraint " << c->getName());
793 debug_print(M, "M");
794 B2INFO("errorpropagation = " << errorpropagation);
795 }
796 // for error propagation after fit,
797 //2nd derivatives of constraints times lambda should _not_ be included!
798 if (!errorpropagation) c->add2ndDerivativesToMatrix(M->block->data, M->tda, gsl_vector_get(x, kglobal));
799 }
800 if (debug > 3) {
801 B2INFO("After adding derivatives of constraints::\n");
802 debug_print(M, "M");
803 B2INFO("===========================================::\n");
804 }
805
806
807 // Finally, treat the soft constraints
808
809 for (auto bsc : softconstraints) {
810 assert(bsc);
811 bsc->add2ndDerivativesToMatrix(M->block->data, M->tda);
812 }
813 if (debug > 3) {
814 B2INFO("After adding soft constraints::\n");
815 debug_print(M, "M");
816 B2INFO("===========================================::\n");
817 }
818
819 // Rescale columns and rows by perr
820 for (unsigned int i = 0; i < idim; ++i)
821 for (unsigned int j = 0; j < idim; ++j)
822 gsl_matrix_set(Mscal, i, j,
823 gsl_vector_get(perr, i)*gsl_vector_get(perr, j)*gsl_matrix_get(M, i, j));
824
825 return 0;
826 }
827
828
829 int NewtonFitterGSL::calcy()
830 {
831 assert(y);
832 assert(y->size == idim);
833 gsl_vector_set_zero(y);
834 // First, for the parameters
835 for (auto fo : fitobjects) {
836 assert(fo);
837 fo->addToGlobalChi2DerVector(y->block->data, y->size);
838 }
839
840 // Now add lambda*derivatives of constraints,
841 // And finally, the derivatives w.r.t. to the constraints, i.e. the constraints themselves
842 for (auto c : constraints) {
843 assert(c);
844 int kglobal = c->getGlobalNum();
845 assert(kglobal >= 0 && kglobal < (int)idim);
846 c->addToGlobalChi2DerVector(y->block->data, y->size, gsl_vector_get(x, kglobal));
847 gsl_vector_set(y, kglobal, c->getValue());
848 }
849
850 // Finally, treat the soft constraints
851
852 for (auto bsc : softconstraints) {
853 assert(bsc);
854 bsc->addToGlobalChi2DerVector(y->block->data, y->size);
855 }
856 gsl_vector_memcpy(yscal, y);
857 gsl_vector_mul(yscal, perr);
858 return 0;
859 }
860
861 int NewtonFitterGSL::optimizeScale()
862 {
863 updateParams(xold);
864 calcy();
865 if (debug > 1) {
866 B2INFO("NewtonFitterGSL::optimizeScale");
867 debug_print(xold, "xold");
868 debug_print(yscal, "yscal");
869 debug_print(dx, "dx");
870 debug_print(dxscal, "dxscal");
871 }
872 scalevals[0] = 0;
873 fvals[0] = 0.5 * pow(gsl_blas_dnrm2(yscal), 2);
874 B2DEBUG(11, "NewtonFitterGSL::optimizeScale: fvals[0] = " << fvals[0]);
875 // -dx is a direction
876 // we want to minimize f=0.5*|y|^2 in that direction with y=grad chi2
877 // The gradient grad f is given by grad f = d^chi^2/dxi dxj * dchi^2/dxj
878 // = Mscal*yscal
879
880 // Calculate grad = 1*Mscal*yscal + 0*grad
881 gsl_blas_dgemv(CblasNoTrans, 1, Mscal, yscal, 0, grad);
882 if (debug > 1) {
883 debug_print(grad, "grad");
884 }
885
886 // Code adapted from Numerical Recipes (3rd ed), page 479
887 // routine lnsrch
888
889 int nite = 0;
890
891 static const double ALF = 1E-4;
892
893 stepsize = std::abs(gsl_vector_get(dxscal, gsl_blas_idamax(dxscal)));
894 static const double maxstepsize = 5;
895 double scalefactor = maxstepsize / stepsize;
896 if (stepsize > maxstepsize) {
897 gsl_vector_scale(dxscal, maxstepsize / stepsize);
898 B2DEBUG(12, "NewtonFitterGSL::optimizeScale: Rescaling dxscal by factor " << scalefactor);
899 stepsize = std::abs(gsl_vector_get(dxscal, gsl_blas_idamax(dxscal)));
900 if (debug > 1) {
901 debug_print(dxscal, "dxscal");
902 }
903 }
904
905 double slope;
906 gsl_blas_ddot(dxscal, grad, &slope);
907 slope *= -1;
908 B2DEBUG(12, "NewtonFitterGSL::optimizeScale: slope=" << slope
909 << ", 2*fvals[0]*factor=" << 2 * fvals[0]*scalefactor);
910
911 scale = 1;
912 double tmpscale, scaleold = 1;
913 do {
914 // sets x = xold
915 gsl_vector_memcpy(x, xold);
916 // x = scale*dx + x
917 if (debug > 1) {
918 debug_print(x, "x(1)");
919 }
920 gsl_blas_daxpy(-scale, dx, x);
921 if (debug > 1) {
922 debug_print(x, "x(2)");
923 }
924
925 updateParams(x);
926 calcy();
927 if (debug > 1) {
928 debug_print(x, "x(3)");
929 debug_print(yscal, "yscal");
930 }
931 ++nite;
932 scalevals[nite] = scale;
933 fvals[nite] = 0.5 * pow(gsl_blas_dnrm2(yscal), 2);
934
935 chi2new = calcChi2();
936
937
938// if (chi2new <= chi2best && fvals[nite] <= fvalbest) {
939// if ((fvals[nite] < fvalbest && chi2new <= chi2best) ||
940// (fvals[nite] < 1E-4 && chi2new < chi2best)) {
941 if ((fvals[nite] < fvalbest)) {
942 B2DEBUG(13, "new best value: "
943 << " scale " << scalevals[nite] << " -> |y|^2 = " << fvals[nite]
944 << ", chi2=" << chi2new << ", old best chi2: " << chi2best);
945 gsl_vector_memcpy(xbest, x);
946 chi2best = chi2new;
947 fvalbest = fvals[nite];
948 scalebest = scale;
949 stepbest = scale * stepsize;
950 }
951
952 if (fvals[nite] < fvals[0] + ALF * scale * slope) break;
953 if (nite == 1) {
954 tmpscale = -slope / (2 * (fvals[nite] - fvals[0] - slope));
955 B2DEBUG(13, "quadratic estimate for best scale: " << tmpscale);
956 } else {
957 double rhs1 = fvals[nite] - fvals[0] - scale * slope;
958 double rhs2 = fvals[nite - 1] - fvals[0] - scaleold * slope;
959 double a = (rhs1 / (scale * scale) - rhs2 / (scaleold * scaleold)) / (scale - scaleold);
960 double b = (-scaleold * rhs1 / (scale * scale) + scale * rhs2 / (scaleold * scaleold)) / (scale - scaleold);
961 if (a == 0) tmpscale = -slope / (2 * b);
962 else {
963 double disc = b * b - 3 * a * slope;
964 if (disc < 0) tmpscale = 0.5 * scale;
965 else if (b <= 0) tmpscale = (-b + sqrt(disc)) / (3 * a);
966 else tmpscale = -slope / (b + sqrt(disc));
967 }
968 B2DEBUG(13, "cubic estimate for best scale: " << tmpscale);
969 if (tmpscale > 0.5 * scale) tmpscale = 0.5 * scale;
970 }
971 scaleold = scale;
972 scale = (tmpscale < 0.1 * scale) ? 0.1 * scale : tmpscale;
973 B2DEBUG(11, "New scale: " << scale);
974
975 } while (nite < NITMAX && scale > 0.0001);
976
977 if (debug > 1) {
978 for (int it = 0; it <= nite; ++it) {
979 B2INFO(" scale " << scalevals[it] << " -> |y|^2 = " << fvals[it]
980 << " should be " << fvals[0] + ALF * scale * slope);
981 }
982 }
983 return chi2best < chi2old;
984 }
985
986 int NewtonFitterGSL::invertM()
987 {
988 gsl_matrix_memcpy(M1, M);
989
990 int ifail = 0;
991
992 int signum;
993 // Calculate LU decomposition of M into M1
994 int result = gsl_linalg_LU_decomp(M1, permM, &signum);
995 B2DEBUG(11, "invertM: gsl_linalg_LU_decomp result=" << result);
996 // Calculate inverse of M
997 ifail = gsl_linalg_LU_invert(M1, permM, M);
998 B2DEBUG(11, "invertM: gsl_linalg_LU_solve result=" << ifail);
999
1000 if (ifail != 0) {
1001 B2ERROR("NewtonFitter::invert: ifail from gsl_linalg_LU_invert=" << ifail);
1002 }
1003 return ifail;
1004 }
1005
1006 void NewtonFitterGSL::setDebug(int debuglevel)
1007 {
1008 debug = debuglevel;
1009 }
1010
1011
1012 void NewtonFitterGSL::calcCovMatrix()
1013 {
1014 // Set up equation system M*dadeta + dydeta = 0
1015 // here, dadeta is d a / d eta, i.e. the derivatives of the fitted
1016 // parameters a w.r.t. to the measured parameters eta,
1017 // and dydeta is the derivative of the set of equations
1018 // w.r.t eta, i.e. simply d^2 chi^2 / d a d eta.
1019 // Now, if chi2 = (a-eta)^T*Vinv((a-eta), we have simply
1020 // d^2 chi^2 / d a d eta = - d^2 chi^2 / d a d a
1021 // and can use the method addToGlobalChi2DerMatrix.
1022
1023 gsl_matrix_set_zero(M1);
1024 gsl_matrix_set_zero(M2);
1025 // First, all terms d^2 chi^2/dx1 dx2
1026 for (auto fo : fitobjects) {
1027 assert(fo);
1028 fo->addToGlobalChi2DerMatrix(M1->block->data, M1->tda);
1029 fo->addToGlobCov(M2->block->data, M2->tda);
1030 }
1031
1032 // multiply by -1
1033 gsl_matrix_scale(M1, -1);
1034
1035 // JL: dy/eta are the derivatives of the "objective function" with respect to the MEASURED parameters.
1036 // Since the soft constraints do not depend on the measured, but only on the fitted (!) parameters,
1037 // dy/deta stays -1*M1 also in the presence of soft constraints!
1038 gsl_matrix_view dydeta = gsl_matrix_submatrix(M1, 0, 0, idim, npar);
1039 gsl_matrix_view Cov_eta = gsl_matrix_submatrix(M2, 0, 0, npar, npar);
1040
1041 if (debug > 3) {
1042 B2INFO("NewtonFitterGSL::calcCovMatrix\n");
1043 debug_print(&dydeta.matrix, "dydeta");
1044 debug_print(&Cov_eta.matrix, "Cov_eta"); \
1045 }
1046
1047 // JL: calculates d^2 chi^2 / dx1 dx2 + first (!) derivatives of hard & soft constraints, and the
1048 // second derivatives of the soft constraints times the values of the fitted parameters
1049 // - all of the with respect to the FITTED parameters, therefore with soft constraints like in the fit itself.
1050 calcM(true);
1051
1052 if (debug > 3) {
1053 debug_print(M, "M");
1054 }
1055
1056 // Now, solve M*dadeta = dydeta
1057
1058 // Calculate LU decomposition of M into M3
1059 int signum;
1060 int result = gsl_linalg_LU_decomp(M, permM, &signum);
1061
1062 if (debug > 3) {
1063 B2INFO("invertM: gsl_linalg_LU_decomp result=" << result);
1064 debug_print(M, "M_LU");
1065 }
1066
1067 // Calculate inverse of M, store in M3
1068 int ifail = gsl_linalg_LU_invert(M, permM, M3);
1069
1070 if (debug > 3) {
1071 B2INFO("invertM: gsl_linalg_LU_invert ifail=" << ifail);
1072 debug_print(M3, "Minv");
1073 }
1074
1075 // Calculate dadeta = M3*dydeta
1076 gsl_matrix_set_zero(M4);
1077 gsl_matrix_view dadeta = gsl_matrix_submatrix(M4, 0, 0, idim, npar);
1078
1079 if (debug > 3) {
1080 debug_print(&dadeta.matrix, "dadeta");
1081 }
1082
1083 // dadeta = 1*M*dydeta + 0*dadeta
1084 gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, M3, &dydeta.matrix, 0, &dadeta.matrix);
1085
1086
1087 // Now calculate Cov_a = dadeta*Cov_eta*dadeta^T
1088
1089 // First, calculate M3 = Cov_eta*dadeta^T as
1090 gsl_matrix_view M3part = gsl_matrix_submatrix(M3, 0, 0, npar, idim);
1091 gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1, &Cov_eta.matrix, &dadeta.matrix, 0, &M3part.matrix);
1092 // Now Cov_a = dadeta*M3part
1093 gsl_matrix_set_zero(M5);
1094 gsl_matrix_view Cov_a = gsl_matrix_submatrix(M5, 0, 0, npar, npar);
1095 gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, &dadeta.matrix, &M3part.matrix, 0, M5);
1096 gsl_matrix_memcpy(CCinv, M5);
1097
1098 if (debug > 3) {
1099 debug_print(&Cov_a.matrix, "Cov_a");
1100 debug_print(CCinv, "full Cov from err prop");
1101 debug_print(M1, "uncorr Cov from err prop");
1102 }
1103
1104 // Finally, copy covariance matrix
1105 if (cov && covDim != npar) {
1106 delete[] cov;
1107 cov = nullptr;
1108 }
1109 covDim = npar;
1110 if (!cov) cov = new double[covDim * covDim];
1111 for (int i = 0; i < covDim; ++i) {
1112 for (int j = 0; j < covDim; ++j) {
1113 cov[i * covDim + j] = gsl_matrix_get(&Cov_a.matrix, i, j);
1114 }
1115 }
1116 covValid = true;
1117 }
1118
1119
1120 double NewtonFitterGSL::meritFunction(double mu)
1121 {
1122 double result = 0;
1123 switch (imerit) {
1124 case 1: // l1 penalty function, Nocedal&Wright Eq. (15.24)
1125 result = chi2;
1126 for (auto c : constraints) {
1127 assert(c);
1128 int kglobal = c->getGlobalNum();
1129 assert(kglobal >= 0 && kglobal < (int)idim);
1130 result += mu * std::fabs(c->getValue());
1131 }
1132 break;
1133 default: assert(0);
1134 }
1135 return result;
1136 }
1137
1138 double NewtonFitterGSL::meritFunctionDeriv()
1139 {
1140 double result = 0;
1141 switch (imerit) {
1142 case 1:
1143 break;
1144 default: assert(0);
1145 }
1146 return result;
1147 }
1148
1149 }// end OrcaKinFit namespace
1151} // end Belle2 namespace
1152
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.
Abstract base class for soft 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
int ncon
total number of hard constraints
int calcDxSVD()
Calculate the vector dx to update the parameters; returns fail code, 0=OK.
int npar
total number of parameters
virtual double calcChi2()
Calculate the chi2.
void printMy(const double M[], const double y[], int idim)
Print a Matrix M and a vector y of dimension idim.
virtual int getNsoft() const
Get the number of soft constraints of the last fit.
virtual double fit() override
The fit method, returns the fit probability.
virtual bool initialize() override
Initialize the fitter.
virtual int getNcon() const
Get the number of hard constraints of the last fit.
int nsoft
total number of soft constraints
virtual int getError() const override
Get the error code of the last fit: 0=OK, 1=failed.
int nunm
total number of unmeasured parameters
int calcDx()
Calculate the vector dx to update the parameters; returns fail code, 0=OK.
virtual int getNunm() const
Get the number of unmeasured parameters of the last fit.
virtual int getIterations() const override
Get the number of iterations of the last fit.
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.
virtual ~NewtonFitterGSL()
Virtual destructor.
Abstract base class for different kinds of events.
Definition: ClusterUtils.h:24