Back to home page

Project CMSSW displayed by LXR

 
 

    


File indexing completed on 2021-02-14 14:28:17

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==(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   double x_v = thePredState.theState().globalPosition().x();
0191   double y_v = thePredState.theState().globalPosition().y();
0192   double z_v = thePredState.theState().globalPosition().z();
0193   double X = x_v - paramPt.x() - sin(phiAtEP) / transverseCurvatureAtEP;
0194   double Y = y_v - paramPt.y() + cos(phiAtEP) / transverseCurvatureAtEP;
0195   double SS = X * X + Y * Y;
0196   double S = sqrt(SS);
0197 
0198   // The track parameters at the expansion point
0199   theExpandedParams[0] = transverseCurvatureAtEP;
0200   theExpandedParams[1] = thetaAtEP;
0201   theExpandedParams[3] = 1 / transverseCurvatureAtEP - signTC * S;
0202 
0203   theExpandedParams[5] = part->currentState().mass();
0204 
0205   double phiFEP;
0206   if (std::abs(X) > std::abs(Y)) {
0207     double signX = (X > 0.0 ? +1.0 : -1.0);
0208     phiFEP = -signTC * signX * acos(signTC * Y / S);
0209   } else {
0210     phiFEP = asin(-signTC * X / S);
0211     if ((signTC * Y) < 0.0)
0212       phiFEP = M_PI - phiFEP;
0213   }
0214   if (phiFEP > M_PI)
0215     phiFEP -= 2 * M_PI;
0216   theExpandedParams[2] = phiFEP;
0217   theExpandedParams[4] =
0218       z_v - paramPt.z() - (phiAtEP - theExpandedParams[2]) / tan(thetaAtEP) / transverseCurvatureAtEP;
0219 
0220   thePositionJacobian(2, 0) = -Y / (SS);
0221   thePositionJacobian(2, 1) = X / (SS);
0222   thePositionJacobian(3, 0) = -signTC * X / S;
0223   thePositionJacobian(3, 1) = -signTC * Y / S;
0224   thePositionJacobian(4, 0) = thePositionJacobian(2, 0) / tan(thetaAtEP) / transverseCurvatureAtEP;
0225   thePositionJacobian(4, 1) = thePositionJacobian(2, 1) / tan(thetaAtEP) / transverseCurvatureAtEP;
0226   thePositionJacobian(4, 2) = 1;
0227 
0228   //debug code - to be removed later
0229   //   cout<<"parameters for momentum jacobian: X "<<X<<endl;
0230   //   cout<<"parameters for momentum jacobian: Y "<<Y<<endl;
0231   //   cout<<"parameters for momentum jacobian: SS "<<SS<<endl;
0232   //   cout<<"parameters for momentum jacobian: PhiAtEP "<<phiAtEP<<endl;
0233   //   cout<<"parameters for momentum jacobian: curv "<<transverseCurvatureAtEP<<endl;
0234   //   cout<<"sin phi Atep "<<sin(phiAtEP)<<endl;
0235   //   cout<<"cos phi at EP "<<cos(phiAtEP)<<endl;
0236   //   cout<<"upper  part is "<<X*cos(phiAtEP) + Y*sin(phiAtEP) <<endl;
0237   //   cout<<"lower part is"<<SS*transverseCurvatureAtEP*transverseCurvatureAtEP<<endl;
0238 
0239   theMomentumJacobian(0, 0) = 1;
0240   theMomentumJacobian(1, 1) = 1;
0241 
0242   theMomentumJacobian(2, 0) =
0243       -(X * cos(phiAtEP) + Y * sin(phiAtEP)) / (SS * transverseCurvatureAtEP * transverseCurvatureAtEP);
0244 
0245   theMomentumJacobian(2, 2) = (Y * cos(phiAtEP) - X * sin(phiAtEP)) / (SS * transverseCurvatureAtEP);
0246 
0247   theMomentumJacobian(3, 0) =
0248       (signTC * (Y * cos(phiAtEP) - X * sin(phiAtEP)) / S - 1) / (transverseCurvatureAtEP * transverseCurvatureAtEP);
0249 
0250   theMomentumJacobian(3, 2) = signTC * (X * cos(phiAtEP) + Y * sin(phiAtEP)) / (S * transverseCurvatureAtEP);
0251 
0252   theMomentumJacobian(4, 0) =
0253       (phiAtEP - theExpandedParams(2)) / tan(thetaAtEP) / (transverseCurvatureAtEP * transverseCurvatureAtEP) +
0254       theMomentumJacobian(2, 0) / tan(thetaAtEP) / transverseCurvatureAtEP;
0255 
0256   theMomentumJacobian(4, 1) =
0257       (phiAtEP - theExpandedParams(2)) * (1 + 1 / (tan(thetaAtEP) * tan(thetaAtEP))) / transverseCurvatureAtEP;
0258 
0259   theMomentumJacobian(4, 2) = (theMomentumJacobian(2, 2) - 1) / tan(thetaAtEP) / transverseCurvatureAtEP;
0260 
0261   theMomentumJacobian(5, 3) = 1;
0262 
0263   // And finally the residuals:
0264   AlgebraicVector3 expansionPoint;
0265   expansionPoint[0] = thePredState.theState().globalPosition().x();
0266   expansionPoint[1] = thePredState.theState().globalPosition().y();
0267   expansionPoint[2] = thePredState.theState().globalPosition().z();
0268   AlgebraicVector4 momentumAtExpansionPoint;
0269   momentumAtExpansionPoint[0] = transverseCurvatureAtEP;  // Transverse Curv
0270   momentumAtExpansionPoint[1] = thetaAtEP;
0271   momentumAtExpansionPoint[2] = phiAtEP;
0272   momentumAtExpansionPoint[3] = theExpandedParams[5];
0273 
0274   theConstantTerm = AlgebraicVector6(theExpandedParams - thePositionJacobian * expansionPoint -
0275                                      theMomentumJacobian * momentumAtExpansionPoint);
0276 }
0277 
0278 void ParticleKinematicLinearizedTrackState::computeNeutralJacobians() const {
0279   GlobalPoint paramPt(theLinPoint);
0280   double thetaAtEP = thePredState.theState().globalMomentum().theta();
0281   double phiAtEP = thePredState.theState().globalMomentum().phi();
0282   double ptAtEP = thePredState.theState().globalMomentum().perp();
0283 
0284   double x_v = thePredState.theState().globalPosition().x();
0285   double y_v = thePredState.theState().globalPosition().y();
0286   double z_v = thePredState.theState().globalPosition().z();
0287   double X = x_v - paramPt.x();
0288   double Y = y_v - paramPt.y();
0289 
0290   // The track parameters at the expansion point
0291   theExpandedParams[0] = 1 / ptAtEP;
0292   theExpandedParams[1] = thetaAtEP;
0293   theExpandedParams[2] = phiAtEP;
0294   theExpandedParams[3] = X * sin(phiAtEP) - Y * cos(phiAtEP);
0295   theExpandedParams[4] = z_v - paramPt.z() - (X * cos(phiAtEP) + Y * sin(phiAtEP)) / tan(thetaAtEP);
0296   theExpandedParams[5] = part->currentState().mass();
0297 
0298   // The Jacobian: (all at the expansion point)
0299   // [i,j]
0300   // i = 0: rho = 1/pt , 1: theta, 2: phi_p, 3: epsilon, 4: z_p
0301   // j = 0: x_v, 1: y_v, 2: z_v
0302   thePositionJacobian(3, 0) = sin(phiAtEP);
0303   thePositionJacobian(3, 1) = -cos(phiAtEP);
0304   thePositionJacobian(4, 0) = -cos(phiAtEP) / tan(thetaAtEP);
0305   thePositionJacobian(4, 1) = -sin(phiAtEP) / tan(thetaAtEP);
0306   thePositionJacobian(4, 2) = 1;
0307 
0308   // [i,j]
0309   // i = 0: rho = 1/pt , 1: theta, 2: phi_p, 3: epsilon, 4: z_p
0310   // j = 0: rho = 1/pt , 1: theta, 2: phi_v
0311   theMomentumJacobian(0, 0) = 1;
0312   theMomentumJacobian(1, 1) = 1;
0313   theMomentumJacobian(2, 2) = 1;
0314 
0315   theMomentumJacobian(3, 2) = X * cos(phiAtEP) + Y * sin(phiAtEP);
0316   theMomentumJacobian(4, 1) = theMomentumJacobian(3, 2) * (1 + 1 / (tan(thetaAtEP) * tan(thetaAtEP)));
0317 
0318   theMomentumJacobian(4, 2) = (X * sin(phiAtEP) - Y * cos(phiAtEP)) / tan(thetaAtEP);
0319   theMomentumJacobian(5, 3) = 1;
0320 
0321   // And finally the residuals:
0322   AlgebraicVector3 expansionPoint;
0323   expansionPoint[0] = thePredState.theState().globalPosition().x();
0324   expansionPoint[1] = thePredState.theState().globalPosition().y();
0325   expansionPoint[2] = thePredState.theState().globalPosition().z();
0326   AlgebraicVector4 momentumAtExpansionPoint;
0327   momentumAtExpansionPoint[0] = 1 / ptAtEP;
0328   momentumAtExpansionPoint[1] = thetaAtEP;
0329   momentumAtExpansionPoint[2] = phiAtEP;
0330   momentumAtExpansionPoint[3] = theExpandedParams[5];
0331 
0332   theConstantTerm = AlgebraicVector6(theExpandedParams - thePositionJacobian * expansionPoint -
0333                                      theMomentumJacobian * momentumAtExpansionPoint);
0334 }
0335 
0336 reco::TransientTrack ParticleKinematicLinearizedTrackState::track() const {
0337   throw VertexException(" ParticleKinematicLinearizedTrackState:: no TransientTrack to return");
0338 }