VertexTools/src/PerigeeLinearizedTrackState.cc

0001 #include "RecoVertex/VertexTools/interface/PerigeeLinearizedTrackState.h"
0002 #include "RecoVertex/VertexTools/interface/PerigeeRefittedTrackState.h"
0003 #include "TrackingTools/TrajectoryState/interface/PerigeeConversions.h"
0004 #include "RecoVertex/VertexPrimitives/interface/VertexException.h"
0005 #include "MagneticField/Engine/interface/MagneticField.h"
0006 #include "FWCore/MessageLogger/interface/MessageLogger.h"
0007 #include "FWCore/Utilities/interface/Likely.h"
0008
0009 void PerigeeLinearizedTrackState::computeJacobians() const {
0010   GlobalPoint paramPt(theLinPoint);
0011
0012   thePredState = builder(theTSOS, paramPt);
0013   if UNLIKELY (!thePredState.isValid())
0014     return;
0015
0016   double field = theTrack.field()->inInverseGeV(thePredState.theState().position()).z();
0017
0018   if ((std::abs(theCharge) < 1e-5) || (fabs(field) < 1.e-10)) {
0019     //neutral track
0020     computeNeutralJacobians();
0021   } else {
0022     //charged track
0023     computeChargedJacobians();
0024   }
0025
0026   jacobiansAvailable = true;
0027 }
0028
0029 bool PerigeeLinearizedTrackState::operator==(const LinearizedTrackState<5>& other) const {
0030   const PerigeeLinearizedTrackState* otherP = dynamic_cast<const PerigeeLinearizedTrackState*>(&other);
0031   if (otherP == nullptr) {
0032     throw VertexException("PerigeeLinearizedTrackState: don't know how to compare myself to non-perigee track state");
0033   }
0034   return (otherP->track() == theTrack);
0035 }
0036
0037 bool PerigeeLinearizedTrackState::operator==(const ReferenceCountingPointer<LinearizedTrackState<5> >& other) const {
0038   const PerigeeLinearizedTrackState* otherP = dynamic_cast<const PerigeeLinearizedTrackState*>(other.get());
0039   if (otherP == nullptr) {
0040     throw VertexException("PerigeeLinearizedTrackState: don't know how to compare myself to non-perigee track state");
0041   }
0042   return (otherP->track() == theTrack);
0043 }
0044
0045 PerigeeLinearizedTrackState::RefCountedLinearizedTrackState PerigeeLinearizedTrackState::stateWithNewLinearizationPoint(
0046     const GlobalPoint& newLP) const {
0047   return RefCountedLinearizedTrackState(new PerigeeLinearizedTrackState(newLP, track(), theTSOS));
0048 }
0049
0050 PerigeeLinearizedTrackState::RefCountedRefittedTrackState PerigeeLinearizedTrackState::createRefittedTrackState(
0051     const GlobalPoint& vertexPosition,
0052     const AlgebraicVector3& vectorParameters,
0053     const AlgebraicSymMatrix66& covarianceMatrix) const {
0054   TrajectoryStateClosestToPoint refittedTSCP = PerigeeConversions::trajectoryStateClosestToPoint(
0055       vectorParameters, vertexPosition, charge(), covarianceMatrix, theTrack.field());
0056   return RefCountedRefittedTrackState(new PerigeeRefittedTrackState(refittedTSCP, vectorParameters));
0057 }
0058
0059 std::vector<PerigeeLinearizedTrackState::RefCountedLinearizedTrackState> PerigeeLinearizedTrackState::components()
0060     const {
0061   std::vector<RefCountedLinearizedTrackState> result;
0062   result.reserve(1);
0063   result.push_back(RefCountedLinearizedTrackState(const_cast<PerigeeLinearizedTrackState*>(this)));
0064   return result;
0065 }
0066
0067 AlgebraicVector5 PerigeeLinearizedTrackState::refittedParamFromEquation(
0068     const RefCountedRefittedTrackState& theRefittedState) const {
0069   auto p = theRefittedState->position();
0070   AlgebraicVector3 vertexPosition(p.x(), p.y(), p.z());
0071   AlgebraicVector3 momentum = theRefittedState->momentumVector();
0072   if ((momentum(2) * predictedStateMomentumParameters()(2) < 0) && (fabs(momentum(2)) > M_PI / 2)) {
0073     if (predictedStateMomentumParameters()(2) < 0.)
0074       momentum(2) -= 2 * M_PI;
0075     if (predictedStateMomentumParameters()(2) > 0.)
0076       momentum(2) += 2 * M_PI;
0077   }
0078   AlgebraicVectorN param = constantTerm() + positionJacobian() * vertexPosition + momentumJacobian() * momentum;
0079   if (param(2) > M_PI)
0080     param(2) -= 2 * M_PI;
0081   if (param(2) < -M_PI)
0082     param(2) += 2 * M_PI;
0083
0084   return param;
0085 }
0086
0087 void PerigeeLinearizedTrackState::checkParameters(AlgebraicVector5& parameters) const {
0088   if (parameters(2) > M_PI)
0089     parameters(2) -= 2 * M_PI;
0090   if (parameters(2) < -M_PI)
0091     parameters(2) += 2 * M_PI;
0092 }
0093
0094 void PerigeeLinearizedTrackState::computeChargedJacobians() const {
0095   GlobalPoint paramPt(theLinPoint);
0096   //tarjectory parameters
0097   double field = theTrack.field()->inInverseGeV(thePredState.theState().position()).z();
0098   double signTC = -theCharge;
0099
0100   double thetaAtEP = thePredState.perigeeParameters().theta();
0101   double phiAtEP = thePredState.perigeeParameters().phi();
0102   double ptAtEP = thePredState.pt();
0103   double transverseCurvatureAtEP = field / ptAtEP * signTC;
0104
0105   // Fix calculation for case where magnetic field swaps sign between previous state and current state
0106   if (field * theTrack.field()->inInverseGeV(theTSOS.globalPosition()).z() < 0.) {
0107     signTC = -signTC;
0108   }
0109
0110   double x_v = thePredState.theState().position().x();
0111   double y_v = thePredState.theState().position().y();
0112   double z_v = thePredState.theState().position().z();
0113   double X = x_v - paramPt.x() - sin(phiAtEP) / transverseCurvatureAtEP;
0114   double Y = y_v - paramPt.y() + cos(phiAtEP) / transverseCurvatureAtEP;
0115   double SS = X * X + Y * Y;
0116   double S = sqrt(SS);
0117
0118   // The track parameters at the expansion point
0119
0120   theExpandedParams[0] = transverseCurvatureAtEP;
0121   theExpandedParams[1] = thetaAtEP;
0122   theExpandedParams[3] = 1 / transverseCurvatureAtEP - signTC * S;
0123   double phiFEP;
0124   if (std::abs(X) > std::abs(Y)) {
0125     double signX = (X > 0.0 ? +1.0 : -1.0);
0126     phiFEP = -signTC * signX * acos(signTC * Y / S);
0127   } else {
0128     phiFEP = asin(-signTC * X / S);
0129     if ((signTC * Y) < 0.0)
0130       phiFEP = M_PI - phiFEP;
0131   }
0132   if (phiFEP > M_PI)
0133     phiFEP -= 2 * M_PI;
0134   theExpandedParams[2] = phiFEP;
0135   theExpandedParams[4] =
0136       z_v - paramPt.z() - (phiAtEP - theExpandedParams[2]) / tan(thetaAtEP) / transverseCurvatureAtEP;
0137
0138   // The Jacobian: (all at the expansion point)
0139   // [i,j]
0140   // i = 0: rho , 1: theta, 2: phi_p, 3: epsilon, 4: z_p
0141   // j = 0: x_v, 1: y_v, 2: z_v
0142
0143   thePositionJacobian(2, 0) = -Y / (SS);
0144   thePositionJacobian(2, 1) = X / (SS);
0145   thePositionJacobian(3, 0) = -signTC * X / S;
0146   thePositionJacobian(3, 1) = -signTC * Y / S;
0147   thePositionJacobian(4, 0) = thePositionJacobian(2, 0) / tan(thetaAtEP) / transverseCurvatureAtEP;
0148   thePositionJacobian(4, 1) = thePositionJacobian(2, 1) / tan(thetaAtEP) / transverseCurvatureAtEP;
0149   thePositionJacobian(4, 2) = 1;
0150
0151   // [i,j]
0152   // i = 0: rho , 1: theta, 2: phi_p, 3: epsilon, 4: z_p
0153   // j = 0: rho, 1: theta, 2: phi_v
0154   theMomentumJacobian(0, 0) = 1;
0155   theMomentumJacobian(1, 1) = 1;
0156
0157   theMomentumJacobian(2, 0) =
0158       -(X * cos(phiAtEP) + Y * sin(phiAtEP)) / (SS * transverseCurvatureAtEP * transverseCurvatureAtEP);
0159
0160   theMomentumJacobian(2, 2) = (Y * cos(phiAtEP) - X * sin(phiAtEP)) / (SS * transverseCurvatureAtEP);
0161
0162   theMomentumJacobian(3, 0) =
0163       (signTC * (Y * cos(phiAtEP) - X * sin(phiAtEP)) / S - 1) / (transverseCurvatureAtEP * transverseCurvatureAtEP);
0164
0165   theMomentumJacobian(3, 2) = signTC * (X * cos(phiAtEP) + Y * sin(phiAtEP)) / (S * transverseCurvatureAtEP);
0166
0167   theMomentumJacobian(4, 0) =
0168       (phiAtEP - theExpandedParams[2]) / tan(thetaAtEP) / (transverseCurvatureAtEP * transverseCurvatureAtEP) +
0169       theMomentumJacobian(2, 0) / tan(thetaAtEP) / transverseCurvatureAtEP;
0170
0171   theMomentumJacobian(4, 1) =
0172       (phiAtEP - theExpandedParams[2]) * (1 + 1 / (tan(thetaAtEP) * tan(thetaAtEP))) / transverseCurvatureAtEP;
0173
0174   theMomentumJacobian(4, 2) = (theMomentumJacobian(2, 2) - 1) / tan(thetaAtEP) / transverseCurvatureAtEP;
0175
0176   // And finally the residuals:
0177
0178   auto p = thePredState.theState().position();
0179   AlgebraicVector3 expansionPoint(p.x(), p.y(), p.z());
0180   AlgebraicVector3 momentumAtExpansionPoint(transverseCurvatureAtEP, thetaAtEP, phiAtEP);
0181
0182   theConstantTerm = AlgebraicVector5(theExpandedParams - thePositionJacobian * expansionPoint -
0183                                      theMomentumJacobian * momentumAtExpansionPoint);
0184 }
0185
0186 void PerigeeLinearizedTrackState::computeNeutralJacobians() const {
0187   GlobalPoint paramPt(theLinPoint);
0188
0189   //tarjectory parameters
0190   double thetaAtEP = thePredState.theState().momentum().theta();
0191   double phiAtEP = thePredState.theState().momentum().phi();
0192   double ptAtEP = thePredState.theState().momentum().perp();
0193
0194   double x_v = thePredState.theState().position().x();
0195   double y_v = thePredState.theState().position().y();
0196   double z_v = thePredState.theState().position().z();
0197   double X = x_v - paramPt.x();
0198   double Y = y_v - paramPt.y();
0199
0200   // The track parameters at the expansion point
0201
0202   theExpandedParams(0) = 1 / ptAtEP;
0203   theExpandedParams(1) = thetaAtEP;
0204   theExpandedParams(2) = phiAtEP;
0205   theExpandedParams(3) = X * sin(phiAtEP) - Y * cos(phiAtEP);
0206   theExpandedParams(4) = z_v - paramPt.z() - (X * cos(phiAtEP) + Y * sin(phiAtEP)) / tan(thetaAtEP);
0207
0208   // The Jacobian: (all at the expansion point)
0209   // [i,j]
0210   // i = 0: rho = 1/pt , 1: theta, 2: phi_p, 3: epsilon, 4: z_p
0211   // j = 0: x_v, 1: y_v, 2: z_v
0212
0213   thePositionJacobian(3, 0) = sin(phiAtEP);
0214   thePositionJacobian(3, 1) = -cos(phiAtEP);
0215   thePositionJacobian(4, 0) = -cos(phiAtEP) / tan(thetaAtEP);
0216   thePositionJacobian(4, 1) = -sin(phiAtEP) / tan(thetaAtEP);
0217   thePositionJacobian(4, 2) = 1;
0218
0219   // [i,j]
0220   // i = 0: rho = 1/pt , 1: theta, 2: phi_p, 3: epsilon, 4: z_p
0221   // j = 0: rho = 1/pt , 1: theta, 2: phi_v
0222
0223   theMomentumJacobian(0, 0) = 1;
0224   theMomentumJacobian(1, 1) = 1;
0225   theMomentumJacobian(2, 2) = 1;
0226
0227   theMomentumJacobian(3, 2) = X * cos(phiAtEP) + Y * sin(phiAtEP);
0228
0229   theMomentumJacobian(4, 1) = theMomentumJacobian(3, 2) * (1 + 1 / (tan(thetaAtEP) * tan(thetaAtEP)));
0230
0231   theMomentumJacobian(4, 2) = (X * sin(phiAtEP) - Y * cos(phiAtEP)) / tan(thetaAtEP);
0232
0233   // And finally the residuals:
0234
0235   auto p = thePredState.theState().position();
0236   AlgebraicVector3 expansionPoint(p.x(), p.y(), p.z());
0237   AlgebraicVector3 momentumAtExpansionPoint(1. / ptAtEP, thetaAtEP, phiAtEP);
0238
0239   theConstantTerm = AlgebraicVector5(theExpandedParams - thePositionJacobian * expansionPoint -
0240                                      theMomentumJacobian * momentumAtExpansionPoint);
0241 }