ConstrainedTreeBuilderT.h

Go to the documentation of this file.

#ifndef ConstrainedTreeBuilderT_H
#define ConstrainedTreeBuilderT_H

#include "RecoVertex/KinematicFitPrimitives/interface/RefCountedKinematicTree.h"
#include "RecoVertex/KinematicFitPrimitives/interface/VirtualKinematicParticleFactory.h"
#include "RecoVertex/KinematicFitPrimitives/interface/KinematicVertexFactory.h"

class ConstrainedTreeBuilderT
{

public:

  ConstrainedTreeBuilderT(){}
 
  ~ConstrainedTreeBuilderT(){}

  template<int nTrk>
  RefCountedKinematicTree 
  buildTree(const std::vector<RefCountedKinematicParticle> & initialParticles, 
        const std::vector<KinematicState> & finalStates,
        const RefCountedKinematicVertex vtx, 
        const ROOT::Math::SMatrix<double,3+7*nTrk,3+7*nTrk,ROOT::Math::MatRepSym<double,3+7*nTrk> >& fCov) const;

private:

  RefCountedKinematicTree buildRealTree(const RefCountedKinematicParticle virtualParticle, 
    const RefCountedKinematicVertex vtx, const std::vector<RefCountedKinematicParticle> & particles) const;

  template<int nTrk>
  static AlgebraicSymMatrix77 
  covarianceMatrix(const std::vector<RefCountedKinematicParticle> &rPart, 
           const AlgebraicVector7& newPar,
           const ROOT::Math::SMatrix<double,3+7*nTrk,3+7*nTrk,ROOT::Math::MatRepSym<double,3+7*nTrk> >& fitCov);
                       
 VirtualKinematicParticleFactory pFactory;                     
 KinematicVertexFactory vFactory;
};


#include "DataFormats/CLHEP/interface/Migration.h"
#include "FWCore/MessageLogger/interface/MessageLogger.h"

template<int nTrk>
RefCountedKinematicTree 
ConstrainedTreeBuilderT::buildTree(const std::vector<RefCountedKinematicParticle> & initialParticles, 
                   const std::vector<KinematicState> & finalStates,
                   const RefCountedKinematicVertex vertex, 
                   const ROOT::Math::SMatrix<double,3+7*nTrk,3+7*nTrk,ROOT::Math::MatRepSym<double,3+7*nTrk> >& fullCov) const
{
  if (!vertex->vertexIsValid()) {
       LogDebug("RecoVertex/ConstrainedTreeBuilder")
    << "Vertex is invalid\n";
       return ReferenceCountingPointer<KinematicTree>(new KinematicTree());
  }
  AlgebraicVector3 vtx;
  vtx(0) = vertex->position().x();
  vtx(1) = vertex->position().y();
  vtx(2) = vertex->position().z();
  AlgebraicMatrix33 vertexCov = fullCov.template Sub<ROOT::Math::SMatrix<double, 3> >(0,0);

// cout << fullCov<<endl;
//  cout << "RecoVertex/ConstrainedTreeBuilder"<<vtx<<endl;

 double ent = 0.;
 int charge = 0;
  AlgebraicVector7 virtualPartPar;
  virtualPartPar(0) = vertex->position().x();
  virtualPartPar(1) = vertex->position().y();
  virtualPartPar(2) = vertex->position().z();

//making refitted particles out of refitted states.
//none of the operations above violates the order of particles

  ROOT::Math::SMatrix<double,7,7,ROOT::Math::MatRepStd<double,7,7> >  aMatrix;
  ROOT::Math::SMatrix<double,7,7,ROOT::Math::MatRepStd<double,7,7> >  aMatrixT;
  aMatrix(3,3) =  aMatrixT(3,3) = 1;
  aMatrix(4,4) =  aMatrixT(4,4)  = 1;
  aMatrix(5,5) =  aMatrixT(5,5)  = 1;
  aMatrix(6,6) =  aMatrixT(6,6)  = 1;
  ROOT::Math::SMatrix<double,7,3,ROOT::Math::MatRepStd<double,7,3> > bMatrix;
  ROOT::Math::SMatrix<double,3,7,ROOT::Math::MatRepStd<double,3,7> > bMatrixT;
  bMatrix(0,0) =  bMatrixT(0,0) = 1;
  bMatrix(1,1) =  bMatrixT(1,1) = 1;
  bMatrix(2,2) =  bMatrixT(2,2) = 1;
  AlgebraicSymMatrix77 trackParCov;
  ROOT::Math::SMatrix<double,3,7,ROOT::Math::MatRepStd<double,3,7> > vtxTrackCov;
  AlgebraicSymMatrix77 nCovariance;
  // AlgebraicSymMatrix77 tmp;

  std::vector<RefCountedKinematicParticle>::const_iterator i = initialParticles.begin();
  std::vector<KinematicState>::const_iterator iStates = finalStates.begin();
  std::vector<RefCountedKinematicParticle> rParticles;
  int n=0;
  // assert(initialParticles.size()==nTrk);
  for( ; i != initialParticles.end() && iStates != finalStates.end(); ++i,++iStates)
  {
    AlgebraicVector7 p = iStates->kinematicParameters().vector();
    double a = - iStates->particleCharge() *
    iStates->magneticField()->inInverseGeV(iStates->globalPosition()).z();

    aMatrix(4,0) = aMatrixT(0,4) = -a;
    aMatrix(3,1) = aMatrixT(1,3) =  a;
    bMatrix(4,0) = bMatrixT(0,4) =  a;
    bMatrix(3,1) = bMatrixT(1,3) = -a;

    AlgebraicVector7 par = aMatrix*p + bMatrix * vtx;

    trackParCov = fullCov.template Sub<AlgebraicSymMatrix77>(3+n*7,3+n*7);
    vtxTrackCov = fullCov.template Sub<ROOT::Math::SMatrix<double, 3, 7> >(0,3+n*7);
    ROOT::Math::AssignSym::Evaluate(nCovariance,
                    aMatrix * trackParCov * aMatrixT
                    + aMatrix * ROOT::Math::Transpose(vtxTrackCov) * bMatrixT
                    + bMatrix * vtxTrackCov * aMatrixT 
                    + bMatrix * vertexCov * bMatrixT
                    );
    /*
    ROOT::Math::AssignSym::Evaluate(tmp, aMatrix * ROOT::Math::Transpose(vtxTrackCov) * bMatrixT);
    nCovariance+=tmp;
    ROOT::Math::AssignSym::Evaluate(tmp, bMatrix * vtxTrackCov * aMatrixT);
    nCovariance+=tmp;
    ROOT::Math::AssignSym::Evaluate(tmp, bMatrix * vertexCov * bMatrixT);
    nCovariance+=tmp;
    */

    KinematicState stateAtVertex(KinematicParameters(par),
                 KinematicParametersError(nCovariance),
                 iStates->particleCharge(), iStates->magneticField());
    rParticles.push_back((*i)->refittedParticle(stateAtVertex, vertex->chiSquared(), vertex->degreesOfFreedom()));
    
    virtualPartPar(3) += par(3);
    virtualPartPar(4) += par(4);
    virtualPartPar(5) += par(5);
    ent += sqrt(par(6)*par(6) +  par(3)*par(3)+par(4)*par(4)+par(5)*par(5) );
    charge += iStates->particleCharge();

    ++n;

  }

 //total reconstructed mass
  double differ = ent*ent - (virtualPartPar(3)*virtualPartPar(3) + virtualPartPar(5)*virtualPartPar(5) + virtualPartPar(4)*virtualPartPar(4));
  if(differ>0.) {
    virtualPartPar(6) = sqrt(differ);
  } else {
   LogDebug("ConstrainedTreeBuilder")
    << "Fit failed: Current precision does not allow to calculate the mass\n";
   return ReferenceCountingPointer<KinematicTree>(new KinematicTree());
  }

 // covariance matrix:

  AlgebraicSymMatrix77 cov = this->covarianceMatrix<nTrk>(rParticles,virtualPartPar,fullCov);

  KinematicState nState(KinematicParameters(virtualPartPar),
            KinematicParametersError(cov),
            charge, initialParticles[0]->magneticField());

 //newborn kinematic particle
  float chi2 = vertex->chiSquared();
  float ndf = vertex->degreesOfFreedom();
  KinematicParticle * zp = nullptr;
  RefCountedKinematicParticle virtualParticle = pFactory.particle(nState,chi2,ndf,zp);

  return buildRealTree(virtualParticle, vertex, rParticles);
}

template<int nTrk>
AlgebraicSymMatrix77 
ConstrainedTreeBuilderT::covarianceMatrix(const std::vector<RefCountedKinematicParticle> &rPart, 
                      const AlgebraicVector7& newPar,
                      const ROOT::Math::SMatrix<double,3+7*nTrk,3+7*nTrk,ROOT::Math::MatRepSym<double,3+7*nTrk> >& fitCov)
 {

   typedef ROOT::Math::SMatrix<double,3+7*nTrk,3+7*nTrk,ROOT::Math::MatRepSym<double,3+7*nTrk> > FitCov;
  //constructing the full matrix using the simple fact
  //that we have never broken the order of tracks
  // during our fit.
  
  int size = nTrk;  

  if ( int(rPart.size())!=size) throw "error in ConstrainedTreeBuilderT ";

  //global propagation to the vertex position
  //Jacobian is done for all the parameters together
  ROOT::Math::SMatrix<double,3+7*nTrk> jac;
  jac(0,0) = 1;
  jac(1,1) = 1;
  jac(2,2) = 1;
  ROOT::Math::SMatrix<double,3,7> upper;
  ROOT::Math::SMatrix<double,7>  diagonal;
  for(  int i_int=0; i_int!=size; ++i_int) {
    RefCountedKinematicParticle const & i = rPart[i_int];   
    //vertex position related components of the matrix
    double a_i = - (i)->currentState().particleCharge() *
      (i)->magneticField()->inInverseGeV((i)->currentState().globalPosition()).z();
    upper(0,0) = 1;
    upper(1,1) = 1;
    upper(2,2) = 1;
    upper(1,3) = -a_i;
    upper(0,4) = a_i;
    jac.Place_at(upper,0,3+i_int*7);
    
    diagonal(3,3) = 1;
    diagonal(4,4) = 1;
    diagonal(5,5) = 1;
    diagonal(6,6) = 1;
    diagonal(1,3) = a_i;
    diagonal(0,4) = -a_i;
    jac.Place_at(diagonal,3+i_int*7,3+i_int*7);
  }
  
  // jacobian is constructed in such a way, that
  // right operation for transformation will be
  // fitCov.similarityT(jac)
  // WARNING: normal similarity operation is
  // not valid in this case
  //now making reduced matrix:
  // int vSize = rPart.size();
  FitCov const & fit_cov_sym = fitCov;
  /*
    for(int i = 0; i<7*vSize+3; ++i)
    {
    for(int j = 0; j<7*vSize+3; ++j)
    {if(i<=j) fit_cov_sym(i,j) = fitCov(i,j);}
    }
  */
  
  ROOT::Math::SMatrix<double,4*nTrk+3,4*nTrk+3, ROOT::Math::MatRepSym<double,4*nTrk+3> > reduced;
  FitCov transform=ROOT::Math::SimilarityT(jac,fit_cov_sym); // similarityT???
  
  //jacobian to add matrix components
  ROOT::Math::SMatrix<double,7,4*nTrk+3> jac_t;
  jac_t(0,0) = 1.;
  jac_t(1,1) = 1.;
  jac_t(2,2) = 1.;
  

  
  double energy_global = sqrt(newPar(3)*newPar(3)+newPar(4)*newPar(4) + newPar(5)*newPar(5)+newPar(6)*newPar(6));
  for(int il_int = 0; il_int!=size; ++il_int) {
    RefCountedKinematicParticle const & rs = rPart[il_int];
    //jacobian components:
    int off1=3; int off2=il_int*4+3;
    jac_t(off1+0,off2+0) = 1.;
    jac_t(off1+1,off2+1) = 1.;
    jac_t(off1+2,off2+2) = 1.;
    
    //non-trival elements: mass correlations:
    AlgebraicVector7 l_Par = (rs)->currentState().kinematicParameters().vector();
    double energy_local  = sqrt(l_Par(6)*l_Par(6) + l_Par(3)*l_Par(3) + l_Par(4)*l_Par(4) + l_Par(5)*l_Par(5));
    jac_t(off1+3,off2+3) = energy_global*l_Par(6)/(newPar(6)*energy_local);
    jac_t(off1+3,off2+0) = ((energy_global*l_Par(3)/energy_local) - newPar(3))/newPar(6);
    jac_t(off1+3,off2+1) = ((energy_global*l_Par(4)/energy_local) - newPar(4))/newPar(6);
    jac_t(off1+3,off2+2) = ((energy_global*l_Par(5)/energy_local) - newPar(5))/newPar(6);
  }

  for(int i = 0; i<7;++i)
    for(int j =0; j<7; ++j)
      reduced(i,j)  = transform(i+3, j+3);

  for(int i = 1; i<size; i++) {
    //non-trival elements: mass correlations:
    //upper row and right column

    int off1=0;
    int off2=3+4*i;
    for(int l1 = 0; l1<3;++l1) 
      for(int l2 = 0; l2<4;++l2)
    reduced(off1+l1,off2+l2) = transform(3+l1,6+7*i +l2);

    //diagonal elements
    off1=off2=3+4*i;
    for(int l1 = 0; l1<4;++l1)
      for(int l2 = 0; l2<4;++l2)
    reduced(off1+l1,off2+l2) = transform(6+7*i+l1, 6+7*i+l2);

     //off diagonal elements
   for(int j = 1; j<size; j++) {
      off1 =  3+4*(i-1); off2=3+4*j;
      for(int l1 = 0; l1<4;++l1)
    for(int l2 = 0; l2<4;++l2)
      reduced(off1+l1,off2+l2)  = transform(6+7*(i-1)+l1,6+7*j+l2);
    }

  }
    
  AlgebraicSymMatrix77 ret;
  ROOT::Math::AssignSym::Evaluate(ret, jac_t*reduced*ROOT::Math::Transpose(jac_t));
  return ret;
}







#endif