KinematicFit/src/LagrangeParentParticleFitter.cc

0001 #include "RecoVertex/KinematicFit/interface/LagrangeParentParticleFitter.h"
0002 #include "RecoVertex/KinematicFitPrimitives/interface/KinematicVertexFactory.h"
0003 #include "RecoVertex/KinematicFit/interface/InputSort.h"
0004 #include "FWCore/MessageLogger/interface/MessageLogger.h"
0005
0006 LagrangeParentParticleFitter::LagrangeParentParticleFitter() { defaultParameters(); }
0007
0008 /*
0009 RefCountedKinematicTree LagrangeParentParticleFitter::fit(RefCountedKinematicTree tree, KinematicConstraint * cs) const
0010 {
0011 //fitting top paticle only
0012  tree->movePointerToTheTop();
0013  RefCountedKinematicParticle particle = tree->currentParticle();
0014  RefCountedKinematicVertex inVertex = tree->currentDecayVertex();
0015
0016 //parameters and covariance matrix of the particle to fit
0017  AlgebraicVector par = particle->currentState().kinematicParameters().vector();
0018  AlgebraicSymMatrix cov = particle->currentState().kinematicParametersError().matrix();
0019
0020 //chi2 and ndf
0021  float chi = 0.;
0022  float ndf = 0.;
0023
0024 //constraint
0025 //here it is defined at 7-point
0026 //taken just at current state parameters
0027  AlgebraicVector vl;
0028  AlgebraicMatrix dr;
0029  AlgebraicVector dev;
0030
0031 //initial expansion point
0032  AlgebraicVector exPoint = particle->currentState().kinematicParameters().vector();
0033
0034 //refitted parameters and covariance matrix:
0035 //simple and symmetric
0036  AlgebraicVector refPar;
0037  AlgebraicSymMatrix refCovS;
0038
0039  int nstep = 0;
0040  double df = 0.;
0041  do{
0042    chi = particle->chiSquared();
0043    ndf = particle->degreesOfFreedom();
0044    df =  0.;
0045
0046 //derivative and value at expansion point
0047    vl = cs->value(exPoint).first;
0048    dr = cs->derivative(exPoint).first;
0049    dev = cs->deviations();
0050
0051 //residual between expansion and current parameters
0052 //0 at the first step
0053    AlgebraicVector delta_alpha = par - exPoint;
0054
0055 //parameters needed for refit
0056 // v_d = (D * V_alpha & * D.T)^(-1)
0057    AlgebraicMatrix drt = dr.T();
0058    AlgebraicMatrix v_d = dr * cov * drt;
0059    int ifail = 0;
0060    v_d.invert(ifail);
0061    if(ifail != 0) throw VertexException("ParentParticleFitter::error inverting covariance matrix");
0062
0063 //lagrangian multipliers
0064 //lambda = V_d * (D * delta_alpha + d)
0065    AlgebraicVector lambda = v_d * (dr*delta_alpha + vl);
0066
0067 //refitted parameters
0068    refPar = par - (cov * drt * lambda);
0069
0070 //refitted covariance: simple and SymMatrix
0071    refCovS = cov;
0072    AlgebraicMatrix sPart = drt * v_d * dr;
0073    AlgebraicMatrix covF = cov * sPart * cov;
0074    AlgebraicSymMatrix sCovF(7,0);
0075
0076    for(int i = 1; i<8; i++)
0077    {
0078      for(int j = 1; j<8; j++)
0079      {if(i<=j) sCovF(i,j) = covF(i,j);}
0080    }
0081    refCovS -= sCovF;
0082
0083    for(int i = 1; i < 8; i++)
0084    {refCovS(i,i) += dev(i);}
0085
0086 //chiSquared
0087    chi +=  (lambda.T() * (dr*delta_alpha + vl))(1);
0088    ndf +=  cs->numberOfEquations();
0089
0090 //new expansionPoint
0091    exPoint = refPar;
0092    AlgebraicVector vlp = cs->value(exPoint).first;
0093    for(int i = 1; i< vl.num_row();i++)
0094    {df += std::abs(vlp(i));}
0095    nstep++;
0096   }while(df>theMaxDiff && nstep<theMaxStep);
0097
0098 //!!!!!!!!!!!!!!!here the math part is finished!!!!!!!!!!!!!!!!!!!!!!
0099 //creating an output KinematicParticle
0100 //and putting it on its place in the tree
0101   KinematicParameters param(refPar);
0102   KinematicParametersError er(refCovS);
0103   KinematicState kState(param,er,particle->initialState().particleCharge());
0104   RefCountedKinematicParticle refParticle  = particle->refittedParticle(kState,chi,ndf,cs->clone());
0105   tree->replaceCurrentParticle(refParticle);
0106
0107 //replacing the  vertex with its refitted version
0108   GlobalPoint nvPos(param.vector()(1), param.vector()(2), param.vector()(3));
0109   AlgebraicSymMatrix nvMatrix = er.matrix().sub(1,3);
0110   GlobalError nvError(asSMatrix<3>(nvMatrix));
0111   VertexState vState(nvPos, nvError, 1.0);
0112   KinematicVertexFactory vFactory;
0113   RefCountedKinematicVertex nVertex = vFactory.vertex(vState, inVertex,chi,ndf);
0114   tree->replaceCurrentVertex(nVertex);
0115
0116   return tree;
0117 }
0118 */
0119
0120 std::vector<RefCountedKinematicTree> LagrangeParentParticleFitter::fit(
0121     const std::vector<RefCountedKinematicTree>& trees, KinematicConstraint* cs) const {
0122   InputSort iSort;
0123   std::vector<RefCountedKinematicParticle> prt = iSort.sort(trees);
0124   int nStates = prt.size();
0125
0126   //making full initial parameters and covariance
0127   AlgebraicVector part(7 * nStates, 0);
0128   AlgebraicSymMatrix cov(7 * nStates, 0);
0129
0130   AlgebraicVector chi_in(nStates, 0);
0131   AlgebraicVector ndf_in(nStates, 0);
0132   int l_c = 0;
0133   for (std::vector<RefCountedKinematicParticle>::const_iterator i = prt.begin(); i != prt.end(); i++) {
0134     AlgebraicVector7 lp = (*i)->currentState().kinematicParameters().vector();
0135     for (int j = 1; j != 8; j++) {
0136       part(7 * l_c + j) = lp(j - 1);
0137     }
0138     AlgebraicSymMatrix lc = asHepMatrix<7>((*i)->currentState().kinematicParametersError().matrix());
0139     cov.sub(7 * l_c + 1, lc);
0140     chi_in(l_c + 1) = (*i)->chiSquared();
0141     ndf_in(l_c + 1) = (*i)->degreesOfFreedom();
0142     l_c++;
0143   }
0144   //refitted parameters and covariance matrix:
0145   //simple and symmetric
0146   AlgebraicVector refPar;
0147   AlgebraicSymMatrix refCovS;
0148
0149   //constraint values, derivatives and deviations:
0150   AlgebraicVector vl;
0151   AlgebraicMatrix dr;
0152   AlgebraicVector dev;
0153   int nstep = 0;
0154   double df = 0.;
0155   AlgebraicVector exPoint = part;
0156
0157   //this piece of code should later be refactored:
0158   // The algorithm is the same as above, but the
0159   // number of refitted particles is >1. Smart way of
0160   // refactoring should be chosen for it.
0161   AlgebraicVector chi;
0162   AlgebraicVector ndf;
0163   // cout<<"Starting the main loop"<<endl;
0164   do {
0165     df = 0.;
0166     chi = chi_in;
0167     ndf = ndf_in;
0168     //  cout<<"Iterational linearization point: "<<exPoint<<endl;
0169     vl = cs->value(exPoint).first;
0170     dr = cs->derivative(exPoint).first;
0171     dev = cs->deviations(nStates);
0172
0173     //  cout<<"The value : "<<vl<<endl;
0174     //  cout<<"The derivative: "<<dr<<endl;
0175     //  cout<<"deviations: "<<dev<<endl;
0176     //  cout<<"covariance "<<cov<<endl;
0177
0178     //residual between expansion and current parameters
0179     //0 at the first step
0180     AlgebraicVector delta_alpha = part - exPoint;
0181
0182     //parameters needed for refit
0183     // v_d = (D * V_alpha & * D.T)^(-1)
0184     AlgebraicMatrix drt = dr.T();
0185     AlgebraicMatrix v_d = dr * cov * drt;
0186     int ifail = 0;
0187     v_d.invert(ifail);
0188     if (ifail != 0) {
0189       LogDebug("KinematicConstrainedVertexFitter") << "Fit failed: unable to invert covariance matrix\n";
0190       return std::vector<RefCountedKinematicTree>();
0191     }
0192
0193     //lagrangian multipliers
0194     //lambda = V_d * (D * delta_alpha + d)
0195     AlgebraicVector lambda = v_d * (dr * delta_alpha + vl);
0196
0197     //refitted parameters
0198     refPar = part - (cov * drt * lambda);
0199
0200     //refitted covariance: simple and SymMatrix
0201     refCovS = cov;
0202     AlgebraicMatrix sPart = drt * v_d * dr;
0203     AlgebraicMatrix covF = cov * sPart * cov;
0204
0205     //total covariance symmatrix
0206     AlgebraicSymMatrix sCovF(nStates * 7, 0);
0207     for (int i = 1; i < nStates * 7 + 1; ++i) {
0208       for (int j = 1; j < nStates * 7 + 1; j++) {
0209         if (i <= j)
0210           sCovF(i, j) = covF(i, j);
0211       }
0212     }
0213
0214     refCovS -= sCovF;
0215
0216     //  cout<<"Fitter: refitted covariance "<<refCovS<<endl;
0217     for (int i = 1; i < nStates + 1; i++) {
0218       for (int j = 1; j < 8; j++) {
0219         refCovS((i - 1) + j, (i - 1) + j) += dev(j);
0220       }
0221     }
0222
0223     //chiSquared
0224     for (int k = 1; k < nStates + 1; k++) {
0225       chi(k) += (lambda.T() * (dr * delta_alpha + vl))(1);
0226       ndf(k) += cs->numberOfEquations();
0227     }
0228     //new expansionPoint
0229     exPoint = refPar;
0230     AlgebraicVector vlp = cs->value(exPoint).first;
0231     for (int i = 1; i < vl.num_row(); i++) {
0232       df += std::abs(vlp(i));
0233     }
0234     nstep++;
0235   } while (df > theMaxDiff && nstep < theMaxStep);
0236   //here math and iterative part is finished, starting an output production
0237   //creating an output KinematicParticle and putting it on its place in the tree
0238
0239   //vector of refitted particles and trees
0240   std::vector<RefCountedKinematicParticle> refPart;
0241   std::vector<RefCountedKinematicTree> refTrees = trees;
0242
0243   int j = 1;
0244   std::vector<RefCountedKinematicTree>::const_iterator tr = refTrees.begin();
0245   for (std::vector<RefCountedKinematicParticle>::const_iterator i = prt.begin(); i != prt.end(); i++) {
0246     AlgebraicVector7 lRefPar;
0247     for (int k = 1; k < 8; k++) {
0248       lRefPar(k - 1) = refPar((j - 1) * 7 + k);
0249     }
0250     AlgebraicSymMatrix77 lRefCovS = asSMatrix<7>(refCovS.sub((j - 1) * 7 + 1, (j - 1) * 7 + 7));
0251
0252     //new refitted parameters and covariance
0253     KinematicParameters param(lRefPar);
0254     KinematicParametersError er(lRefCovS);
0255     KinematicState kState(param, er, (*i)->initialState().particleCharge(), (**i).magneticField());
0256     RefCountedKinematicParticle refParticle = (*i)->refittedParticle(kState, chi(j), ndf(j), cs->clone());
0257
0258     //replacing particle itself
0259     (*tr)->findParticle(*i);
0260     RefCountedKinematicVertex inVertex = (*tr)->currentDecayVertex();
0261     (*tr)->replaceCurrentParticle(refParticle);
0262
0263     //replacing the  vertex with its refitted version
0264     GlobalPoint nvPos(param.position());
0265     AlgebraicSymMatrix nvMatrix = asHepMatrix<7>(er.matrix()).sub(1, 3);
0266     GlobalError nvError(asSMatrix<3>(nvMatrix));
0267     VertexState vState(nvPos, nvError, 1.0);
0268     KinematicVertexFactory vFactory;
0269     RefCountedKinematicVertex nVertex = vFactory.vertex(vState, inVertex, chi(j), ndf(j));
0270     (*tr)->replaceCurrentVertex(nVertex);
0271     tr++;
0272     j++;
0273   }
0274   return refTrees;
0275 }
0276
0277 void LagrangeParentParticleFitter::setParameters(const edm::ParameterSet& pSet) {
0278   theMaxDiff = pSet.getParameter<double>("maxDistance");
0279   theMaxStep = pSet.getParameter<int>("maxNbrOfIterations");
0280   ;
0281 }
0282
0283 void LagrangeParentParticleFitter::defaultParameters() {
0284   theMaxDiff = 0.001;
0285   theMaxStep = 100;
0286 }