KalmanTrackToTrackCovCalculator.cc

Go to the documentation of this file.

#include "RecoVertex/KalmanVertexFit/interface/KalmanTrackToTrackCovCalculator.h"
#include "RecoVertex/VertexPrimitives/interface/VertexException.h"

template <unsigned int N>
typename CachingVertex<N>::TrackToTrackMap KalmanTrackToTrackCovCalculator<N>::operator()(
    const CachingVertex<N>& vertex) const {
  typedef ROOT::Math::SMatrix<double, N, 3, ROOT::Math::MatRepStd<double, N, 3> > AlgebraicMatrixN3;
  typedef ROOT::Math::SMatrix<double, N, N - 2, ROOT::Math::MatRepStd<double, N, N - 2> > AlgebraicMatrixNM;
  typedef ROOT::Math::SMatrix<double, N - 2, 3, ROOT::Math::MatRepStd<double, N - 2, 3> > AlgebraicMatrixM3;
  typedef ROOT::Math::SMatrix<double, 3, N - 2, ROOT::Math::MatRepStd<double, 3, N - 2> > AlgebraicMatrix3M;
  typedef ROOT::Math::SMatrix<double, N, N, ROOT::Math::MatRepSym<double, N> > AlgebraicSymMatrixNN;
  typedef ROOT::Math::SMatrix<double, N - 2, N - 2, ROOT::Math::MatRepSym<double, N - 2> > AlgebraicSymMatrixMM;
  typedef ROOT::Math::SMatrix<double, N - 2, N - 2, ROOT::Math::MatRepStd<double, N - 2, N - 2> > AlgebraicMatrixMM;

  typename CachingVertex<N>::TrackToTrackMap returnMap;
  int ifail = 0;
  std::vector<RefCountedVertexTrack> tracks = vertex.tracks();

  //vertex initial data needed
  AlgebraicSymMatrix33 vertexC = vertex.error().matrix();

  for (typename std::vector<RefCountedVertexTrack>::iterator i = tracks.begin(); i != tracks.end(); i++) {
    const AlgebraicMatrixN3& leftA = (*i)->linearizedTrack()->positionJacobian();
    const AlgebraicMatrixNM& leftB = (*i)->linearizedTrack()->momentumJacobian();
    AlgebraicSymMatrixNN leftG = (*i)->linearizedTrack()->predictedStateWeight(ifail);
    AlgebraicSymMatrixMM leftW = ROOT::Math::SimilarityT(leftB, leftG);

    ifail = !leftW.Invert();
    if (ifail != 0)
      throw VertexException("KalmanTrackToTrackCovarianceCalculator::leftW matrix inversion failed");
    AlgebraicMatrixM3 leftPart = leftW * (ROOT::Math::Transpose(leftB)) * leftG * leftA;
    typename CachingVertex<N>::TrackMap internalMap;
    for (typename std::vector<RefCountedVertexTrack>::iterator j = tracks.begin(); j != tracks.end(); j++) {
      if (*i < *j) {
        const AlgebraicMatrixN3& rightA = (*j)->linearizedTrack()->positionJacobian();
        const AlgebraicMatrixNM& rightB = (*j)->linearizedTrack()->momentumJacobian();
        AlgebraicSymMatrixNN rightG = (*j)->linearizedTrack()->predictedStateWeight(ifail);
        AlgebraicSymMatrixMM rightW = ROOT::Math::SimilarityT(rightB, rightG);

        ifail = !rightW.Invert();

        if (ifail != 0)
          throw VertexException("KalmanTrackToTrackCovarianceCalculator::rightW matrix inversion failed");
        AlgebraicMatrix3M rightPart = (ROOT::Math::Transpose(rightA)) * rightG * rightB * rightW;
        internalMap[(*j)] = leftPart * vertexC * rightPart;
      }
    }
    returnMap[*i] = internalMap;
  }
  return returnMap;
}

template class KalmanTrackToTrackCovCalculator<5>;
template class KalmanTrackToTrackCovCalculator<6>;