KalmanVertexFit/src/KalmanVertexTrackUpdator.cc

0001 #include "RecoVertex/KalmanVertexFit/interface/KalmanVertexTrackUpdator.h"
0002 #include "TrackingTools/TrajectoryState/interface/FreeTrajectoryState.h"
0003 //#include "Utilities/GenUtil/interface/ReferenceCountingPointer.h"
0004 #include "RecoVertex/VertexPrimitives/interface/VertexTrack.h"
0005 #include "RecoVertex/VertexPrimitives/interface/CachingVertex.h"
0006 #include "RecoVertex/VertexPrimitives/interface/VertexException.h"
0007 #include "DataFormats/Math/interface/invertPosDefMatrix.h"
0008
0009 #include <iostream>
0010
0011 template <unsigned int N>
0012 typename CachingVertex<N>::RefCountedVertexTrack KalmanVertexTrackUpdator<N>::update(const CachingVertex<N>& vertex,
0013                                                                                      RefCountedVertexTrack track) const
0014
0015 {
0016   trackMatrixPair thePair = trackRefit(vertex.vertexState(), track->linearizedTrack(), track->weight());
0017
0018   VertexState rVert = updator.positionUpdate(vertex.vertexState(), track->linearizedTrack(), track->weight(), -1);
0019
0020   std::pair<bool, double> result = helper.trackParameterChi2(track->linearizedTrack(), thePair.first);
0021   float smoothedChi2 = helper.vertexChi2(rVert, vertex.vertexState()) + result.second;
0022
0023   return theVTFactory.vertexTrack(
0024       track->linearizedTrack(), vertex.vertexState(), thePair.first, smoothedChi2, thePair.second, track->weight());
0025 }
0026
0027 template <unsigned int N>
0028 typename KalmanVertexTrackUpdator<N>::trackMatrixPair KalmanVertexTrackUpdator<N>::trackRefit(
0029     const VertexState& vertex,
0030     KalmanVertexTrackUpdator<N>::RefCountedLinearizedTrackState linTrackState,
0031     float weight) const {
0032   typedef ROOT::Math::SVector<double, N> AlgebraicVectorN;
0033   typedef ROOT::Math::SVector<double, N - 2> AlgebraicVectorM;
0034   typedef ROOT::Math::SMatrix<double, N, 3, ROOT::Math::MatRepStd<double, N, 3> > AlgebraicMatrixN3;
0035   typedef ROOT::Math::SMatrix<double, 3, N, ROOT::Math::MatRepStd<double, 3, N> > AlgebraicMatrix3N;
0036   typedef ROOT::Math::SMatrix<double, 3, N - 2, ROOT::Math::MatRepStd<double, 3, N - 2> > AlgebraicMatrix3M;
0037   typedef ROOT::Math::SMatrix<double, N, N - 2, ROOT::Math::MatRepStd<double, N, N - 2> > AlgebraicMatrixNM;
0038   typedef ROOT::Math::SMatrix<double, N - 2, 3, ROOT::Math::MatRepStd<double, N - 2, 3> > AlgebraicMatrixM3;
0039   typedef ROOT::Math::SMatrix<double, N, N, ROOT::Math::MatRepSym<double, N> > AlgebraicSymMatrixNN;
0040   //   typedef ROOT::Math::SMatrix<double,N+1,N+1,ROOT::Math::MatRepSym<double,N+1> > AlgebraicSymMatrixOO;
0041   typedef ROOT::Math::SMatrix<double, N + 1, N + 1, ROOT::Math::MatRepStd<double, N + 1, N + 1> > AlgebraicMatrixOO;
0042   typedef ROOT::Math::SMatrix<double, N - 2, N - 2, ROOT::Math::MatRepSym<double, N - 2> > AlgebraicSymMatrixMM;
0043
0044   //Vertex position
0045   GlobalPoint vertexPosition = vertex.position();
0046
0047   AlgebraicVector3 vertexCoord;
0048   vertexCoord(0) = vertexPosition.x();
0049   vertexCoord(1) = vertexPosition.y();
0050   vertexCoord(2) = vertexPosition.z();
0051   const AlgebraicSymMatrix33 vertexErrorMatrix = vertex.error().matrix();
0052
0053   //track information
0054   const AlgebraicMatrixN3 a = linTrackState->positionJacobian();
0055   const AlgebraicMatrixNM b = linTrackState->momentumJacobian();
0056
0057   //   AlgebraicVectorN trackParameters =
0058   //    linTrackState->predictedStateParameters();
0059
0060   int ifail;
0061   AlgebraicSymMatrixNN trackParametersWeight = linTrackState->predictedStateWeight(ifail);
0062
0063   AlgebraicSymMatrixMM s = ROOT::Math::SimilarityT(b, trackParametersWeight);
0064
0065   if (!invertPosDefMatrix(s))
0066     throw VertexException("KalmanVertexTrackUpdator::S matrix inversion failed");
0067
0068   //                                    NN           NM  MM
0069   AlgebraicMatrixNM twbs = trackParametersWeight * b * s;
0070
0071   AlgebraicVectorN vv = linTrackState->predictedStateParameters() - linTrackState->constantTerm() - a * vertexCoord;
0072   //                                   MM                MN                    NN
0073   //  AlgebraicVectorM newTrackMomentumP =  s * (ROOT::Math::Transpose(b)) * trackParametersWeight * vv;
0074   AlgebraicVectorM newTrackMomentumP = ROOT::Math::Transpose(twbs) * vv;
0075
0076   //AlgebraicMatrix3M refittedPositionMomentumConvariance =
0077   //        33                        3N                    NN                  NM  MM
0078   //  -vertexErrorMatrix * (ROOT::Math::Transpose(a)) * trackParametersWeight * b * s;
0079
0080   AlgebraicMatrix3N tmpM1 = -vertexErrorMatrix * (ROOT::Math::Transpose(a));
0081   AlgebraicMatrix3M refittedPositionMomentumConvariance = tmpM1 * twbs;
0082
0083   AlgebraicSymMatrixMM refittedMomentumConvariance =
0084       s / weight + ROOT::Math::SimilarityT(refittedPositionMomentumConvariance, vertex.weight().matrix());
0085
0086   // int matrixSize = 3+3; //refittedMomentumConvariance.num_col();
0087   AlgebraicMatrixOO covMatrix;  //(matrixSize, matrixSize);
0088   covMatrix.Place_at(refittedPositionMomentumConvariance, 0, 3);
0089   covMatrix.Place_at(ROOT::Math::Transpose(refittedPositionMomentumConvariance), 3, 0);
0090   covMatrix.Place_at(vertexErrorMatrix, 0, 0);
0091   covMatrix.Place_at(refittedMomentumConvariance, 3, 3);
0092
0093   AlgebraicSymMatrixOO covSymMatrix(covMatrix.LowerBlock());
0094
0095   RefCountedRefittedTrackState refittedTrackState =
0096       linTrackState->createRefittedTrackState(vertexPosition, newTrackMomentumP, covSymMatrix);
0097
0098   return trackMatrixPair(refittedTrackState, covSymMatrix);
0099   //        (refittedTrackState, refittedPositionMomentumConvariance);
0100 }
0101
0102 template class KalmanVertexTrackUpdator<5>;
0103 template class KalmanVertexTrackUpdator<6>;