KinematicConstrainedVertexUpdatorT.h

Go to the documentation of this file.

#ifndef KinematicConstrainedVertexUpdatorT_H
#define KinematicConstrainedVertexUpdatorT_H

#include <atomic>
#include <cassert>
#include <iostream>

#include "DataFormats/Math/interface/invertPosDefMatrix.h"
#include "FWCore/Utilities/interface/thread_safety_macros.h"
#include "RecoVertex/KinematicFitPrimitives/interface/MultiTrackKinematicConstraintT.h"
#include "RecoVertex/KinematicFitPrimitives/interface/KinematicVertexFactory.h"
#include "RecoVertex/KinematicFit/interface/VertexKinematicConstraintT.h"

// the usual stupid counter
namespace KineDebug3 {
  struct Count {
  std::atomic<int> n;
    Count() : n(0){}
    ~Count() {
    }

  };
  inline void count() {
    CMS_THREAD_SAFE static Count c;
    ++c.n;
  }

}

template < int nTrk, int nConstraint> class KinematicConstrainedVertexUpdatorT
{
public:
  
  KinematicConstrainedVertexUpdatorT();
  
  ~KinematicConstrainedVertexUpdatorT();
  
  RefCountedKinematicVertex  
  update(const ROOT::Math::SVector<double, 3+7*nTrk> & inState, 
     ROOT::Math::SMatrix<double, 3+7*nTrk,3+7*nTrk  ,ROOT::Math::MatRepSym<double,3+7*nTrk> >& inCov, std::vector<KinematicState> & lStates, 
     const GlobalPoint& lPoint, GlobalVector const & fieldValue, MultiTrackKinematicConstraintT< nTrk, nConstraint > * cs);
  
private:
  
  KinematicVertexFactory vFactory;
  VertexKinematicConstraintT vConstraint;                   
  ROOT::Math::SVector<double,3+7*nTrk> delta_alpha;
  ROOT::Math::SMatrix<double,nConstraint+4,3+7*nTrk> g;
  ROOT::Math::SVector<double,nConstraint+4> val;
  ROOT::Math::SVector<double, 3+7*nTrk> finPar;
  ROOT::Math::SVector<double, nConstraint+4> lambda;
  ROOT::Math::SMatrix<double,3,3,ROOT::Math::MatRepSym<double,3> > pCov = ROOT::Math::SMatrixNoInit(); 
  ROOT::Math::SMatrix<double,7,7,ROOT::Math::MatRepSym<double,7> > nCovariance = ROOT::Math::SMatrixNoInit();
  ROOT::Math::SMatrix<double,nConstraint+4,nConstraint+4,ROOT::Math::MatRepSym<double,nConstraint+4> > v_g_sym = ROOT::Math::SMatrixNoInit();   
  
};

#include "FWCore/MessageLogger/interface/MessageLogger.h"

template < int nTrk, int nConstraint> 
KinematicConstrainedVertexUpdatorT< nTrk, nConstraint >::KinematicConstrainedVertexUpdatorT()
{}

template < int nTrk, int nConstraint> 
KinematicConstrainedVertexUpdatorT< nTrk, nConstraint >::~KinematicConstrainedVertexUpdatorT()
{}

template < int nTrk, int nConstraint> 
RefCountedKinematicVertex 
KinematicConstrainedVertexUpdatorT< nTrk, nConstraint >::update(const ROOT::Math::SVector<double,3+7*nTrk>& inPar,
                                ROOT::Math::SMatrix<double, 3+7*nTrk,3+7*nTrk ,ROOT::Math::MatRepSym<double,3+7*nTrk> >& inCov,
                                std::vector<KinematicState> & lStates,
                                const GlobalPoint& lPoint,
                                GlobalVector const & fieldValue, 
                                MultiTrackKinematicConstraintT< nTrk, nConstraint > * cs)
{
  KineDebug3::count();

  int vSize = lStates.size();

  assert( nConstraint==0 || cs!=nullptr);
  assert(vSize == nConstraint);

  const MagneticField* field=lStates.front().magneticField();
  
  delta_alpha=inPar;
  delta_alpha(0)-=lPoint.x();
  delta_alpha(1)-=lPoint.y();
  delta_alpha(2)-=lPoint.z();
  int cst=3;
  for(std::vector<KinematicState>::const_iterator i = lStates.begin(); i != lStates.end(); i++) 
    for ( int j=0; j<7; j++) {
      delta_alpha(cst)-=i->kinematicParameters()(j);
      cst++;
    }
  
  // cout<<"delta_alpha"<<delta_alpha<<endl;
  //resulting matrix of derivatives and vector of values.
  //their size  depends of number of tracks to analyze and number of
  //additional constraints to apply 
  
  if( nConstraint !=0) {
    cs->init(lStates, lPoint, fieldValue);
    val.Place_at(cs->value(),0);
    g.Place_at(cs->positionDerivative(),0,0);
    g.Place_at(cs->parametersDerivative(),0,3);
  }

  vConstraint.init(lStates, lPoint, fieldValue);
  val.Place_at(vConstraint.value(),nConstraint);
  g.Place_at(vConstraint.positionDerivative(),nConstraint, 0);
  g.Place_at(vConstraint.parametersDerivative(),nConstraint, 3);
  
  

  
  //debug code   
  v_g_sym = ROOT::Math::Similarity(g,inCov);
  
  // bool ifl1 = v_g_sym.Invert();
  bool ifl1 = invertPosDefMatrix(v_g_sym);
  if(!ifl1) {
    edm::LogWarning("KinematicConstrainedVertexUpdatorFailed")<< "invert failed\n"
                                  << v_g_sym;
    LogDebug("KinematicConstrainedVertexFitter3")
      << "Fit failed: unable to invert SYM gain matrix\n";
    return  RefCountedKinematicVertex();    
  }
  
  // delta alpha is now valid!
  //full math case now!
  val += g*delta_alpha;
  lambda = v_g_sym *val;
  
  //final parameters  
  finPar = inPar -  inCov * (ROOT::Math::Transpose(g) * lambda);
  
  //refitted covariance 
  ROOT::Math::SMatrix<double,3+7*nTrk,3+7*nTrk,ROOT::Math::MatRepSym<double,3+7*nTrk> > prod = ROOT::Math::SimilarityT(g,v_g_sym);
  ROOT::Math::SMatrix<double,3+7*nTrk,3+7*nTrk,ROOT::Math::MatRepSym<double,3+7*nTrk> > prod1;
  ROOT::Math::AssignSym::Evaluate(prod1, inCov * prod * inCov); 
  // ROOT::Math::AssignSym::Evaluate(prod, prod1 * inCov); 
  inCov -= prod1;
  
  pCov = inCov.template Sub< ROOT::Math::SMatrix<double,3,3,ROOT::Math::MatRepSym<double,3> > >(0,0);
  
  // chi2
  double chi  = ROOT::Math::Dot(lambda,val); //??
  
  //this is ndf without significant prior
  //vertex so -3 factor exists here 
  float ndf = 2*vSize - 3;
  ndf += nConstraint;
 
  
  //making resulting vertex 
  GlobalPoint vPos (finPar(0),finPar(1),finPar(2)); 
  VertexState st(vPos,GlobalError(pCov));
  RefCountedKinematicVertex rVtx = vFactory.vertex(st,chi,ndf);
  
  //making refitted states of Kinematic Particles
  AlgebraicVector7 newPar; 
  int i_int = 0;
  for(std::vector<KinematicState>::iterator i_st=lStates.begin(); i_st != lStates.end(); i_st++)
    {
      for(int i =0; i<7; i++)
    {newPar(i) = finPar(3 + i_int*7 + i);}
      
   nCovariance = inCov.template Sub<ROOT::Math::SMatrix<double, 7,7,ROOT::Math::MatRepSym<double,7> > >(3 + i_int*7, 3 + i_int*7);
   TrackCharge chl = i_st->particleCharge();
   KinematicParameters nrPar(newPar);
   KinematicParametersError nrEr(nCovariance);
   KinematicState newState(nrPar,nrEr,chl, field);
   (*i_st) = newState;
   i_int++;
  }
  return rVtx;  
}

#endif