| File: | analysis/OrcaKinFit/src/BaseHardConstraint.cc |
| Warning: | line 170, column 17 Assigned value is uninitialized |
Press '?' to see keyboard shortcuts
Keyboard shortcuts:
| 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 | ||||
| 27 | namespace Belle2 { | |||
| 28 | namespace OrcaKinFit { | |||
| 29 | ||||
| 30 | BaseHardConstraint::~BaseHardConstraint() = default; | |||
| 31 | ||||
| 32 | ||||
| 33 | void BaseHardConstraint::add1stDerivativesToMatrix(double* M, int idim) const | |||
| 34 | { | |||
| 35 | double dgdpi[BaseDefs::MAXINTERVARS]; | |||
| 36 | for (unsigned int i = 0; i < fitobjects.size(); ++i) { | |||
| 37 | const BaseFitObject* foi = fitobjects[i]; | |||
| 38 | assert(foi)(static_cast <bool> (foi) ? void (0) : __assert_fail ("foi" , "analysis/OrcaKinFit/src/BaseHardConstraint.cc", 38, __extension__ __PRETTY_FUNCTION__)); | |||
| 39 | if (firstDerivatives(i, dgdpi)) { | |||
| 40 | foi->addTo1stDerivatives(M, idim, dgdpi, getGlobalNum(), getVarBasis()); | |||
| 41 | } | |||
| 42 | } | |||
| 43 | } | |||
| 44 | ||||
| 45 | ||||
| 46 | /** | |||
| 47 | * Calculates the second derivative of the constraint g w.r.t. the various parameters, | |||
| 48 | * multiplies it by lambda and adds it to the global covariance matrix | |||
| 49 | * | |||
| 50 | * in case of particlefitobject: | |||
| 51 | * We denote with P_i the 4-vector of the i-th ParticleFitObject, | |||
| 52 | * then | |||
| 53 | * \f[ | |||
| 54 | * \frac{\partial ^2 g}{\partial a_k \partial a_l} | |||
| 55 | * = \sum_i \sum_j \frac{\partial ^2 g}{\partial P_i \partial P_j} \cdot | |||
| 56 | * \frac{\partial P_i}{\partial a_k} \cdot \frac{\partial P_j}{\partial a_l} | |||
| 57 | * + \sum_i \frac{\partial g}{\partial P_i} \cdot | |||
| 58 | * \frac{\partial^2 P_i}{\partial a_k \partial a_l} | |||
| 59 | * \f] | |||
| 60 | * Here, \f$\frac{\partial P_i}{\partial a_k}\f$ is a \f$4 \times n_i\f$ Matrix, where | |||
| 61 | * \f$n_i\f$ is the number of parameters of FitObject i; | |||
| 62 | * Correspondingly, \f$\frac{\partial^2 P_i}{\partial a_k \partial a_l}\f$ is a | |||
| 63 | * \f$4 \times n_i \times n_i\f$ matrix. | |||
| 64 | * Also, \f$\frac{\partial ^2 g}{\partial P_i \partial P_j}\f$ is a \f$4\times 4\f$ matrix | |||
| 65 | * for a given i and j, and \f$\frac{\partial g}{\partial P_i}\f$ is a 4-vector | |||
| 66 | * (though not a Lorentz-vector!). | |||
| 67 | * | |||
| 68 | * but here it's been generalised | |||
| 69 | */ | |||
| 70 | ||||
| 71 | ||||
| 72 | void BaseHardConstraint::add2ndDerivativesToMatrix(double* M, int idim, double lambda) const | |||
| 73 | { | |||
| 74 | ||||
| 75 | /** First, treat the part | |||
| 76 | * \f[ | |||
| 77 | * \frac{\partial ^2 g}{\partial P_i \partial P_j} \cdot | |||
| 78 | * \frac{\partial P_i}{\partial a_k} \cdot \frac{\partial P_j}{\partial a_l} | |||
| 79 | * \f] | |||
| 80 | */ | |||
| 81 | // Derivatives \f$\frac{\partial ^2 g}{\partial P_i \partial P_j}\f$ at fixed i, j | |||
| 82 | // 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 | |||
| 83 | double d2GdPidPj[BaseDefs::MAXINTERVARS * BaseDefs::MAXINTERVARS]; | |||
| 84 | ||||
| 85 | // Derivatives \f$\frac {\partial P_i}{\partial a_k}\f$ for all i; | |||
| 86 | // k is local parameter number | |||
| 87 | // dPidAk[KMAX*4*i + 4*k + ii] is \f$\frac {\partial P_{i,ii}}{\partial a_k}\f$, | |||
| 88 | // with ii=0, 1, 2, 3 for E, px, py, pz | |||
| 89 | const int n = fitobjects.size(); | |||
| 90 | auto* dPidAk = new double[n * BaseDefs::MAXPAR * BaseDefs::MAXINTERVARS]; | |||
| 91 | bool* dPidAkval = new bool[n]; | |||
| 92 | ||||
| 93 | for (int i = 0; i
| |||
| ||||
| 94 | ||||
| 95 | // Derivatives \f$\frac{\partial ^2 g}{\partial P_i \partial a_l}\f$ at fixed i | |||
| 96 | // d2GdPdAl[4*l + ii] is \f$\frac{\partial ^2 g}{\partial P_{i,ii} \partial a_l}\f$ | |||
| 97 | double d2GdPdAl[static_cast<int>(BaseDefs::MAXINTERVARS) * BaseDefs::MAXPAR]; | |||
| 98 | // Derivatives \f$\frac{\partial ^2 g}{\partial a_k \partial a_l}\f$ | |||
| 99 | double d2GdAkdAl[BaseDefs::MAXPAR * BaseDefs::MAXPAR] = {0}; | |||
| 100 | ||||
| 101 | // Global parameter numbers: parglobal[BaseDefs::MAXPAR*i+klocal] | |||
| 102 | // is global parameter number of local parameter klocal of i-th Fit object | |||
| 103 | int* parglobal = new int[BaseDefs::MAXPAR * n]; | |||
| 104 | ||||
| 105 | for (int i = 0; i < n; ++i) { | |||
| 106 | const BaseFitObject* foi = fitobjects[i]; | |||
| 107 | assert(foi)(static_cast <bool> (foi) ? void (0) : __assert_fail ("foi" , "analysis/OrcaKinFit/src/BaseHardConstraint.cc", 107, __extension__ __PRETTY_FUNCTION__)); | |||
| 108 | for (int klocal = 0; klocal < foi->getNPar(); ++klocal) { | |||
| 109 | parglobal [BaseDefs::MAXPAR * i + klocal] = foi->getGlobalParNum(klocal); | |||
| 110 | } | |||
| 111 | } | |||
| 112 | ||||
| 113 | ||||
| 114 | for (int i = 0; i < n; ++i) { | |||
| 115 | const BaseFitObject* foi = fitobjects[i]; | |||
| 116 | assert(foi)(static_cast <bool> (foi) ? void (0) : __assert_fail ("foi" , "analysis/OrcaKinFit/src/BaseHardConstraint.cc", 116, __extension__ __PRETTY_FUNCTION__)); | |||
| 117 | for (int j = 0; j < n; ++j) { | |||
| 118 | const BaseFitObject* foj = fitobjects[j]; | |||
| 119 | assert(foj)(static_cast <bool> (foj) ? void (0) : __assert_fail ("foj" , "analysis/OrcaKinFit/src/BaseHardConstraint.cc", 119, __extension__ __PRETTY_FUNCTION__)); | |||
| 120 | if (secondDerivatives(i, j, d2GdPidPj)) { | |||
| 121 | if (!dPidAkval[i]) { | |||
| 122 | foi->getDerivatives(dPidAk + i * (static_cast<int>(BaseDefs::MAXPAR) * BaseDefs::MAXINTERVARS), | |||
| 123 | static_cast<int>(BaseDefs::MAXPAR) * BaseDefs::MAXINTERVARS); | |||
| 124 | dPidAkval[i] = true; | |||
| 125 | } | |||
| 126 | if (!dPidAkval[j]) { | |||
| 127 | foj->getDerivatives(dPidAk + j * (static_cast<int>(BaseDefs::MAXPAR) * BaseDefs::MAXINTERVARS), | |||
| 128 | static_cast<int>(BaseDefs::MAXPAR) * BaseDefs::MAXINTERVARS); | |||
| 129 | dPidAkval[j] = true; | |||
| 130 | } | |||
| 131 | // Now sum over E/px/Py/Pz for object j: | |||
| 132 | // \f[ | |||
| 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 | // \f] | |||
| 137 | // We're summing over jj here | |||
| 138 | for (int llocal = 0; llocal < foj->getNPar(); ++llocal) { | |||
| 139 | for (int ii = 0; ii < BaseDefs::MAXINTERVARS; ++ii) { | |||
| 140 | int ind1 = BaseDefs::MAXINTERVARS * ii; | |||
| 141 | int ind2 = (static_cast<int>(BaseDefs::MAXPAR) * BaseDefs::MAXINTERVARS) * j + BaseDefs::MAXINTERVARS * llocal; | |||
| 142 | double& r = d2GdPdAl[BaseDefs::MAXINTERVARS * llocal + ii]; | |||
| 143 | r = d2GdPidPj[ ind1] * dPidAk[ ind2]; // E | |||
| 144 | r += d2GdPidPj[++ind1] * dPidAk[++ind2]; // px | |||
| 145 | r += d2GdPidPj[++ind1] * dPidAk[++ind2]; // py | |||
| 146 | r += d2GdPidPj[++ind1] * dPidAk[++ind2]; // pz | |||
| 147 | } | |||
| 148 | } | |||
| 149 | // Now sum over E/px/Py/Pz for object i, i.e. sum over ii: | |||
| 150 | // \f[ | |||
| 151 | // \frac{\partial ^2 g}{\partial a_k \partial a_l} | |||
| 152 | // = \sum_{ii} \frac{\partial ^2 g}{\partial P_{i,ii} \partial a_l} \cdot | |||
| 153 | // \frac{\partial P_{i,ii}}{\partial a_k} | |||
| 154 | // \f] | |||
| 155 | for (int klocal = 0; klocal < foi->getNPar(); ++klocal) { | |||
| 156 | for (int llocal = 0; llocal < foj->getNPar(); ++llocal) { | |||
| 157 | int ind1 = BaseDefs::MAXINTERVARS * llocal; | |||
| 158 | int ind2 = (static_cast<int>(BaseDefs::MAXPAR) * BaseDefs::MAXINTERVARS) * i + BaseDefs::MAXINTERVARS * klocal; | |||
| 159 | double& r = d2GdAkdAl[BaseDefs::MAXPAR * klocal + llocal]; | |||
| 160 | r = d2GdPdAl[ ind1] * dPidAk[ ind2]; //E | |||
| 161 | r += d2GdPdAl[++ind1] * dPidAk[++ind2]; // px | |||
| 162 | r += d2GdPdAl[++ind1] * dPidAk[++ind2]; // py | |||
| 163 | r += d2GdPdAl[++ind1] * dPidAk[++ind2]; // pz | |||
| 164 | } | |||
| 165 | } | |||
| 166 | // Now expand the local parameter numbers to global ones | |||
| 167 | for (int klocal = 0; klocal < foi->getNPar(); ++klocal) { | |||
| 168 | int kglobal = parglobal [BaseDefs::MAXPAR * i + klocal]; | |||
| 169 | for (int llocal = 0; llocal < foj->getNPar(); ++llocal) { | |||
| 170 | int lglobal = parglobal [BaseDefs::MAXPAR * j + llocal]; | |||
| ||||
| 171 | M [idim * kglobal + lglobal] += lambda * d2GdAkdAl[BaseDefs::MAXPAR * klocal + llocal]; | |||
| 172 | } | |||
| 173 | } | |||
| 174 | } | |||
| 175 | } | |||
| 176 | } | |||
| 177 | /** Second, treat the part | |||
| 178 | * \f[ | |||
| 179 | * \sum_i \frac{\partial g}{\partial P_i} \cdot | |||
| 180 | * \frac{\partial^2 P_i}{\partial a_k \partial a_l} | |||
| 181 | * \f] | |||
| 182 | * Here, \f$\frac{\partial g}{\partial P_i}\f$ is a 4-vector, which we pass on to | |||
| 183 | * the FitObject | |||
| 184 | */ | |||
| 185 | ||||
| 186 | double dgdpi[BaseDefs::MAXINTERVARS]; | |||
| 187 | for (int i = 0; i < n; ++i) { | |||
| 188 | const BaseFitObject* foi = fitobjects[i]; | |||
| 189 | assert(foi)(static_cast <bool> (foi) ? void (0) : __assert_fail ("foi" , "analysis/OrcaKinFit/src/BaseHardConstraint.cc", 189, __extension__ __PRETTY_FUNCTION__)); | |||
| 190 | if (firstDerivatives(i, dgdpi)) { | |||
| 191 | foi->addTo2ndDerivatives(M, idim, lambda, dgdpi, getVarBasis()); | |||
| 192 | } | |||
| 193 | } | |||
| 194 | ||||
| 195 | delete[] dPidAk; | |||
| 196 | delete[] dPidAkval; | |||
| 197 | delete[] parglobal; | |||
| 198 | } | |||
| 199 | ||||
| 200 | void BaseHardConstraint::addToGlobalChi2DerVector(double* y, int idim, double lambda) const | |||
| 201 | { | |||
| 202 | double dgdpi[BaseDefs::MAXINTERVARS]; | |||
| 203 | for (unsigned int i = 0; i < fitobjects.size(); ++i) { | |||
| 204 | const BaseFitObject* foi = fitobjects[i]; | |||
| 205 | assert(foi)(static_cast <bool> (foi) ? void (0) : __assert_fail ("foi" , "analysis/OrcaKinFit/src/BaseHardConstraint.cc", 205, __extension__ __PRETTY_FUNCTION__)); | |||
| 206 | if (firstDerivatives(i, dgdpi)) { | |||
| 207 | foi->addToGlobalChi2DerVector(y, idim, lambda, dgdpi, getVarBasis()); | |||
| 208 | } | |||
| 209 | } | |||
| 210 | } | |||
| 211 | ||||
| 212 | ||||
| 213 | double BaseHardConstraint::getError() const | |||
| 214 | { | |||
| 215 | double dgdpi[BaseDefs::MAXINTERVARS]; | |||
| 216 | double error2 = 0; | |||
| 217 | for (unsigned int i = 0; i < fitobjects.size(); ++i) { | |||
| 218 | const BaseFitObject* foi = fitobjects[i]; | |||
| 219 | assert(foi)(static_cast <bool> (foi) ? void (0) : __assert_fail ("foi" , "analysis/OrcaKinFit/src/BaseHardConstraint.cc", 219, __extension__ __PRETTY_FUNCTION__)); | |||
| 220 | if (firstDerivatives(i, dgdpi)) { | |||
| 221 | error2 += foi->getError2(dgdpi, getVarBasis()); | |||
| 222 | } | |||
| 223 | } | |||
| 224 | return std::sqrt(std::abs(error2)); | |||
| 225 | } | |||
| 226 | ||||
| 227 | ||||
| 228 | double BaseHardConstraint::dirDer(double* p, double* w, int idim, double mu) | |||
| 229 | { | |||
| 230 | double* pw, *pp; | |||
| 231 | for (pw = w; pw < w + idim; * (pw++) = 0); | |||
| 232 | addToGlobalChi2DerVector(w, idim, mu); | |||
| 233 | double result = 0; | |||
| 234 | for (pw = w, pp = p; pw < w + idim; result += *(pp++) * *(pw++)); | |||
| 235 | return mu * result; | |||
| 236 | } | |||
| 237 | ||||
| 238 | double BaseHardConstraint::dirDerAbs(double* p, double* w, int idim, double mu) | |||
| 239 | { | |||
| 240 | double val = getValue(); | |||
| 241 | if (val == 0) return mu * std::fabs(dirDer(p, w, idim, 1)); | |||
| 242 | else if (val > 0) return mu * dirDer(p, w, idim, 1); | |||
| 243 | else return -mu * dirDer(p, w, idim, 1); | |||
| 244 | } | |||
| 245 | ||||
| 246 | ||||
| 247 | void BaseHardConstraint::test1stDerivatives() | |||
| 248 | { | |||
| 249 | B2INFO("BaseConstraint::test1stDerivatives for " << getName())do { if (Belle2::LogSystem::Instance().isLevelEnabled(Belle2:: LogConfig::c_Info, 0, "analysis")) { { LogVariableStream varStream ; varStream << "BaseConstraint::test1stDerivatives for " << getName(); Belle2::LogSystem::Instance().sendMessage (Belle2::LogMessage(Belle2::LogConfig::c_Info, std::move(varStream ), "analysis", __PRETTY_FUNCTION__, "analysis/OrcaKinFit/src/BaseHardConstraint.cc" , 249, 0)); }; } } while(false); | |||
| 250 | double y[100]; | |||
| 251 | for (double& i : y) i = 0; | |||
| 252 | addToGlobalChi2DerVector(y, 100, 1); | |||
| 253 | double eps = 0.00001; | |||
| 254 | for (unsigned int ifo = 0; ifo < fitobjects.size(); ++ifo) { | |||
| 255 | BaseFitObject* fo = fitobjects[ifo]; | |||
| 256 | assert(fo)(static_cast <bool> (fo) ? void (0) : __assert_fail ("fo" , "analysis/OrcaKinFit/src/BaseHardConstraint.cc", 256, __extension__ __PRETTY_FUNCTION__)); | |||
| 257 | for (int ilocal = 0; ilocal < fo->getNPar(); ++ilocal) { | |||
| 258 | int iglobal = fo->getGlobalParNum(ilocal); | |||
| 259 | double calc = y[iglobal]; | |||
| 260 | double num = num1stDerivative(ifo, ilocal, eps); | |||
| 261 | B2INFO("fo: " << fo->getName() << " par " << ilocal << "/"do { if (Belle2::LogSystem::Instance().isLevelEnabled(Belle2:: LogConfig::c_Info, 0, "analysis")) { { LogVariableStream varStream ; varStream << "fo: " << fo->getName() << " par " << ilocal << "/" << iglobal << " (" << fo->getParamName(ilocal) << ") calc: " << calc << " - num: " << num << " = " << calc - num; Belle2::LogSystem::Instance().sendMessage (Belle2::LogMessage(Belle2::LogConfig::c_Info, std::move(varStream ), "analysis", __PRETTY_FUNCTION__, "analysis/OrcaKinFit/src/BaseHardConstraint.cc" , 263, 0)); }; } } while(false) | |||
| 262 | << iglobal << " (" << fo->getParamName(ilocal)do { if (Belle2::LogSystem::Instance().isLevelEnabled(Belle2:: LogConfig::c_Info, 0, "analysis")) { { LogVariableStream varStream ; varStream << "fo: " << fo->getName() << " par " << ilocal << "/" << iglobal << " (" << fo->getParamName(ilocal) << ") calc: " << calc << " - num: " << num << " = " << calc - num; Belle2::LogSystem::Instance().sendMessage (Belle2::LogMessage(Belle2::LogConfig::c_Info, std::move(varStream ), "analysis", __PRETTY_FUNCTION__, "analysis/OrcaKinFit/src/BaseHardConstraint.cc" , 263, 0)); }; } } while(false) | |||
| 263 | << ") calc: " << calc << " - num: " << num << " = " << calc - num)do { if (Belle2::LogSystem::Instance().isLevelEnabled(Belle2:: LogConfig::c_Info, 0, "analysis")) { { LogVariableStream varStream ; varStream << "fo: " << fo->getName() << " par " << ilocal << "/" << iglobal << " (" << fo->getParamName(ilocal) << ") calc: " << calc << " - num: " << num << " = " << calc - num; Belle2::LogSystem::Instance().sendMessage (Belle2::LogMessage(Belle2::LogConfig::c_Info, std::move(varStream ), "analysis", __PRETTY_FUNCTION__, "analysis/OrcaKinFit/src/BaseHardConstraint.cc" , 263, 0)); }; } } while(false); | |||
| 264 | } | |||
| 265 | } | |||
| 266 | } | |||
| 267 | ||||
| 268 | void BaseHardConstraint::test2ndDerivatives() | |||
| 269 | { | |||
| 270 | B2INFO("BaseConstraint::test2ndDerivatives for " << getName())do { if (Belle2::LogSystem::Instance().isLevelEnabled(Belle2:: LogConfig::c_Info, 0, "analysis")) { { LogVariableStream varStream ; varStream << "BaseConstraint::test2ndDerivatives for " << getName(); Belle2::LogSystem::Instance().sendMessage (Belle2::LogMessage(Belle2::LogConfig::c_Info, std::move(varStream ), "analysis", __PRETTY_FUNCTION__, "analysis/OrcaKinFit/src/BaseHardConstraint.cc" , 270, 0)); }; } } while(false); | |||
| 271 | const int idim = 100; | |||
| 272 | auto* M = new double[idim * idim]; | |||
| 273 | for (int i = 0; i < idim * idim; ++i) M[i] = 0; | |||
| 274 | add2ndDerivativesToMatrix(M, idim, 1); | |||
| 275 | double eps = 0.0001; | |||
| 276 | B2INFO("eps=" << eps)do { if (Belle2::LogSystem::Instance().isLevelEnabled(Belle2:: LogConfig::c_Info, 0, "analysis")) { { LogVariableStream varStream ; varStream << "eps=" << eps; Belle2::LogSystem:: Instance().sendMessage(Belle2::LogMessage(Belle2::LogConfig:: c_Info, std::move(varStream), "analysis", __PRETTY_FUNCTION__ , "analysis/OrcaKinFit/src/BaseHardConstraint.cc", 276, 0)); } ; } } while(false); | |||
| 277 | ||||
| 278 | for (unsigned int ifo1 = 0; ifo1 < fitobjects.size(); ++ifo1) { | |||
| 279 | BaseFitObject* fo1 = fitobjects[ifo1]; | |||
| 280 | assert(fo1)(static_cast <bool> (fo1) ? void (0) : __assert_fail ("fo1" , "analysis/OrcaKinFit/src/BaseHardConstraint.cc", 280, __extension__ __PRETTY_FUNCTION__)); | |||
| 281 | for (unsigned int ifo2 = ifo1; ifo2 < fitobjects.size(); ++ifo2) { | |||
| 282 | BaseFitObject* fo2 = fitobjects[ifo2]; | |||
| 283 | assert(fo2)(static_cast <bool> (fo2) ? void (0) : __assert_fail ("fo2" , "analysis/OrcaKinFit/src/BaseHardConstraint.cc", 283, __extension__ __PRETTY_FUNCTION__)); | |||
| 284 | for (int ilocal1 = 0; ilocal1 < fo1->getNPar(); ++ilocal1) { | |||
| 285 | int iglobal1 = fo1->getGlobalParNum(ilocal1); | |||
| 286 | for (int ilocal2 = (ifo1 == ifo2 ? ilocal1 : 0); ilocal2 < fo2->getNPar(); ++ilocal2) { | |||
| 287 | int iglobal2 = fo2->getGlobalParNum(ilocal2); | |||
| 288 | double calc = M[idim * iglobal1 + iglobal2]; | |||
| 289 | double num = num2ndDerivative(ifo1, ilocal1, eps, ifo2, ilocal2, eps); | |||
| 290 | B2INFO("fo1: " << fo1->getName() << " par " << ilocal1 << "/"do { if (Belle2::LogSystem::Instance().isLevelEnabled(Belle2:: LogConfig::c_Info, 0, "analysis")) { { LogVariableStream varStream ; varStream << "fo1: " << fo1->getName() << " par " << ilocal1 << "/" << iglobal1 << " (" << fo1->getParamName(ilocal1) << "), fo2: " << fo2->getName() << " par " << ilocal2 << "/" << iglobal2 << " (" << fo2-> getParamName(ilocal2) << ") calc: " << calc << " - num: " << num << " = " << calc - num; Belle2 ::LogSystem::Instance().sendMessage(Belle2::LogMessage(Belle2 ::LogConfig::c_Info, std::move(varStream), "analysis", __PRETTY_FUNCTION__ , "analysis/OrcaKinFit/src/BaseHardConstraint.cc", 294, 0)); } ; } } while(false) | |||
| 291 | << iglobal1 << " (" << fo1->getParamName(ilocal1)do { if (Belle2::LogSystem::Instance().isLevelEnabled(Belle2:: LogConfig::c_Info, 0, "analysis")) { { LogVariableStream varStream ; varStream << "fo1: " << fo1->getName() << " par " << ilocal1 << "/" << iglobal1 << " (" << fo1->getParamName(ilocal1) << "), fo2: " << fo2->getName() << " par " << ilocal2 << "/" << iglobal2 << " (" << fo2-> getParamName(ilocal2) << ") calc: " << calc << " - num: " << num << " = " << calc - num; Belle2 ::LogSystem::Instance().sendMessage(Belle2::LogMessage(Belle2 ::LogConfig::c_Info, std::move(varStream), "analysis", __PRETTY_FUNCTION__ , "analysis/OrcaKinFit/src/BaseHardConstraint.cc", 294, 0)); } ; } } while(false) | |||
| 292 | << "), fo2: " << fo2->getName() << " par " << ilocal2 << "/"do { if (Belle2::LogSystem::Instance().isLevelEnabled(Belle2:: LogConfig::c_Info, 0, "analysis")) { { LogVariableStream varStream ; varStream << "fo1: " << fo1->getName() << " par " << ilocal1 << "/" << iglobal1 << " (" << fo1->getParamName(ilocal1) << "), fo2: " << fo2->getName() << " par " << ilocal2 << "/" << iglobal2 << " (" << fo2-> getParamName(ilocal2) << ") calc: " << calc << " - num: " << num << " = " << calc - num; Belle2 ::LogSystem::Instance().sendMessage(Belle2::LogMessage(Belle2 ::LogConfig::c_Info, std::move(varStream), "analysis", __PRETTY_FUNCTION__ , "analysis/OrcaKinFit/src/BaseHardConstraint.cc", 294, 0)); } ; } } while(false) | |||
| 293 | << iglobal2 << " (" << fo2->getParamName(ilocal2)do { if (Belle2::LogSystem::Instance().isLevelEnabled(Belle2:: LogConfig::c_Info, 0, "analysis")) { { LogVariableStream varStream ; varStream << "fo1: " << fo1->getName() << " par " << ilocal1 << "/" << iglobal1 << " (" << fo1->getParamName(ilocal1) << "), fo2: " << fo2->getName() << " par " << ilocal2 << "/" << iglobal2 << " (" << fo2-> getParamName(ilocal2) << ") calc: " << calc << " - num: " << num << " = " << calc - num; Belle2 ::LogSystem::Instance().sendMessage(Belle2::LogMessage(Belle2 ::LogConfig::c_Info, std::move(varStream), "analysis", __PRETTY_FUNCTION__ , "analysis/OrcaKinFit/src/BaseHardConstraint.cc", 294, 0)); } ; } } while(false) | |||
| 294 | << ") calc: " << calc << " - num: " << num << " = " << calc - num)do { if (Belle2::LogSystem::Instance().isLevelEnabled(Belle2:: LogConfig::c_Info, 0, "analysis")) { { LogVariableStream varStream ; varStream << "fo1: " << fo1->getName() << " par " << ilocal1 << "/" << iglobal1 << " (" << fo1->getParamName(ilocal1) << "), fo2: " << fo2->getName() << " par " << ilocal2 << "/" << iglobal2 << " (" << fo2-> getParamName(ilocal2) << ") calc: " << calc << " - num: " << num << " = " << calc - num; Belle2 ::LogSystem::Instance().sendMessage(Belle2::LogMessage(Belle2 ::LogConfig::c_Info, std::move(varStream), "analysis", __PRETTY_FUNCTION__ , "analysis/OrcaKinFit/src/BaseHardConstraint.cc", 294, 0)); } ; } } while(false); | |||
| 295 | } | |||
| 296 | } | |||
| 297 | } | |||
| 298 | } | |||
| 299 | delete[] M; | |||
| 300 | } | |||
| 301 | ||||
| 302 | ||||
| 303 | double BaseHardConstraint::num1stDerivative(int ifo, int ilocal, double eps) | |||
| 304 | { | |||
| 305 | BaseFitObject* fo = fitobjects[ifo]; | |||
| 306 | assert(fo)(static_cast <bool> (fo) ? void (0) : __assert_fail ("fo" , "analysis/OrcaKinFit/src/BaseHardConstraint.cc", 306, __extension__ __PRETTY_FUNCTION__)); | |||
| 307 | double save = fo->getParam(ilocal); | |||
| 308 | fo->setParam(ilocal, save + eps); | |||
| 309 | double v1 = getValue(); | |||
| 310 | fo->setParam(ilocal, save - eps); | |||
| 311 | double v2 = getValue(); | |||
| 312 | double result = (v1 - v2) / (2 * eps); | |||
| 313 | fo->setParam(ilocal, save); | |||
| 314 | return result; | |||
| 315 | } | |||
| 316 | ||||
| 317 | double BaseHardConstraint::num2ndDerivative(int ifo1, int ilocal1, double eps1, | |||
| 318 | int ifo2, int ilocal2, double eps2) | |||
| 319 | { | |||
| 320 | double result; | |||
| 321 | ||||
| 322 | if (ifo1 == ifo2 && ilocal1 == ilocal2) { | |||
| 323 | BaseFitObject* fo = fitobjects[ifo1]; | |||
| 324 | assert(fo)(static_cast <bool> (fo) ? void (0) : __assert_fail ("fo" , "analysis/OrcaKinFit/src/BaseHardConstraint.cc", 324, __extension__ __PRETTY_FUNCTION__)); | |||
| 325 | double save = fo->getParam(ilocal1); | |||
| 326 | double v0 = getValue(); | |||
| 327 | fo->setParam(ilocal1, save + eps1); | |||
| 328 | double v1 = getValue(); | |||
| 329 | fo->setParam(ilocal1, save - eps1); | |||
| 330 | double v2 = getValue(); | |||
| 331 | result = (v1 + v2 - 2 * v0) / (eps1 * eps1); | |||
| 332 | fo->setParam(ilocal1, save); | |||
| 333 | } else { | |||
| 334 | BaseFitObject* fo1 = fitobjects[ifo1]; | |||
| 335 | assert(fo1)(static_cast <bool> (fo1) ? void (0) : __assert_fail ("fo1" , "analysis/OrcaKinFit/src/BaseHardConstraint.cc", 335, __extension__ __PRETTY_FUNCTION__)); | |||
| 336 | BaseFitObject* fo2 = fitobjects[ifo2]; | |||
| 337 | assert(fo2)(static_cast <bool> (fo2) ? void (0) : __assert_fail ("fo2" , "analysis/OrcaKinFit/src/BaseHardConstraint.cc", 337, __extension__ __PRETTY_FUNCTION__)); | |||
| 338 | double save1 = fo1->getParam(ilocal1); | |||
| 339 | double save2 = fo2->getParam(ilocal2); | |||
| 340 | fo1->setParam(ilocal1, save1 + eps1); | |||
| 341 | fo2->setParam(ilocal2, save2 + eps2); | |||
| 342 | double v11 = getValue(); | |||
| 343 | fo2->setParam(ilocal2, save2 - eps2); | |||
| 344 | double v12 = getValue(); | |||
| 345 | fo1->setParam(ilocal1, save1 - eps1); | |||
| 346 | double v22 = getValue(); | |||
| 347 | fo2->setParam(ilocal2, save2 + eps2); | |||
| 348 | double v21 = getValue(); | |||
| 349 | result = (v11 + v22 - v12 - v21) / (4 * eps1 * eps2); | |||
| 350 | fo1->setParam(ilocal1, save1); | |||
| 351 | fo2->setParam(ilocal2, save2); | |||
| 352 | } | |||
| 353 | return result; | |||
| 354 | } | |||
| 355 | ||||
| 356 | void BaseHardConstraint::printFirstDerivatives() const | |||
| 357 | { | |||
| 358 | ||||
| 359 | B2INFO("BaseHardConstraint::printFirstDerivatives " << fitobjects.size())do { if (Belle2::LogSystem::Instance().isLevelEnabled(Belle2:: LogConfig::c_Info, 0, "analysis")) { { LogVariableStream varStream ; varStream << "BaseHardConstraint::printFirstDerivatives " << fitobjects.size(); Belle2::LogSystem::Instance().sendMessage (Belle2::LogMessage(Belle2::LogConfig::c_Info, std::move(varStream ), "analysis", __PRETTY_FUNCTION__, "analysis/OrcaKinFit/src/BaseHardConstraint.cc" , 359, 0)); }; } } while(false); | |||
| 360 | ||||
| 361 | double dgdpi[BaseDefs::MAXINTERVARS]; | |||
| 362 | for (unsigned int i = 0; i < fitobjects.size(); ++i) { | |||
| 363 | const BaseFitObject* foi = fitobjects[i]; | |||
| 364 | assert(foi)(static_cast <bool> (foi) ? void (0) : __assert_fail ("foi" , "analysis/OrcaKinFit/src/BaseHardConstraint.cc", 364, __extension__ __PRETTY_FUNCTION__)); | |||
| 365 | if (firstDerivatives(i, dgdpi)) { | |||
| 366 | B2INFO("first derivs for obj " << i << " : ")do { if (Belle2::LogSystem::Instance().isLevelEnabled(Belle2:: LogConfig::c_Info, 0, "analysis")) { { LogVariableStream varStream ; varStream << "first derivs for obj " << i << " : "; Belle2::LogSystem::Instance().sendMessage(Belle2::LogMessage (Belle2::LogConfig::c_Info, std::move(varStream), "analysis", __PRETTY_FUNCTION__, "analysis/OrcaKinFit/src/BaseHardConstraint.cc" , 366, 0)); }; } } while(false); | |||
| 367 | for (double j : dgdpi) | |||
| 368 | B2INFO(j << " ")do { if (Belle2::LogSystem::Instance().isLevelEnabled(Belle2:: LogConfig::c_Info, 0, "analysis")) { { LogVariableStream varStream ; varStream << j << " "; Belle2::LogSystem::Instance ().sendMessage(Belle2::LogMessage(Belle2::LogConfig::c_Info, std ::move(varStream), "analysis", __PRETTY_FUNCTION__, "analysis/OrcaKinFit/src/BaseHardConstraint.cc" , 368, 0)); }; } } while(false); | |||
| 369 | } | |||
| 370 | } | |||
| 371 | ||||
| 372 | return; | |||
| 373 | } | |||
| 374 | ||||
| 375 | void BaseHardConstraint::printSecondDerivatives() const | |||
| 376 | { | |||
| 377 | ||||
| 378 | double d2GdPidPj[BaseDefs::MAXINTERVARS * BaseDefs::MAXINTERVARS]; | |||
| 379 | ||||
| 380 | int n = fitobjects.size(); | |||
| 381 | ||||
| 382 | for (int i = 0; i < n; ++i) { | |||
| 383 | const BaseFitObject* foi = fitobjects[i]; | |||
| 384 | assert(foi)(static_cast <bool> (foi) ? void (0) : __assert_fail ("foi" , "analysis/OrcaKinFit/src/BaseHardConstraint.cc", 384, __extension__ __PRETTY_FUNCTION__)); | |||
| 385 | for (int j = 0; j < n; ++j) { | |||
| 386 | const BaseFitObject* foj = fitobjects[j]; | |||
| 387 | assert(foj)(static_cast <bool> (foj) ? void (0) : __assert_fail ("foj" , "analysis/OrcaKinFit/src/BaseHardConstraint.cc", 387, __extension__ __PRETTY_FUNCTION__)); | |||
| 388 | if (secondDerivatives(i, j, d2GdPidPj)) { | |||
| 389 | ||||
| 390 | B2INFO("second derivs for objs " << i << " " << j)do { if (Belle2::LogSystem::Instance().isLevelEnabled(Belle2:: LogConfig::c_Info, 0, "analysis")) { { LogVariableStream varStream ; varStream << "second derivs for objs " << i << " " << j; Belle2::LogSystem::Instance().sendMessage(Belle2 ::LogMessage(Belle2::LogConfig::c_Info, std::move(varStream), "analysis", __PRETTY_FUNCTION__, "analysis/OrcaKinFit/src/BaseHardConstraint.cc" , 390, 0)); }; } } while(false); | |||
| 391 | ||||
| 392 | int k(0); | |||
| 393 | for (int k1 = 0; k1 < BaseDefs::MAXINTERVARS; k1++) { | |||
| 394 | for (int k2 = 0; k2 < BaseDefs::MAXINTERVARS; k2++) { | |||
| 395 | B2INFO(d2GdPidPj[k++] << " ")do { if (Belle2::LogSystem::Instance().isLevelEnabled(Belle2:: LogConfig::c_Info, 0, "analysis")) { { LogVariableStream varStream ; varStream << d2GdPidPj[k++] << " "; Belle2::LogSystem ::Instance().sendMessage(Belle2::LogMessage(Belle2::LogConfig ::c_Info, std::move(varStream), "analysis", __PRETTY_FUNCTION__ , "analysis/OrcaKinFit/src/BaseHardConstraint.cc", 395, 0)); } ; } } while(false); | |||
| 396 | } | |||
| 397 | } | |||
| 398 | } | |||
| 399 | } | |||
| 400 | } | |||
| 401 | ||||
| 402 | return; | |||
| 403 | } | |||
| 404 | ||||
| 405 | }// end OrcaKinFit namespace | |||
| 406 | } // end Belle2 namespace |