KalmanVertexFit/src/KalmanTrackToTrackCovCalculator.cc

0001 #include "RecoVertex/KalmanVertexFit/interface/KalmanTrackToTrackCovCalculator.h"
0002 #include "RecoVertex/VertexPrimitives/interface/VertexException.h"
0003
0004 template <unsigned int N>
0005 typename CachingVertex<N>::TrackToTrackMap KalmanTrackToTrackCovCalculator<N>::operator()(
0006     const CachingVertex<N>& vertex) const {
0007   typedef ROOT::Math::SMatrix<double, N, 3, ROOT::Math::MatRepStd<double, N, 3> > AlgebraicMatrixN3;
0008   typedef ROOT::Math::SMatrix<double, N, N - 2, ROOT::Math::MatRepStd<double, N, N - 2> > AlgebraicMatrixNM;
0009   typedef ROOT::Math::SMatrix<double, N - 2, 3, ROOT::Math::MatRepStd<double, N - 2, 3> > AlgebraicMatrixM3;
0010   typedef ROOT::Math::SMatrix<double, 3, N - 2, ROOT::Math::MatRepStd<double, 3, N - 2> > AlgebraicMatrix3M;
0011   typedef ROOT::Math::SMatrix<double, N, N, ROOT::Math::MatRepSym<double, N> > AlgebraicSymMatrixNN;
0012   typedef ROOT::Math::SMatrix<double, N - 2, N - 2, ROOT::Math::MatRepSym<double, N - 2> > AlgebraicSymMatrixMM;
0013   typedef ROOT::Math::SMatrix<double, N - 2, N - 2, ROOT::Math::MatRepStd<double, N - 2, N - 2> > AlgebraicMatrixMM;
0014
0015   typename CachingVertex<N>::TrackToTrackMap returnMap;
0016   int ifail = 0;
0017   std::vector<RefCountedVertexTrack> tracks = vertex.tracks();
0018
0019   //vertex initial data needed
0020   AlgebraicSymMatrix33 vertexC = vertex.error().matrix();
0021
0022   for (typename std::vector<RefCountedVertexTrack>::iterator i = tracks.begin(); i != tracks.end(); i++) {
0023     const AlgebraicMatrixN3& leftA = (*i)->linearizedTrack()->positionJacobian();
0024     const AlgebraicMatrixNM& leftB = (*i)->linearizedTrack()->momentumJacobian();
0025     AlgebraicSymMatrixNN leftG = (*i)->linearizedTrack()->predictedStateWeight(ifail);
0026     AlgebraicSymMatrixMM leftW = ROOT::Math::SimilarityT(leftB, leftG);
0027
0028     ifail = !leftW.Invert();
0029     if (ifail != 0)
0030       throw VertexException("KalmanTrackToTrackCovarianceCalculator::leftW matrix inversion failed");
0031     AlgebraicMatrixM3 leftPart = leftW * (ROOT::Math::Transpose(leftB)) * leftG * leftA;
0032     typename CachingVertex<N>::TrackMap internalMap;
0033     for (typename std::vector<RefCountedVertexTrack>::iterator j = tracks.begin(); j != tracks.end(); j++) {
0034       if (*i < *j) {
0035         const AlgebraicMatrixN3& rightA = (*j)->linearizedTrack()->positionJacobian();
0036         const AlgebraicMatrixNM& rightB = (*j)->linearizedTrack()->momentumJacobian();
0037         AlgebraicSymMatrixNN rightG = (*j)->linearizedTrack()->predictedStateWeight(ifail);
0038         AlgebraicSymMatrixMM rightW = ROOT::Math::SimilarityT(rightB, rightG);
0039
0040         ifail = !rightW.Invert();
0041
0042         if (ifail != 0)
0043           throw VertexException("KalmanTrackToTrackCovarianceCalculator::rightW matrix inversion failed");
0044         AlgebraicMatrix3M rightPart = (ROOT::Math::Transpose(rightA)) * rightG * rightB * rightW;
0045         internalMap[(*j)] = leftPart * vertexC * rightPart;
0046       }
0047     }
0048     returnMap[*i] = internalMap;
0049   }
0050   return returnMap;
0051 }
0052
0053 template class KalmanTrackToTrackCovCalculator<5>;
0054 template class KalmanTrackToTrackCovCalculator<6>;