Back to home page

Project CMSSW displayed by LXR
 
 

    

File indexing completed on 2023-03-17 11:15:59 

0001 // Classname: TKinFitter
0002 // Author: Jan E. Sundermann, Verena Klose (TU Dresden)
0003 
0004 //________________________________________________________________
0005 //
0006 // TKinFitter::
0007 // --------------------
0008 //
0009 // Class to perform kinematic fit with non-linear constraints
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   // reset all internal parameters of the fitter
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   // Set to default values
0142   _maxNbIter = 50;
0143   _maxDeltaS = 5e-3;
0144   _maxF = 1e-4;
0145 }
0146 
0147 void TKinFitter::resetStatus() {
0148   // reset status of the fit
0149 
0150   _status = -1;
0151   _nbIter = 0;
0152 }
0153 
0154 void TKinFitter::resetParams() {
0155   // reset all particles to their initial parameters
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   // count number of measured parameters
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   // count number of unmeasured parameters
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   // add one measured particle
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   // add many measured particles
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   // add one unmeasured particle
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   // add many unmeasured particles
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   // add a constraint
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   // Set verbosity of the fitter:
0306   // 0: quiet
0307   // 1: print information before and after the fit
0308   // 2: print output for every iteration of the fit
0309   // 3: print debug information
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   // Perform the fit
0320   // Returns:
0321   // 0: converged
0322   // 1: not converged
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   // Calculate covariance matrix V
0335   calcV();
0336 
0337   // print status
0338   if (_verbosity >= 1) {
0339     print();
0340   }
0341 
0342   do {
0343     // Reset status to "RUNNING"
0344     // (if it was not aborted already due to singular covariance matrix)
0345     if (_status != -10) {
0346       _status = 10;
0347     }
0348 
0349     // Calculate matrices
0350     calcB();
0351     calcVB();
0352     if (_nParA > 0) {
0353       calcA();
0354       calcVA();
0355       calcC32();
0356     }
0357     calcC31();
0358     calcC33();
0359     calcC();
0360 
0361     // Calculate corretion for a, y, and lambda
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     // Apply calculated corrections to measured and unmeasured particles
0385     if (_nParA > 0) {
0386       applyDeltaA();
0387     }
0388     applyDeltaY();
0389 
0390     _nbIter++;
0391 
0392     //calculate F and S
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     // Reduce step width if F is not getting smaller
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     // Test convergence
0426     isConverged = converged(currF, prevS, currS);
0427 
0428   } while ((!isConverged) && (_nbIter < _maxNbIter) && (_status != -10));
0429 
0430   // Calculate covariance matrices
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   // Set status information
0448   if (isConverged) {
0449     _status = 0;
0450   } else if (_status != -10) {
0451     _status = 1;
0452   }
0453 
0454   // print status
0455   if (_verbosity >= 1) {
0456     print();
0457   }
0458 
0459   return _status;
0460 }
0461 
0462 void TKinFitter::setCovMatrix(TMatrixD& V) {
0463   // Set the covariance matrix of the measured particles
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   // Combines the covariance matrices of all measured particles
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   // Calculate the Jacobi matrix of unmeasured parameters df_i/da_i
0523   // Row i contains the derivatives of constraint f_i. Column q contains
0524   // the derivative wrt. a_q.
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       // Calculate matrix product  df/dP * dP/dy = (df/dr, df/dtheta, df/dphi, ...)
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   // Calculate transposed matrix
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   // Calculate the Jacobi matrix of measured parameters df_i/da_i
0558   // Row i contains the derivatives of constraint f_i. Column q contains
0559   // the derivative wrt. a_q.
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       // Calculate matrix product  df/dP * dP/dy = (df/dr, df/dtheta, df/dphi, ...)
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   // Calculate the matrix V_B = (B*V*B^T)^-1
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   // Calculate the matrix VA = (A^T*VB*A)
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   // Calculate the matrix C11 = V^(-1) - V^(-1)*BT*VB*B*V^(-1) + V^(-1)*BT*VB*A*VA^(-1)*AT*VB*B*V^(-1)
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   // Calculate the matrix  C21 = -VA^(-1)*AT*VB*B*V^(-1)
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   //  Calculate the matrix C22 = VA^(-1)
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   // Calculate the matrix  C31 = VB*B*V^(-1) - VB*A*VA^(-1)*AT*VB*B*V^(-1)
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   // Calculate the matrix  C32 = VB*A*VA^(-1)
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   // Calculate the matrix C33 = -VB + VB*A*VA^(-1)*AT*VB
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   // Calculate the matrix c = A*deltaAStar + B*deltaYStar - fStar
0783 
0784   int offsetParam = 0;
0785 
0786   // calculate delta(a*), = 0 in the first iteration
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   // calculate delta(y*), = 0 in the first iteration
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   // calculate f*
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   // calculate c
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   // Calculate the matrix deltaA = C32T * c
0867   // (corrections to unmeasured parameters)
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   // Calculate the matrix deltaY = C31T * c
0885   // (corrections to measured parameters)
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   // Calculate the matrix Lambda = C33 * c
0903   // (Lagrange Multipliers)
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   // Calculate the covariance matrix of fitted parameters
0918   //
0919   // Vfit(y) = ( C11  C21T )
0920   //     (a)   ( C21  C22  )
0921   //
0922   // Vfit(lambda) = (-C33)
0923 
0924   // Calculate covariance matrix of lambda
0925   _lambdaVFit.ResizeTo(_C33);
0926   _lambdaVFit = _C33;
0927   _lambdaVFit *= -1.;
0928 
0929   // Calculate combined covariance matrix of y and a
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   // apply fit covariance matrix to measured and unmeasured  particles
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   //apply corrections to unmeasured particles
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(&params);
1017     offsetParam += nbParams;
1018   }
1019 
1020   return true;
1021 }
1022 
1023 Bool_t TKinFitter::applyDeltaY() {
1024   //apply corrections to measured particles
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(&params);
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(&params);
1047       offsetParam += nbParams;
1048     }
1049   }
1050 
1051   return true;
1052 }
1053 
1054 Double_t TKinFitter::getF() {
1055   // calculate current absolut value of constraints
1056   // F = Sum[ Abs(f_k( aStar, yStar)) ]
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   // calculate current value of Chi2
1068   // S = deltaYT * V^-1 * deltaY
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   // check whether convergence criteria are fulfilled
1083 
1084   Bool_t isConverged = false;
1085 
1086   // calculate F, delta(a) and delta(y) already applied
1087   isConverged = (F < _maxF);
1088 
1089   // Calculate current Chi^2 and delta(S)
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   // Print status of fit
1130   log << "Status: " << getStatusString() << "   F=" << getF() << "   S=" << getS() << "   N=" << _nbIter
1131       << "   NDF=" << getNDF() << "\n";
1132   // Print measured particles
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   // Print unmeasured particles
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   // Print constraints
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   // produce a tabular printout for matrices
1207   // this function is a modified version of Root's TMatrixTBase<Element>::Print method
1208   // which could not be used together with the MessageLogger
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 }
[ Source navigation ] [ Diff markup ] [ Identifier search ] [ general search ]

This page was automatically generated by the 2.2.1 LXR engine.

The LXR team
Valid CSS 2.1! Valid HTML 4.01!