KalmanVertexUpdator.cc

Go to the documentation of this file.

#include "RecoVertex/KalmanVertexFit/interface/KalmanVertexUpdator.h"
#include "DataFormats/GeometryVector/interface/GlobalVector.h"
#include "DataFormats/CLHEP/interface/AlgebraicObjects.h"
#include "RecoVertex/VertexPrimitives/interface/VertexException.h"
#include "DataFormats/GeometrySurface/interface/ReferenceCounted.h"
#include "FWCore/MessageLogger/interface/MessageLogger.h"
#include "DataFormats/Math/interface/invertPosDefMatrix.h"
 
#include <algorithm>
 
// Based on the R.Fruhwirth et al Computer Physics Communications 96 (1996) 189-208
template <unsigned int N>
CachingVertex<N> KalmanVertexUpdator<N>::update(const CachingVertex<N>& oldVertex,
                                                const RefCountedVertexTrack track,
                                                float weight,
                                                int sign) const {
  if (abs(sign) != 1)
    throw VertexException("KalmanVertexUpdator::abs(sign) not equal to 1");
 
  VertexState newVertexState = positionUpdate(oldVertex.vertexState(), track->linearizedTrack(), weight, sign);
  if (!newVertexState.isValid())
    return CachingVertex<N>();
 
  float chi1 = oldVertex.totalChiSquared();
  std::pair<bool, double> chi2P =
      chi2Increment(oldVertex.vertexState(), newVertexState, track->linearizedTrack(), weight);
  if (!chi2P.first)
    return CachingVertex<N>();  // return invalid vertex
 
  chi1 += sign * chi2P.second;
 
  //adding or removing track from the CachingVertex::VertexTracks
  std::vector<RefCountedVertexTrack> newVertexTracks = oldVertex.tracks();
 
  if (sign > 0) {
    newVertexTracks.push_back(track);
  } else {
    typename std::vector<RefCountedVertexTrack>::iterator pos =
        find(newVertexTracks.begin(), newVertexTracks.end(), track);
    if (pos != newVertexTracks.end()) {
      newVertexTracks.erase(pos);
    } else {
      std::cout << "KalmanVertexUpdator::Unable to find requested track in the current vertex" << std::endl;
      throw VertexException("KalmanVertexUpdator::Unable to find requested track in the current vertex");
    }
  }
 
  if (oldVertex.hasPrior()) {
    return CachingVertex<N>(oldVertex.priorVertexState(), newVertexState, newVertexTracks, chi1);
  } else {
    return CachingVertex<N>(newVertexState, newVertexTracks, chi1);
  }
}
 
template <unsigned int N>
CachingVertex<N> KalmanVertexUpdator<N>::add(const CachingVertex<N>& oldVertex,
                                             const RefCountedVertexTrack track) const {
  float weight = track->weight();
  return update(oldVertex, track, weight, +1);
}
 
template <unsigned int N>
CachingVertex<N> KalmanVertexUpdator<N>::remove(const CachingVertex<N>& oldVertex,
                                                const RefCountedVertexTrack track) const {
  float weight = track->weight();
  return update(oldVertex, track, weight, -1);
}
 
template <unsigned int N>
VertexState KalmanVertexUpdator<N>::positionUpdate(const VertexState& oldVertex,
                                                   const RefCountedLinearizedTrackState linearizedTrack,
                                                   const float weight,
                                                   int sign) const {
  int error;
 
  if (!linearizedTrack->isValid())
    return VertexState();
 
  const AlgebraicMatrixN3& a = linearizedTrack->positionJacobian();
  const AlgebraicMatrixNM& b = linearizedTrack->momentumJacobian();
 
  //   AlgebraicVectorN trackParameters =
  //    linearizedTrack->predictedStateParameters();
 
  AlgebraicSymMatrixNN trackParametersWeight = linearizedTrack->predictedStateWeight(error);
  if (error != 0) {
    edm::LogWarning("KalmanVertexUpdator")
        << "predictedState error matrix inversion failed. An invalid vertex will be returned.";
    return VertexState();
  }
 
  // Jacobians
  //  edm::LogInfo("RecoVertex/KalmanVertexUpdator")
  //    << "Now updating position" << "\n";
 
  //vertex information
  //   AlgebraicSymMatrix33 oldVertexWeight = oldVertex.weight().matrix();
  AlgebraicSymMatrixMM s = ROOT::Math::SimilarityT(b, trackParametersWeight);
  if (!invertPosDefMatrix(s)) {
    edm::LogWarning("KalmanVertexUpdator") << "S matrix inversion failed. An invalid vertex will be returned.";
    return VertexState();
  }
 
  AlgebraicSymMatrixNN gB =
      trackParametersWeight - ROOT::Math::Similarity(trackParametersWeight, ROOT::Math::Similarity(b, s));
 
  // Getting the new covariance matrix of the vertex.
 
  AlgebraicSymMatrix33 newVertexWeight = oldVertex.weight().matrix() + (weight * sign) * ROOT::Math::SimilarityT(a, gB);
  //  edm::LogInfo("RecoVertex/KalmanVertexUpdator")
  //    << "weight matrix" << newVertexWeight << "\n";
 
  AlgebraicVector3 newSwr =
      oldVertex.weightTimesPosition() +
      (weight * sign) * ((ROOT::Math::Transpose(a) * gB) *
                         (linearizedTrack->predictedStateParameters() - linearizedTrack->constantTerm()));
  //  edm::LogInfo("RecoVertex/KalmanVertexUpdator")
  //    << "weighttimespos" << newSwr << "\n";
 
  VertexState newpos(newSwr, GlobalWeight(newVertexWeight), 1.0);
 
  //  edm::LogInfo("RecoVertex/KalmanVertexUpdator")
  //    << "pos" << newpos.position() << "\n";
 
  return newpos;
}
 
template <unsigned int N>
std::pair<bool, double> KalmanVertexUpdator<N>::chi2Increment(const VertexState& oldVertex,
                                                              const VertexState& newVertexState,
                                                              const RefCountedLinearizedTrackState linearizedTrack,
                                                              float weight) const {
  int error;
  GlobalPoint newVertexPosition = newVertexState.position();
 
  if (!linearizedTrack->isValid())
    return std::pair<bool, double>(false, -1.);
 
  AlgebraicVector3 newVertexPositionV;
  newVertexPositionV(0) = newVertexPosition.x();
  newVertexPositionV(1) = newVertexPosition.y();
  newVertexPositionV(2) = newVertexPosition.z();
 
  const AlgebraicMatrixN3& a = linearizedTrack->positionJacobian();
  const AlgebraicMatrixNM& b = linearizedTrack->momentumJacobian();
 
  AlgebraicVectorN trackParameters = linearizedTrack->predictedStateParameters();
 
  AlgebraicSymMatrixNN trackParametersWeight = linearizedTrack->predictedStateWeight(error);
  if (error != 0) {
    edm::LogWarning("KalmanVertexUpdator")
        << "predictedState error matrix inversion failed. An invalid vertex will be returned.";
    return std::pair<bool, double>(false, -1.);
  }
 
  AlgebraicSymMatrixMM s = ROOT::Math::SimilarityT(b, trackParametersWeight);
  if (!invertPosDefMatrix(s)) {
    edm::LogWarning("KalmanVertexUpdator") << "S matrix inversion failed. An invalid vertex will be returned.";
    return std::pair<bool, double>(false, -1.);
  }
 
  const AlgebraicVectorN& theResidual = linearizedTrack->constantTerm();
  AlgebraicVectorN vv = trackParameters - theResidual - a * newVertexPositionV;
  AlgebraicVectorM newTrackMomentumP = s * ROOT::Math::Transpose(b) * trackParametersWeight * vv;
 
  //   AlgebraicVectorN rtp = ( theResidual +  a * newVertexPositionV + b * newTrackMomentumP);
 
  AlgebraicVectorN parameterResiduals = vv - b * newTrackMomentumP;
  linearizedTrack->checkParameters(parameterResiduals);
 
  double chi2 = weight * ROOT::Math::Similarity(parameterResiduals, trackParametersWeight);
 
  //   chi2 += vertexPositionChi2(oldVertex, newVertexPosition);
  chi2 += helper.vertexChi2(oldVertex, newVertexState);
 
  return std::pair<bool, double>(true, chi2);
}
 
template class KalmanVertexUpdator<5>;
template class KalmanVertexUpdator<6>;