KinematicFit/src/KinematicConstrainedVertexUpdator.cc

0001 #include "RecoVertex/KinematicFit/interface/KinematicConstrainedVertexUpdator.h"
0002 #include "FWCore/MessageLogger/interface/MessageLogger.h"
0003 #include "FWCore/Utilities/interface/isFinite.h"
0004
0005 KinematicConstrainedVertexUpdator::KinematicConstrainedVertexUpdator() {
0006   vFactory = new KinematicVertexFactory();
0007   vConstraint = new VertexKinematicConstraint();
0008 }
0009
0010 KinematicConstrainedVertexUpdator::~KinematicConstrainedVertexUpdator() {
0011   delete vFactory;
0012   delete vConstraint;
0013 }
0014
0015 std::pair<std::pair<std::vector<KinematicState>, AlgebraicMatrix>, RefCountedKinematicVertex>
0016 KinematicConstrainedVertexUpdator::update(const AlgebraicVector& inPar,
0017                                           const AlgebraicMatrix& inCov,
0018                                           const std::vector<KinematicState>& lStates,
0019                                           const GlobalPoint& lPoint,
0020                                           MultiTrackKinematicConstraint* cs) const {
0021   const MagneticField* field = lStates.front().magneticField();
0022   AlgebraicMatrix d_matrix = vConstraint->parametersDerivative(lStates, lPoint);
0023   AlgebraicMatrix e_matrix = vConstraint->positionDerivative(lStates, lPoint);
0024   AlgebraicVector val_s = vConstraint->value(lStates, lPoint);
0025   int vSize = lStates.size();
0026
0027   //delta alpha
0028   AlgebraicVector d_a(7 * vSize + 3);
0029   d_a(1) = lPoint.x();
0030   d_a(2) = lPoint.y();
0031   d_a(3) = lPoint.z();
0032
0033   int cst = 0;
0034   for (std::vector<KinematicState>::const_iterator i = lStates.begin(); i != lStates.end(); i++) {
0035     AlgebraicVector lst_par = asHepVector<7>(i->kinematicParameters().vector());
0036     for (int j = 1; j < lst_par.num_row() + 1; j++) {
0037       d_a(3 + 7 * cst + j) = lst_par(j);
0038     }
0039     cst++;
0040   }
0041
0042   AlgebraicVector delta_alpha = inPar - d_a;
0043
0044   // cout<<"delta_alpha"<<delta_alpha<<endl;
0045   //resulting matrix of derivatives and vector of values.
0046   //their size  depends of number of tracks to analyze and number of
0047   //additional constraints to apply
0048   AlgebraicMatrix g;
0049   AlgebraicVector val;
0050   if (cs == nullptr) {
0051     //unconstrained vertex fitter case
0052     g = AlgebraicMatrix(2 * vSize, 7 * vSize + 3, 0);
0053     val = AlgebraicVector(2 * vSize);
0054
0055     //filling the derivative matrix
0056     g.sub(1, 1, e_matrix);
0057     g.sub(1, 4, d_matrix);
0058
0059     //filling the vector of values
0060     val = val_s;
0061   } else {
0062     //constrained vertex fitter case
0063     int n_eq = cs->numberOfEquations();
0064     g = AlgebraicMatrix(n_eq + 2 * vSize, 7 * vSize + 3, 0);
0065     val = AlgebraicVector(n_eq + 2 * vSize);
0066     AlgebraicMatrix n_x = cs->positionDerivative(lStates, lPoint);
0067     AlgebraicMatrix n_alpha = cs->parametersDerivative(lStates, lPoint);
0068     AlgebraicVector c_val = cs->value(lStates, lPoint);
0069
0070     //filling the derivative matrix
0071     //   cout<<"n_x:"<<n_x<<endl;
0072     //   cout<<"n_alpha"<<n_alpha<<endl;
0073
0074     g.sub(1, 1, n_x);
0075     g.sub(1, 4, n_alpha);
0076     g.sub(n_eq + 1, 1, e_matrix);
0077     g.sub(n_eq + 1, 4, d_matrix);
0078
0079     //filling the vector of values
0080     for (int i = 1; i < n_eq + 1; i++) {
0081       val(i) = c_val(i);
0082     }
0083     for (int i = 1; i < (2 * vSize + 1); i++) {
0084       val(i + n_eq) = val_s(i);
0085     }
0086   }
0087
0088   //check for NaN
0089   for (int i = 1; i <= val.num_row(); ++i) {
0090     if (edm::isNotFinite(val(i))) {
0091       LogDebug("KinematicConstrainedVertexUpdator") << "catched NaN.\n";
0092       return std::pair<std::pair<std::vector<KinematicState>, AlgebraicMatrix>, RefCountedKinematicVertex>(
0093           std::pair<std::vector<KinematicState>, AlgebraicMatrix>(std::vector<KinematicState>(), AlgebraicMatrix(1, 0)),
0094           RefCountedKinematicVertex());
0095     }
0096   }
0097
0098   //debug feature
0099   AlgebraicSymMatrix in_cov_sym(7 * vSize + 3, 0);
0100
0101   for (int i = 1; i < 7 * vSize + 4; ++i) {
0102     for (int j = 1; j < 7 * vSize + 4; ++j) {
0103       if (i <= j)
0104         in_cov_sym(i, j) = inCov(i, j);
0105     }
0106   }
0107
0108   //debug code
0109   AlgebraicSymMatrix v_g_sym = in_cov_sym.similarity(g);
0110
0111   int ifl1 = 0;
0112   v_g_sym.invert(ifl1);
0113   if (ifl1 != 0) {
0114     LogDebug("KinematicConstrainedVertexFitter") << "Fit failed: unable to invert SYM gain matrix\n";
0115     return std::pair<std::pair<std::vector<KinematicState>, AlgebraicMatrix>, RefCountedKinematicVertex>(
0116         std::pair<std::vector<KinematicState>, AlgebraicMatrix>(std::vector<KinematicState>(), AlgebraicMatrix(1, 0)),
0117         RefCountedKinematicVertex());
0118   }
0119
0120   // delta alpha is now valid!
0121   //full math case now!
0122   AlgebraicVector lambda = v_g_sym * (g * delta_alpha + val);
0123
0124   //final parameters
0125   AlgebraicVector finPar = inPar - in_cov_sym * g.T() * lambda;
0126
0127   //covariance matrix business:
0128   AlgebraicMatrix mFactor = in_cov_sym * (v_g_sym.similarityT(g)) * in_cov_sym;
0129
0130   //refitted covariance
0131   AlgebraicMatrix rCov = in_cov_sym - mFactor;
0132
0133   //symmetric covariance:
0134   AlgebraicSymMatrix r_cov_sym(7 * vSize + 3, 0);
0135   for (int i = 1; i < 7 * vSize + 4; ++i) {
0136     for (int j = 1; j < 7 * vSize + 4; ++j) {
0137       if (i <= j)
0138         r_cov_sym(i, j) = rCov(i, j);
0139     }
0140   }
0141
0142   AlgebraicSymMatrix pCov = r_cov_sym.sub(1, 3);
0143
0144   // chi2
0145   AlgebraicVector chi = lambda.T() * (g * delta_alpha + val);
0146
0147   //this is ndf without significant prior
0148   //vertex so -3 factor exists here
0149   float ndf = 2 * vSize - 3;
0150   if (cs != nullptr) {
0151     ndf += cs->numberOfEquations();
0152   }
0153
0154   //making resulting vertex
0155   GlobalPoint vPos(finPar(1), finPar(2), finPar(3));
0156   VertexState st(vPos, GlobalError(asSMatrix<3>(pCov)));
0157   RefCountedKinematicVertex rVtx = vFactory->vertex(st, chi(1), ndf);
0158
0159   //making refitted states of Kinematic Particles
0160   int i_int = 0;
0161   std::vector<KinematicState> ns;
0162   for (std::vector<KinematicState>::const_iterator i_st = lStates.begin(); i_st != lStates.end(); i_st++) {
0163     AlgebraicVector7 newPar;
0164     for (int i = 0; i < 7; i++) {
0165       newPar(i) = finPar(4 + i_int * 7 + i);
0166     }
0167
0168     AlgebraicSymMatrix nCovariance = r_cov_sym.sub(4 + i_int * 7, 10 + i_int * 7);
0169     TrackCharge chl = i_st->particleCharge();
0170     KinematicParameters nrPar(newPar);
0171     KinematicParametersError nrEr(asSMatrix<7>(nCovariance));
0172     KinematicState newState(nrPar, nrEr, chl, field);
0173     ns.push_back(newState);
0174     i_int++;
0175   }
0176   std::pair<std::vector<KinematicState>, AlgebraicMatrix> ns_m(ns, rCov);
0177   return std::pair<std::pair<std::vector<KinematicState>, AlgebraicMatrix>, RefCountedKinematicVertex>(ns_m, rVtx);
0178 }