File indexing completed on 2024-04-06 12:29:10
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
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 }
0027
0028
0029
0030
0031
0032
0033 template <int nTrk, int nConstraint>
0034 class KinematicConstrainedVertexUpdatorT {
0035 public:
0036
0037
0038
0039 KinematicConstrainedVertexUpdatorT();
0040
0041 ~KinematicConstrainedVertexUpdatorT();
0042
0043
0044
0045
0046
0047
0048
0049
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
0110
0111
0112
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
0127 v_g_sym = ROOT::Math::Similarity(g, inCov);
0128
0129
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
0138
0139 val += g * delta_alpha;
0140 lambda = v_g_sym * val;
0141
0142
0143 finPar = inPar - inCov * (ROOT::Math::Transpose(g) * lambda);
0144
0145
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
0151 inCov -= prod1;
0152
0153 pCov = inCov.template Sub<ROOT::Math::SMatrix<double, 3, 3, ROOT::Math::MatRepSym<double, 3> > >(0, 0);
0154
0155
0156 double chi = ROOT::Math::Dot(lambda, val);
0157
0158
0159
0160 float ndf = 2 * vSize - 3;
0161 ndf += nConstraint;
0162
0163
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
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