KinematicConstrainedVertexUpdator.cc

Go to the documentation of this file.

#include "RecoVertex/KinematicFit/interface/KinematicConstrainedVertexUpdator.h"
#include "FWCore/MessageLogger/interface/MessageLogger.h"
#include "FWCore/Utilities/interface/isFinite.h"

KinematicConstrainedVertexUpdator::KinematicConstrainedVertexUpdator() {
  vFactory = new KinematicVertexFactory();
  vConstraint = new VertexKinematicConstraint();
}

KinematicConstrainedVertexUpdator::~KinematicConstrainedVertexUpdator() {
  delete vFactory;
  delete vConstraint;
}

std::pair<std::pair<std::vector<KinematicState>, AlgebraicMatrix>, RefCountedKinematicVertex>
KinematicConstrainedVertexUpdator::update(const AlgebraicVector& inPar,
                                          const AlgebraicMatrix& inCov,
                                          const std::vector<KinematicState>& lStates,
                                          const GlobalPoint& lPoint,
                                          MultiTrackKinematicConstraint* cs) const {
  const MagneticField* field = lStates.front().magneticField();
  AlgebraicMatrix d_matrix = vConstraint->parametersDerivative(lStates, lPoint);
  AlgebraicMatrix e_matrix = vConstraint->positionDerivative(lStates, lPoint);
  AlgebraicVector val_s = vConstraint->value(lStates, lPoint);
  int vSize = lStates.size();

  //delta alpha
  AlgebraicVector d_a(7 * vSize + 3);
  d_a(1) = lPoint.x();
  d_a(2) = lPoint.y();
  d_a(3) = lPoint.z();

  int cst = 0;
  for (std::vector<KinematicState>::const_iterator i = lStates.begin(); i != lStates.end(); i++) {
    AlgebraicVector lst_par = asHepVector<7>(i->kinematicParameters().vector());
    for (int j = 1; j < lst_par.num_row() + 1; j++) {
      d_a(3 + 7 * cst + j) = lst_par(j);
    }
    cst++;
  }

  AlgebraicVector delta_alpha = inPar - d_a;

  // cout<<"delta_alpha"<<delta_alpha<<endl;
  //resulting matrix of derivatives and vector of values.
  //their size  depends of number of tracks to analyze and number of
  //additional constraints to apply
  AlgebraicMatrix g;
  AlgebraicVector val;
  if (cs == nullptr) {
    //unconstrained vertex fitter case
    g = AlgebraicMatrix(2 * vSize, 7 * vSize + 3, 0);
    val = AlgebraicVector(2 * vSize);

    //filling the derivative matrix
    g.sub(1, 1, e_matrix);
    g.sub(1, 4, d_matrix);

    //filling the vector of values
    val = val_s;
  } else {
    //constrained vertex fitter case
    int n_eq = cs->numberOfEquations();
    g = AlgebraicMatrix(n_eq + 2 * vSize, 7 * vSize + 3, 0);
    val = AlgebraicVector(n_eq + 2 * vSize);
    AlgebraicMatrix n_x = cs->positionDerivative(lStates, lPoint);
    AlgebraicMatrix n_alpha = cs->parametersDerivative(lStates, lPoint);
    AlgebraicVector c_val = cs->value(lStates, lPoint);

    //filling the derivative matrix
    //   cout<<"n_x:"<<n_x<<endl;
    //   cout<<"n_alpha"<<n_alpha<<endl;

    g.sub(1, 1, n_x);
    g.sub(1, 4, n_alpha);
    g.sub(n_eq + 1, 1, e_matrix);
    g.sub(n_eq + 1, 4, d_matrix);

    //filling the vector of values
    for (int i = 1; i < n_eq + 1; i++) {
      val(i) = c_val(i);
    }
    for (int i = 1; i < (2 * vSize + 1); i++) {
      val(i + n_eq) = val_s(i);
    }
  }

  //check for NaN
  for (int i = 1; i <= val.num_row(); ++i) {
    if (edm::isNotFinite(val(i))) {
      LogDebug("KinematicConstrainedVertexUpdator") << "catched NaN.\n";
      return std::pair<std::pair<std::vector<KinematicState>, AlgebraicMatrix>, RefCountedKinematicVertex>(
          std::pair<std::vector<KinematicState>, AlgebraicMatrix>(std::vector<KinematicState>(), AlgebraicMatrix(1, 0)),
          RefCountedKinematicVertex());
    }
  }

  //debug feature
  AlgebraicSymMatrix in_cov_sym(7 * vSize + 3, 0);

  for (int i = 1; i < 7 * vSize + 4; ++i) {
    for (int j = 1; j < 7 * vSize + 4; ++j) {
      if (i <= j)
        in_cov_sym(i, j) = inCov(i, j);
    }
  }

  //debug code
  AlgebraicSymMatrix v_g_sym = in_cov_sym.similarity(g);

  int ifl1 = 0;
  v_g_sym.invert(ifl1);
  if (ifl1 != 0) {
    LogDebug("KinematicConstrainedVertexFitter") << "Fit failed: unable to invert SYM gain matrix\n";
    return std::pair<std::pair<std::vector<KinematicState>, AlgebraicMatrix>, RefCountedKinematicVertex>(
        std::pair<std::vector<KinematicState>, AlgebraicMatrix>(std::vector<KinematicState>(), AlgebraicMatrix(1, 0)),
        RefCountedKinematicVertex());
  }

  // delta alpha is now valid!
  //full math case now!
  AlgebraicVector lambda = v_g_sym * (g * delta_alpha + val);

  //final parameters
  AlgebraicVector finPar = inPar - in_cov_sym * g.T() * lambda;

  //covariance matrix business:
  AlgebraicMatrix mFactor = in_cov_sym * (v_g_sym.similarityT(g)) * in_cov_sym;

  //refitted covariance
  AlgebraicMatrix rCov = in_cov_sym - mFactor;

  //symmetric covariance:
  AlgebraicSymMatrix r_cov_sym(7 * vSize + 3, 0);
  for (int i = 1; i < 7 * vSize + 4; ++i) {
    for (int j = 1; j < 7 * vSize + 4; ++j) {
      if (i <= j)
        r_cov_sym(i, j) = rCov(i, j);
    }
  }

  AlgebraicSymMatrix pCov = r_cov_sym.sub(1, 3);

  // chi2
  AlgebraicVector chi = lambda.T() * (g * delta_alpha + val);

  //this is ndf without significant prior
  //vertex so -3 factor exists here
  float ndf = 2 * vSize - 3;
  if (cs != nullptr) {
    ndf += cs->numberOfEquations();
  }

  //making resulting vertex
  GlobalPoint vPos(finPar(1), finPar(2), finPar(3));
  VertexState st(vPos, GlobalError(asSMatrix<3>(pCov)));
  RefCountedKinematicVertex rVtx = vFactory->vertex(st, chi(1), ndf);

  //making refitted states of Kinematic Particles
  int i_int = 0;
  std::vector<KinematicState> ns;
  for (std::vector<KinematicState>::const_iterator i_st = lStates.begin(); i_st != lStates.end(); i_st++) {
    AlgebraicVector7 newPar;
    for (int i = 0; i < 7; i++) {
      newPar(i) = finPar(4 + i_int * 7 + i);
    }

    AlgebraicSymMatrix nCovariance = r_cov_sym.sub(4 + i_int * 7, 10 + i_int * 7);
    TrackCharge chl = i_st->particleCharge();
    KinematicParameters nrPar(newPar);
    KinematicParametersError nrEr(asSMatrix<7>(nCovariance));
    KinematicState newState(nrPar, nrEr, chl, field);
    ns.push_back(newState);
    i_int++;
  }
  std::pair<std::vector<KinematicState>, AlgebraicMatrix> ns_m(ns, rCov);
  return std::pair<std::pair<std::vector<KinematicState>, AlgebraicMatrix>, RefCountedKinematicVertex>(ns_m, rVtx);
}