MultipleKinematicConstraint.cc

Go to the documentation of this file.

#include "RecoVertex/KinematicFitPrimitives/interface/MultipleKinematicConstraint.h"

void MultipleKinematicConstraint::addConstraint(KinematicConstraint *newConst) const {
  if (newConst == nullptr)
    throw VertexException("MultipleKinematicConstraint::zero constraint pointer passed");
  cts.push_back(newConst);
  em = false;
}

std::pair<AlgebraicVector, AlgebraicVector> MultipleKinematicConstraint::value(const AlgebraicVector &exPoint) const {
  if (cts.empty())
    throw VertexException("MultipleKinematicConstraint::value requested for empty constraint");
  if (exPoint.num_row() == 0)
    throw VertexException("MultipleKinematicConstraint::value requested for zero Linearization point");

  //looking for total number of states, represented by this  point.
  //Since this version only works with extended  cartesian parametrization,
  //we check whether the dimensionof he point is n*7
  AlgebraicVector expansion = exPoint;
  int inSize = exPoint.num_row();
  if ((inSize % 7) != 0)
    throw VertexException("MultipleKinematicConstraint::linearization point has a wrong dimension");

  int total = 0;
  for (std::vector<KinematicConstraint *>::const_iterator i = cts.begin(); i != cts.end(); i++) {
    total += (*i)->numberOfEquations();
  }
  AlgebraicVector vl(total, 0);

  int cr_size = 0;
  for (std::vector<KinematicConstraint *>::const_iterator i = cts.begin(); i != cts.end(); i++) {
    AlgebraicVector vlc = (*i)->value(expansion).first;
    int sz = vlc.num_row();
    for (int j = 1; j < sz + 1; j++) {
      vl(cr_size + j) = vlc(j);
    }
    cr_size += sz;
  }
  return std::pair<AlgebraicVector, AlgebraicVector>(vl, expansion);
}

std::pair<AlgebraicMatrix, AlgebraicVector> MultipleKinematicConstraint::derivative(
    const AlgebraicVector &exPoint) const {
  if (cts.empty())
    throw VertexException("MultipleKinematicConstraint::derivative requested for empty constraint");
  if (exPoint.num_row() == 0)
    throw VertexException("MultipleKinematicConstraint::value requested for zero Linearization point");

  //security check for extended cartesian parametrization
  int inSize = exPoint.num_row();
  if ((inSize % 7) != 0)
    throw VertexException("MultipleKinematicConstraint::linearization point has a wrong dimension");

  AlgebraicVector par = exPoint;
  int total = 0;
  for (std::vector<KinematicConstraint *>::const_iterator i = cts.begin(); i != cts.end(); i++) {
    total += (*i)->numberOfEquations();
  }

  //Full derivative matrix: (numberOfConstraints x 7*numberOfStates)
  AlgebraicMatrix dr(total, inSize);

  int cr_size = 0;
  for (std::vector<KinematicConstraint *>::const_iterator i = cts.begin(); i != cts.end(); i++) {
    //matrix should be (nx7*NumberOfStates)
    AlgebraicMatrix lConst = (*i)->derivative(par).first;
    dr.sub(cr_size + 1, 1, lConst);
    cr_size += (*i)->numberOfEquations();
  }
  return std::pair<AlgebraicMatrix, AlgebraicVector>(dr, par);
}

int MultipleKinematicConstraint::numberOfEquations() const {
  int ne = 0;
  if (cts.empty())
    throw VertexException("MultipleKinematicConstraint::number of equations requested for empty constraint");
  for (std::vector<KinematicConstraint *>::const_iterator i = cts.begin(); i != cts.end(); i++) {
    ne += (*i)->numberOfEquations();
  }
  return ne;
}

std::pair<AlgebraicVector, AlgebraicVector> MultipleKinematicConstraint::value(
    const std::vector<RefCountedKinematicParticle> &par) const {
  if (cts.empty())
    throw VertexException("MultipleKinematicConstraint::derivative requested for empty constraint");
  int nStates = par.size();
  AlgebraicVector param(7 * nStates, 0);
  int count = 1;
  for (std::vector<RefCountedKinematicParticle>::const_iterator i = par.begin(); i != par.end(); i++) {
    for (int j = 1; j < 8; j++) {
      param((count - 1) * 7 + j) = (*i)->currentState().kinematicParameters().vector()(j - 1);
    }
    count++;
  }

  //looking for total number of equations
  int total = 0;
  for (std::vector<KinematicConstraint *>::const_iterator i = cts.begin(); i != cts.end(); i++) {
    total += (*i)->numberOfEquations();
  }
  AlgebraicVector vl(total, 0);

  int cr_size = 0;
  for (std::vector<KinematicConstraint *>::const_iterator i = cts.begin(); i != cts.end(); i++) {
    AlgebraicVector vlc = (*i)->value(par).first;
    int sz = vlc.num_row();
    for (int j = 1; j <= sz; j++) {
      vl(cr_size + j) = vlc(j);
    }
    cr_size += sz;
  }
  return std::pair<AlgebraicVector, AlgebraicVector>(vl, param);
}

std::pair<AlgebraicMatrix, AlgebraicVector> MultipleKinematicConstraint::derivative(
    const std::vector<RefCountedKinematicParticle> &par) const {
  if (cts.empty())
    throw VertexException("MultipleKinematicConstraint::derivative requested for empty constraint");
  int nStates = par.size();
  AlgebraicVector param(7 * nStates, 0);

  int count = 1;
  for (std::vector<RefCountedKinematicParticle>::const_iterator i = par.begin(); i != par.end(); i++) {
    for (int j = 1; j < 8; j++) {
      param((count - 1) * 7 + j) = (*i)->currentState().kinematicParameters().vector()(j - 1);
    }
    count++;
  }
  int total = 0;
  for (std::vector<KinematicConstraint *>::const_iterator i = cts.begin(); i != cts.end(); i++) {
    total += (*i)->numberOfEquations();
  }
  AlgebraicMatrix dr(total, 7 * nStates);

  int cr_size = 0;
  for (std::vector<KinematicConstraint *>::const_iterator i = cts.begin(); i != cts.end(); i++) {
    //matrix should be (TotalNumberOfEquations x 7* TotalNumberOfStates)
    //Derivative matrix for given constraint
    AlgebraicMatrix lConst = (*i)->derivative(param).first;

    //putting it into the appropriate line
    dr.sub(cr_size + 1, 1, lConst);
    cr_size += (*i)->numberOfEquations();
  }
  return std::pair<AlgebraicMatrix, AlgebraicVector>(dr, param);
}

AlgebraicVector MultipleKinematicConstraint::deviations(int nStates) const {
  AlgebraicVector dev(nStates * 7, 0);
  if (cts.empty())
    throw VertexException("MultipleKinematicConstraint::deviations requested for empty constraint");
  for (std::vector<KinematicConstraint *>::const_iterator i = cts.begin(); i != cts.end(); i++) {
    AlgebraicVector dev_loc = (*i)->deviations(nStates);
    for (int j = 1; j < nStates * 7 + 1; j++) {
      dev(j) = dev(j) + dev_loc(j);
    }
  }
  return dev;
}