Belle II Software light-2406-ragdoll
BaseHardConstraint.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/BaseHardConstraint.h"
18#include <framework/logging/Logger.h>
19
20#undef NDEBUG
21#include <cassert>
22
23#include <cstring>
24#include <iostream>
25#include <cmath>
26
27namespace Belle2 {
32 namespace OrcaKinFit {
33
35
36
37 void BaseHardConstraint::add1stDerivativesToMatrix(double* M, int idim) const
38 {
39 double dgdpi[BaseDefs::MAXINTERVARS];
40 for (unsigned int i = 0; i < fitobjects.size(); ++i) {
41 const BaseFitObject* foi = fitobjects[i];
42 assert(foi);
43 if (firstDerivatives(i, dgdpi)) {
44 foi->addTo1stDerivatives(M, idim, dgdpi, getGlobalNum(), getVarBasis());
45 }
46 }
47 }
48
49
76 void BaseHardConstraint::add2ndDerivativesToMatrix(double* M, int idim, double lambda) const
77 {
78
85 // Derivatives \f$\frac{\partial ^2 g}{\partial P_i \partial P_j}\f$ at fixed i, j
86 // 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
87 double d2GdPidPj[BaseDefs::MAXINTERVARS * BaseDefs::MAXINTERVARS];
88
89 // Derivatives \f$\frac {\partial P_i}{\partial a_k}\f$ for all i;
90 // k is local parameter number
91 // dPidAk[KMAX*4*i + 4*k + ii] is \f$\frac {\partial P_{i,ii}}{\partial a_k}\f$,
92 // with ii=0, 1, 2, 3 for E, px, py, pz
93 const int n = fitobjects.size();
94 auto* dPidAk = new double[n * BaseDefs::MAXPAR * BaseDefs::MAXINTERVARS];
95 bool* dPidAkval = new bool[n];
96
97 for (int i = 0; i < n; ++i) dPidAkval[i] = false;
98
99 // Derivatives \f$\frac{\partial ^2 g}{\partial P_i \partial a_l}\f$ at fixed i
100 // d2GdPdAl[4*l + ii] is \f$\frac{\partial ^2 g}{\partial P_{i,ii} \partial a_l}\f$
101 double d2GdPdAl[static_cast<int>(BaseDefs::MAXINTERVARS) * BaseDefs::MAXPAR];
102 // Derivatives \f$\frac{\partial ^2 g}{\partial a_k \partial a_l}\f$
103 double d2GdAkdAl[BaseDefs::MAXPAR * BaseDefs::MAXPAR] = {0};
104
105 // Global parameter numbers: parglobal[BaseDefs::MAXPAR*i+klocal]
106 // is global parameter number of local parameter klocal of i-th Fit object
107 int* parglobal = new int[BaseDefs::MAXPAR * n];
108
109 for (int i = 0; i < n; ++i) {
110 const BaseFitObject* foi = fitobjects[i];
111 assert(foi);
112 for (int klocal = 0; klocal < foi->getNPar(); ++klocal) {
113 parglobal [BaseDefs::MAXPAR * i + klocal] = foi->getGlobalParNum(klocal);
114 }
115 }
116
117
118 for (int i = 0; i < n; ++i) {
119 const BaseFitObject* foi = fitobjects[i];
120 assert(foi);
121 for (int j = 0; j < n; ++j) {
122 const BaseFitObject* foj = fitobjects[j];
123 assert(foj);
124 if (secondDerivatives(i, j, d2GdPidPj)) {
125 if (!dPidAkval[i]) {
126 foi->getDerivatives(dPidAk + i * (static_cast<int>(BaseDefs::MAXPAR) * BaseDefs::MAXINTERVARS),
127 static_cast<int>(BaseDefs::MAXPAR) * BaseDefs::MAXINTERVARS);
128 dPidAkval[i] = true;
129 }
130 if (!dPidAkval[j]) {
131 foj->getDerivatives(dPidAk + j * (static_cast<int>(BaseDefs::MAXPAR) * BaseDefs::MAXINTERVARS),
132 static_cast<int>(BaseDefs::MAXPAR) * BaseDefs::MAXINTERVARS);
133 dPidAkval[j] = true;
134 }
135 // Now sum over E/px/Py/Pz for object j:
136 // \f[
137 // \frac{\partial ^2 g}{\partial P_{i,ii} \partial a_l}
138 // = (sum_{j}) sum_{jj} frac{\partial ^2 g}{\partial P_{i,ii} \partial P_{j,jj}}
139 // \cdot \frac{\partial P_{j,jj}}{\partial a_l}
140 // \f]
141 // We're summing over jj here
142 for (int llocal = 0; llocal < foj->getNPar(); ++llocal) {
143 for (int ii = 0; ii < BaseDefs::MAXINTERVARS; ++ii) {
144 int ind1 = BaseDefs::MAXINTERVARS * ii;
145 int ind2 = (static_cast<int>(BaseDefs::MAXPAR) * BaseDefs::MAXINTERVARS) * j + BaseDefs::MAXINTERVARS * llocal;
146 double& r = d2GdPdAl[BaseDefs::MAXINTERVARS * llocal + ii];
147 r = d2GdPidPj[ ind1] * dPidAk[ ind2]; // E
148 r += d2GdPidPj[++ind1] * dPidAk[++ind2]; // px
149 r += d2GdPidPj[++ind1] * dPidAk[++ind2]; // py
150 r += d2GdPidPj[++ind1] * dPidAk[++ind2]; // pz
151 }
152 }
153 // Now sum over E/px/Py/Pz for object i, i.e. sum over ii:
154 // \f[
155 // \frac{\partial ^2 g}{\partial a_k \partial a_l}
156 // = \sum_{ii} \frac{\partial ^2 g}{\partial P_{i,ii} \partial a_l} \cdot
157 // \frac{\partial P_{i,ii}}{\partial a_k}
158 // \f]
159 for (int klocal = 0; klocal < foi->getNPar(); ++klocal) {
160 for (int llocal = 0; llocal < foj->getNPar(); ++llocal) {
161 int ind1 = BaseDefs::MAXINTERVARS * llocal;
162 int ind2 = (static_cast<int>(BaseDefs::MAXPAR) * BaseDefs::MAXINTERVARS) * i + BaseDefs::MAXINTERVARS * klocal;
163 double& r = d2GdAkdAl[BaseDefs::MAXPAR * klocal + llocal];
164 r = d2GdPdAl[ ind1] * dPidAk[ ind2]; //E
165 r += d2GdPdAl[++ind1] * dPidAk[++ind2]; // px
166 r += d2GdPdAl[++ind1] * dPidAk[++ind2]; // py
167 r += d2GdPdAl[++ind1] * dPidAk[++ind2]; // pz
168 }
169 }
170 // Now expand the local parameter numbers to global ones
171 for (int klocal = 0; klocal < foi->getNPar(); ++klocal) {
172 int kglobal = parglobal [BaseDefs::MAXPAR * i + klocal];
173 for (int llocal = 0; llocal < foj->getNPar(); ++llocal) {
174 int lglobal = parglobal [BaseDefs::MAXPAR * j + llocal];
175 M [idim * kglobal + lglobal] += lambda * d2GdAkdAl[BaseDefs::MAXPAR * klocal + llocal];
176 }
177 }
178 }
179 }
180 }
190 double dgdpi[BaseDefs::MAXINTERVARS];
191 for (int i = 0; i < n; ++i) {
192 const BaseFitObject* foi = fitobjects[i];
193 assert(foi);
194 if (firstDerivatives(i, dgdpi)) {
195 foi->addTo2ndDerivatives(M, idim, lambda, dgdpi, getVarBasis());
196 }
197 }
198
199 delete[] dPidAk;
200 delete[] dPidAkval;
201 delete[] parglobal;
202 }
203
204 void BaseHardConstraint::addToGlobalChi2DerVector(double* y, int idim, double lambda) const
205 {
206 double dgdpi[BaseDefs::MAXINTERVARS];
207 for (unsigned int i = 0; i < fitobjects.size(); ++i) {
208 const BaseFitObject* foi = fitobjects[i];
209 assert(foi);
210 if (firstDerivatives(i, dgdpi)) {
211 foi->addToGlobalChi2DerVector(y, idim, lambda, dgdpi, getVarBasis());
212 }
213 }
214 }
215
216
218 {
219 double dgdpi[BaseDefs::MAXINTERVARS];
220 double error2 = 0;
221 for (unsigned int i = 0; i < fitobjects.size(); ++i) {
222 const BaseFitObject* foi = fitobjects[i];
223 assert(foi);
224 if (firstDerivatives(i, dgdpi)) {
225 error2 += foi->getError2(dgdpi, getVarBasis());
226 }
227 }
228 return std::sqrt(std::abs(error2));
229 }
230
231
232 double BaseHardConstraint::dirDer(double* p, double* w, int idim, double mu)
233 {
234 double* pw, *pp;
235 for (pw = w; pw < w + idim; * (pw++) = 0);
236 addToGlobalChi2DerVector(w, idim, mu);
237 double result = 0;
238 for (pw = w, pp = p; pw < w + idim; result += *(pp++) * *(pw++));
239 return mu * result;
240 }
241
242 double BaseHardConstraint::dirDerAbs(double* p, double* w, int idim, double mu)
243 {
244 double val = getValue();
245 if (val == 0) return mu * std::fabs(dirDer(p, w, idim, 1));
246 else if (val > 0) return mu * dirDer(p, w, idim, 1);
247 else return -mu * dirDer(p, w, idim, 1);
248 }
249
250
251 void BaseHardConstraint::test1stDerivatives()
252 {
253 B2INFO("BaseConstraint::test1stDerivatives for " << getName());
254 double y[100];
255 for (double& i : y) i = 0;
256 addToGlobalChi2DerVector(y, 100, 1);
257 double eps = 0.00001;
258 for (unsigned int ifo = 0; ifo < fitobjects.size(); ++ifo) {
259 BaseFitObject* fo = fitobjects[ifo];
260 assert(fo);
261 for (int ilocal = 0; ilocal < fo->getNPar(); ++ilocal) {
262 int iglobal = fo->getGlobalParNum(ilocal);
263 double calc = y[iglobal];
264 double num = num1stDerivative(ifo, ilocal, eps);
265 B2INFO("fo: " << fo->getName() << " par " << ilocal << "/"
266 << iglobal << " (" << fo->getParamName(ilocal)
267 << ") calc: " << calc << " - num: " << num << " = " << calc - num);
268 }
269 }
270 }
271
272 void BaseHardConstraint::test2ndDerivatives()
273 {
274 B2INFO("BaseConstraint::test2ndDerivatives for " << getName());
275 const int idim = 100;
276 auto* M = new double[idim * idim];
277 for (int i = 0; i < idim * idim; ++i) M[i] = 0;
278 add2ndDerivativesToMatrix(M, idim, 1);
279 double eps = 0.0001;
280 B2INFO("eps=" << eps);
281
282 for (unsigned int ifo1 = 0; ifo1 < fitobjects.size(); ++ifo1) {
283 BaseFitObject* fo1 = fitobjects[ifo1];
284 assert(fo1);
285 for (unsigned int ifo2 = ifo1; ifo2 < fitobjects.size(); ++ifo2) {
286 BaseFitObject* fo2 = fitobjects[ifo2];
287 assert(fo2);
288 for (int ilocal1 = 0; ilocal1 < fo1->getNPar(); ++ilocal1) {
289 int iglobal1 = fo1->getGlobalParNum(ilocal1);
290 for (int ilocal2 = (ifo1 == ifo2 ? ilocal1 : 0); ilocal2 < fo2->getNPar(); ++ilocal2) {
291 int iglobal2 = fo2->getGlobalParNum(ilocal2);
292 double calc = M[idim * iglobal1 + iglobal2];
293 double num = num2ndDerivative(ifo1, ilocal1, eps, ifo2, ilocal2, eps);
294 B2INFO("fo1: " << fo1->getName() << " par " << ilocal1 << "/"
295 << iglobal1 << " (" << fo1->getParamName(ilocal1)
296 << "), fo2: " << fo2->getName() << " par " << ilocal2 << "/"
297 << iglobal2 << " (" << fo2->getParamName(ilocal2)
298 << ") calc: " << calc << " - num: " << num << " = " << calc - num);
299 }
300 }
301 }
302 }
303 delete[] M;
304 }
305
306
307 double BaseHardConstraint::num1stDerivative(int ifo, int ilocal, double eps)
308 {
309 BaseFitObject* fo = fitobjects[ifo];
310 assert(fo);
311 double save = fo->getParam(ilocal);
312 fo->setParam(ilocal, save + eps);
313 double v1 = getValue();
314 fo->setParam(ilocal, save - eps);
315 double v2 = getValue();
316 double result = (v1 - v2) / (2 * eps);
317 fo->setParam(ilocal, save);
318 return result;
319 }
320
321 double BaseHardConstraint::num2ndDerivative(int ifo1, int ilocal1, double eps1,
322 int ifo2, int ilocal2, double eps2)
323 {
324 double result;
325
326 if (ifo1 == ifo2 && ilocal1 == ilocal2) {
327 BaseFitObject* fo = fitobjects[ifo1];
328 assert(fo);
329 double save = fo->getParam(ilocal1);
330 double v0 = getValue();
331 fo->setParam(ilocal1, save + eps1);
332 double v1 = getValue();
333 fo->setParam(ilocal1, save - eps1);
334 double v2 = getValue();
335 result = (v1 + v2 - 2 * v0) / (eps1 * eps1);
336 fo->setParam(ilocal1, save);
337 } else {
338 BaseFitObject* fo1 = fitobjects[ifo1];
339 assert(fo1);
340 BaseFitObject* fo2 = fitobjects[ifo2];
341 assert(fo2);
342 double save1 = fo1->getParam(ilocal1);
343 double save2 = fo2->getParam(ilocal2);
344 fo1->setParam(ilocal1, save1 + eps1);
345 fo2->setParam(ilocal2, save2 + eps2);
346 double v11 = getValue();
347 fo2->setParam(ilocal2, save2 - eps2);
348 double v12 = getValue();
349 fo1->setParam(ilocal1, save1 - eps1);
350 double v22 = getValue();
351 fo2->setParam(ilocal2, save2 + eps2);
352 double v21 = getValue();
353 result = (v11 + v22 - v12 - v21) / (4 * eps1 * eps2);
354 fo1->setParam(ilocal1, save1);
355 fo2->setParam(ilocal2, save2);
356 }
357 return result;
358 }
359
360 void BaseHardConstraint::printFirstDerivatives() const
361 {
362
363 B2INFO("BaseHardConstraint::printFirstDerivatives " << fitobjects.size());
364
365 double dgdpi[BaseDefs::MAXINTERVARS];
366 for (unsigned int i = 0; i < fitobjects.size(); ++i) {
367 const BaseFitObject* foi = fitobjects[i];
368 assert(foi);
369 if (firstDerivatives(i, dgdpi)) {
370 B2INFO("first derivs for obj " << i << " : ");
371 for (double j : dgdpi)
372 B2INFO(j << " ");
373 }
374 }
375
376 return;
377 }
378
379 void BaseHardConstraint::printSecondDerivatives() const
380 {
381
382 double d2GdPidPj[BaseDefs::MAXINTERVARS * BaseDefs::MAXINTERVARS];
383
384 int n = fitobjects.size();
385
386 for (int i = 0; i < n; ++i) {
387 const BaseFitObject* foi = fitobjects[i];
388 assert(foi);
389 for (int j = 0; j < n; ++j) {
390 const BaseFitObject* foj = fitobjects[j];
391 assert(foj);
392 if (secondDerivatives(i, j, d2GdPidPj)) {
393
394 B2INFO("second derivs for objs " << i << " " << j);
395
396 int k(0);
397 for (int k1 = 0; k1 < BaseDefs::MAXINTERVARS; k1++) {
398 for (int k2 = 0; k2 < BaseDefs::MAXINTERVARS; k2++) {
399 B2INFO(d2GdPidPj[k++] << " ");
400 }
401 }
402 }
403 }
404 }
405
406 return;
407 }
408
409 }// end OrcaKinFit namespace
411} // end Belle2 namespace
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 ~BaseHardConstraint()
Virtual destructor.
virtual int getGlobalNum() const
Accesses position of constraint in global constraint list.
virtual double getValue() const override=0
Returns the value of the constraint.
FitObjectContainer fitobjects
The FitObjectContainer.
virtual void add1stDerivativesToMatrix(double *M, int idim) const
Adds first 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, double lambda) const
Add lambda times derivatives of chi squared to global derivative vector.
virtual double dirDerAbs(double *p, double *w, int idim, double mu=1)
Calculate directional derivative for abs(c)
virtual double dirDer(double *p, double *w, int idim, double mu=1)
Calculate directional derivative.
virtual bool secondDerivatives(int i, int j, double *derivatives) const =0
Second derivatives with respect to the meta-variables of Fit objects i and j; result false if all der...
virtual bool firstDerivatives(int i, double *derivatives) const =0
First derivatives with respect to the meta-variables of Fit objects i; result false if all derivative...
virtual void add2ndDerivativesToMatrix(double *M, int idim, double lambda) const
Adds second order derivatives to global covariance matrix M.
virtual 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 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