Belle II Software light-2406-ragdoll
SoftGaussParticleConstraint.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#include "analysis/OrcaKinFit/SoftGaussParticleConstraint.h"
18#include "analysis/OrcaKinFit/ParticleFitObject.h"
19#include <framework/logging/Logger.h>
20#include <iostream>
21#include <cmath>
22using namespace std;
23
24namespace Belle2 {
29 namespace OrcaKinFit {
30
32 : sigma(sigma_)
33 {
35 }
36
38 {
39 return sigma;
40 }
41
43 {
44 if (sigma_ != 0) sigma = std::abs(sigma_);
45 return sigma;
46 }
47
49 {
50 double r = getValue() / getSigma();
51 return r * r;
52 }
53
55 {
56 double dgdpi[4];
57 double error2 = 0;
58 for (unsigned int i = 0; i < fitobjects.size(); ++i) {
59 const ParticleFitObject* foi = fitobjects[i];
60 assert(foi);
61 if (firstDerivatives(i, dgdpi)) {
62 error2 += foi->getError2(dgdpi, getVarBasis());
63 }
64 }
65 return std::sqrt(std::abs(error2));
66 }
67
93 {
94
101 double s = getSigma();
102 double fact = 2 * getValue() / (s * s);
103
104 // Derivatives \f$\frac{\partial ^2 g}{\partial P_i \partial P_j}\f$ at fixed i, j
105 // 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
106 double d2GdPidPj[16];
107 // Derivatives \f$\frac {\partial P_i}{\partial a_k}\f$ for all i;
108 // k is local parameter number
109 // dPidAk[KMAX*4*i + 4*k + ii] is \f$\frac {\partial P_{i,ii}}{\partial a_k}\f$,
110 // with ii=0, 1, 2, 3 for E, px, py, pz
111 const int KMAX = 4;
112 const int n = fitobjects.size();
113 auto* dPidAk = new double[n * KMAX * 4];
114 bool* dPidAkval = new bool[n];
115
116 for (int i = 0; i < n; ++i) dPidAkval[i] = false;
117
118 // Derivatives \f$\frac{\partial ^2 g}{\partial P_i \partial a_l}\f$ at fixed i
119 // d2GdPdAl[4*l + ii] is \f$\frac{\partial ^2 g}{\partial P_{i,ii} \partial a_l}\f$
120 double d2GdPdAl[4 * KMAX];
121 // Derivatives \f$\frac{\partial ^2 g}{\partial a_k \partial a_l}\f$
122 double d2GdAkdAl[KMAX * KMAX] = {0};
123
124 // Global parameter numbers: parglobal[KMAX*i+klocal]
125 // is global parameter number of local parameter klocal of i-th Fit object
126 int* parglobal = new int[KMAX * n];
127
128 for (int i = 0; i < n; ++i) {
129 const ParticleFitObject* foi = fitobjects[i];
130 assert(foi);
131 for (int klocal = 0; klocal < foi->getNPar(); ++klocal) {
132 parglobal [KMAX * i + klocal] = foi->getGlobalParNum(klocal);
133 }
134 }
135
136
137 for (int i = 0; i < n; ++i) {
138 const ParticleFitObject* foi = fitobjects[i];
139 assert(foi);
140 for (int j = 0; j < n; ++j) {
141 const ParticleFitObject* foj = fitobjects[j];
142 assert(foj);
143 if (secondDerivatives(i, j, d2GdPidPj)) {
144 if (!dPidAkval[i]) {
145 foi->getDerivatives(dPidAk + i * (KMAX * 4), KMAX * 4);
146 dPidAkval[i] = true;
147 }
148 if (!dPidAkval[j]) {
149 foj->getDerivatives(dPidAk + j * (KMAX * 4), KMAX * 4);
150 dPidAkval[j] = true;
151 }
152 // Now sum over E/px/Py/Pz for object j:
153 // \f[
154 // \frac{\partial ^2 g}{\partial P_{i,ii} \partial a_l}
155 // = (sum_{j}) sum_{jj} frac{\partial ^2 g}{\partial P_{i,ii} \partial P_{j,jj}}
156 // \cdot \frac{\partial P_{j,jj}}{\partial a_l}
157 // \f]
158 // We're summing over jj here
159 for (int llocal = 0; llocal < foj->getNPar(); ++llocal) {
160 for (int ii = 0; ii < 4; ++ii) {
161 int ind1 = 4 * ii;
162 int ind2 = (KMAX * 4) * j + 4 * llocal;
163 double& r = d2GdPdAl[4 * llocal + ii];
164 r = d2GdPidPj[ ind1] * dPidAk[ ind2]; // E
165 r += d2GdPidPj[++ind1] * dPidAk[++ind2]; // px
166 r += d2GdPidPj[++ind1] * dPidAk[++ind2]; // py
167 r += d2GdPidPj[++ind1] * dPidAk[++ind2]; // pz
168 }
169 }
170 // Now sum over E/px/Py/Pz for object i, i.e. sum over ii:
171 // \f[
172 // \frac{\partial ^2 g}{\partial a_k \partial a_l}
173 // = \sum_{ii} \frac{\partial ^2 g}{\partial P_{i,ii} \partial a_l} \cdot
174 // \frac{\partial P_{i,ii}}{\partial a_k}
175 // \f]
176 for (int klocal = 0; klocal < foi->getNPar(); ++klocal) {
177 for (int llocal = 0; llocal < foj->getNPar(); ++llocal) {
178 int ind1 = 4 * llocal;
179 int ind2 = (KMAX * 4) * i + 4 * klocal;
180 double& r = d2GdAkdAl[KMAX * klocal + llocal];
181 r = d2GdPdAl[ ind1] * dPidAk[ ind2]; //E
182 r += d2GdPdAl[++ind1] * dPidAk[++ind2]; // px
183 r += d2GdPdAl[++ind1] * dPidAk[++ind2]; // py
184 r += d2GdPdAl[++ind1] * dPidAk[++ind2]; // pz
185 }
186 }
187 // Now expand the local parameter numbers to global ones
188 for (int klocal = 0; klocal < foi->getNPar(); ++klocal) {
189 int kglobal = parglobal [KMAX * i + klocal];
190 for (int llocal = 0; llocal < foj->getNPar(); ++llocal) {
191 int lglobal = parglobal [KMAX * j + llocal];
192 M [idim * kglobal + lglobal] += fact * d2GdAkdAl[KMAX * klocal + llocal];
193 }
194 }
195 }
196 }
197 }
216 auto* v = new double[idim];
217 for (int i = 0; i < idim; ++i) v[i] = 0;
218 double sqrtfact2 = sqrt(2.0) / s;
219
220 double dgdpi[4];
221 for (int i = 0; i < n; ++i) {
222 const ParticleFitObject* foi = fitobjects[i];
223 assert(foi);
224 if (firstDerivatives(i, dgdpi)) {
225 foi->addTo2ndDerivatives(M, idim, fact, dgdpi, getVarBasis());
226 foi->addToGlobalChi2DerVector(v, idim, sqrtfact2, dgdpi, getVarBasis());
227 }
228 }
229
230 for (int i = 0; i < idim; ++i) {
231 if (double vi = v[i]) {
232 int ioffs = i * idim;
233 for (double* pvj = v; pvj < v + idim; ++pvj) {
234 M[ioffs++] += vi * (*pvj);
235 }
236 }
237 }
238
239
240 delete[] dPidAk;
241 delete[] dPidAkval;
242 delete[] parglobal;
243 delete[] v;
244 }
245
247 {
248 double dgdpi[4];
249 double s = getSigma();
250 double r = 2 * getValue() / (s * s);
251 for (unsigned int i = 0; i < fitobjects.size(); ++i) {
252 const ParticleFitObject* foi = fitobjects[i];
253 assert(foi);
254 if (firstDerivatives(i, dgdpi)) {
255 foi->addToGlobalChi2DerVector(y, idim, r, dgdpi, getVarBasis());
256 }
257 }
258 }
259
260 void SoftGaussParticleConstraint::test1stDerivatives()
261 {
262 B2INFO("SoftGaussParticleConstraint::test1stDerivatives for " << getName());
263 double y[100];
264 for (double& i : y) i = 0;
266 double eps = 0.00001;
267 for (unsigned int ifo = 0; ifo < fitobjects.size(); ++ifo) {
268 ParticleFitObject* fo = fitobjects[ifo];
269 assert(fo);
270 for (int ilocal = 0; ilocal < fo->getNPar(); ++ilocal) {
271 int iglobal = fo->getGlobalParNum(ilocal);
272 double calc = y[iglobal];
273 double num = num1stDerivative(ifo, ilocal, eps);
274 B2INFO("fo: " << fo->getName() << " par " << ilocal << "/"
275 << iglobal << " (" << fo->getParamName(ilocal)
276 << ") calc: " << calc << " - num: " << num << " = " << calc - num);
277 }
278 }
279 }
280 void SoftGaussParticleConstraint::test2ndDerivatives()
281 {
282 B2INFO("SoftGaussParticleConstraint::test2ndDerivatives for " << getName());
283 const int idim = 100;
284 auto* M = new double[idim * idim];
285 for (int i = 0; i < idim * idim; ++i) M[i] = 0;
287 double eps = 0.0001;
288 B2INFO("eps=" << eps);
289
290 for (unsigned int ifo1 = 0; ifo1 < fitobjects.size(); ++ifo1) {
291 ParticleFitObject* fo1 = fitobjects[ifo1];
292 assert(fo1);
293 for (unsigned int ifo2 = ifo1; ifo2 < fitobjects.size(); ++ifo2) {
294 ParticleFitObject* fo2 = fitobjects[ifo2];
295 assert(fo2);
296 for (int ilocal1 = 0; ilocal1 < fo1->getNPar(); ++ilocal1) {
297 int iglobal1 = fo1->getGlobalParNum(ilocal1);
298 for (int ilocal2 = (ifo1 == ifo2 ? ilocal1 : 0); ilocal2 < fo2->getNPar(); ++ilocal2) {
299 int iglobal2 = fo2->getGlobalParNum(ilocal2);
300 double calc = M[idim * iglobal1 + iglobal2];
301 double num = num2ndDerivative(ifo1, ilocal1, eps, ifo2, ilocal2, eps);
302 B2INFO("fo1: " << fo1->getName() << " par " << ilocal1 << "/"
303 << iglobal1 << " (" << fo1->getParamName(ilocal1)
304 << "), fo2: " << fo2->getName() << " par " << ilocal2 << "/"
305 << iglobal2 << " (" << fo2->getParamName(ilocal2)
306 << ") calc: " << calc << " - num: " << num << " = " << calc - num);
307 }
308 }
309 }
310 }
311 delete[] M;
312 }
313
314
315 double SoftGaussParticleConstraint::num1stDerivative(int ifo, int ilocal, double eps)
316 {
317 ParticleFitObject* fo = fitobjects[ifo];
318 assert(fo);
319 double save = fo->getParam(ilocal);
320 fo->setParam(ilocal, save + eps);
321 double v1 = getChi2();
322 fo->setParam(ilocal, save - eps);
323 double v2 = getChi2();
324 double result = (v1 - v2) / (2 * eps);
325 fo->setParam(ilocal, save);
326 return result;
327 }
328
329 double SoftGaussParticleConstraint::num2ndDerivative(int ifo1, int ilocal1, double eps1,
330 int ifo2, int ilocal2, double eps2)
331 {
332 double result;
333
334 if (ifo1 == ifo2 && ilocal1 == ilocal2) {
335 ParticleFitObject* fo = fitobjects[ifo1];
336 assert(fo);
337 double save = fo->getParam(ilocal1);
338 double v0 = getChi2();
339 fo->setParam(ilocal1, save + eps1);
340 double v1 = getChi2();
341 fo->setParam(ilocal1, save - eps1);
342 double v2 = getChi2();
343 result = (v1 + v2 - 2 * v0) / (eps1 * eps1);
344 fo->setParam(ilocal1, save);
345 } else {
346 ParticleFitObject* fo1 = fitobjects[ifo1];
347 assert(fo1);
348 ParticleFitObject* fo2 = fitobjects[ifo2];
349 assert(fo2);
350 double save1 = fo1->getParam(ilocal1);
351 double save2 = fo2->getParam(ilocal2);
352 fo1->setParam(ilocal1, save1 + eps1);
353 fo2->setParam(ilocal2, save2 + eps2);
354 double v11 = getChi2();
355 fo2->setParam(ilocal2, save2 - eps2);
356 double v12 = getChi2();
357 fo1->setParam(ilocal1, save1 - eps1);
358 double v22 = getChi2();
359 fo2->setParam(ilocal2, save2 + eps2);
360 double v21 = getChi2();
361 result = (v11 + v22 - v12 - v21) / (4 * eps1 * eps2);
362 fo1->setParam(ilocal1, save1);
363 fo2->setParam(ilocal2, save2);
364 }
365 return result;
366 }
367
368 int SoftGaussParticleConstraint::getVarBasis() const
369 {
370 return VAR_BASIS;
371 }
372 }// end OrcaKinFit namespace
374} // end Belle2 namespace
375
virtual const char * getName() const
Returns the name of the constraint.
virtual int getNPar() const =0
Get total number of parameters of this FitObject.
virtual const char * getParamName(int ilocal) const =0
Get name of parameter ilocal.
virtual bool setParam(int ilocal, double par_, bool measured_, bool fixed_=false)
Set value and measured flag of parameter i; return: significant change.
virtual int getGlobalParNum(int ilocal) const
Get global parameter number of parameter ilocal.
virtual const char * getName() const
Get object's name.
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 double getValue() const override=0
Returns the value of the constraint function.
FitObjectContainer fitobjects
The FitObjectContainer.
virtual void add2ndDerivativesToMatrix(double *M, int idim) const override
Adds second order derivatives to global covariance matrix M.
virtual double getError() const override
Returns the error on the value of the constraint.
virtual void addToGlobalChi2DerVector(double *y, int idim) const override
Add derivatives of chi squared to global derivative matrix.
virtual bool secondDerivatives(int i, int j, double *derivatives) const =0
Second derivatives with respect to the 4-vectors of Fit objects i and j; result false if all derivati...
virtual bool firstDerivatives(int i, double *derivatives) const =0
First derivatives with respect to the 4-vector of Fit objects i; result false if all derivatives are ...
void invalidateCache() const
Invalidates any cached values for the next event.
virtual double setSigma(double sigma_)
Sets the sigma.
virtual double getSigma() const
Returns the sigma.
SoftGaussParticleConstraint(double sigma_)
Creates an empty SoftGaussParticleConstraint object.
double num2ndDerivative(int ifo1, int ilocal1, double eps1, int ifo2, int ilocal2, double eps2)
Evaluates numerically the 2nd derivative w.r.t. 2 parameters.
virtual double getChi2() const override
Returns the chi2.
double num1stDerivative(int ifo, int ilocal, double eps)
Evaluates numerically the 1st derivative w.r.t. a parameter.
Abstract base class for different kinds of events.
Definition: ClusterUtils.h:24
STL namespace.