KinematicFitPrimitives/src/ParticleKinematicLinearizedTrackState.cc

0001 #include "RecoVertex/KinematicFitPrimitives/interface/ParticleKinematicLinearizedTrackState.h"
0002 #include "RecoVertex/KinematicFitPrimitives/interface/KinematicRefittedTrackState.h"
0003 #include "RecoVertex/KinematicFitPrimitives/interface/KinematicPerigeeConversions.h"
0004
0005 const AlgebraicVector6& ParticleKinematicLinearizedTrackState::constantTerm() const {
0006   if (!jacobiansAvailable)
0007     computeJacobians();
0008   return theConstantTerm;
0009 }
0010
0011 const AlgebraicMatrix63& ParticleKinematicLinearizedTrackState::positionJacobian() const {
0012   if (!jacobiansAvailable)
0013     computeJacobians();
0014   return thePositionJacobian;
0015 }
0016
0017 const AlgebraicMatrix64& ParticleKinematicLinearizedTrackState::momentumJacobian() const {
0018   if (!jacobiansAvailable)
0019     computeJacobians();
0020   return theMomentumJacobian;
0021 }
0022
0023 const AlgebraicVector6& ParticleKinematicLinearizedTrackState::parametersFromExpansion() const {
0024   if (!jacobiansAvailable)
0025     computeJacobians();
0026   return theExpandedParams;
0027 }
0028
0029 AlgebraicVector6 ParticleKinematicLinearizedTrackState::predictedStateParameters() const {
0030   if (!jacobiansAvailable)
0031     computeJacobians();
0032   return thePredState.perigeeParameters().vector();
0033 }
0034
0035 AlgebraicSymMatrix66 ParticleKinematicLinearizedTrackState::predictedStateWeight(int& error) const {
0036   if (!jacobiansAvailable)
0037     computeJacobians();
0038   int i = 0;
0039   AlgebraicSymMatrix66 z = thePredState.perigeeError().weightMatrix(i);
0040   error = i;
0041   return z;
0042   //   return thePredState.perigeeError().weightMatrix(error);
0043 }
0044
0045 AlgebraicSymMatrix66 ParticleKinematicLinearizedTrackState::predictedStateError() const {
0046   if (!jacobiansAvailable)
0047     computeJacobians();
0048   return thePredState.perigeeError().covarianceMatrix();
0049 }
0050
0051 // ImpactPointMeasurement  ParticleKinematicLinearizedTrackState::impactPointMeasurement() const
0052 // { throw VertexException(" ParticleKinematicLinearizedTrackState::impact point measurement is not implemented for kinematic classes!");}
0053
0054 TrackCharge ParticleKinematicLinearizedTrackState::charge() const { return part->initialState().particleCharge(); }
0055
0056 RefCountedKinematicParticle ParticleKinematicLinearizedTrackState::particle() const { return part; }
0057
0058 bool ParticleKinematicLinearizedTrackState::operator==(const LinearizedTrackState<6>& other) const {
0059   const ParticleKinematicLinearizedTrackState* otherP =
0060       dynamic_cast<const ParticleKinematicLinearizedTrackState*>(&other);
0061   if (otherP == nullptr) {
0062     throw VertexException(
0063         " ParticleKinematicLinearizedTrackState:: don't know how to compare myself to non-kinematic track state");
0064   }
0065   return (*(otherP->particle()) == *part);
0066 }
0067
0068 bool ParticleKinematicLinearizedTrackState::hasError() const {
0069   if (!jacobiansAvailable)
0070     computeJacobians();
0071   return thePredState.isValid();
0072 }
0073
0074 // here we make a and b matrices of
0075 // our measurement function expansion.
0076 // marices will be almost the same as for
0077 // classical perigee, but bigger:
0078 // (6x3) and (6x4) respectivelly.
0079 void ParticleKinematicLinearizedTrackState::computeJacobians() const {
0080   GlobalPoint paramPt(theLinPoint);
0081   thePredState = builder(part->currentState(), paramPt);
0082   //  bool valid = thePredState.isValid();
0083   //  if (!valid) std::cout <<"Help!!!!!!!!! State is invalid\n";
0084   //  if (!valid) return;
0085   if (std::abs(theCharge) < 1e-5) {
0086     //neutral track
0087     computeNeutralJacobians();
0088   } else {
0089     //charged track
0090     computeChargedJacobians();
0091   }
0092   jacobiansAvailable = true;
0093 }
0094
0095 ReferenceCountingPointer<LinearizedTrackState<6> >
0096 ParticleKinematicLinearizedTrackState::stateWithNewLinearizationPoint(const GlobalPoint& newLP) const {
0097   RefCountedKinematicParticle pr = part;
0098   return new ParticleKinematicLinearizedTrackState(newLP, pr);
0099 }
0100
0101 ParticleKinematicLinearizedTrackState::RefCountedRefittedTrackState
0102 ParticleKinematicLinearizedTrackState::createRefittedTrackState(const GlobalPoint& vertexPosition,
0103                                                                 const AlgebraicVector4& vectorParameters,
0104                                                                 const AlgebraicSymMatrix77& covarianceMatrix) const {
0105   KinematicPerigeeConversions conversions;
0106   KinematicState lst = conversions.kinematicState(
0107       vectorParameters, vertexPosition, charge(), covarianceMatrix, part->currentState().magneticField());
0108   RefCountedRefittedTrackState rst =
0109       RefCountedRefittedTrackState(new KinematicRefittedTrackState(lst, vectorParameters));
0110   return rst;
0111 }
0112
0113 AlgebraicVector4 ParticleKinematicLinearizedTrackState::predictedStateMomentumParameters() const {
0114   if (!jacobiansAvailable)
0115     computeJacobians();
0116   AlgebraicVector4 res;
0117   res[0] = thePredState.perigeeParameters().vector()(0);
0118   res[1] = thePredState.perigeeParameters().vector()(1);
0119   res[2] = thePredState.perigeeParameters().vector()(2);
0120   res[3] = thePredState.perigeeParameters().vector()(5);
0121   return res;
0122 }
0123
0124 AlgebraicSymMatrix44 ParticleKinematicLinearizedTrackState::predictedStateMomentumError() const {
0125   int error;
0126   if (!jacobiansAvailable)
0127     computeJacobians();
0128   AlgebraicSymMatrix44 res;
0129   AlgebraicSymMatrix33 m3 = thePredState.perigeeError().weightMatrix(error).Sub<AlgebraicSymMatrix33>(0, 0);
0130   res.Place_at(m3, 0, 0);
0131   res(3, 0) = thePredState.perigeeError().weightMatrix(error)(5, 5);
0132   res(3, 1) = thePredState.perigeeError().weightMatrix(error)(5, 0);
0133   res(3, 2) = thePredState.perigeeError().weightMatrix(error)(5, 1);
0134   res(3, 3) = thePredState.perigeeError().weightMatrix(error)(5, 2);
0135   return res;
0136 }
0137
0138 double ParticleKinematicLinearizedTrackState::weightInMixture() const { return 1.; }
0139
0140 std::vector<ReferenceCountingPointer<LinearizedTrackState<6> > > ParticleKinematicLinearizedTrackState::components()
0141     const {
0142   std::vector<ReferenceCountingPointer<LinearizedTrackState<6> > > res;
0143   res.reserve(1);
0144   res.push_back(RefCountedLinearizedTrackState(const_cast<ParticleKinematicLinearizedTrackState*>(this)));
0145   return res;
0146 }
0147
0148 AlgebraicVector6 ParticleKinematicLinearizedTrackState::refittedParamFromEquation(
0149     const RefCountedRefittedTrackState& theRefittedState) const {
0150   AlgebraicVectorM momentum = theRefittedState->momentumVector();
0151   if ((momentum(2) * predictedStateMomentumParameters()(2) < 0) && (std::fabs(momentum(2)) > M_PI / 2)) {
0152     if (predictedStateMomentumParameters()(2) < 0.)
0153       momentum(2) -= 2 * M_PI;
0154     if (predictedStateMomentumParameters()(2) > 0.)
0155       momentum(2) += 2 * M_PI;
0156   }
0157
0158   AlgebraicVector3 vertexPosition;
0159   vertexPosition(0) = theRefittedState->position().x();
0160   vertexPosition(1) = theRefittedState->position().y();
0161   vertexPosition(2) = theRefittedState->position().z();
0162   AlgebraicVector6 param = constantTerm() + positionJacobian() * vertexPosition + momentumJacobian() * momentum;
0163
0164   if (param(2) > M_PI)
0165     param(2) -= 2 * M_PI;
0166   if (param(2) < -M_PI)
0167     param(2) += 2 * M_PI;
0168
0169   return param;
0170 }
0171
0172 void ParticleKinematicLinearizedTrackState::checkParameters(AlgebraicVector6& parameters) const {
0173   if (parameters(2) > M_PI)
0174     parameters(2) -= 2 * M_PI;
0175   if (parameters(2) < -M_PI)
0176     parameters(2) += 2 * M_PI;
0177 }
0178
0179 void ParticleKinematicLinearizedTrackState::computeChargedJacobians() const {
0180   GlobalPoint paramPt(theLinPoint);
0181
0182   double field = part->currentState().magneticField()->inInverseGeV(thePredState.theState().globalPosition()).z();
0183   double signTC = -part->currentState().particleCharge();
0184
0185   double thetaAtEP = thePredState.theState().globalMomentum().theta();
0186   double phiAtEP = thePredState.theState().globalMomentum().phi();
0187   double ptAtEP = thePredState.theState().globalMomentum().perp();
0188   double transverseCurvatureAtEP = field / ptAtEP * signTC;
0189
0190   // Fix calculation for case where magnetic field swaps sign between previous state and current state
0191   if (field * part->currentState().magneticField()->inInverseGeV(part->currentState().globalPosition()).z() < 0.) {
0192     signTC = -signTC;
0193   }
0194
0195   double x_v = thePredState.theState().globalPosition().x();
0196   double y_v = thePredState.theState().globalPosition().y();
0197   double z_v = thePredState.theState().globalPosition().z();
0198   double X = x_v - paramPt.x() - sin(phiAtEP) / transverseCurvatureAtEP;
0199   double Y = y_v - paramPt.y() + cos(phiAtEP) / transverseCurvatureAtEP;
0200   double SS = X * X + Y * Y;
0201   double S = sqrt(SS);
0202
0203   // The track parameters at the expansion point
0204   theExpandedParams[0] = transverseCurvatureAtEP;
0205   theExpandedParams[1] = thetaAtEP;
0206   theExpandedParams[3] = 1 / transverseCurvatureAtEP - signTC * S;
0207
0208   theExpandedParams[5] = part->currentState().mass();
0209
0210   double phiFEP;
0211   if (std::abs(X) > std::abs(Y)) {
0212     double signX = (X > 0.0 ? +1.0 : -1.0);
0213     phiFEP = -signTC * signX * acos(signTC * Y / S);
0214   } else {
0215     phiFEP = asin(-signTC * X / S);
0216     if ((signTC * Y) < 0.0)
0217       phiFEP = M_PI - phiFEP;
0218   }
0219   if (phiFEP > M_PI)
0220     phiFEP -= 2 * M_PI;
0221   theExpandedParams[2] = phiFEP;
0222   theExpandedParams[4] =
0223       z_v - paramPt.z() - (phiAtEP - theExpandedParams[2]) / tan(thetaAtEP) / transverseCurvatureAtEP;
0224
0225   thePositionJacobian(2, 0) = -Y / (SS);
0226   thePositionJacobian(2, 1) = X / (SS);
0227   thePositionJacobian(3, 0) = -signTC * X / S;
0228   thePositionJacobian(3, 1) = -signTC * Y / S;
0229   thePositionJacobian(4, 0) = thePositionJacobian(2, 0) / tan(thetaAtEP) / transverseCurvatureAtEP;
0230   thePositionJacobian(4, 1) = thePositionJacobian(2, 1) / tan(thetaAtEP) / transverseCurvatureAtEP;
0231   thePositionJacobian(4, 2) = 1;
0232
0233   //debug code - to be removed later
0234   //   cout<<"parameters for momentum jacobian: X "<<X<<endl;
0235   //   cout<<"parameters for momentum jacobian: Y "<<Y<<endl;
0236   //   cout<<"parameters for momentum jacobian: SS "<<SS<<endl;
0237   //   cout<<"parameters for momentum jacobian: PhiAtEP "<<phiAtEP<<endl;
0238   //   cout<<"parameters for momentum jacobian: curv "<<transverseCurvatureAtEP<<endl;
0239   //   cout<<"sin phi Atep "<<sin(phiAtEP)<<endl;
0240   //   cout<<"cos phi at EP "<<cos(phiAtEP)<<endl;
0241   //   cout<<"upper  part is "<<X*cos(phiAtEP) + Y*sin(phiAtEP) <<endl;
0242   //   cout<<"lower part is"<<SS*transverseCurvatureAtEP*transverseCurvatureAtEP<<endl;
0243
0244   theMomentumJacobian(0, 0) = 1;
0245   theMomentumJacobian(1, 1) = 1;
0246
0247   theMomentumJacobian(2, 0) =
0248       -(X * cos(phiAtEP) + Y * sin(phiAtEP)) / (SS * transverseCurvatureAtEP * transverseCurvatureAtEP);
0249
0250   theMomentumJacobian(2, 2) = (Y * cos(phiAtEP) - X * sin(phiAtEP)) / (SS * transverseCurvatureAtEP);
0251
0252   theMomentumJacobian(3, 0) =
0253       (signTC * (Y * cos(phiAtEP) - X * sin(phiAtEP)) / S - 1) / (transverseCurvatureAtEP * transverseCurvatureAtEP);
0254
0255   theMomentumJacobian(3, 2) = signTC * (X * cos(phiAtEP) + Y * sin(phiAtEP)) / (S * transverseCurvatureAtEP);
0256
0257   theMomentumJacobian(4, 0) =
0258       (phiAtEP - theExpandedParams(2)) / tan(thetaAtEP) / (transverseCurvatureAtEP * transverseCurvatureAtEP) +
0259       theMomentumJacobian(2, 0) / tan(thetaAtEP) / transverseCurvatureAtEP;
0260
0261   theMomentumJacobian(4, 1) =
0262       (phiAtEP - theExpandedParams(2)) * (1 + 1 / (tan(thetaAtEP) * tan(thetaAtEP))) / transverseCurvatureAtEP;
0263
0264   theMomentumJacobian(4, 2) = (theMomentumJacobian(2, 2) - 1) / tan(thetaAtEP) / transverseCurvatureAtEP;
0265
0266   theMomentumJacobian(5, 3) = 1;
0267
0268   // And finally the residuals:
0269   AlgebraicVector3 expansionPoint;
0270   expansionPoint[0] = thePredState.theState().globalPosition().x();
0271   expansionPoint[1] = thePredState.theState().globalPosition().y();
0272   expansionPoint[2] = thePredState.theState().globalPosition().z();
0273   AlgebraicVector4 momentumAtExpansionPoint;
0274   momentumAtExpansionPoint[0] = transverseCurvatureAtEP;  // Transverse Curv
0275   momentumAtExpansionPoint[1] = thetaAtEP;
0276   momentumAtExpansionPoint[2] = phiAtEP;
0277   momentumAtExpansionPoint[3] = theExpandedParams[5];
0278
0279   theConstantTerm = AlgebraicVector6(theExpandedParams - thePositionJacobian * expansionPoint -
0280                                      theMomentumJacobian * momentumAtExpansionPoint);
0281 }
0282
0283 void ParticleKinematicLinearizedTrackState::computeNeutralJacobians() const {
0284   GlobalPoint paramPt(theLinPoint);
0285   double thetaAtEP = thePredState.theState().globalMomentum().theta();
0286   double phiAtEP = thePredState.theState().globalMomentum().phi();
0287   double ptAtEP = thePredState.theState().globalMomentum().perp();
0288
0289   double x_v = thePredState.theState().globalPosition().x();
0290   double y_v = thePredState.theState().globalPosition().y();
0291   double z_v = thePredState.theState().globalPosition().z();
0292   double X = x_v - paramPt.x();
0293   double Y = y_v - paramPt.y();
0294
0295   // The track parameters at the expansion point
0296   theExpandedParams[0] = 1 / ptAtEP;
0297   theExpandedParams[1] = thetaAtEP;
0298   theExpandedParams[2] = phiAtEP;
0299   theExpandedParams[3] = X * sin(phiAtEP) - Y * cos(phiAtEP);
0300   theExpandedParams[4] = z_v - paramPt.z() - (X * cos(phiAtEP) + Y * sin(phiAtEP)) / tan(thetaAtEP);
0301   theExpandedParams[5] = part->currentState().mass();
0302
0303   // The Jacobian: (all at the expansion point)
0304   // [i,j]
0305   // i = 0: rho = 1/pt , 1: theta, 2: phi_p, 3: epsilon, 4: z_p
0306   // j = 0: x_v, 1: y_v, 2: z_v
0307   thePositionJacobian(3, 0) = sin(phiAtEP);
0308   thePositionJacobian(3, 1) = -cos(phiAtEP);
0309   thePositionJacobian(4, 0) = -cos(phiAtEP) / tan(thetaAtEP);
0310   thePositionJacobian(4, 1) = -sin(phiAtEP) / tan(thetaAtEP);
0311   thePositionJacobian(4, 2) = 1;
0312
0313   // [i,j]
0314   // i = 0: rho = 1/pt , 1: theta, 2: phi_p, 3: epsilon, 4: z_p
0315   // j = 0: rho = 1/pt , 1: theta, 2: phi_v
0316   theMomentumJacobian(0, 0) = 1;
0317   theMomentumJacobian(1, 1) = 1;
0318   theMomentumJacobian(2, 2) = 1;
0319
0320   theMomentumJacobian(3, 2) = X * cos(phiAtEP) + Y * sin(phiAtEP);
0321   theMomentumJacobian(4, 1) = theMomentumJacobian(3, 2) * (1 + 1 / (tan(thetaAtEP) * tan(thetaAtEP)));
0322
0323   theMomentumJacobian(4, 2) = (X * sin(phiAtEP) - Y * cos(phiAtEP)) / tan(thetaAtEP);
0324   theMomentumJacobian(5, 3) = 1;
0325
0326   // And finally the residuals:
0327   AlgebraicVector3 expansionPoint;
0328   expansionPoint[0] = thePredState.theState().globalPosition().x();
0329   expansionPoint[1] = thePredState.theState().globalPosition().y();
0330   expansionPoint[2] = thePredState.theState().globalPosition().z();
0331   AlgebraicVector4 momentumAtExpansionPoint;
0332   momentumAtExpansionPoint[0] = 1 / ptAtEP;
0333   momentumAtExpansionPoint[1] = thetaAtEP;
0334   momentumAtExpansionPoint[2] = phiAtEP;
0335   momentumAtExpansionPoint[3] = theExpandedParams[5];
0336
0337   theConstantTerm = AlgebraicVector6(theExpandedParams - thePositionJacobian * expansionPoint -
0338                                      theMomentumJacobian * momentumAtExpansionPoint);
0339 }
0340
0341 reco::TransientTrack ParticleKinematicLinearizedTrackState::track() const {
0342   throw VertexException(" ParticleKinematicLinearizedTrackState:: no TransientTrack to return");
0343 }