Belle II Software  release-05-02-19
BaseHardConstraint.cc
1 /**************************************************************************
2  * BASF2 (Belle Analysis Framework 2) *
3  * See https://github.com/tferber/OrcaKinfit, forked from *
4  * https://github.com/iLCSoft/MarlinKinfit *
5  * *
6  * Further information about the fit engine and the user interface *
7  * provided in MarlinKinfit can be found at *
8  * https://www.desy.de/~blist/kinfit/doc/html/ *
9  * and in the LCNotes LC-TOOL-2009-001 and LC-TOOL-2009-004 available *
10  * from http://www-flc.desy.de/lcnotes/ *
11  * *
12  * Adopted by: Torben Ferber (torben.ferber@desy.de) (TF) *
13  * *
14  * This software is provided "as is" without any warranty. *
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 
27 namespace Belle2 {
32  namespace OrcaKinFit {
33 
34  BaseHardConstraint::~BaseHardConstraint() = default;
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 
75  void BaseHardConstraint::add2ndDerivativesToMatrix(double* M, int idim, double lambda) const
76  {
77 
84  // Derivatives $\frac{\partial ^2 g}{\partial P_i \partial P_j}$ at fixed i, j
85  // 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
86  double d2GdPidPj[BaseDefs::MAXINTERVARS * BaseDefs::MAXINTERVARS];
87 
88  // Derivatives $\frac {\partial P_i}{\partial a_k}$ for all i;
89  // k is local parameter number
90  // dPidAk[KMAX*4*i + 4*k + ii] is $\frac {\partial P_{i,ii}}{\partial a_k}$,
91  // with ii=0, 1, 2, 3 for E, px, py, pz
92  const int n = fitobjects.size();
93  auto* dPidAk = new double[n * BaseDefs::MAXPAR * BaseDefs::MAXINTERVARS];
94  bool* dPidAkval = new bool[n];
95 
96  for (int i = 0; i < n; ++i) dPidAkval[i] = false;
97 
98  // Derivatives $\frac{\partial ^2 g}{\partial P_i \partial a_l}$ at fixed i
99  // d2GdPdAl[4*l + ii] is $\frac{\partial ^2 g}{\partial P_{i,ii} \partial a_l}$
100  double d2GdPdAl[BaseDefs::MAXINTERVARS * BaseDefs::MAXPAR];
101  // Derivatives $\frac{\partial ^2 g}{\partial a_k \partial a_l}$
102  double d2GdAkdAl[BaseDefs::MAXPAR * BaseDefs::MAXPAR] = {0};
103 
104  // Global parameter numbers: parglobal[BaseDefs::MAXPAR*i+klocal]
105  // is global parameter number of local parameter klocal of i-th Fit object
106  int* parglobal = new int[BaseDefs::MAXPAR * n];
107 
108  for (int i = 0; i < n; ++i) {
109  const BaseFitObject* foi = fitobjects[i];
110  assert(foi);
111  for (int klocal = 0; klocal < foi->getNPar(); ++klocal) {
112  parglobal [BaseDefs::MAXPAR * i + klocal] = foi->getGlobalParNum(klocal);
113  }
114  }
115 
116 
117  for (int i = 0; i < n; ++i) {
118  const BaseFitObject* foi = fitobjects[i];
119  assert(foi);
120  for (int j = 0; j < n; ++j) {
121  const BaseFitObject* foj = fitobjects[j];
122  assert(foj);
123  if (secondDerivatives(i, j, d2GdPidPj)) {
124  if (!dPidAkval[i]) {
125  foi->getDerivatives(dPidAk + i * (BaseDefs::MAXPAR * BaseDefs::MAXINTERVARS), BaseDefs::MAXPAR * BaseDefs::MAXINTERVARS);
126  dPidAkval[i] = true;
127  }
128  if (!dPidAkval[j]) {
129  foj->getDerivatives(dPidAk + j * (BaseDefs::MAXPAR * BaseDefs::MAXINTERVARS), BaseDefs::MAXPAR * BaseDefs::MAXINTERVARS);
130  dPidAkval[j] = true;
131  }
132  // Now sum over E/px/Py/Pz for object j:
133  // $$\frac{\partial ^2 g}{\partial P_{i,ii} \partial a_l}
134  // = (sum_{j}) sum_{jj} frac{\partial ^2 g}{\partial P_{i,ii} \partial P_{j,jj}}
135  // \cdot \frac{\partial P_{j,jj}}{\partial a_l}
136  // We're summing over jj here
137  for (int llocal = 0; llocal < foj->getNPar(); ++llocal) {
138  for (int ii = 0; ii < BaseDefs::MAXINTERVARS; ++ii) {
139  int ind1 = BaseDefs::MAXINTERVARS * ii;
140  int ind2 = (BaseDefs::MAXPAR * BaseDefs::MAXINTERVARS) * j + BaseDefs::MAXINTERVARS * llocal;
141  double& r = d2GdPdAl[BaseDefs::MAXINTERVARS * llocal + ii];
142  r = d2GdPidPj[ ind1] * dPidAk[ ind2]; // E
143  r += d2GdPidPj[++ind1] * dPidAk[++ind2]; // px
144  r += d2GdPidPj[++ind1] * dPidAk[++ind2]; // py
145  r += d2GdPidPj[++ind1] * dPidAk[++ind2]; // pz
146  }
147  }
148  // Now sum over E/px/Py/Pz for object i, i.e. sum over ii:
149  // $$
150  // \frac{\partial ^2 g}{\partial a_k \partial a_l}
151  // = \sum_{ii} \frac{\partial ^2 g}{\partial P_{i,ii} \partial a_l} \cdot
152  // \frac{\partial P_{i,ii}}{\partial a_k}
153  // $$
154  for (int klocal = 0; klocal < foi->getNPar(); ++klocal) {
155  for (int llocal = 0; llocal < foj->getNPar(); ++llocal) {
156  int ind1 = BaseDefs::MAXINTERVARS * llocal;
157  int ind2 = (BaseDefs::MAXPAR * BaseDefs::MAXINTERVARS) * i + BaseDefs::MAXINTERVARS * klocal;
158  double& r = d2GdAkdAl[BaseDefs::MAXPAR * klocal + llocal];
159  r = d2GdPdAl[ ind1] * dPidAk[ ind2]; //E
160  r += d2GdPdAl[++ind1] * dPidAk[++ind2]; // px
161  r += d2GdPdAl[++ind1] * dPidAk[++ind2]; // py
162  r += d2GdPdAl[++ind1] * dPidAk[++ind2]; // pz
163  }
164  }
165  // Now expand the local parameter numbers to global ones
166  for (int klocal = 0; klocal < foi->getNPar(); ++klocal) {
167  int kglobal = parglobal [BaseDefs::MAXPAR * i + klocal];
168  for (int llocal = 0; llocal < foj->getNPar(); ++llocal) {
169  int lglobal = parglobal [BaseDefs::MAXPAR * j + llocal];
170  M [idim * kglobal + lglobal] += lambda * d2GdAkdAl[BaseDefs::MAXPAR * klocal + llocal];
171  }
172  }
173  }
174  }
175  }
185  double dgdpi[BaseDefs::MAXINTERVARS];
186  for (int i = 0; i < n; ++i) {
187  const BaseFitObject* foi = fitobjects[i];
188  assert(foi);
189  if (firstDerivatives(i, dgdpi)) {
190  foi->addTo2ndDerivatives(M, idim, lambda, dgdpi, getVarBasis());
191  }
192  }
193 
194  delete[] dPidAk;
195  delete[] dPidAkval;
196  delete[] parglobal;
197  }
198 
199  void BaseHardConstraint::addToGlobalChi2DerVector(double* y, int idim, double lambda) const
200  {
201  double dgdpi[BaseDefs::MAXINTERVARS];
202  for (unsigned int i = 0; i < fitobjects.size(); ++i) {
203  const BaseFitObject* foi = fitobjects[i];
204  assert(foi);
205  if (firstDerivatives(i, dgdpi)) {
206  foi->addToGlobalChi2DerVector(y, idim, lambda, dgdpi, getVarBasis());
207  }
208  }
209  }
210 
211 
212  double BaseHardConstraint::getError() const
213  {
214  double dgdpi[BaseDefs::MAXINTERVARS];
215  double error2 = 0;
216  for (unsigned int i = 0; i < fitobjects.size(); ++i) {
217  const BaseFitObject* foi = fitobjects[i];
218  assert(foi);
219  if (firstDerivatives(i, dgdpi)) {
220  error2 += foi->getError2(dgdpi, getVarBasis());
221  }
222  }
223  return std::sqrt(std::abs(error2));
224  }
225 
226 
227  double BaseHardConstraint::dirDer(double* p, double* w, int idim, double mu)
228  {
229  double* pw, *pp;
230  for (pw = w; pw < w + idim; * (pw++) = 0);
231  addToGlobalChi2DerVector(w, idim, mu);
232  double result = 0;
233  for (pw = w, pp = p; pw < w + idim; result += *(pp++) * *(pw++));
234  return mu * result;
235  }
236 
237  double BaseHardConstraint::dirDerAbs(double* p, double* w, int idim, double mu)
238  {
239  double val = getValue();
240  if (val == 0) return mu * std::fabs(dirDer(p, w, idim, 1));
241  else if (val > 0) return mu * dirDer(p, w, idim, 1);
242  else return -mu * dirDer(p, w, idim, 1);
243  }
244 
245 
246  void BaseHardConstraint::test1stDerivatives()
247  {
248  B2INFO("BaseConstraint::test1stDerivatives for " << getName());
249  double y[100];
250  for (double& i : y) i = 0;
251  addToGlobalChi2DerVector(y, 100, 1);
252  double eps = 0.00001;
253  for (unsigned int ifo = 0; ifo < fitobjects.size(); ++ifo) {
254  BaseFitObject* fo = fitobjects[ifo];
255  assert(fo);
256  for (int ilocal = 0; ilocal < fo->getNPar(); ++ilocal) {
257  int iglobal = fo->getGlobalParNum(ilocal);
258  double calc = y[iglobal];
259  double num = num1stDerivative(ifo, ilocal, eps);
260  B2INFO("fo: " << fo->getName() << " par " << ilocal << "/"
261  << iglobal << " (" << fo->getParamName(ilocal)
262  << ") calc: " << calc << " - num: " << num << " = " << calc - num);
263  }
264  }
265  }
266 
267  void BaseHardConstraint::test2ndDerivatives()
268  {
269  B2INFO("BaseConstraint::test2ndDerivatives for " << getName());
270  const int idim = 100;
271  auto* M = new double[idim * idim];
272  for (int i = 0; i < idim * idim; ++i) M[i] = 0;
273  add2ndDerivativesToMatrix(M, idim, 1);
274  double eps = 0.0001;
275  B2INFO("eps=" << eps);
276 
277  for (unsigned int ifo1 = 0; ifo1 < fitobjects.size(); ++ifo1) {
278  BaseFitObject* fo1 = fitobjects[ifo1];
279  assert(fo1);
280  for (unsigned int ifo2 = ifo1; ifo2 < fitobjects.size(); ++ifo2) {
281  BaseFitObject* fo2 = fitobjects[ifo2];
282  assert(fo2);
283  for (int ilocal1 = 0; ilocal1 < fo1->getNPar(); ++ilocal1) {
284  int iglobal1 = fo1->getGlobalParNum(ilocal1);
285  for (int ilocal2 = (ifo1 == ifo2 ? ilocal1 : 0); ilocal2 < fo2->getNPar(); ++ilocal2) {
286  int iglobal2 = fo2->getGlobalParNum(ilocal2);
287  double calc = M[idim * iglobal1 + iglobal2];
288  double num = num2ndDerivative(ifo1, ilocal1, eps, ifo2, ilocal2, eps);
289  B2INFO("fo1: " << fo1->getName() << " par " << ilocal1 << "/"
290  << iglobal1 << " (" << fo1->getParamName(ilocal1)
291  << "), fo2: " << fo2->getName() << " par " << ilocal2 << "/"
292  << iglobal2 << " (" << fo2->getParamName(ilocal2)
293  << ") calc: " << calc << " - num: " << num << " = " << calc - num);
294  }
295  }
296  }
297  }
298  delete[] M;
299  }
300 
301 
302  double BaseHardConstraint::num1stDerivative(int ifo, int ilocal, double eps)
303  {
304  BaseFitObject* fo = fitobjects[ifo];
305  assert(fo);
306  double save = fo->getParam(ilocal);
307  fo->setParam(ilocal, save + eps);
308  double v1 = getValue();
309  fo->setParam(ilocal, save - eps);
310  double v2 = getValue();
311  double result = (v1 - v2) / (2 * eps);
312  fo->setParam(ilocal, save);
313  return result;
314  }
315 
316  double BaseHardConstraint::num2ndDerivative(int ifo1, int ilocal1, double eps1,
317  int ifo2, int ilocal2, double eps2)
318  {
319  double result;
320 
321  if (ifo1 == ifo2 && ilocal1 == ilocal2) {
322  BaseFitObject* fo = fitobjects[ifo1];
323  assert(fo);
324  double save = fo->getParam(ilocal1);
325  double v0 = getValue();
326  fo->setParam(ilocal1, save + eps1);
327  double v1 = getValue();
328  fo->setParam(ilocal1, save - eps1);
329  double v2 = getValue();
330  result = (v1 + v2 - 2 * v0) / (eps1 * eps1);
331  fo->setParam(ilocal1, save);
332  } else {
333  BaseFitObject* fo1 = fitobjects[ifo1];
334  assert(fo1);
335  BaseFitObject* fo2 = fitobjects[ifo2];
336  assert(fo2);
337  double save1 = fo1->getParam(ilocal1);
338  double save2 = fo2->getParam(ilocal2);
339  fo1->setParam(ilocal1, save1 + eps1);
340  fo2->setParam(ilocal2, save2 + eps2);
341  double v11 = getValue();
342  fo2->setParam(ilocal2, save2 - eps2);
343  double v12 = getValue();
344  fo1->setParam(ilocal1, save1 - eps1);
345  double v22 = getValue();
346  fo2->setParam(ilocal2, save2 + eps2);
347  double v21 = getValue();
348  result = (v11 + v22 - v12 - v21) / (4 * eps1 * eps2);
349  fo1->setParam(ilocal1, save1);
350  fo2->setParam(ilocal2, save2);
351  }
352  return result;
353  }
354 
355  void BaseHardConstraint::printFirstDerivatives() const
356  {
357 
358  B2INFO("BaseHardConstraint::printFirstDerivatives " << fitobjects.size());
359 
360  double dgdpi[BaseDefs::MAXINTERVARS];
361  for (unsigned int i = 0; i < fitobjects.size(); ++i) {
362  const BaseFitObject* foi = fitobjects[i];
363  assert(foi);
364  if (firstDerivatives(i, dgdpi)) {
365  B2INFO("first derivs for obj " << i << " : ");
366  for (double j : dgdpi)
367  B2INFO(j << " ");
368  }
369  }
370 
371  return;
372  }
373 
374  void BaseHardConstraint::printSecondDerivatives() const
375  {
376 
377  double d2GdPidPj[BaseDefs::MAXINTERVARS * BaseDefs::MAXINTERVARS];
378 
379  int n = fitobjects.size();
380 
381  for (int i = 0; i < n; ++i) {
382  const BaseFitObject* foi = fitobjects[i];
383  assert(foi);
384  for (int j = 0; j < n; ++j) {
385  const BaseFitObject* foj = fitobjects[j];
386  assert(foj);
387  if (secondDerivatives(i, j, d2GdPidPj)) {
388 
389  B2INFO("second derivs for objs " << i << " " << j);
390 
391  int k(0);
392  for (int k1 = 0; k1 < BaseDefs::MAXINTERVARS; k1++) {
393  for (int k2 = 0; k2 < BaseDefs::MAXINTERVARS; k2++) {
394  B2INFO(d2GdPidPj[k++] << " ");
395  }
396  }
397  }
398  }
399  }
400 
401  return;
402  }
403 
404  }// end OrcaKinFit namespace
406 } // end Belle2 namespace
VXDTFFilterTest::v2
const std::vector< double > v2
MATLAB generated random vector.
Definition: decorrelationMatrix.cc:44
Belle2::OrcaKinFit::BaseConstraint::getName
virtual const char * getName() const
Returns the name of the constraint.
Definition: BaseConstraint.cc:70
Belle2::OrcaKinFit::BaseHardConstraint::add2ndDerivativesToMatrix
virtual void add2ndDerivativesToMatrix(double *M, int idim, double lambda) const
Adds second order derivatives to global covariance matrix M.
Definition: BaseHardConstraint.cc:89
VXDTFFilterTest::v1
const std::vector< double > v1
MATLAB generated random vector.
Definition: decorrelationMatrix.cc:30
Belle2::OrcaKinFit::BaseHardConstraint::num1stDerivative
virtual double num1stDerivative(int ifo, int ilocal, double eps)
Evaluates numerically the 1st derivative w.r.t. a parameter.
Definition: BaseHardConstraint.cc:316
Belle2::OrcaKinFit::BaseHardConstraint::addToGlobalChi2DerVector
virtual void addToGlobalChi2DerVector(double *y, int idim, double lambda) const
Add lambda times derivatives of chi squared to global derivative vector.
Definition: BaseHardConstraint.cc:213
Belle2::OrcaKinFit::BaseHardConstraint::getError
virtual double getError() const override
Returns the error on the value of the constraint.
Definition: BaseHardConstraint.cc:226
Belle2::OrcaKinFit::BaseHardConstraint::fitobjects
FitObjectContainer fitobjects
The FitObjectContainer.
Definition: BaseHardConstraint.h:191
Belle2::OrcaKinFit::BaseHardConstraint::num2ndDerivative
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.
Definition: BaseHardConstraint.cc:330
Belle2::OrcaKinFit::BaseHardConstraint::getGlobalNum
virtual int getGlobalNum() const
Accesses position of constraint in global constraint list.
Definition: BaseHardConstraint.h:151
Belle2
Abstract base class for different kinds of events.
Definition: MillepedeAlgorithm.h:19
Belle2::OrcaKinFit::BaseHardConstraint::secondDerivatives
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...
Belle2::OrcaKinFit::BaseHardConstraint::dirDer
virtual double dirDer(double *p, double *w, int idim, double mu=1)
Calculate directional derivative.
Definition: BaseHardConstraint.cc:241
Belle2::OrcaKinFit::BaseHardConstraint::firstDerivatives
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...
Belle2::OrcaKinFit::BaseHardConstraint::add1stDerivativesToMatrix
virtual void add1stDerivativesToMatrix(double *M, int idim) const
Adds first order derivatives to global covariance matrix M.
Definition: BaseHardConstraint.cc:51
Belle2::OrcaKinFit::BaseHardConstraint::~BaseHardConstraint
virtual ~BaseHardConstraint()
Virtual destructor.
Belle2::OrcaKinFit::BaseHardConstraint::getValue
virtual double getValue() const override=0
Returns the value of the constraint.
Belle2::OrcaKinFit::BaseHardConstraint::dirDerAbs
virtual double dirDerAbs(double *p, double *w, int idim, double mu=1)
Calculate directional derivative for abs(c)
Definition: BaseHardConstraint.cc:251