KalmanVertexFit/src/KalmanVertexUpdator.cc

0001 #include "RecoVertex/KalmanVertexFit/interface/KalmanVertexUpdator.h"
0002 #include "DataFormats/GeometryVector/interface/GlobalVector.h"
0003 #include "DataFormats/CLHEP/interface/AlgebraicObjects.h"
0004 #include "RecoVertex/VertexPrimitives/interface/VertexException.h"
0005 #include "DataFormats/GeometrySurface/interface/ReferenceCounted.h"
0006 #include "FWCore/MessageLogger/interface/MessageLogger.h"
0007 #include "DataFormats/Math/interface/invertPosDefMatrix.h"
0008
0009 #include <algorithm>
0010
0011 // Based on the R.Fruhwirth et al Computer Physics Communications 96 (1996) 189-208
0012 template <unsigned int N>
0013 CachingVertex<N> KalmanVertexUpdator<N>::update(const CachingVertex<N>& oldVertex,
0014                                                 const RefCountedVertexTrack track,
0015                                                 float weight,
0016                                                 int sign) const {
0017   if (abs(sign) != 1)
0018     throw VertexException("KalmanVertexUpdator::abs(sign) not equal to 1");
0019
0020   VertexState newVertexState = positionUpdate(oldVertex.vertexState(), track->linearizedTrack(), weight, sign);
0021   if (!newVertexState.isValid())
0022     return CachingVertex<N>();
0023
0024   float chi1 = oldVertex.totalChiSquared();
0025   std::pair<bool, double> chi2P =
0026       chi2Increment(oldVertex.vertexState(), newVertexState, track->linearizedTrack(), weight);
0027   if (!chi2P.first)
0028     return CachingVertex<N>();  // return invalid vertex
0029
0030   chi1 += sign * chi2P.second;
0031
0032   //adding or removing track from the CachingVertex::VertexTracks
0033   std::vector<RefCountedVertexTrack> newVertexTracks = oldVertex.tracks();
0034
0035   if (sign > 0) {
0036     newVertexTracks.push_back(track);
0037   } else {
0038     typename std::vector<RefCountedVertexTrack>::iterator pos =
0039         find(newVertexTracks.begin(), newVertexTracks.end(), track);
0040     if (pos != newVertexTracks.end()) {
0041       newVertexTracks.erase(pos);
0042     } else {
0043       std::cout << "KalmanVertexUpdator::Unable to find requested track in the current vertex" << std::endl;
0044       throw VertexException("KalmanVertexUpdator::Unable to find requested track in the current vertex");
0045     }
0046   }
0047
0048   if (oldVertex.hasPrior()) {
0049     return CachingVertex<N>(oldVertex.priorVertexState(), newVertexState, newVertexTracks, chi1);
0050   } else {
0051     return CachingVertex<N>(newVertexState, newVertexTracks, chi1);
0052   }
0053 }
0054
0055 template <unsigned int N>
0056 CachingVertex<N> KalmanVertexUpdator<N>::add(const CachingVertex<N>& oldVertex,
0057                                              const RefCountedVertexTrack track) const {
0058   float weight = track->weight();
0059   return update(oldVertex, track, weight, +1);
0060 }
0061
0062 template <unsigned int N>
0063 CachingVertex<N> KalmanVertexUpdator<N>::remove(const CachingVertex<N>& oldVertex,
0064                                                 const RefCountedVertexTrack track) const {
0065   float weight = track->weight();
0066   return update(oldVertex, track, weight, -1);
0067 }
0068
0069 template <unsigned int N>
0070 VertexState KalmanVertexUpdator<N>::positionUpdate(const VertexState& oldVertex,
0071                                                    const RefCountedLinearizedTrackState linearizedTrack,
0072                                                    const float weight,
0073                                                    int sign) const {
0074   int error;
0075
0076   if (!linearizedTrack->isValid())
0077     return VertexState();
0078
0079   const AlgebraicMatrixN3& a = linearizedTrack->positionJacobian();
0080   const AlgebraicMatrixNM& b = linearizedTrack->momentumJacobian();
0081
0082   //   AlgebraicVectorN trackParameters =
0083   //    linearizedTrack->predictedStateParameters();
0084
0085   AlgebraicSymMatrixNN trackParametersWeight = linearizedTrack->predictedStateWeight(error);
0086   if (error != 0) {
0087     edm::LogWarning("KalmanVertexUpdator")
0088         << "predictedState error matrix inversion failed. An invalid vertex will be returned.";
0089     return VertexState();
0090   }
0091
0092   // Jacobians
0093   //  edm::LogInfo("RecoVertex/KalmanVertexUpdator")
0094   //    << "Now updating position" << "\n";
0095
0096   //vertex information
0097   //   AlgebraicSymMatrix33 oldVertexWeight = oldVertex.weight().matrix();
0098   AlgebraicSymMatrixMM s = ROOT::Math::SimilarityT(b, trackParametersWeight);
0099   if (!invertPosDefMatrix(s)) {
0100     edm::LogWarning("KalmanVertexUpdator") << "S matrix inversion failed. An invalid vertex will be returned.";
0101     return VertexState();
0102   }
0103
0104   AlgebraicSymMatrixNN gB =
0105       trackParametersWeight - ROOT::Math::Similarity(trackParametersWeight, ROOT::Math::Similarity(b, s));
0106
0107   // Getting the new covariance matrix of the vertex.
0108
0109   AlgebraicSymMatrix33 newVertexWeight = oldVertex.weight().matrix() + (weight * sign) * ROOT::Math::SimilarityT(a, gB);
0110   //  edm::LogInfo("RecoVertex/KalmanVertexUpdator")
0111   //    << "weight matrix" << newVertexWeight << "\n";
0112
0113   AlgebraicVector3 newSwr =
0114       oldVertex.weightTimesPosition() +
0115       (weight * sign) * ((ROOT::Math::Transpose(a) * gB) *
0116                          (linearizedTrack->predictedStateParameters() - linearizedTrack->constantTerm()));
0117   //  edm::LogInfo("RecoVertex/KalmanVertexUpdator")
0118   //    << "weighttimespos" << newSwr << "\n";
0119
0120   VertexState newpos(newSwr, GlobalWeight(newVertexWeight), 1.0);
0121
0122   //  edm::LogInfo("RecoVertex/KalmanVertexUpdator")
0123   //    << "pos" << newpos.position() << "\n";
0124
0125   return newpos;
0126 }
0127
0128 template <unsigned int N>
0129 std::pair<bool, double> KalmanVertexUpdator<N>::chi2Increment(const VertexState& oldVertex,
0130                                                               const VertexState& newVertexState,
0131                                                               const RefCountedLinearizedTrackState linearizedTrack,
0132                                                               float weight) const {
0133   int error;
0134   GlobalPoint newVertexPosition = newVertexState.position();
0135
0136   if (!linearizedTrack->isValid())
0137     return std::pair<bool, double>(false, -1.);
0138
0139   AlgebraicVector3 newVertexPositionV;
0140   newVertexPositionV(0) = newVertexPosition.x();
0141   newVertexPositionV(1) = newVertexPosition.y();
0142   newVertexPositionV(2) = newVertexPosition.z();
0143
0144   const AlgebraicMatrixN3& a = linearizedTrack->positionJacobian();
0145   const AlgebraicMatrixNM& b = linearizedTrack->momentumJacobian();
0146
0147   AlgebraicVectorN trackParameters = linearizedTrack->predictedStateParameters();
0148
0149   AlgebraicSymMatrixNN trackParametersWeight = linearizedTrack->predictedStateWeight(error);
0150   if (error != 0) {
0151     edm::LogWarning("KalmanVertexUpdator")
0152         << "predictedState error matrix inversion failed. An invalid vertex will be returned.";
0153     return std::pair<bool, double>(false, -1.);
0154   }
0155
0156   AlgebraicSymMatrixMM s = ROOT::Math::SimilarityT(b, trackParametersWeight);
0157   if (!invertPosDefMatrix(s)) {
0158     edm::LogWarning("KalmanVertexUpdator") << "S matrix inversion failed. An invalid vertex will be returned.";
0159     return std::pair<bool, double>(false, -1.);
0160   }
0161
0162   const AlgebraicVectorN& theResidual = linearizedTrack->constantTerm();
0163   AlgebraicVectorN vv = trackParameters - theResidual - a * newVertexPositionV;
0164   AlgebraicVectorM newTrackMomentumP = s * ROOT::Math::Transpose(b) * trackParametersWeight * vv;
0165
0166   //   AlgebraicVectorN rtp = ( theResidual +  a * newVertexPositionV + b * newTrackMomentumP);
0167
0168   AlgebraicVectorN parameterResiduals = vv - b * newTrackMomentumP;
0169   linearizedTrack->checkParameters(parameterResiduals);
0170
0171   double chi2 = weight * ROOT::Math::Similarity(parameterResiduals, trackParametersWeight);
0172
0173   //   chi2 += vertexPositionChi2(oldVertex, newVertexPosition);
0174   chi2 += helper.vertexChi2(oldVertex, newVertexState);
0175
0176   return std::pair<bool, double>(true, chi2);
0177 }
0178
0179 template class KalmanVertexUpdator<5>;
0180 template class KalmanVertexUpdator<6>;