Belle II Software light-2406-ragdoll
SoftBWParticleConstraint.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#ifdef MARLIN_USE_ROOT
18
19
20#include "analysis/OrcaKinFit/SoftBWParticleConstraint.h"
21#include "analysis/OrcaKinFit/ParticleFitObject.h"
22#include <framework/logging/Logger.h>
23
24#include "Math/ProbFuncMathCore.h"
25#include "Math/QuantFuncMathCore.h"
26
27#include <iostream>
28#include <cmath>
29
30using namespace std;
31
32namespace Belle2 {
37 namespace OrcaKinFit {
38
39 SoftBWParticleConstraint::SoftBWParticleConstraint(double gamma_, double emin_, double emax_)
40 :
41 fitobjects(FitObjectContainer()), derivatives(std::vector <double> ()), flags(std::vector <int> ()),
42 gamma(gamma_), emin(emin_), emax(emax_),
43 cachevalid(false),
44 atanxmin(0), atanxmax(0), diffatanx(0)
45 {
46 invalidateCache();
47 }
48
49 double SoftBWParticleConstraint::getGamma() const
50 {
51 return gamma;
52 }
53
54 double SoftBWParticleConstraint::setGamma(double gamma_)
55 {
56 if (gamma_ != 0) gamma = std::abs(gamma_);
57 invalidateCache();
58 return gamma;
59 }
60
61 double SoftBWParticleConstraint::getChi2() const
62 {
63 return penalty(getValue());
64 }
65
66 double SoftBWParticleConstraint::getError() const
67 {
68 double dgdpi[4];
69 double error2 = 0;
70 for (unsigned int i = 0; i < fitobjects.size(); ++i) {
71 const ParticleFitObject* foi = fitobjects[i];
72 assert(foi);
73 if (firstDerivatives(i, dgdpi)) {
74 error2 += foi->getError2(dgdpi, getVarBasis());
75 }
76 }
77 return std::sqrt(std::abs(error2));
78 }
79
103 void SoftBWParticleConstraint::add2ndDerivativesToMatrix(double* M, int idim) const
104 {
105
113 double e = getValue();
114 double fact = penalty1stder(e);
115 double fact2 = penalty2ndder(e);
116
117 // Derivatives $\frac{\partial ^2 g}{\partial P_i \partial P_j}$ at fixed i, j
118 // d2GdPidPj[4*ii+jj] is derivative w.r.t. P_i,ii and P_j,jj, where ii=0,1,2,3 for E,px,py,pz
119 double d2GdPidPj[16];
120 // Derivatives $\frac {\partial P_i}{\partial a_k}$ for all i;
121 // k is local parameter number
122 // dPidAk[KMAX*4*i + 4*k + ii] is $\frac {\partial P_{i,ii}}{\partial a_k}$,
123 // with ii=0, 1, 2, 3 for E, px, py, pz
124 const int KMAX = 4;
125 const int n = fitobjects.size();
126 double* dPidAk = new double[n * KMAX * 4];
127 bool* dPidAkval = new bool[n];
128
129 for (int i = 0; i < n; ++i) dPidAkval[i] = false;
130
131 // Derivatives $\frac{\partial ^2 g}{\partial P_i \partial a_l}$ at fixed i
132 // d2GdPdAl[4*l + ii] is $\frac{\partial ^2 g}{\partial P_{i,ii} \partial a_l}$
133 double d2GdPdAl[4 * KMAX];
134 // Derivatives $\frac{\partial ^2 g}{\partial a_k \partial a_l}$
135 double d2GdAkdAl[KMAX * KMAX] = {0};
136
137
138
139 // Global parameter numbers: parglobal[KMAX*i+klocal]
140 // is global parameter number of local parameter klocal of i-th Fit object
141 int* parglobal = new int[KMAX * n];
142
143 for (int i = 0; i < n; ++i) {
144 const ParticleFitObject* foi = fitobjects[i];
145 assert(foi);
146 for (int klocal = 0; klocal < foi->getNPar(); ++klocal) {
147 parglobal [KMAX * i + klocal] = foi->getGlobalParNum(klocal);
148 }
149 }
150
151
152 for (int i = 0; i < n; ++i) {
153 const ParticleFitObject* foi = fitobjects[i];
154 assert(foi);
155 for (int j = 0; j < n; ++j) {
156 const ParticleFitObject* foj = fitobjects[j];
157 assert(foj);
158 if (secondDerivatives(i, j, d2GdPidPj)) {
159 if (!dPidAkval[i]) {
160 foi->getDerivatives(dPidAk + i * (KMAX * 4), KMAX * 4);
161 dPidAkval[i] = true;
162 }
163 if (!dPidAkval[j]) {
164 foj->getDerivatives(dPidAk + j * (KMAX * 4), KMAX * 4);
165 dPidAkval[j] = true;
166 }
167 // Now sum over E/px/Py/Pz for object j:
168 // $$\frac{\partial ^2 g}{\partial P_{i,ii} \partial a_l}
169 // = (sum_{j}) sum_{jj} frac{\partial ^2 g}{\partial P_{i,ii} \partial P_{j,jj}}
170 // \cdot \frac{\partial P_{j,jj}}{\partial a_l}
171 // We're summing over jj here
172 for (int llocal = 0; llocal < foj->getNPar(); ++llocal) {
173 for (int ii = 0; ii < 4; ++ii) {
174 int ind1 = 4 * ii;
175 int ind2 = (KMAX * 4) * j + 4 * llocal;
176 double& r = d2GdPdAl[4 * llocal + ii];
177 r = d2GdPidPj[ ind1] * dPidAk[ ind2]; // E
178 r += d2GdPidPj[++ind1] * dPidAk[++ind2]; // px
179 r += d2GdPidPj[++ind1] * dPidAk[++ind2]; // py
180 r += d2GdPidPj[++ind1] * dPidAk[++ind2]; // pz
181 }
182 }
183 // Now sum over E/px/Py/Pz for object i, i.e. sum over ii:
184 // $$
185 // \frac{\partial ^2 g}{\partial a_k \partial a_l}
186 // = \sum_{ii} \frac{\partial ^2 g}{\partial P_{i,ii} \partial a_l} \cdot
187 // \frac{\partial P_{i,ii}}{\partial a_k}
188 // $$
189 for (int klocal = 0; klocal < foi->getNPar(); ++klocal) {
190 for (int llocal = 0; llocal < foj->getNPar(); ++llocal) {
191 int ind1 = 4 * llocal;
192 int ind2 = (KMAX * 4) * i + 4 * klocal;
193 double& r = d2GdAkdAl[KMAX * klocal + llocal];
194 r = d2GdPdAl[ ind1] * dPidAk[ ind2]; //E
195 r += d2GdPdAl[++ind1] * dPidAk[++ind2]; // px
196 r += d2GdPdAl[++ind1] * dPidAk[++ind2]; // py
197 r += d2GdPdAl[++ind1] * dPidAk[++ind2]; // pz
198 }
199 }
200 // Now expand the local parameter numbers to global ones
201 for (int klocal = 0; klocal < foi->getNPar(); ++klocal) {
202 int kglobal = parglobal [KMAX * i + klocal];
203 for (int llocal = 0; llocal < foj->getNPar(); ++llocal) {
204 int lglobal = parglobal [KMAX * j + llocal];
205 M [idim * kglobal + lglobal] += fact * d2GdAkdAl[KMAX * klocal + llocal];
206 }
207 }
208 }
209 }
210 }
230 double* v = new double[idim];
231 for (int i = 0; i < idim; ++i) v[i] = 0;
232
233 // fact2 may be negative, so don't use sqrt(fact2)
234 double dgdpi[4];
235 for (int i = 0; i < n; ++i) {
236 const ParticleFitObject* foi = fitobjects[i];
237 assert(foi);
238 if (firstDerivatives(i, dgdpi)) {
239 foi->addTo2ndDerivatives(M, idim, fact, dgdpi, getVarBasis());
240 foi->addToGlobalChi2DerVector(v, idim, 1, dgdpi, getVarBasis());
241 }
242 }
243
244 for (int i = 0; i < idim; ++i) {
245 if (double vi = v[i]) {
246 int ioffs = i * idim;
247 for (double* pvj = v; pvj < v + idim; ++pvj) {
248 M[ioffs++] += fact2 * vi * (*pvj);
249 }
250 }
251 }
252
253
254 delete[] dPidAk;
255 delete[] dPidAkval;
256 delete[] parglobal;
257 delete[] v;
258 }
259
260 void SoftBWParticleConstraint::addToGlobalChi2DerVector(double* y, int idim) const
261 {
262 double dgdpi[4];
263 double r = penalty1stder(getValue());
264 for (unsigned int i = 0; i < fitobjects.size(); ++i) {
265 const ParticleFitObject* foi = fitobjects[i];
266 assert(foi);
267 if (firstDerivatives(i, dgdpi)) {
268 foi->addToGlobalChi2DerVector(y, idim, r, dgdpi, getVarBasis());
269 }
270 }
271 }
272
273 void SoftBWParticleConstraint::test1stDerivatives()
274 {
275 B2INFO("SoftBWParticleConstraint::test1stDerivatives for " << getName());
276 double y[100];
277 for (int i = 0; i < 100; ++i) y[i] = 0;
278 addToGlobalChi2DerVector(y, 100);
279 double eps = 0.00001;
280 for (unsigned int ifo = 0; ifo < fitobjects.size(); ++ifo) {
281 ParticleFitObject* fo = fitobjects[ifo];
282 assert(fo);
283 for (int ilocal = 0; ilocal < fo->getNPar(); ++ilocal) {
284 int iglobal = fo->getGlobalParNum(ilocal);
285 double calc = y[iglobal];
286 double num = num1stDerivative(ifo, ilocal, eps);
287 B2INFO("fo: " << fo->getName() << " par " << ilocal << "/"
288 << iglobal << " (" << fo->getParamName(ilocal)
289 << ") calc: " << calc << " - num: " << num << " = " << calc - num);
290 }
291 }
292 }
293 void SoftBWParticleConstraint::test2ndDerivatives()
294 {
295 B2INFO("SoftBWParticleConstraint::test2ndDerivatives for " << getName());
296 const int idim = 100;
297 double* M = new double[idim * idim];
298 for (int i = 0; i < idim * idim; ++i) M[i] = 0;
299 add2ndDerivativesToMatrix(M, idim);
300 double eps = 0.0001;
301 B2INFO("eps=" << eps);
302
303 for (unsigned int ifo1 = 0; ifo1 < fitobjects.size(); ++ifo1) {
304 ParticleFitObject* fo1 = fitobjects[ifo1];
305 assert(fo1);
306 for (unsigned int ifo2 = ifo1; ifo2 < fitobjects.size(); ++ifo2) {
307 ParticleFitObject* fo2 = fitobjects[ifo2];
308 assert(fo2);
309 for (int ilocal1 = 0; ilocal1 < fo1->getNPar(); ++ilocal1) {
310 int iglobal1 = fo1->getGlobalParNum(ilocal1);
311 for (int ilocal2 = (ifo1 == ifo2 ? ilocal1 : 0); ilocal2 < fo2->getNPar(); ++ilocal2) {
312 int iglobal2 = fo2->getGlobalParNum(ilocal2);
313 double calc = M[idim * iglobal1 + iglobal2];
314 double num = num2ndDerivative(ifo1, ilocal1, eps, ifo2, ilocal2, eps);
315 B2INFO("fo1: " << fo1->getName() << " par " << ilocal1 << "/"
316 << iglobal1 << " (" << fo1->getParamName(ilocal1)
317 << "), fo2: " << fo2->getName() << " par " << ilocal2 << "/"
318 << iglobal2 << " (" << fo2->getParamName(ilocal2)
319 << ") calc: " << calc << " - num: " << num << " = " << calc - num);
320 }
321 }
322 }
323 }
324 delete[] M;
325 }
326
327
328 double SoftBWParticleConstraint::num1stDerivative(int ifo, int ilocal, double eps)
329 {
330 ParticleFitObject* fo = fitobjects[ifo];
331 assert(fo);
332 double save = fo->getParam(ilocal);
333 fo->setParam(ilocal, save + eps);
334 double v1 = getChi2();
335 fo->setParam(ilocal, save - eps);
336 double v2 = getChi2();
337 double result = (v1 - v2) / (2 * eps);
338 fo->setParam(ilocal, save);
339 return result;
340 }
341
342 double SoftBWParticleConstraint::num2ndDerivative(int ifo1, int ilocal1, double eps1,
343 int ifo2, int ilocal2, double eps2)
344 {
345 double result;
346
347 if (ifo1 == ifo2 && ilocal1 == ilocal2) {
348 ParticleFitObject* fo = fitobjects[ifo1];
349 assert(fo);
350 double save = fo->getParam(ilocal1);
351 double v0 = getChi2();
352 fo->setParam(ilocal1, save + eps1);
353 double v1 = getChi2();
354 fo->setParam(ilocal1, save - eps1);
355 double v2 = getChi2();
356 result = (v1 + v2 - 2 * v0) / (eps1 * eps1);
357 fo->setParam(ilocal1, save);
358 } else {
359 ParticleFitObject* fo1 = fitobjects[ifo1];
360 assert(fo1);
361 ParticleFitObject* fo2 = fitobjects[ifo2];
362 assert(fo2);
363 double save1 = fo1->getParam(ilocal1);
364 double save2 = fo2->getParam(ilocal2);
365 fo1->setParam(ilocal1, save1 + eps1);
366 fo2->setParam(ilocal2, save2 + eps2);
367 double v11 = getChi2();
368 fo2->setParam(ilocal2, save2 - eps2);
369 double v12 = getChi2();
370 fo1->setParam(ilocal1, save1 - eps1);
371 double v22 = getChi2();
372 fo2->setParam(ilocal2, save2 + eps2);
373 double v21 = getChi2();
374 result = (v11 + v22 - v12 - v21) / (4 * eps1 * eps2);
375 fo1->setParam(ilocal1, save1);
376 fo2->setParam(ilocal2, save2);
377 }
378 return result;
379 }
380
381 double SoftBWParticleConstraint::erfinv(double x)
382 {
383// static const double a = 8*(M_PI-3)/(3*M_PI*(4-M_PI));
384// static const double aa = 3*(4-M_PI)/(4*(M_PI-3)); // = 2/(M_PI*a);
385// double s = (x<0) ? -1 : 1;
386// x *= s;
387// if (x >= 1) return s*HUGE_VAL;
388// double ll = std::log (1 - x*x);
389// double xx = aa + 0.5*ll;
390// return s * std::sqrt(-xx + std::sqrt (xx*xx - ll/a));
391 return 2 * ROOT::Math::normal_quantile(std::sqrt(2.0) * x, 1.0) - 1;
392 }
393
394 double SoftBWParticleConstraint::normal_quantile(double x)
395 {
396 return ROOT::Math::normal_quantile(x, 1.0);
397 }
398
399 double SoftBWParticleConstraint::normal_quantile_1stderiv(double x)
400 {
401 double y = ROOT::Math::normal_quantile(x, 1.0);
402 return 1 / normal_pdf(y);
403 }
404
405 double SoftBWParticleConstraint::normal_quantile_2ndderiv(double x)
406 {
407 double y = ROOT::Math::normal_quantile(x, 1.0);
408 return -y / pow(normal_pdf(y), 2);
409 }
410
411 double SoftBWParticleConstraint::normal_pdf(double x)
412 {
413 static const double C_1_SQRT2PI = 1 / (std::sqrt(2 * M_PI));
414 return C_1_SQRT2PI * std::exp(-0.5 * x * x);
415 }
416
417 double SoftBWParticleConstraint::normal_pdf_deriv(double x)
418 {
419 static const double C_1_SQRT2PI = 1 / (std::sqrt(2 * M_PI));
420 return -C_1_SQRT2PI * x * std::exp(-0.5 * x * x);
421 }
422
423 double SoftBWParticleConstraint::penalty(double e) const
424 {
425 double x = e / gamma;
426 // x is distributed according to the Cauchy distribution
427 // f(x) = 1/pi 1/(1 + x^2)
428 // or (if limits are active)
429 // f(x) = 1/(arctan(x_max) - arctan(x_min)) 1/(1 + x^2)
430
431 // The integral pdf is
432 // F(x) = 1/2 + 1/pi arctan (x)
433 // or (if limits are active)
434 // F(x) = 1/2 + arctan (x)/(arctan(x_max) - arctan(x_min))
435
436 // So, chi2 = 2 (erf^-1 (1 + 2 F(x)) )^2
437 // or chi2 = norm_quantile (F(x))^2
438
439 if (!cachevalid) updateCache();
440 double F = 0.5 + std::atan(x) / diffatanx;
441 if (F < 0 || F > 1 || !std::isfinite(F))
442 B2INFO("SoftBWParticleConstraint::penalty: error for e=" << e
443 << ", gamma=" << gamma << " -> x=" << x << " => F=" << F);
444
445 assert(F >= 0);
446 assert(F <= 1);
447 double result = std::pow(normal_quantile(F), 2);
448 assert(std::isfinite(result));
449 return result;
450
451
452
453 //double chi = erfinv (2/M_PI *std:arctan (x));
454 //return 2*chi*chi;;
455
456 // anyway, a very good and much simpler approximation is
457 // return 0.75*std::log (1 + x*x);
458 }
459
460 double SoftBWParticleConstraint::penalty1stder(double e) const
461 {
462 double x = e / gamma;
463 if (!cachevalid) updateCache();
464 double F = 0.5 + std::atan(x) / diffatanx;
465 double dF_de = 1. / ((1 + x * x) * diffatanx * gamma);
466 double result = 2 * normal_quantile(F) * normal_quantile_1stderiv(F) * dF_de;
467 assert(std::isfinite(result));
468 return result;
469
470 }
471 double SoftBWParticleConstraint::penalty2ndder(double e) const
472 {
473 double x = e / gamma;
474 if (!cachevalid) updateCache();
475 double F = 0.5 + std::atan(x) / diffatanx;
476 double dF_de = 1. / ((1 + x * x) * diffatanx * gamma);
477 double d2F_de2 = -2 * diffatanx * x * dF_de * dF_de;
478
479 double result = 2 * pow(normal_quantile_1stderiv(F) * dF_de, 2)
480 + 2 * normal_quantile(F) * normal_quantile_2ndderiv(F) * dF_de * dF_de
481 + 2 * normal_quantile(F) * normal_quantile_1stderiv(F) * d2F_de2;
482 assert(std::isfinite(result));
483 return result;
484
485
486 }
487
488 void SoftBWParticleConstraint::invalidateCache() const
489 {
490 cachevalid = false;
491 }
492
493 void SoftBWParticleConstraint::updateCache() const
494 {
495 if (emin == -numeric_limits<double>::infinity())
496 atanxmin = -M_PI_2;
497 else if (emin == numeric_limits<double>::infinity())
498 atanxmin = M_PI_2;
499 else atanxmin = std::atan(emin / gamma);
500 if (emax == -numeric_limits<double>::infinity())
501 atanxmax = -M_PI_2;
502 else if (emax == numeric_limits<double>::infinity())
503 atanxmax = M_PI_2;
504 else atanxmax = std::atan(emax / gamma);
505 diffatanx = atanxmax - atanxmin;
506 cachevalid = true;
507
508 B2INFO("SoftBWParticleConstraint::updateCache(): "
509 << "gamma=" << gamma
510 << ", emin=" << emin << " -> atanxmin=" << atanxmin
511 << ", emax=" << emax << " -> atanxmax=" << atanxmax
512 << " => diffatanx=" << diffatanx);
513
514 }
515
516 bool SoftBWParticleConstraint::cacheValid() const
517 {
518 return cachevalid;
519 }
520
521 int SoftBWParticleConstraint::getVarBasis() const
522 {
523 return VAR_BASIS;
524 }
525
526 }// end OrcaKinFit namespace
528} // end Belle2 namespace
529
530
531#endif // MARLIN_USE_ROOT
Abstract base class for different kinds of events.
Definition: ClusterUtils.h:24
STL namespace.