File indexing completed on 2023-03-17 11:15:59
0001
0002
0003
0004
0005
0006
0007
0008
0009
0010
0011
0012 #include <iostream>
0013 #include <iomanip>
0014 #include "FWCore/MessageLogger/interface/MessageLogger.h"
0015 #include "PhysicsTools/KinFitter/interface/TKinFitter.h"
0016 #include "PhysicsTools/KinFitter/interface/TAbsFitParticle.h"
0017 #include "PhysicsTools/KinFitter/interface/TAbsFitConstraint.h"
0018
0019 TKinFitter::TKinFitter()
0020 : TNamed("UnNamed", "UnTitled"),
0021 _A(1, 1),
0022 _AT(1, 1),
0023 _B(1, 1),
0024 _BT(1, 1),
0025 _V(1, 1),
0026 _Vinv(1, 1),
0027 _VB(1, 1),
0028 _VBinv(1, 1),
0029 _VA(1, 1),
0030 _VAinv(1, 1),
0031 _c(1, 1),
0032 _C11(1, 1),
0033 _C11T(1, 1),
0034 _C21(1, 1),
0035 _C21T(1, 1),
0036 _C22(1, 1),
0037 _C22T(1, 1),
0038 _C31(1, 1),
0039 _C31T(1, 1),
0040 _C32(1, 1),
0041 _C32T(1, 1),
0042 _C33(1, 1),
0043 _C33T(1, 1),
0044 _deltaA(1, 1),
0045 _deltaY(1, 1),
0046 _deltaAstar(1, 1),
0047 _deltaYstar(1, 1),
0048 _lambda(1, 1),
0049 _lambdaT(1, 1),
0050 _lambdaVFit(1, 1),
0051 _yaVFit(1, 1),
0052 _constraints(0),
0053 _measParticles(0),
0054 _unmeasParticles(0) {
0055 reset();
0056 }
0057
0058 TKinFitter::TKinFitter(const TString& name, const TString& title)
0059 : TNamed(name, title),
0060 _A(1, 1),
0061 _AT(1, 1),
0062 _B(1, 1),
0063 _BT(1, 1),
0064 _V(1, 1),
0065 _Vinv(1, 1),
0066 _VB(1, 1),
0067 _VBinv(1, 1),
0068 _VA(1, 1),
0069 _VAinv(1, 1),
0070 _c(1, 1),
0071 _C11(1, 1),
0072 _C11T(1, 1),
0073 _C21(1, 1),
0074 _C21T(1, 1),
0075 _C22(1, 1),
0076 _C22T(1, 1),
0077 _C31(1, 1),
0078 _C31T(1, 1),
0079 _C32(1, 1),
0080 _C32T(1, 1),
0081 _C33(1, 1),
0082 _C33T(1, 1),
0083 _deltaA(1, 1),
0084 _deltaY(1, 1),
0085 _deltaAstar(1, 1),
0086 _deltaYstar(1, 1),
0087 _lambda(1, 1),
0088 _lambdaT(1, 1),
0089 _lambdaVFit(1, 1),
0090 _yaVFit(1, 1),
0091 _constraints(0),
0092 _measParticles(0),
0093 _unmeasParticles(0) {
0094 reset();
0095 }
0096
0097 void TKinFitter::reset() {
0098
0099
0100 _status = -1;
0101 _nbIter = 0;
0102 _nParA = 0;
0103 _nParB = 0;
0104 _verbosity = 1;
0105 _A.ResizeTo(1, 1);
0106 _AT.ResizeTo(1, 1);
0107 _B.ResizeTo(1, 1);
0108 _BT.ResizeTo(1, 1);
0109 _V.ResizeTo(1, 1);
0110 _Vinv.ResizeTo(1, 1);
0111 _VB.ResizeTo(1, 1);
0112 _VBinv.ResizeTo(1, 1);
0113 _VA.ResizeTo(1, 1);
0114 _VAinv.ResizeTo(1, 1);
0115 _c.ResizeTo(1, 1);
0116 _C11.ResizeTo(1, 1);
0117 _C11T.ResizeTo(1, 1);
0118 _C21.ResizeTo(1, 1);
0119 _C21T.ResizeTo(1, 1);
0120 _C22.ResizeTo(1, 1);
0121 _C22T.ResizeTo(1, 1);
0122 _C31.ResizeTo(1, 1);
0123 _C31T.ResizeTo(1, 1);
0124 _C32.ResizeTo(1, 1);
0125 _C32T.ResizeTo(1, 1);
0126 _C33.ResizeTo(1, 1);
0127 _C33T.ResizeTo(1, 1);
0128 _deltaA.ResizeTo(1, 1);
0129 _deltaY.ResizeTo(1, 1);
0130 _deltaAstar.ResizeTo(1, 1);
0131 _deltaYstar.ResizeTo(1, 1);
0132 _lambda.ResizeTo(1, 1);
0133 _lambdaT.ResizeTo(1, 1);
0134 _lambdaVFit.ResizeTo(1, 1);
0135 _yaVFit.ResizeTo(1, 1);
0136
0137 _constraints.clear();
0138 _measParticles.clear();
0139 _unmeasParticles.clear();
0140
0141
0142 _maxNbIter = 50;
0143 _maxDeltaS = 5e-3;
0144 _maxF = 1e-4;
0145 }
0146
0147 void TKinFitter::resetStatus() {
0148
0149
0150 _status = -1;
0151 _nbIter = 0;
0152 }
0153
0154 void TKinFitter::resetParams() {
0155
0156
0157 for (unsigned int iP = 0; iP < _measParticles.size(); iP++) {
0158 TAbsFitParticle* particle = _measParticles[iP];
0159 particle->reset();
0160 }
0161 for (unsigned int iP = 0; iP < _unmeasParticles.size(); iP++) {
0162 TAbsFitParticle* particle = _unmeasParticles[iP];
0163 particle->reset();
0164 }
0165 for (unsigned int iC = 0; iC < _constraints.size(); iC++) {
0166 TAbsFitConstraint* constraint = _constraints[iC];
0167 constraint->reset();
0168 }
0169 }
0170
0171 TKinFitter::~TKinFitter() {}
0172
0173 void TKinFitter::countMeasParams() {
0174
0175
0176 _nParB = 0;
0177 for (unsigned int indexParticle = 0; indexParticle < _measParticles.size(); indexParticle++) {
0178 _nParB += _measParticles[indexParticle]->getNPar();
0179 }
0180 for (unsigned int indexConstraint = 0; indexConstraint < _constraints.size(); indexConstraint++) {
0181 _nParB += _constraints[indexConstraint]->getNPar();
0182 }
0183 }
0184
0185 void TKinFitter::countUnmeasParams() {
0186
0187
0188 _nParA = 0;
0189 for (unsigned int indexParticle = 0; indexParticle < _unmeasParticles.size(); indexParticle++) {
0190 _nParA += _unmeasParticles[indexParticle]->getNPar();
0191 }
0192 }
0193
0194 void TKinFitter::addMeasParticle(TAbsFitParticle* particle) {
0195
0196
0197 resetStatus();
0198
0199 if (particle != nullptr) {
0200 _measParticles.push_back(particle);
0201 } else {
0202 edm::LogError("NullPointer") << "Measured particle points to NULL. It will not be added to the KinFitter.";
0203 }
0204
0205 countMeasParams();
0206 }
0207
0208 void TKinFitter::addMeasParticles(TAbsFitParticle* p1,
0209 TAbsFitParticle* p2,
0210 TAbsFitParticle* p3,
0211 TAbsFitParticle* p4,
0212 TAbsFitParticle* p5,
0213 TAbsFitParticle* p6,
0214 TAbsFitParticle* p7,
0215 TAbsFitParticle* p8,
0216 TAbsFitParticle* p9) {
0217
0218
0219 resetStatus();
0220
0221 if (p1 != nullptr)
0222 _measParticles.push_back(p1);
0223 if (p2 != nullptr)
0224 _measParticles.push_back(p2);
0225 if (p3 != nullptr)
0226 _measParticles.push_back(p3);
0227 if (p4 != nullptr)
0228 _measParticles.push_back(p4);
0229 if (p5 != nullptr)
0230 _measParticles.push_back(p5);
0231 if (p6 != nullptr)
0232 _measParticles.push_back(p6);
0233 if (p7 != nullptr)
0234 _measParticles.push_back(p7);
0235 if (p8 != nullptr)
0236 _measParticles.push_back(p8);
0237 if (p9 != nullptr)
0238 _measParticles.push_back(p9);
0239
0240 countMeasParams();
0241 }
0242
0243 void TKinFitter::addUnmeasParticle(TAbsFitParticle* particle) {
0244
0245
0246 resetStatus();
0247
0248 if (particle != nullptr) {
0249 _unmeasParticles.push_back(particle);
0250 } else {
0251 edm::LogError("NullPointer") << "Unmeasured particle points to NULL. It will not be added to the KinFitter.";
0252 }
0253
0254 countUnmeasParams();
0255 }
0256
0257 void TKinFitter::addUnmeasParticles(TAbsFitParticle* p1,
0258 TAbsFitParticle* p2,
0259 TAbsFitParticle* p3,
0260 TAbsFitParticle* p4,
0261 TAbsFitParticle* p5,
0262 TAbsFitParticle* p6,
0263 TAbsFitParticle* p7,
0264 TAbsFitParticle* p8,
0265 TAbsFitParticle* p9) {
0266
0267
0268 resetStatus();
0269
0270 if (p1 != nullptr)
0271 _unmeasParticles.push_back(p1);
0272 if (p2 != nullptr)
0273 _unmeasParticles.push_back(p2);
0274 if (p3 != nullptr)
0275 _unmeasParticles.push_back(p3);
0276 if (p4 != nullptr)
0277 _unmeasParticles.push_back(p4);
0278 if (p5 != nullptr)
0279 _unmeasParticles.push_back(p5);
0280 if (p6 != nullptr)
0281 _unmeasParticles.push_back(p6);
0282 if (p7 != nullptr)
0283 _unmeasParticles.push_back(p7);
0284 if (p8 != nullptr)
0285 _unmeasParticles.push_back(p8);
0286 if (p9 != nullptr)
0287 _unmeasParticles.push_back(p9);
0288
0289 countUnmeasParams();
0290 }
0291
0292 void TKinFitter::addConstraint(TAbsFitConstraint* constraint) {
0293
0294
0295 resetStatus();
0296
0297 if (constraint != nullptr) {
0298 _constraints.push_back(constraint);
0299 }
0300
0301 countMeasParams();
0302 }
0303
0304 void TKinFitter::setVerbosity(Int_t verbosity) {
0305
0306
0307
0308
0309
0310
0311 if (verbosity < 0)
0312 verbosity = 0;
0313 if (verbosity > 3)
0314 verbosity = 3;
0315 _verbosity = verbosity;
0316 }
0317
0318 Int_t TKinFitter::fit() {
0319
0320
0321
0322
0323
0324 resetParams();
0325 resetStatus();
0326
0327 _nbIter = 0;
0328 Bool_t isConverged = false;
0329 Double_t prevF;
0330 Double_t currF = getF();
0331 Double_t prevS;
0332 Double_t currS = 0.;
0333
0334
0335 calcV();
0336
0337
0338 if (_verbosity >= 1) {
0339 print();
0340 }
0341
0342 do {
0343
0344
0345 if (_status != -10) {
0346 _status = 10;
0347 }
0348
0349
0350 calcB();
0351 calcVB();
0352 if (_nParA > 0) {
0353 calcA();
0354 calcVA();
0355 calcC32();
0356 }
0357 calcC31();
0358 calcC33();
0359 calcC();
0360
0361
0362 if (_nParA > 0) {
0363 calcDeltaA();
0364 }
0365 calcDeltaY();
0366 calcLambda();
0367
0368 if (_verbosity >= 3) {
0369 printMatrix(_A, "A");
0370 printMatrix(_B, "B");
0371 printMatrix(_VBinv, "VBinv");
0372 printMatrix(_VB, "VB");
0373 printMatrix(_V, "V");
0374 printMatrix(_deltaY, "deltaY");
0375 printMatrix(_deltaA, "deltaA");
0376 printMatrix(_C32T, "C32T");
0377 printMatrix(_c, "C");
0378 }
0379
0380 if (_verbosity >= 2) {
0381 print();
0382 }
0383
0384
0385 if (_nParA > 0) {
0386 applyDeltaA();
0387 }
0388 applyDeltaY();
0389
0390 _nbIter++;
0391
0392
0393 prevF = currF;
0394 currF = getF();
0395 prevS = currS;
0396 currS = getS();
0397
0398 if (TMath::IsNaN(currF)) {
0399 edm::LogInfo("KinFitter") << "The current value of F is NaN. Fit will be aborted.";
0400 _status = -10;
0401 }
0402 if (TMath::IsNaN(currS)) {
0403 edm::LogInfo("KinFitter") << "The current value of S is NaN. Fit will be aborted.";
0404 _status = -10;
0405 }
0406
0407
0408 Int_t nstep = 0;
0409 while (currF >= prevF) {
0410 nstep++;
0411 if (nstep == 10)
0412 break;
0413 if (_nParA > 0)
0414 _deltaA -= (0.5) * (_deltaA - _deltaAstar);
0415 _deltaY -= (0.5) * (_deltaY - _deltaYstar);
0416 _lambda *= 0.5;
0417 _lambdaT *= 0.5;
0418 if (_nParA > 0)
0419 applyDeltaA();
0420 applyDeltaY();
0421 currF = getF();
0422 currS = getS();
0423 }
0424
0425
0426 isConverged = converged(currF, prevS, currS);
0427
0428 } while ((!isConverged) && (_nbIter < _maxNbIter) && (_status != -10));
0429
0430
0431 calcB();
0432 calcVB();
0433
0434 if (_nParA > 0) {
0435 calcA();
0436 calcVA();
0437 calcC21();
0438 calcC22();
0439 calcC32();
0440 }
0441 calcC11();
0442 calcC31();
0443 calcC33();
0444 calcVFit();
0445 applyVFit();
0446
0447
0448 if (isConverged) {
0449 _status = 0;
0450 } else if (_status != -10) {
0451 _status = 1;
0452 }
0453
0454
0455 if (_verbosity >= 1) {
0456 print();
0457 }
0458
0459 return _status;
0460 }
0461
0462 void TKinFitter::setCovMatrix(TMatrixD& V) {
0463
0464
0465 if ((V.GetNrows() != _nParB) || (V.GetNcols() != _nParB)) {
0466 edm::LogError("WrongMatrixSize") << "Covariance matrix of measured particles needs to be a " << _nParB << "x"
0467 << _nParB << " matrix.";
0468 } else {
0469 _V.ResizeTo(V);
0470 _V = V;
0471 }
0472 }
0473
0474 Bool_t TKinFitter::calcV() {
0475
0476
0477 _V.ResizeTo(_nParB, _nParB);
0478 _V.Zero();
0479
0480 Int_t offsetP = 0;
0481 for (unsigned int iP = 0; iP < _measParticles.size(); iP++) {
0482 TAbsFitParticle* particle = _measParticles[iP];
0483 Int_t nParP = particle->getNPar();
0484 const TMatrixD* covMatrix = particle->getCovMatrix();
0485
0486 for (int iX = offsetP; iX < offsetP + nParP; iX++) {
0487 for (int iY = offsetP; iY < offsetP + nParP; iY++) {
0488 _V(iX, iY) = (*covMatrix)(iX - offsetP, iY - offsetP);
0489 }
0490 }
0491
0492 offsetP += nParP;
0493 }
0494
0495 for (unsigned int iC = 0; iC < _constraints.size(); iC++) {
0496 TAbsFitConstraint* constraint = _constraints[iC];
0497 Int_t nParP = constraint->getNPar();
0498 const TMatrixD* covMatrix = constraint->getCovMatrix();
0499
0500 for (int iX = offsetP; iX < offsetP + nParP; iX++) {
0501 for (int iY = offsetP; iY < offsetP + nParP; iY++) {
0502 _V(iX, iY) = (*covMatrix)(iX - offsetP, iY - offsetP);
0503 }
0504 }
0505
0506 offsetP += nParP;
0507 }
0508
0509 _Vinv.ResizeTo(_V);
0510 _Vinv = _V;
0511 try {
0512 _Vinv.Invert();
0513 } catch (cms::Exception& e) {
0514 edm::LogInfo("KinFitter") << "Failed to invert covariance matrix V. Fit will be aborted.";
0515 _status = -10;
0516 }
0517
0518 return true;
0519 }
0520
0521 Bool_t TKinFitter::calcA() {
0522
0523
0524
0525
0526 _A.ResizeTo(_constraints.size(), _nParA);
0527
0528 for (unsigned int indexConstr = 0; indexConstr < _constraints.size(); indexConstr++) {
0529 int offsetParam = 0;
0530 for (unsigned int indexParticle = 0; indexParticle < _unmeasParticles.size(); indexParticle++) {
0531
0532
0533 TAbsFitParticle* particle = _unmeasParticles[indexParticle];
0534 TMatrixD* derivParticle = particle->getDerivative();
0535 TMatrixD* derivConstr = _constraints[indexConstr]->getDerivative(particle);
0536 TMatrixD deriv(*derivConstr, TMatrixD::kMult, *derivParticle);
0537
0538 for (int indexParam = 0; indexParam < deriv.GetNcols(); indexParam++) {
0539 _A(indexConstr, indexParam + offsetParam) = deriv(0, indexParam);
0540 }
0541 offsetParam += deriv.GetNcols();
0542
0543 delete derivParticle;
0544 delete derivConstr;
0545 }
0546 }
0547
0548
0549 TMatrixD AT(TMatrixD::kTransposed, _A);
0550 _AT.ResizeTo(AT);
0551 _AT = AT;
0552
0553 return true;
0554 }
0555
0556 Bool_t TKinFitter::calcB() {
0557
0558
0559
0560
0561 _B.ResizeTo(_constraints.size(), _nParB);
0562
0563 int offsetParam = 0;
0564 for (unsigned int indexConstr = 0; indexConstr < _constraints.size(); indexConstr++) {
0565 offsetParam = 0;
0566 for (unsigned int indexParticle = 0; indexParticle < _measParticles.size(); indexParticle++) {
0567
0568 TAbsFitParticle* particle = _measParticles[indexParticle];
0569 TMatrixD* derivParticle = particle->getDerivative();
0570 TMatrixD* derivConstr = _constraints[indexConstr]->getDerivative(particle);
0571 TMatrixD deriv(*derivConstr, TMatrixD::kMult, *derivParticle);
0572 if (_verbosity >= 3) {
0573 TString matrixName = "B deriv: Particle -> ";
0574 matrixName += particle->GetName();
0575 printMatrix(*derivParticle, matrixName);
0576 matrixName = "B deriv: Constraint -> ";
0577 matrixName += _constraints[indexConstr]->GetName();
0578 matrixName += " , Particle -> ";
0579 matrixName += particle->GetName();
0580 printMatrix(*derivConstr, matrixName);
0581 }
0582 for (int indexParam = 0; indexParam < deriv.GetNcols(); indexParam++) {
0583 _B(indexConstr, indexParam + offsetParam) = deriv(0, indexParam);
0584 }
0585 offsetParam += deriv.GetNcols();
0586
0587 delete derivParticle;
0588 delete derivConstr;
0589 }
0590 }
0591
0592 for (unsigned int iC = 0; iC < _constraints.size(); iC++) {
0593 TAbsFitConstraint* constraint = _constraints[iC];
0594 TMatrixD* deriv = constraint->getDerivativeAlpha();
0595
0596 if (deriv != nullptr) {
0597 if (_verbosity >= 3) {
0598 TString matrixName = "B deriv alpha: Constraint -> ";
0599 matrixName += constraint->GetName();
0600 printMatrix(*deriv, matrixName);
0601 }
0602 for (int indexParam = 0; indexParam < deriv->GetNcols(); indexParam++) {
0603 _B(iC, indexParam + offsetParam) = (*deriv)(0, indexParam);
0604 }
0605 offsetParam += deriv->GetNcols();
0606
0607 delete deriv;
0608 }
0609 }
0610
0611 TMatrixD BT(TMatrixD::kTransposed, _B);
0612 _BT.ResizeTo(BT);
0613 _BT = BT;
0614
0615 return true;
0616 }
0617
0618 Bool_t TKinFitter::calcVB() {
0619
0620
0621 TMatrixD BV(_B, TMatrixD::kMult, _V);
0622 TMatrixD VBinv(BV, TMatrixD::kMult, _BT);
0623 _VBinv.ResizeTo(VBinv);
0624 _VBinv = VBinv;
0625
0626 _VB.ResizeTo(_VBinv);
0627 _VB = _VBinv;
0628 try {
0629 _VB.Invert();
0630 } catch (cms::Exception& e) {
0631 edm::LogInfo("KinFitter") << "Failed to invert matrix VB. Fit will be aborted.";
0632 _status = -10;
0633 }
0634
0635 return true;
0636 }
0637
0638 Bool_t TKinFitter::calcVA() {
0639
0640
0641 TMatrixD ATVB(_AT, TMatrixD::kMult, _VB);
0642 TMatrixD VA(ATVB, TMatrixD::kMult, _A);
0643 _VA.ResizeTo(VA);
0644 _VA = VA;
0645
0646 _VAinv.ResizeTo(_VA);
0647 _VAinv = _VA;
0648 try {
0649 _VAinv.Invert();
0650 } catch (cms::Exception& e) {
0651 edm::LogInfo("KinFitter") << "Failed to invert matrix VA. Fit will be aborted.";
0652 _status = -10;
0653 }
0654
0655 return true;
0656 }
0657
0658 Bool_t TKinFitter::calcC11() {
0659
0660
0661 TMatrixD VBT(_V, TMatrixD::kMult, _BT);
0662 TMatrixD VBB(_VB, TMatrixD::kMult, _B);
0663 TMatrixD VBTVBB(VBT, TMatrixD::kMult, VBB);
0664 TMatrixD m2(VBTVBB, TMatrixD::kMult, _V);
0665
0666 _C11.ResizeTo(_V);
0667 _C11 = _V;
0668 _C11 -= m2;
0669
0670 if (_nParA > 0) {
0671 TMatrixD VBA(_VB, TMatrixD::kMult, _A);
0672 TMatrixD VBTVBA(VBT, TMatrixD::kMult, VBA);
0673 TMatrixD VAinvAT(_VAinv, TMatrixD::kMult, _AT);
0674 TMatrixD VBTVBAVAinvAT(VBTVBA, TMatrixD::kMult, VAinvAT);
0675 TMatrixD VBTVBAVAinvATVBB(VBTVBAVAinvAT, TMatrixD::kMult, VBB);
0676 TMatrixD m3(VBTVBAVAinvATVBB, TMatrixD::kMult, _V);
0677 _C11 += m3;
0678 }
0679
0680 TMatrixD C11T(TMatrixD::kTransposed, _C11);
0681 _C11T.ResizeTo(C11T);
0682 _C11T = C11T;
0683
0684 return true;
0685 }
0686
0687 Bool_t TKinFitter::calcC21() {
0688
0689
0690 TMatrixD VAinvAT(_VAinv, TMatrixD::kMult, _AT);
0691 TMatrixD VBB(_VB, TMatrixD::kMult, _B);
0692 TMatrixD VAinvATVBB(VAinvAT, TMatrixD::kMult, VBB);
0693 TMatrixD C21(VAinvATVBB, TMatrixD::kMult, _V);
0694 C21 *= -1.;
0695 _C21.ResizeTo(C21);
0696 _C21 = C21;
0697
0698 TMatrixD C21T(TMatrixD::kTransposed, _C21);
0699 _C21T.ResizeTo(C21T);
0700 _C21T = C21T;
0701
0702 return true;
0703 }
0704
0705 Bool_t TKinFitter::calcC22() {
0706
0707
0708 _C22.ResizeTo(_VAinv);
0709 _C22 = _VAinv;
0710
0711 TMatrixD C22T(TMatrixD::kTransposed, _C22);
0712 _C22T.ResizeTo(C22T);
0713 _C22T = C22T;
0714
0715 return true;
0716 }
0717
0718 Bool_t TKinFitter::calcC31() {
0719
0720
0721 TMatrixD VbB(_VB, TMatrixD::kMult, _B);
0722 TMatrixD m1(VbB, TMatrixD::kMult, _V);
0723
0724 _C31.ResizeTo(m1);
0725 _C31 = m1;
0726
0727 if (_nParA > 0) {
0728 TMatrixD VbA(_VB, TMatrixD::kMult, _A);
0729 TMatrixD VAinvAT(_VAinv, TMatrixD::kMult, _AT);
0730 TMatrixD VbBV(VbB, TMatrixD::kMult, _V);
0731 TMatrixD VbAVAinvAT(VbA, TMatrixD::kMult, VAinvAT);
0732 TMatrixD m2(VbAVAinvAT, TMatrixD::kMult, VbBV);
0733
0734 _C31 -= m2;
0735 }
0736
0737 TMatrixD C31T(TMatrixD::kTransposed, _C31);
0738 _C31T.ResizeTo(C31T);
0739 _C31T = C31T;
0740
0741 return true;
0742 }
0743
0744 Bool_t TKinFitter::calcC32() {
0745
0746
0747 TMatrixD VbA(_VB, TMatrixD::kMult, _A);
0748 TMatrixD C32(VbA, TMatrixD::kMult, _VAinv);
0749 _C32.ResizeTo(C32);
0750 _C32 = C32;
0751
0752 TMatrixD C32T(TMatrixD::kTransposed, _C32);
0753 _C32T.ResizeTo(C32T);
0754 _C32T = C32T;
0755
0756 return true;
0757 }
0758
0759 Bool_t TKinFitter::calcC33() {
0760
0761
0762 _C33.ResizeTo(_VB);
0763 _C33 = _VB;
0764 _C33 *= -1.;
0765
0766 if (_nParA > 0) {
0767 TMatrixD VbA(_VB, TMatrixD::kMult, _A);
0768 TMatrixD VAinvAT(_VAinv, TMatrixD::kMult, _AT);
0769 TMatrixD VbAVAinvAT(VbA, TMatrixD::kMult, VAinvAT);
0770 TMatrixD C33(VbAVAinvAT, TMatrixD::kMult, _VB);
0771 _C33 += C33;
0772 }
0773
0774 TMatrixD C33T(TMatrixD::kTransposed, _C33);
0775 _C33T.ResizeTo(C33T);
0776 _C33T = C33T;
0777
0778 return true;
0779 }
0780
0781 Bool_t TKinFitter::calcC() {
0782
0783
0784 int offsetParam = 0;
0785
0786
0787 TMatrixD deltaastar(1, 1);
0788 if (_nParA > 0) {
0789 deltaastar.ResizeTo(_nParA, 1);
0790 for (unsigned int indexParticle = 0; indexParticle < _unmeasParticles.size(); indexParticle++) {
0791 TAbsFitParticle* particle = _unmeasParticles[indexParticle];
0792 const TMatrixD* astar = particle->getParCurr();
0793 const TMatrixD* a = particle->getParIni();
0794 TMatrixD deltaastarpart(*astar);
0795 deltaastarpart -= *a;
0796
0797 for (int indexParam = 0; indexParam < deltaastarpart.GetNrows(); indexParam++) {
0798 deltaastar(indexParam + offsetParam, 0) = deltaastarpart(indexParam, 0);
0799 }
0800 offsetParam += deltaastarpart.GetNrows();
0801 }
0802
0803 if (_verbosity >= 3) {
0804 printMatrix(deltaastar, "deltaastar");
0805 }
0806 }
0807
0808
0809 TMatrixD deltaystar(_nParB, 1);
0810 offsetParam = 0;
0811 for (unsigned int indexParticle = 0; indexParticle < _measParticles.size(); indexParticle++) {
0812 TAbsFitParticle* particle = _measParticles[indexParticle];
0813 const TMatrixD* ystar = particle->getParCurr();
0814 const TMatrixD* y = particle->getParIni();
0815 TMatrixD deltaystarpart(*ystar);
0816 deltaystarpart -= *y;
0817
0818 for (int indexParam = 0; indexParam < deltaystarpart.GetNrows(); indexParam++) {
0819 deltaystar(indexParam + offsetParam, 0) = deltaystarpart(indexParam, 0);
0820 }
0821 offsetParam += deltaystarpart.GetNrows();
0822 }
0823
0824 for (unsigned int iC = 0; iC < _constraints.size(); iC++) {
0825 TAbsFitConstraint* constraint = _constraints[iC];
0826
0827 if (constraint->getNPar() > 0) {
0828 const TMatrixD* alphastar = constraint->getParCurr();
0829 const TMatrixD* alpha = constraint->getParIni();
0830
0831 TMatrixD deltaalphastarpart(*alphastar);
0832 deltaalphastarpart -= *alpha;
0833
0834 for (int indexParam = 0; indexParam < deltaalphastarpart.GetNrows(); indexParam++) {
0835 deltaystar(indexParam + offsetParam, 0) = deltaalphastarpart(indexParam, 0);
0836 }
0837 offsetParam += deltaalphastarpart.GetNrows();
0838 }
0839 }
0840
0841 if (_verbosity >= 3) {
0842 printMatrix(deltaystar, "deltaystar");
0843 }
0844
0845
0846 TMatrixD fstar(_constraints.size(), 1);
0847 for (unsigned int indexConstr = 0; indexConstr < _constraints.size(); indexConstr++) {
0848 fstar(indexConstr, 0) = _constraints[indexConstr]->getCurrentValue();
0849 }
0850
0851
0852 _c.ResizeTo(fstar);
0853 _c = fstar;
0854 _c *= (-1.);
0855 TMatrixD Bdeltaystar(_B, TMatrixD::kMult, deltaystar);
0856 _c += Bdeltaystar;
0857 if (_nParA) {
0858 TMatrixD Adeltaastar(_A, TMatrixD::kMult, deltaastar);
0859 _c += Adeltaastar;
0860 }
0861
0862 return true;
0863 }
0864
0865 Bool_t TKinFitter::calcDeltaA() {
0866
0867
0868
0869 TMatrixD deltaA(_C32T, TMatrixD::kMult, _c);
0870 _deltaA.ResizeTo(deltaA);
0871
0872 if (_nbIter == 0) {
0873 _deltaAstar.ResizeTo(deltaA);
0874 _deltaAstar.Zero();
0875 } else
0876 _deltaAstar = _deltaA;
0877
0878 _deltaA = deltaA;
0879
0880 return true;
0881 }
0882
0883 Bool_t TKinFitter::calcDeltaY() {
0884
0885
0886
0887 TMatrixD deltaY(_C31T, TMatrixD::kMult, _c);
0888 _deltaY.ResizeTo(deltaY);
0889
0890 if (_nbIter == 0) {
0891 _deltaYstar.ResizeTo(deltaY);
0892 _deltaYstar.Zero();
0893 } else
0894 _deltaYstar = _deltaY;
0895
0896 _deltaY = deltaY;
0897
0898 return true;
0899 }
0900
0901 Bool_t TKinFitter::calcLambda() {
0902
0903
0904
0905 TMatrixD lambda(_C33, TMatrixD::kMult, _c);
0906 _lambda.ResizeTo(lambda);
0907 _lambda = lambda;
0908
0909 TMatrixD lambdaT(TMatrixD::kTransposed, _lambda);
0910 _lambdaT.ResizeTo(lambdaT);
0911 _lambdaT = lambdaT;
0912
0913 return true;
0914 }
0915
0916 Bool_t TKinFitter::calcVFit() {
0917
0918
0919
0920
0921
0922
0923
0924
0925 _lambdaVFit.ResizeTo(_C33);
0926 _lambdaVFit = _C33;
0927 _lambdaVFit *= -1.;
0928
0929
0930 Int_t nbRows = _C11.GetNrows();
0931 Int_t nbCols = _C11.GetNcols();
0932 if (_nParA > 0) {
0933 nbRows += _C21.GetNrows();
0934 nbCols += _C21T.GetNcols();
0935 }
0936 _yaVFit.ResizeTo(nbRows, nbCols);
0937
0938 for (int iRow = 0; iRow < nbRows; iRow++) {
0939 for (int iCol = 0; iCol < nbCols; iCol++) {
0940 if (iRow >= _C11.GetNrows()) {
0941 if (iCol >= _C11.GetNcols()) {
0942 _yaVFit(iRow, iCol) = _C22(iRow - _C11.GetNrows(), iCol - _C11.GetNcols());
0943 } else {
0944 _yaVFit(iRow, iCol) = _C21(iRow - _C11.GetNrows(), iCol);
0945 }
0946 } else {
0947 if (iCol >= _C11.GetNcols()) {
0948 _yaVFit(iRow, iCol) = _C21T(iRow, iCol - _C11.GetNcols());
0949 } else {
0950 _yaVFit(iRow, iCol) = _C11(iRow, iCol);
0951 }
0952 }
0953 }
0954 }
0955
0956 return true;
0957 }
0958
0959 void TKinFitter::applyVFit() {
0960
0961
0962 int offsetParam = 0;
0963 for (unsigned int indexParticle = 0; indexParticle < _measParticles.size(); indexParticle++) {
0964 TAbsFitParticle* particle = _measParticles[indexParticle];
0965 Int_t nbParams = particle->getNPar();
0966 TMatrixD vfit(nbParams, nbParams);
0967 for (Int_t c = 0; c < nbParams; c++) {
0968 for (Int_t r = 0; r < nbParams; r++) {
0969 vfit(r, c) = _yaVFit(r + offsetParam, c + offsetParam);
0970 }
0971 }
0972 particle->setCovMatrixFit(&vfit);
0973 offsetParam += nbParams;
0974 }
0975
0976 for (unsigned int indexConstraint = 0; indexConstraint < _constraints.size(); indexConstraint++) {
0977 TAbsFitConstraint* constraint = _constraints[indexConstraint];
0978 Int_t nbParams = constraint->getNPar();
0979 if (nbParams > 0) {
0980 TMatrixD vfit(nbParams, nbParams);
0981 for (Int_t c = 0; c < nbParams; c++) {
0982 for (Int_t r = 0; r < nbParams; r++) {
0983 vfit(r, c) = _yaVFit(r + offsetParam, c + offsetParam);
0984 }
0985 }
0986 constraint->setCovMatrixFit(&vfit);
0987 offsetParam += nbParams;
0988 }
0989 }
0990
0991 for (unsigned int indexParticle = 0; indexParticle < _unmeasParticles.size(); indexParticle++) {
0992 TAbsFitParticle* particle = _unmeasParticles[indexParticle];
0993 Int_t nbParams = particle->getNPar();
0994 TMatrixD vfit(nbParams, nbParams);
0995 for (Int_t c = 0; c < nbParams; c++) {
0996 for (Int_t r = 0; r < nbParams; r++) {
0997 vfit(r, c) = _yaVFit(r + offsetParam, c + offsetParam);
0998 }
0999 }
1000 particle->setCovMatrixFit(&vfit);
1001 offsetParam += nbParams;
1002 }
1003 }
1004
1005 Bool_t TKinFitter::applyDeltaA() {
1006
1007
1008 int offsetParam = 0;
1009 for (unsigned int indexParticle = 0; indexParticle < _unmeasParticles.size(); indexParticle++) {
1010 TAbsFitParticle* particle = _unmeasParticles[indexParticle];
1011 Int_t nbParams = particle->getNPar();
1012 TMatrixD params(nbParams, 1);
1013 for (Int_t index = 0; index < nbParams; index++) {
1014 params(index, 0) = _deltaA(index + offsetParam, 0);
1015 }
1016 particle->applycorr(¶ms);
1017 offsetParam += nbParams;
1018 }
1019
1020 return true;
1021 }
1022
1023 Bool_t TKinFitter::applyDeltaY() {
1024
1025
1026 int offsetParam = 0;
1027 for (unsigned int indexParticle = 0; indexParticle < _measParticles.size(); indexParticle++) {
1028 TAbsFitParticle* particle = _measParticles[indexParticle];
1029 Int_t nbParams = particle->getNPar();
1030 TMatrixD params(nbParams, 1);
1031 for (Int_t index = 0; index < nbParams; index++) {
1032 params(index, 0) = _deltaY(index + offsetParam, 0);
1033 }
1034 particle->applycorr(¶ms);
1035 offsetParam += nbParams;
1036 }
1037
1038 for (unsigned int indexConstraint = 0; indexConstraint < _constraints.size(); indexConstraint++) {
1039 TAbsFitConstraint* constraint = _constraints[indexConstraint];
1040 Int_t nbParams = constraint->getNPar();
1041 if (nbParams > 0) {
1042 TMatrixD params(nbParams, 1);
1043 for (Int_t index = 0; index < nbParams; index++) {
1044 params(index, 0) = _deltaY(index + offsetParam, 0);
1045 }
1046 constraint->applyDeltaAlpha(¶ms);
1047 offsetParam += nbParams;
1048 }
1049 }
1050
1051 return true;
1052 }
1053
1054 Double_t TKinFitter::getF() {
1055
1056
1057
1058 Double_t F = 0.;
1059 for (unsigned int indexConstr = 0; indexConstr < _constraints.size(); indexConstr++) {
1060 F += TMath::Abs(_constraints[indexConstr]->getCurrentValue());
1061 }
1062
1063 return F;
1064 }
1065
1066 Double_t TKinFitter::getS() {
1067
1068
1069
1070 Double_t S = 0.;
1071
1072 if (_nbIter > 0) {
1073 TMatrixD deltaYTVinv(_deltaY, TMatrixD::kTransposeMult, _Vinv);
1074 TMatrixD S2(deltaYTVinv, TMatrixD::kMult, _deltaY);
1075 S = S2(0, 0);
1076 }
1077
1078 return S;
1079 }
1080
1081 Bool_t TKinFitter::converged(Double_t F, Double_t prevS, Double_t currS) {
1082
1083
1084 Bool_t isConverged = false;
1085
1086
1087 isConverged = (F < _maxF);
1088
1089
1090 Double_t deltaS = currS - prevS;
1091 isConverged = isConverged && (TMath::Abs(deltaS) < _maxDeltaS);
1092
1093 return isConverged;
1094 }
1095
1096 TString TKinFitter::getStatusString() {
1097 TString statusstring = "";
1098
1099 switch (_status) {
1100 case -1: {
1101 statusstring = "NO FIT PERFORMED";
1102 break;
1103 }
1104 case 10: {
1105 statusstring = "RUNNING";
1106 break;
1107 }
1108 case -10: {
1109 statusstring = "ABORTED";
1110 break;
1111 }
1112 case 0: {
1113 statusstring = "CONVERGED";
1114 break;
1115 }
1116 case 1: {
1117 statusstring = "NOT CONVERGED";
1118 break;
1119 }
1120 }
1121
1122 return statusstring;
1123 }
1124
1125 void TKinFitter::print() {
1126 edm::LogVerbatim log("KinFitter");
1127 log << "\n"
1128 << "\n";
1129
1130 log << "Status: " << getStatusString() << " F=" << getF() << " S=" << getS() << " N=" << _nbIter
1131 << " NDF=" << getNDF() << "\n";
1132
1133 log << "measured particles: \n";
1134 Int_t parIndex = 0;
1135 for (unsigned int iP = 0; iP < _measParticles.size(); iP++) {
1136 TAbsFitParticle* particle = _measParticles[iP];
1137 Int_t nParP = particle->getNPar();
1138 const TMatrixD* par = particle->getParCurr();
1139 const TMatrixD* covP = particle->getCovMatrix();
1140 log << std::setw(3) << setiosflags(std::ios::right) << iP;
1141 log << std::setw(15) << setiosflags(std::ios::right) << particle->GetName();
1142 log << std::setw(3) << " ";
1143 for (int iPar = 0; iPar < nParP; iPar++) {
1144 if (iPar > 0) {
1145 log << setiosflags(std::ios::right) << std::setw(21) << " ";
1146 }
1147 TString colstr = "";
1148 colstr += parIndex;
1149 colstr += ":";
1150 log << std::setw(4) << colstr;
1151 log << std::setw(2) << " ";
1152 log << setiosflags(std::ios::left) << setiosflags(std::ios::scientific) << std::setprecision(3);
1153 log << std::setw(15) << (*par)(iPar, 0);
1154 if (_nbIter > 0 && _status < 10) {
1155 log << std::setw(15) << TMath::Sqrt(_yaVFit(iPar, iPar));
1156 } else {
1157 log << std::setw(15) << " ";
1158 }
1159 log << std::setw(15) << TMath::Sqrt((*covP)(iPar, iPar));
1160 log << "\n";
1161 parIndex++;
1162 }
1163 log << particle->getInfoString();
1164 }
1165
1166 log << "unmeasured particles: \n";
1167 parIndex = 0;
1168 for (unsigned int iP = 0; iP < _unmeasParticles.size(); iP++) {
1169 TAbsFitParticle* particle = _unmeasParticles[iP];
1170 Int_t nParP = particle->getNPar();
1171 const TMatrixD* par = particle->getParCurr();
1172 log << std::setw(3) << setiosflags(std::ios::right) << iP;
1173 log << std::setw(15) << particle->GetName();
1174 log << std::setw(3) << " ";
1175 for (int iPar = 0; iPar < nParP; iPar++) {
1176 if (iPar > 0) {
1177 log << setiosflags(std::ios::right) << std::setw(21) << " ";
1178 }
1179 TString colstr = "";
1180 colstr += parIndex;
1181 colstr += ":";
1182 log << std::setw(4) << colstr;
1183 log << std::setw(2) << " ";
1184 log << setiosflags(std::ios::left) << setiosflags(std::ios::scientific) << std::setprecision(3);
1185 log << std::setw(15) << (*par)(iPar, 0);
1186 if (_nbIter > 0 && _status < 10) {
1187 log << std::setw(15) << TMath::Sqrt(_yaVFit(iPar + _nParB, iPar + _nParB));
1188 } else {
1189 log << std::setw(15) << " ";
1190 }
1191 log << "\n";
1192 parIndex++;
1193 }
1194 log << particle->getInfoString();
1195 }
1196 log << "\n";
1197
1198 log << "constraints: \n";
1199 for (unsigned int indexConstr = 0; indexConstr < _constraints.size(); indexConstr++) {
1200 log << _constraints[indexConstr]->getInfoString();
1201 }
1202 log << "\n";
1203 }
1204
1205 void TKinFitter::printMatrix(const TMatrixD& matrix, const TString& name) {
1206
1207
1208
1209
1210 if (!matrix.IsValid()) {
1211 edm::LogWarning("InvalidMatrix") << "Matrix " << name << " is invalid.";
1212 return;
1213 }
1214
1215 edm::LogVerbatim log("KinFitter");
1216
1217 const Int_t nCols = matrix.GetNcols();
1218 const Int_t nRows = matrix.GetNrows();
1219 const Int_t colsPerSheet = 5;
1220 char topbar[100];
1221 Int_t nk = 6 + 13 * TMath::Min(colsPerSheet, nCols);
1222 for (Int_t i = 0; i < nk; i++)
1223 topbar[i] = '-';
1224 topbar[nk] = 0;
1225
1226 Int_t sw = (70 - name.Length()) / 2;
1227
1228 log << std::setfill('=') << std::setw(sw) << "= " << name << std::setw(sw) << std::left << " ="
1229 << "\n"
1230 << std::setfill(' ') << std::right << "\n";
1231
1232 log << nRows << "x" << nCols << " matrix is as follows \n";
1233
1234 for (Int_t iSheet = 0; iSheet < nCols; iSheet += colsPerSheet) {
1235 log << "\n"
1236 << " |";
1237 for (Int_t iCol = iSheet; iCol < nCols; iCol++) {
1238 if (iCol < iSheet + colsPerSheet)
1239 log << std::setw(8) << iCol << " |";
1240 }
1241 log << "\n" << topbar << " \n";
1242 for (Int_t iRow = 0; iRow < nRows; iRow++) {
1243 log << std::setw(4) << iRow << " |";
1244 for (Int_t iCol = iSheet; iCol < nCols; iCol++) {
1245 if (iCol < iSheet + colsPerSheet)
1246 log << std::setw(12) << matrix(iRow, iCol) << " ";
1247 }
1248 log << "\n";
1249 }
1250 }
1251 }