17 #include "analysis/OrcaKinFit/SoftGaussParticleConstraint.h"
18 #include "analysis/OrcaKinFit/ParticleFitObject.h"
19 #include <framework/logging/Logger.h>
29 namespace OrcaKinFit {
31 SoftGaussParticleConstraint::SoftGaussParticleConstraint(
double sigma_)
44 if (sigma_ != 0)
sigma = std::abs(sigma_);
58 for (
unsigned int i = 0; i <
fitobjects.size(); ++i) {
62 error2 += foi->getError2(dgdpi, getVarBasis());
65 return std::sqrt(std::abs(error2));
101 double fact = 2 *
getValue() / (s * s);
105 double d2GdPidPj[16];
112 auto* dPidAk =
new double[n * KMAX * 4];
113 bool* dPidAkval =
new bool[n];
115 for (
int i = 0; i < n; ++i) dPidAkval[i] =
false;
119 double d2GdPdAl[4 * KMAX];
121 double d2GdAkdAl[KMAX * KMAX] = {0};
125 int* parglobal =
new int[KMAX * n];
127 for (
int i = 0; i < n; ++i) {
130 for (
int klocal = 0; klocal < foi->getNPar(); ++klocal) {
131 parglobal [KMAX * i + klocal] = foi->getGlobalParNum(klocal);
136 for (
int i = 0; i < n; ++i) {
139 for (
int j = 0; j < n; ++j) {
144 foi->getDerivatives(dPidAk + i * (KMAX * 4), KMAX * 4);
148 foj->getDerivatives(dPidAk + j * (KMAX * 4), KMAX * 4);
156 for (
int llocal = 0; llocal < foj->getNPar(); ++llocal) {
157 for (
int ii = 0; ii < 4; ++ii) {
159 int ind2 = (KMAX * 4) * j + 4 * llocal;
160 double& r = d2GdPdAl[4 * llocal + ii];
161 r = d2GdPidPj[ ind1] * dPidAk[ ind2];
162 r += d2GdPidPj[++ind1] * dPidAk[++ind2];
163 r += d2GdPidPj[++ind1] * dPidAk[++ind2];
164 r += d2GdPidPj[++ind1] * dPidAk[++ind2];
173 for (
int klocal = 0; klocal < foi->getNPar(); ++klocal) {
174 for (
int llocal = 0; llocal < foj->getNPar(); ++llocal) {
175 int ind1 = 4 * llocal;
176 int ind2 = (KMAX * 4) * i + 4 * klocal;
177 double& r = d2GdAkdAl[KMAX * klocal + llocal];
178 r = d2GdPdAl[ ind1] * dPidAk[ ind2];
179 r += d2GdPdAl[++ind1] * dPidAk[++ind2];
180 r += d2GdPdAl[++ind1] * dPidAk[++ind2];
181 r += d2GdPdAl[++ind1] * dPidAk[++ind2];
185 for (
int klocal = 0; klocal < foi->getNPar(); ++klocal) {
186 int kglobal = parglobal [KMAX * i + klocal];
187 for (
int llocal = 0; llocal < foj->getNPar(); ++llocal) {
188 int lglobal = parglobal [KMAX * j + llocal];
189 M [idim * kglobal + lglobal] += fact * d2GdAkdAl[KMAX * klocal + llocal];
213 auto* v =
new double[idim];
214 for (
int i = 0; i < idim; ++i) v[i] = 0;
215 double sqrtfact2 = sqrt(2.0) / s;
218 for (
int i = 0; i < n; ++i) {
222 foi->addTo2ndDerivatives(M, idim, fact, dgdpi, getVarBasis());
223 foi->addToGlobalChi2DerVector(v, idim, sqrtfact2, dgdpi, getVarBasis());
227 for (
int i = 0; i < idim; ++i) {
228 if (
double vi = v[i]) {
229 int ioffs = i * idim;
230 for (
double* pvj = v; pvj < v + idim; ++pvj) {
231 M[ioffs++] += vi * (*pvj);
247 double r = 2 *
getValue() / (s * s);
248 for (
unsigned int i = 0; i <
fitobjects.size(); ++i) {
252 foi->addToGlobalChi2DerVector(y, idim, r, dgdpi, getVarBasis());
257 void SoftGaussParticleConstraint::test1stDerivatives()
259 B2INFO(
"SoftGaussParticleConstraint::test1stDerivatives for " <<
getName());
261 for (
double& i : y) i = 0;
263 double eps = 0.00001;
264 for (
unsigned int ifo = 0; ifo <
fitobjects.size(); ++ifo) {
267 for (
int ilocal = 0; ilocal < fo->getNPar(); ++ilocal) {
268 int iglobal = fo->getGlobalParNum(ilocal);
269 double calc = y[iglobal];
271 B2INFO(
"fo: " << fo->getName() <<
" par " << ilocal <<
"/"
272 << iglobal <<
" (" << fo->getParamName(ilocal)
273 <<
") calc: " << calc <<
" - num: " << num <<
" = " << calc - num);
277 void SoftGaussParticleConstraint::test2ndDerivatives()
279 B2INFO(
"SoftGaussParticleConstraint::test2ndDerivatives for " <<
getName());
280 const int idim = 100;
281 auto* M =
new double[idim * idim];
282 for (
int i = 0; i < idim * idim; ++i) M[i] = 0;
285 B2INFO(
"eps=" << eps);
287 for (
unsigned int ifo1 = 0; ifo1 <
fitobjects.size(); ++ifo1) {
290 for (
unsigned int ifo2 = ifo1; ifo2 <
fitobjects.size(); ++ifo2) {
293 for (
int ilocal1 = 0; ilocal1 < fo1->getNPar(); ++ilocal1) {
294 int iglobal1 = fo1->getGlobalParNum(ilocal1);
295 for (
int ilocal2 = (ifo1 == ifo2 ? ilocal1 : 0); ilocal2 < fo2->getNPar(); ++ilocal2) {
296 int iglobal2 = fo2->getGlobalParNum(ilocal2);
297 double calc = M[idim * iglobal1 + iglobal2];
299 B2INFO(
"fo1: " << fo1->getName() <<
" par " << ilocal1 <<
"/"
300 << iglobal1 <<
" (" << fo1->getParamName(ilocal1)
301 <<
"), fo2: " << fo2->getName() <<
" par " << ilocal2 <<
"/"
302 << iglobal2 <<
" (" << fo2->getParamName(ilocal2)
303 <<
") calc: " << calc <<
" - num: " << num <<
" = " << calc - num);
316 double save = fo->getParam(ilocal);
317 fo->setParam(ilocal, save + eps);
319 fo->setParam(ilocal, save - eps);
321 double result = (v1 - v2) / (2 * eps);
322 fo->setParam(ilocal, save);
327 int ifo2,
int ilocal2,
double eps2)
331 if (ifo1 == ifo2 && ilocal1 == ilocal2) {
334 double save = fo->getParam(ilocal1);
336 fo->setParam(ilocal1, save + eps1);
338 fo->setParam(ilocal1, save - eps1);
340 result = (v1 + v2 - 2 * v0) / (eps1 * eps1);
341 fo->setParam(ilocal1, save);
347 double save1 = fo1->getParam(ilocal1);
348 double save2 = fo2->getParam(ilocal2);
349 fo1->setParam(ilocal1, save1 + eps1);
350 fo2->setParam(ilocal2, save2 + eps2);
352 fo2->setParam(ilocal2, save2 - eps2);
354 fo1->setParam(ilocal1, save1 - eps1);
356 fo2->setParam(ilocal2, save2 + eps2);
358 result = (v11 + v22 - v12 - v21) / (4 * eps1 * eps2);
359 fo1->setParam(ilocal1, save1);
360 fo2->setParam(ilocal2, save2);
365 int SoftGaussParticleConstraint::getVarBasis()
const