LagrangeParentParticleFitter.cc

Go to the documentation of this file.

#include "RecoVertex/KinematicFit/interface/LagrangeParentParticleFitter.h"
#include "RecoVertex/KinematicFitPrimitives/interface/KinematicVertexFactory.h"
#include "RecoVertex/KinematicFit/interface/InputSort.h"
#include "FWCore/MessageLogger/interface/MessageLogger.h"

LagrangeParentParticleFitter::LagrangeParentParticleFitter()
{defaultParameters();}

/*
RefCountedKinematicTree LagrangeParentParticleFitter::fit(RefCountedKinematicTree tree, KinematicConstraint * cs) const
{
//fitting top paticle only
 tree->movePointerToTheTop();
 RefCountedKinematicParticle particle = tree->currentParticle(); 
 RefCountedKinematicVertex inVertex = tree->currentDecayVertex();

//parameters and covariance matrix of the particle to fit 
 AlgebraicVector par = particle->currentState().kinematicParameters().vector();
 AlgebraicSymMatrix cov = particle->currentState().kinematicParametersError().matrix();

//chi2 and ndf
 float chi = 0.;
 float ndf = 0.;
 
//constraint
//here it is defined at 7-point
//taken just at current state parameters
 AlgebraicVector vl;
 AlgebraicMatrix dr;
 AlgebraicVector dev;
 
//initial expansion point 
 AlgebraicVector exPoint = particle->currentState().kinematicParameters().vector();
 
//refitted parameters and covariance matrix:
//simple and symmetric 
 AlgebraicVector refPar;
 AlgebraicSymMatrix refCovS; 
 
 int nstep = 0;
 double df = 0.;
 do{ 
   chi = particle->chiSquared();
   ndf = particle->degreesOfFreedom();
   df =  0.;
   
//derivative and value at expansion point  
   vl = cs->value(exPoint).first;
   dr = cs->derivative(exPoint).first;
   dev = cs->deviations();  
   
//residual between expansion and current parameters
//0 at the first step   
   AlgebraicVector delta_alpha = par - exPoint;  
  
//parameters needed for refit
// v_d = (D * V_alpha & * D.T)^(-1)   
   AlgebraicMatrix drt = dr.T();
   AlgebraicMatrix v_d = dr * cov * drt;
   int ifail = 0;
   v_d.invert(ifail);
   if(ifail != 0) throw VertexException("ParentParticleFitter::error inverting covariance matrix");
  
//lagrangian multipliers  
//lambda = V_d * (D * delta_alpha + d)
   AlgebraicVector lambda = v_d * (dr*delta_alpha + vl);
  
//refitted parameters
   refPar = par - (cov * drt * lambda);  
  
//refitted covariance: simple and SymMatrix  
   refCovS = cov;
   AlgebraicMatrix sPart = drt * v_d * dr;
   AlgebraicMatrix covF = cov * sPart * cov; 
   AlgebraicSymMatrix sCovF(7,0);
  
   for(int i = 1; i<8; i++)
   {
     for(int j = 1; j<8; j++)
     {if(i<=j) sCovF(i,j) = covF(i,j);}
   }
   refCovS -= sCovF; 
  
   for(int i = 1; i < 8; i++)
   {refCovS(i,i) += dev(i);}
  
//chiSquared  
   chi +=  (lambda.T() * (dr*delta_alpha + vl))(1);
   ndf +=  cs->numberOfEquations();
   
//new expansionPoint
   exPoint = refPar;
   AlgebraicVector vlp = cs->value(exPoint).first;
   for(int i = 1; i< vl.num_row();i++)
   {df += std::abs(vlp(i));}
   nstep++; 
  }while(df>theMaxDiff && nstep<theMaxStep);
  
//creating an output KinematicParticle 
//and putting it on its place in the tree
  KinematicParameters param(refPar);
  KinematicParametersError er(refCovS);
  KinematicState kState(param,er,particle->initialState().particleCharge());
  RefCountedKinematicParticle refParticle  = particle->refittedParticle(kState,chi,ndf,cs->clone());                             
  tree->replaceCurrentParticle(refParticle);
  
//replacing the  vertex with its refitted version
  GlobalPoint nvPos(param.vector()(1), param.vector()(2), param.vector()(3)); 
  AlgebraicSymMatrix nvMatrix = er.matrix().sub(1,3);
  GlobalError nvError(asSMatrix<3>(nvMatrix));
  VertexState vState(nvPos, nvError, 1.0);
  KinematicVertexFactory vFactory;
  RefCountedKinematicVertex nVertex = vFactory.vertex(vState, inVertex,chi,ndf);
  tree->replaceCurrentVertex(nVertex);
   
  return tree;
}
*/

std::vector<RefCountedKinematicTree>  LagrangeParentParticleFitter::fit(const std::vector<RefCountedKinematicTree> &trees, 
                                                                           KinematicConstraint * cs)const                     
{
 
 InputSort iSort;
 std::vector<RefCountedKinematicParticle> prt = iSort.sort(trees);
 int nStates = prt.size(); 

//making full initial parameters and covariance
 AlgebraicVector part(7*nStates,0);
 AlgebraicSymMatrix cov(7*nStates,0);
 
 AlgebraicVector chi_in(nStates,0);
 AlgebraicVector ndf_in(nStates,0);
 int l_c=0;
 for(std::vector<RefCountedKinematicParticle>::const_iterator i = prt.begin(); i != prt.end(); i++)
 {
  AlgebraicVector7 lp = (*i)->currentState().kinematicParameters().vector();
  for(int j = 1; j != 8; j++){part(7*l_c + j) = lp(j-1);}
  AlgebraicSymMatrix lc= asHepMatrix<7>((*i)->currentState().kinematicParametersError().matrix());
  cov.sub(7*l_c+1,lc);
  chi_in(l_c+1) = (*i)->chiSquared();
  ndf_in(l_c+1) = (*i)->degreesOfFreedom();
  l_c++;
 }
//refitted parameters and covariance matrix:
//simple and symmetric 
 AlgebraicVector refPar;
 AlgebraicSymMatrix refCovS; 
 
//constraint values, derivatives and deviations:  
 AlgebraicVector vl;
 AlgebraicMatrix dr;
 AlgebraicVector dev;
 int nstep = 0;
 double df = 0.; 
 AlgebraicVector exPoint = part;
 
//this piece of code should later be refactored:
// The algorithm is the same as above, but the
// number of refitted particles is >1. Smart way of
// refactoring should be chosen for it.
 AlgebraicVector chi;
 AlgebraicVector ndf;
// cout<<"Starting the main loop"<<endl;
 do{
  df = 0.;
  chi = chi_in;
  ndf = ndf_in;
//  cout<<"Iterational linearization point: "<<exPoint<<endl;
  vl = cs->value(exPoint).first;
  dr = cs->derivative(exPoint).first;
  dev = cs->deviations(nStates);

//  cout<<"The value : "<<vl<<endl;
//  cout<<"The derivative: "<<dr<<endl;
//  cout<<"deviations: "<<dev<<endl;
//  cout<<"covariance "<<cov<<endl;

//residual between expansion and current parameters
//0 at the first step   
  AlgebraicVector delta_alpha = part - exPoint;  

//parameters needed for refit
// v_d = (D * V_alpha & * D.T)^(-1)   
   AlgebraicMatrix drt = dr.T();
   AlgebraicMatrix v_d = dr * cov * drt;
   int ifail = 0;
   v_d.invert(ifail);
   if(ifail != 0) {
     LogDebug("KinematicConstrainedVertexFitter")
    << "Fit failed: unable to invert covariance matrix\n";
     return std::vector<RefCountedKinematicTree>();
   }
  
//lagrangian multipliers  
//lambda = V_d * (D * delta_alpha + d)
   AlgebraicVector lambda = v_d * (dr*delta_alpha + vl);
  
//refitted parameters
   refPar = part - (cov * drt * lambda);  
  
//refitted covariance: simple and SymMatrix  
   refCovS = cov;
   AlgebraicMatrix sPart = drt * v_d * dr;
   AlgebraicMatrix covF = cov * sPart * cov; 

//total covariance symmatrix    
  AlgebraicSymMatrix sCovF(nStates*7,0);
  for(int i = 1; i< nStates*7 +1; ++i)
  {
   for(int j = 1; j< nStates*7 +1; j++)
   {if(i<=j) sCovF(i,j) = covF(i,j);}
  }
  
  refCovS -= sCovF; 
  
//  cout<<"Fitter: refitted covariance "<<refCovS<<endl;
  for(int i = 1; i < nStates+1; i++)
  {for(int j = 1; j<8; j++){refCovS((i-1)+j,(i-1)+j) += dev(j);}} 
  
//chiSquared  
  for(int k =1; k<nStates+1; k++)
  {
   chi(k) +=  (lambda.T() * (dr*delta_alpha + vl))(1);
   ndf(k) +=  cs->numberOfEquations();
  }  
//new expansionPoint
  exPoint = refPar;
  AlgebraicVector vlp = cs->value(exPoint).first;
  for(int i = 1; i< vl.num_row();i++)
  {df += std::abs(vlp(i));}
  nstep++; 
 }while(df>theMaxDiff && nstep<theMaxStep);
//here math and iterative part is finished, starting an output production
//creating an output KinematicParticle and putting it on its place in the tree
 
//vector of refitted particles and trees 
 std::vector<RefCountedKinematicParticle> refPart;
 std::vector<RefCountedKinematicTree> refTrees = trees;
 
 int j=1;
 std::vector<RefCountedKinematicTree>::const_iterator tr = refTrees.begin();
 for(std::vector<RefCountedKinematicParticle>::const_iterator i = prt.begin(); i!= prt.end(); i++)
 {
  AlgebraicVector7 lRefPar;
  for(int k = 1; k<8 ; k++)
  {lRefPar(k-1) = refPar((j-1)*7+k);}
  AlgebraicSymMatrix77 lRefCovS = asSMatrix<7>(refCovS.sub((j-1)*7 +1,(j-1)*7+7));
  
//new refitted parameters and covariance  
  KinematicParameters param(lRefPar);
  KinematicParametersError er(lRefCovS); 
  KinematicState kState(param,er,(*i)->initialState().particleCharge(), (**i).magneticField());
  RefCountedKinematicParticle refParticle  = (*i)->refittedParticle(kState,chi(j),ndf(j),cs->clone());
  
//replacing particle itself  
  (*tr)->findParticle(*i);
  RefCountedKinematicVertex inVertex =  (*tr)->currentDecayVertex();
  (*tr)->replaceCurrentParticle(refParticle);
  
//replacing the  vertex with its refitted version
  GlobalPoint nvPos(param.position()); 
  AlgebraicSymMatrix nvMatrix = asHepMatrix<7>(er.matrix()).sub(1,3);
  GlobalError nvError(asSMatrix<3>(nvMatrix));
  VertexState vState(nvPos, nvError, 1.0);
  KinematicVertexFactory vFactory;
  RefCountedKinematicVertex nVertex = vFactory.vertex(vState,inVertex,chi(j),ndf(j));
  (*tr)->replaceCurrentVertex(nVertex);  
  tr++;
  j++;
 }
 return refTrees; 
}     

void LagrangeParentParticleFitter::setParameters(const edm::ParameterSet& pSet)
{
  theMaxDiff = pSet.getParameter<double>("maxDistance");
  theMaxStep = pSet.getParameter<int>("maxNbrOfIterations");;
}

void LagrangeParentParticleFitter::defaultParameters()
{
  theMaxDiff = 0.001;
  theMaxStep = 100;
}