KinematicFit/interface/KinematicConstrainedVertexUpdatorT.h

0001 #ifndef KinematicConstrainedVertexUpdatorT_H
0002 #define KinematicConstrainedVertexUpdatorT_H
0003
0004 #include <atomic>
0005 #include <cassert>
0006 #include <iostream>
0007
0008 #include "DataFormats/Math/interface/invertPosDefMatrix.h"
0009 #include "FWCore/Utilities/interface/thread_safety_macros.h"
0010 #include "RecoVertex/KinematicFitPrimitives/interface/MultiTrackKinematicConstraintT.h"
0011 #include "RecoVertex/KinematicFitPrimitives/interface/KinematicVertexFactory.h"
0012 #include "RecoVertex/KinematicFit/interface/VertexKinematicConstraintT.h"
0013
0014 // the usual stupid counter
0015 namespace KineDebug3 {
0016   struct Count {
0017     std::atomic<int> n;
0018     Count() : n(0) {}
0019     ~Count() {}
0020   };
0021   inline void count() {
0022     CMS_THREAD_SAFE static Count c;
0023     ++c.n;
0024   }
0025
0026 }  // namespace KineDebug3
0027
0028 /**
0029  * Class caching the math part for
0030  * KinematicConstrainedVertexFitter
0031  */
0032
0033 template <int nTrk, int nConstraint>
0034 class KinematicConstrainedVertexUpdatorT {
0035 public:
0036   /**
0037    * Default constructor and destructor
0038    */
0039   KinematicConstrainedVertexUpdatorT();
0040
0041   ~KinematicConstrainedVertexUpdatorT();
0042
0043   /**
0044    * Method updating the states. Takes a vector of full parameters:
0045    * (x,y,z,particle_1,...,particle_n), corresponding linearization
0046    * point: vector of states and GlobalPoint,
0047    * and constraint to be applied during the vertex fit.
0048    * Returns refitted vector of 7n+3 parameters and corresponding
0049    * covariance matrix, where n - number of tracks.
0050    */
0051   RefCountedKinematicVertex update(
0052       const ROOT::Math::SVector<double, 3 + 7 * nTrk>& inState,
0053       ROOT::Math::SMatrix<double, 3 + 7 * nTrk, 3 + 7 * nTrk, ROOT::Math::MatRepSym<double, 3 + 7 * nTrk> >& inCov,
0054       std::vector<KinematicState>& lStates,
0055       const GlobalPoint& lPoint,
0056       GlobalVector const& fieldValue,
0057       MultiTrackKinematicConstraintT<nTrk, nConstraint>* cs);
0058
0059 private:
0060   KinematicVertexFactory vFactory;
0061   VertexKinematicConstraintT vConstraint;
0062   ROOT::Math::SVector<double, 3 + 7 * nTrk> delta_alpha;
0063   ROOT::Math::SMatrix<double, nConstraint + 4, 3 + 7 * nTrk> g;
0064   ROOT::Math::SVector<double, nConstraint + 4> val;
0065   ROOT::Math::SVector<double, 3 + 7 * nTrk> finPar;
0066   ROOT::Math::SVector<double, nConstraint + 4> lambda;
0067   ROOT::Math::SMatrix<double, 3, 3, ROOT::Math::MatRepSym<double, 3> > pCov = ROOT::Math::SMatrixNoInit();
0068   ROOT::Math::SMatrix<double, 7, 7, ROOT::Math::MatRepSym<double, 7> > nCovariance = ROOT::Math::SMatrixNoInit();
0069   ROOT::Math::SMatrix<double, nConstraint + 4, nConstraint + 4, ROOT::Math::MatRepSym<double, nConstraint + 4> >
0070       v_g_sym = ROOT::Math::SMatrixNoInit();
0071 };
0072
0073 #include "FWCore/MessageLogger/interface/MessageLogger.h"
0074
0075 template <int nTrk, int nConstraint>
0076 KinematicConstrainedVertexUpdatorT<nTrk, nConstraint>::KinematicConstrainedVertexUpdatorT() {}
0077
0078 template <int nTrk, int nConstraint>
0079 KinematicConstrainedVertexUpdatorT<nTrk, nConstraint>::~KinematicConstrainedVertexUpdatorT() {}
0080
0081 template <int nTrk, int nConstraint>
0082 RefCountedKinematicVertex KinematicConstrainedVertexUpdatorT<nTrk, nConstraint>::update(
0083     const ROOT::Math::SVector<double, 3 + 7 * nTrk>& inPar,
0084     ROOT::Math::SMatrix<double, 3 + 7 * nTrk, 3 + 7 * nTrk, ROOT::Math::MatRepSym<double, 3 + 7 * nTrk> >& inCov,
0085     std::vector<KinematicState>& lStates,
0086     const GlobalPoint& lPoint,
0087     GlobalVector const& fieldValue,
0088     MultiTrackKinematicConstraintT<nTrk, nConstraint>* cs) {
0089   KineDebug3::count();
0090
0091   int vSize = lStates.size();
0092
0093   assert(nConstraint == 0 || cs != nullptr);
0094   assert(vSize == nConstraint);
0095
0096   const MagneticField* field = lStates.front().magneticField();
0097
0098   delta_alpha = inPar;
0099   delta_alpha(0) -= lPoint.x();
0100   delta_alpha(1) -= lPoint.y();
0101   delta_alpha(2) -= lPoint.z();
0102   int cst = 3;
0103   for (std::vector<KinematicState>::const_iterator i = lStates.begin(); i != lStates.end(); i++)
0104     for (int j = 0; j < 7; j++) {
0105       delta_alpha(cst) -= i->kinematicParameters()(j);
0106       cst++;
0107     }
0108
0109   // cout<<"delta_alpha"<<delta_alpha<<endl;
0110   //resulting matrix of derivatives and vector of values.
0111   //their size  depends of number of tracks to analyze and number of
0112   //additional constraints to apply
0113
0114   if (nConstraint != 0) {
0115     cs->init(lStates, lPoint, fieldValue);
0116     val.Place_at(cs->value(), 0);
0117     g.Place_at(cs->positionDerivative(), 0, 0);
0118     g.Place_at(cs->parametersDerivative(), 0, 3);
0119   }
0120
0121   vConstraint.init(lStates, lPoint, fieldValue);
0122   val.Place_at(vConstraint.value(), nConstraint);
0123   g.Place_at(vConstraint.positionDerivative(), nConstraint, 0);
0124   g.Place_at(vConstraint.parametersDerivative(), nConstraint, 3);
0125
0126   //debug code
0127   v_g_sym = ROOT::Math::Similarity(g, inCov);
0128
0129   // bool ifl1 = v_g_sym.Invert();
0130   bool ifl1 = invertPosDefMatrix(v_g_sym);
0131   if (!ifl1) {
0132     edm::LogWarning("KinematicConstrainedVertexUpdatorFailed") << "invert failed\n" << v_g_sym;
0133     LogDebug("KinematicConstrainedVertexFitter3") << "Fit failed: unable to invert SYM gain matrix\n";
0134     return RefCountedKinematicVertex();
0135   }
0136
0137   // delta alpha is now valid!
0138   //full math case now!
0139   val += g * delta_alpha;
0140   lambda = v_g_sym * val;
0141
0142   //final parameters
0143   finPar = inPar - inCov * (ROOT::Math::Transpose(g) * lambda);
0144
0145   //refitted covariance
0146   ROOT::Math::SMatrix<double, 3 + 7 * nTrk, 3 + 7 * nTrk, ROOT::Math::MatRepSym<double, 3 + 7 * nTrk> > prod =
0147       ROOT::Math::SimilarityT(g, v_g_sym);
0148   ROOT::Math::SMatrix<double, 3 + 7 * nTrk, 3 + 7 * nTrk, ROOT::Math::MatRepSym<double, 3 + 7 * nTrk> > prod1;
0149   ROOT::Math::AssignSym::Evaluate(prod1, inCov * prod * inCov);
0150   // ROOT::Math::AssignSym::Evaluate(prod, prod1 * inCov);
0151   inCov -= prod1;
0152
0153   pCov = inCov.template Sub<ROOT::Math::SMatrix<double, 3, 3, ROOT::Math::MatRepSym<double, 3> > >(0, 0);
0154
0155   // chi2
0156   double chi = ROOT::Math::Dot(lambda, val);  //??
0157
0158   //this is ndf without significant prior
0159   //vertex so -3 factor exists here
0160   float ndf = 2 * vSize - 3;
0161   ndf += nConstraint;
0162
0163   //making resulting vertex
0164   GlobalPoint vPos(finPar(0), finPar(1), finPar(2));
0165   VertexState st(vPos, GlobalError(pCov));
0166   RefCountedKinematicVertex rVtx = vFactory.vertex(st, chi, ndf);
0167
0168   //making refitted states of Kinematic Particles
0169   AlgebraicVector7 newPar;
0170   int i_int = 0;
0171   for (std::vector<KinematicState>::iterator i_st = lStates.begin(); i_st != lStates.end(); i_st++) {
0172     for (int i = 0; i < 7; i++) {
0173       newPar(i) = finPar(3 + i_int * 7 + i);
0174     }
0175
0176     nCovariance = inCov.template Sub<ROOT::Math::SMatrix<double, 7, 7, ROOT::Math::MatRepSym<double, 7> > >(
0177         3 + i_int * 7, 3 + i_int * 7);
0178     TrackCharge chl = i_st->particleCharge();
0179     KinematicParameters nrPar(newPar);
0180     KinematicParametersError nrEr(nCovariance);
0181     KinematicState newState(nrPar, nrEr, chl, field);
0182     (*i_st) = newState;
0183     i_int++;
0184   }
0185   return rVtx;
0186 }
0187
0188 #endif