KinematicFitPrimitives/src/MultipleKinematicConstraint.cc

0001 #include "RecoVertex/KinematicFitPrimitives/interface/MultipleKinematicConstraint.h"
0002
0003 void MultipleKinematicConstraint::addConstraint(KinematicConstraint *newConst) const {
0004   if (newConst == nullptr)
0005     throw VertexException("MultipleKinematicConstraint::zero constraint pointer passed");
0006   cts.push_back(newConst);
0007   em = false;
0008 }
0009
0010 std::pair<AlgebraicVector, AlgebraicVector> MultipleKinematicConstraint::value(const AlgebraicVector &exPoint) const {
0011   if (cts.empty())
0012     throw VertexException("MultipleKinematicConstraint::value requested for empty constraint");
0013   if (exPoint.num_row() == 0)
0014     throw VertexException("MultipleKinematicConstraint::value requested for zero Linearization point");
0015
0016   //looking for total number of states, represented by this  point.
0017   //Since this version only works with extended  cartesian parametrization,
0018   //we check whether the dimensionof he point is n*7
0019   AlgebraicVector expansion = exPoint;
0020   int inSize = exPoint.num_row();
0021   if ((inSize % 7) != 0)
0022     throw VertexException("MultipleKinematicConstraint::linearization point has a wrong dimension");
0023
0024   int total = 0;
0025   for (std::vector<KinematicConstraint *>::const_iterator i = cts.begin(); i != cts.end(); i++) {
0026     total += (*i)->numberOfEquations();
0027   }
0028   AlgebraicVector vl(total, 0);
0029
0030   int cr_size = 0;
0031   for (std::vector<KinematicConstraint *>::const_iterator i = cts.begin(); i != cts.end(); i++) {
0032     AlgebraicVector vlc = (*i)->value(expansion).first;
0033     int sz = vlc.num_row();
0034     for (int j = 1; j < sz + 1; j++) {
0035       vl(cr_size + j) = vlc(j);
0036     }
0037     cr_size += sz;
0038   }
0039   return std::pair<AlgebraicVector, AlgebraicVector>(vl, expansion);
0040 }
0041
0042 std::pair<AlgebraicMatrix, AlgebraicVector> MultipleKinematicConstraint::derivative(
0043     const AlgebraicVector &exPoint) const {
0044   if (cts.empty())
0045     throw VertexException("MultipleKinematicConstraint::derivative requested for empty constraint");
0046   if (exPoint.num_row() == 0)
0047     throw VertexException("MultipleKinematicConstraint::value requested for zero Linearization point");
0048
0049   //security check for extended cartesian parametrization
0050   int inSize = exPoint.num_row();
0051   if ((inSize % 7) != 0)
0052     throw VertexException("MultipleKinematicConstraint::linearization point has a wrong dimension");
0053
0054   AlgebraicVector par = exPoint;
0055   int total = 0;
0056   for (std::vector<KinematicConstraint *>::const_iterator i = cts.begin(); i != cts.end(); i++) {
0057     total += (*i)->numberOfEquations();
0058   }
0059
0060   //Full derivative matrix: (numberOfConstraints x 7*numberOfStates)
0061   AlgebraicMatrix dr(total, inSize);
0062
0063   int cr_size = 0;
0064   for (std::vector<KinematicConstraint *>::const_iterator i = cts.begin(); i != cts.end(); i++) {
0065     //matrix should be (nx7*NumberOfStates)
0066     AlgebraicMatrix lConst = (*i)->derivative(par).first;
0067     dr.sub(cr_size + 1, 1, lConst);
0068     cr_size += (*i)->numberOfEquations();
0069   }
0070   return std::pair<AlgebraicMatrix, AlgebraicVector>(dr, par);
0071 }
0072
0073 int MultipleKinematicConstraint::numberOfEquations() const {
0074   int ne = 0;
0075   if (cts.empty())
0076     throw VertexException("MultipleKinematicConstraint::number of equations requested for empty constraint");
0077   for (std::vector<KinematicConstraint *>::const_iterator i = cts.begin(); i != cts.end(); i++) {
0078     ne += (*i)->numberOfEquations();
0079   }
0080   return ne;
0081 }
0082
0083 std::pair<AlgebraicVector, AlgebraicVector> MultipleKinematicConstraint::value(
0084     const std::vector<RefCountedKinematicParticle> &par) const {
0085   if (cts.empty())
0086     throw VertexException("MultipleKinematicConstraint::derivative requested for empty constraint");
0087   int nStates = par.size();
0088   AlgebraicVector param(7 * nStates, 0);
0089   int count = 1;
0090   for (std::vector<RefCountedKinematicParticle>::const_iterator i = par.begin(); i != par.end(); i++) {
0091     for (int j = 1; j < 8; j++) {
0092       param((count - 1) * 7 + j) = (*i)->currentState().kinematicParameters().vector()(j - 1);
0093     }
0094     count++;
0095   }
0096
0097   //looking for total number of equations
0098   int total = 0;
0099   for (std::vector<KinematicConstraint *>::const_iterator i = cts.begin(); i != cts.end(); i++) {
0100     total += (*i)->numberOfEquations();
0101   }
0102   AlgebraicVector vl(total, 0);
0103
0104   int cr_size = 0;
0105   for (std::vector<KinematicConstraint *>::const_iterator i = cts.begin(); i != cts.end(); i++) {
0106     AlgebraicVector vlc = (*i)->value(par).first;
0107     int sz = vlc.num_row();
0108     for (int j = 1; j <= sz; j++) {
0109       vl(cr_size + j) = vlc(j);
0110     }
0111     cr_size += sz;
0112   }
0113   return std::pair<AlgebraicVector, AlgebraicVector>(vl, param);
0114 }
0115
0116 std::pair<AlgebraicMatrix, AlgebraicVector> MultipleKinematicConstraint::derivative(
0117     const std::vector<RefCountedKinematicParticle> &par) const {
0118   if (cts.empty())
0119     throw VertexException("MultipleKinematicConstraint::derivative requested for empty constraint");
0120   int nStates = par.size();
0121   AlgebraicVector param(7 * nStates, 0);
0122
0123   int count = 1;
0124   for (std::vector<RefCountedKinematicParticle>::const_iterator i = par.begin(); i != par.end(); i++) {
0125     for (int j = 1; j < 8; j++) {
0126       param((count - 1) * 7 + j) = (*i)->currentState().kinematicParameters().vector()(j - 1);
0127     }
0128     count++;
0129   }
0130   int total = 0;
0131   for (std::vector<KinematicConstraint *>::const_iterator i = cts.begin(); i != cts.end(); i++) {
0132     total += (*i)->numberOfEquations();
0133   }
0134   AlgebraicMatrix dr(total, 7 * nStates);
0135
0136   int cr_size = 0;
0137   for (std::vector<KinematicConstraint *>::const_iterator i = cts.begin(); i != cts.end(); i++) {
0138     //matrix should be (TotalNumberOfEquations x 7* TotalNumberOfStates)
0139     //Derivative matrix for given constraint
0140     AlgebraicMatrix lConst = (*i)->derivative(param).first;
0141
0142     //putting it into the appropriate line
0143     dr.sub(cr_size + 1, 1, lConst);
0144     cr_size += (*i)->numberOfEquations();
0145   }
0146   return std::pair<AlgebraicMatrix, AlgebraicVector>(dr, param);
0147 }
0148
0149 AlgebraicVector MultipleKinematicConstraint::deviations(int nStates) const {
0150   AlgebraicVector dev(nStates * 7, 0);
0151   if (cts.empty())
0152     throw VertexException("MultipleKinematicConstraint::deviations requested for empty constraint");
0153   for (std::vector<KinematicConstraint *>::const_iterator i = cts.begin(); i != cts.end(); i++) {
0154     AlgebraicVector dev_loc = (*i)->deviations(nStates);
0155     for (int j = 1; j < nStates * 7 + 1; j++) {
0156       dev(j) = dev(j) + dev_loc(j);
0157     }
0158   }
0159   return dev;
0160 }