00001 #include "RecoVertex/KinematicFitPrimitives/interface/ParticleKinematicLinearizedTrackState.h"
00002 #include "RecoVertex/KinematicFitPrimitives/interface/KinematicRefittedTrackState.h"
00003 #include "RecoVertex/KinematicFitPrimitives/interface/KinematicPerigeeConversions.h"
00004
00005 const AlgebraicVector6 & ParticleKinematicLinearizedTrackState::constantTerm() const
00006 {
00007 if (!jacobiansAvailable) computeJacobians();
00008 return theConstantTerm;
00009 }
00010
00011 const AlgebraicMatrix63 & ParticleKinematicLinearizedTrackState::positionJacobian() const
00012 {
00013 if (!jacobiansAvailable) computeJacobians();
00014 return thePositionJacobian;
00015 }
00016
00017 const AlgebraicMatrix64 & ParticleKinematicLinearizedTrackState::momentumJacobian() const
00018 {
00019 if (!jacobiansAvailable)computeJacobians();
00020 return theMomentumJacobian;
00021 }
00022
00023 const AlgebraicVector6 & ParticleKinematicLinearizedTrackState::parametersFromExpansion() const
00024 {
00025 if (!jacobiansAvailable) computeJacobians();
00026 return theExpandedParams;
00027 }
00028
00029 AlgebraicVector6 ParticleKinematicLinearizedTrackState::predictedStateParameters() const
00030 {
00031 if(!jacobiansAvailable) computeJacobians();
00032 return thePredState.perigeeParameters().vector();
00033 }
00034
00035 AlgebraicSymMatrix66 ParticleKinematicLinearizedTrackState::predictedStateWeight(int & error) const
00036 {
00037 if(!jacobiansAvailable) computeJacobians();
00038 int i = 0;
00039 AlgebraicSymMatrix66 z = thePredState.perigeeError().weightMatrix(i);
00040 error = i;
00041 return z;
00042
00043 }
00044
00045 AlgebraicSymMatrix66 ParticleKinematicLinearizedTrackState::predictedStateError() const
00046 {
00047 if(!jacobiansAvailable) computeJacobians();
00048 return thePredState.perigeeError().covarianceMatrix();
00049 }
00050
00051
00052
00053
00054 TrackCharge ParticleKinematicLinearizedTrackState::charge() const
00055 {return part->initialState().particleCharge();}
00056
00057 RefCountedKinematicParticle ParticleKinematicLinearizedTrackState::particle() const
00058 {return part;}
00059
00060 bool ParticleKinematicLinearizedTrackState::operator ==(LinearizedTrackState<6>& other)const
00061 {
00062 const ParticleKinematicLinearizedTrackState* otherP =
00063 dynamic_cast<const ParticleKinematicLinearizedTrackState*>(&other);
00064 if (otherP == 0) {
00065 throw VertexException(" ParticleKinematicLinearizedTrackState:: don't know how to compare myself to non-kinematic track state");
00066 }
00067 return (*(otherP->particle()) == *part);}
00068
00069 bool ParticleKinematicLinearizedTrackState::hasError() const
00070 {
00071 if (!jacobiansAvailable) computeJacobians();
00072 return thePredState.isValid();
00073 }
00074
00075
00076
00077
00078
00079
00080
00081 void ParticleKinematicLinearizedTrackState::computeJacobians() const
00082 {
00083 GlobalPoint paramPt(theLinPoint);
00084 thePredState = builder(part->currentState(), paramPt);
00085
00086
00087
00088 if (std::abs(theCharge)<1e-5) {
00089
00090
00091 computeNeutralJacobians();
00092 }else{
00093
00094
00095 computeChargedJacobians();
00096 }
00097 jacobiansAvailable = true;
00098 }
00099
00100 ReferenceCountingPointer<LinearizedTrackState<6> >
00101 ParticleKinematicLinearizedTrackState::stateWithNewLinearizationPoint
00102 (const GlobalPoint & newLP) const
00103 {
00104 RefCountedKinematicParticle pr = part;
00105 return new ParticleKinematicLinearizedTrackState(newLP, pr);
00106 }
00107
00108 ParticleKinematicLinearizedTrackState::RefCountedRefittedTrackState
00109 ParticleKinematicLinearizedTrackState::createRefittedTrackState(
00110 const GlobalPoint & vertexPosition, const AlgebraicVector4 & vectorParameters,
00111 const AlgebraicSymMatrix77 & covarianceMatrix) const
00112 {
00113 KinematicPerigeeConversions conversions;
00114 KinematicState lst = conversions.kinematicState(vectorParameters, vertexPosition,
00115 charge(), covarianceMatrix, part->currentState().magneticField());
00116 RefCountedRefittedTrackState rst = RefCountedRefittedTrackState(new KinematicRefittedTrackState(lst,vectorParameters));
00117 return rst;
00118 }
00119
00120 AlgebraicVector4 ParticleKinematicLinearizedTrackState::predictedStateMomentumParameters() const
00121 {
00122 if(!jacobiansAvailable) computeJacobians();
00123 AlgebraicVector4 res;
00124 res[0] = thePredState.perigeeParameters().vector()(0);
00125 res[1] = thePredState.perigeeParameters().vector()(1);
00126 res[2] = thePredState.perigeeParameters().vector()(2);
00127 res[3] = thePredState.perigeeParameters().vector()(5);
00128 return res;
00129 }
00130
00131 AlgebraicSymMatrix44 ParticleKinematicLinearizedTrackState::predictedStateMomentumError() const
00132 {
00133 int error;
00134 if(!jacobiansAvailable) computeJacobians();
00135 AlgebraicSymMatrix44 res;
00136 AlgebraicSymMatrix33 m3 =
00137 thePredState.perigeeError().weightMatrix(error).Sub<AlgebraicSymMatrix33>(0,0);
00138 res.Place_at(m3,0,0);
00139 res(3,0) = thePredState.perigeeError().weightMatrix(error)(5,5);
00140 res(3,1) = thePredState.perigeeError().weightMatrix(error)(5,0);
00141 res(3,2) = thePredState.perigeeError().weightMatrix(error)(5,1);
00142 res(3,3) = thePredState.perigeeError().weightMatrix(error)(5,2);
00143 return res;
00144 }
00145
00146 double ParticleKinematicLinearizedTrackState::weightInMixture() const
00147 {return 1.;}
00148
00149
00150 std::vector<ReferenceCountingPointer<LinearizedTrackState<6> > >
00151 ParticleKinematicLinearizedTrackState::components()const
00152 {
00153 std::vector<ReferenceCountingPointer<LinearizedTrackState<6> > > res;
00154 res.reserve(1);
00155 res.push_back(RefCountedLinearizedTrackState(
00156 const_cast< ParticleKinematicLinearizedTrackState*>(this)));
00157 return res;
00158 }
00159
00160
00161 AlgebraicVector6 ParticleKinematicLinearizedTrackState::refittedParamFromEquation(
00162 const RefCountedRefittedTrackState & theRefittedState) const
00163 {
00164 AlgebraicVectorM momentum = theRefittedState->momentumVector();
00165 if ((momentum(2)*predictedStateMomentumParameters()(2) < 0)&&(std::fabs(momentum(2))>M_PI/2) ) {
00166 if (predictedStateMomentumParameters()(2) < 0.) momentum(2)-= 2*M_PI;
00167 if (predictedStateMomentumParameters()(2) > 0.) momentum(2)+= 2*M_PI;
00168 }
00169
00170 AlgebraicVector3 vertexPosition;
00171 vertexPosition(0) = theRefittedState->position().x();
00172 vertexPosition(1) = theRefittedState->position().y();
00173 vertexPosition(2) = theRefittedState->position().z();
00174 AlgebraicVector6 param = constantTerm() +
00175 positionJacobian() * vertexPosition +
00176 momentumJacobian() * momentum;
00177
00178 if (param(2) > M_PI) param(2)-= 2*M_PI;
00179 if (param(2) < -M_PI) param(2)+= 2*M_PI;
00180
00181 return param;
00182 }
00183
00184
00185 void ParticleKinematicLinearizedTrackState::checkParameters(AlgebraicVector6 & parameters) const
00186 {
00187 if (parameters(2) > M_PI) parameters(2)-= 2*M_PI;
00188 if (parameters(2) < -M_PI) parameters(2)+= 2*M_PI;
00189 }
00190
00191
00192 void ParticleKinematicLinearizedTrackState::computeChargedJacobians() const
00193 {
00194 GlobalPoint paramPt(theLinPoint);
00195
00196 double field = part->currentState().magneticField()->inInverseGeV(thePredState.theState().globalPosition()).z();
00197 double signTC = -part->currentState().particleCharge();
00198
00199 double thetaAtEP = thePredState.theState().globalMomentum().theta();
00200 double phiAtEP = thePredState.theState().globalMomentum().phi();
00201 double ptAtEP = thePredState.theState().globalMomentum().perp();
00202 double transverseCurvatureAtEP = field / ptAtEP*signTC;
00203
00204 double x_v = thePredState.theState().globalPosition().x();
00205 double y_v = thePredState.theState().globalPosition().y();
00206 double z_v = thePredState.theState().globalPosition().z();
00207 double X = x_v - paramPt.x() - sin(phiAtEP) / transverseCurvatureAtEP;
00208 double Y = y_v - paramPt.y() + cos(phiAtEP) / transverseCurvatureAtEP;
00209 double SS = X*X + Y*Y;
00210 double S = sqrt(SS);
00211
00212
00213 theExpandedParams[0] = transverseCurvatureAtEP;
00214 theExpandedParams[1] = thetaAtEP;
00215 theExpandedParams[3] = 1/transverseCurvatureAtEP - signTC * S;
00216
00217 theExpandedParams[5] = part->currentState().mass();
00218
00219 double phiFEP;
00220 if (std::abs(X)>std::abs(Y)) {
00221 double signX = (X>0.0? +1.0:-1.0);
00222 phiFEP = -signTC * signX*acos(signTC*Y/S);
00223 } else {
00224 phiFEP = asin(-signTC*X/S);
00225 if ((signTC*Y)<0.0)
00226 phiFEP = M_PI - phiFEP;
00227 }
00228 if (phiFEP>M_PI) phiFEP-= 2*M_PI;
00229 theExpandedParams[2] = phiFEP;
00230 theExpandedParams[4] = z_v - paramPt.z() -
00231 (phiAtEP - theExpandedParams[2]) / tan(thetaAtEP)/transverseCurvatureAtEP;
00232
00233 thePositionJacobian(2, 0) = - Y / (SS);
00234 thePositionJacobian(2, 1) = X / (SS);
00235 thePositionJacobian(3, 0) = - signTC * X / S;
00236 thePositionJacobian(3, 1) = - signTC * Y / S;
00237 thePositionJacobian(4, 0) = thePositionJacobian(2, 0)/tan(thetaAtEP)/transverseCurvatureAtEP;
00238 thePositionJacobian(4, 1) = thePositionJacobian(2, 1)/tan(thetaAtEP)/transverseCurvatureAtEP;
00239 thePositionJacobian(4, 2) = 1;
00240
00241
00242
00243
00244
00245
00246
00247
00248
00249
00250
00251
00252 theMomentumJacobian(0, 0) = 1;
00253 theMomentumJacobian(1, 1) = 1;
00254
00255 theMomentumJacobian(2, 0) = -
00256 (X*cos(phiAtEP) + Y*sin(phiAtEP))/
00257 (SS*transverseCurvatureAtEP*transverseCurvatureAtEP);
00258
00259 theMomentumJacobian(2, 2) = (Y*cos(phiAtEP) - X*sin(phiAtEP)) /
00260 (SS*transverseCurvatureAtEP);
00261
00262 theMomentumJacobian(3, 0) =
00263 (signTC * (Y*cos(phiAtEP) - X*sin(phiAtEP)) / S - 1)/
00264 (transverseCurvatureAtEP*transverseCurvatureAtEP);
00265
00266 theMomentumJacobian(3, 2) = signTC *(X*cos(phiAtEP) + Y*sin(phiAtEP))/
00267 (S*transverseCurvatureAtEP);
00268
00269 theMomentumJacobian(4, 0) = (phiAtEP - theExpandedParams(2)) /
00270 tan(thetaAtEP)/(transverseCurvatureAtEP*transverseCurvatureAtEP)+
00271 theMomentumJacobian(2, 0) / tan(thetaAtEP)/transverseCurvatureAtEP;
00272
00273 theMomentumJacobian(4, 1) = (phiAtEP - theExpandedParams(2)) *
00274 (1 + 1/(tan(thetaAtEP)*tan(thetaAtEP)))/transverseCurvatureAtEP;
00275
00276 theMomentumJacobian(4, 2) = (theMomentumJacobian(2, 2) - 1) /
00277 tan(thetaAtEP)/transverseCurvatureAtEP;
00278
00279
00280 theMomentumJacobian(5, 3) = 1;
00281
00282
00283 AlgebraicVector3 expansionPoint;
00284 expansionPoint[0] = thePredState.theState().globalPosition().x();
00285 expansionPoint[1] = thePredState.theState().globalPosition().y();
00286 expansionPoint[2] = thePredState.theState().globalPosition().z();
00287 AlgebraicVector4 momentumAtExpansionPoint;
00288 momentumAtExpansionPoint[0] = transverseCurvatureAtEP;
00289 momentumAtExpansionPoint[1] = thetaAtEP;
00290 momentumAtExpansionPoint[2] = phiAtEP;
00291 momentumAtExpansionPoint[3] = theExpandedParams[5];
00292
00293 theConstantTerm = AlgebraicVector6( theExpandedParams -
00294 thePositionJacobian * expansionPoint -
00295 theMomentumJacobian * momentumAtExpansionPoint );
00296 }
00297
00298
00299 void ParticleKinematicLinearizedTrackState::computeNeutralJacobians() const
00300 {
00301 GlobalPoint paramPt(theLinPoint);
00302 double thetaAtEP = thePredState.theState().globalMomentum().theta();
00303 double phiAtEP = thePredState.theState().globalMomentum().phi();
00304 double ptAtEP = thePredState.theState().globalMomentum().perp();
00305
00306
00307 double x_v = thePredState.theState().globalPosition().x();
00308 double y_v = thePredState.theState().globalPosition().y();
00309 double z_v = thePredState.theState().globalPosition().z();
00310 double X = x_v - paramPt.x();
00311 double Y = y_v - paramPt.y();
00312
00313
00314 theExpandedParams[0] = 1 / ptAtEP;
00315 theExpandedParams[1] = thetaAtEP;
00316 theExpandedParams[2] = phiAtEP;
00317 theExpandedParams[3] = X*sin(phiAtEP) - Y*cos(phiAtEP);
00318 theExpandedParams[4] = z_v - paramPt.z() -
00319 (X*cos(phiAtEP) + Y*sin(phiAtEP)) / tan(thetaAtEP);
00320 theExpandedParams[5] = part->currentState().mass();
00321
00322
00323
00324
00325
00326 thePositionJacobian(3, 0) = sin(phiAtEP);
00327 thePositionJacobian(3, 1) = - cos(phiAtEP);
00328 thePositionJacobian(4, 0) = - cos(phiAtEP)/tan(thetaAtEP);
00329 thePositionJacobian(4, 1) = - sin(phiAtEP)/tan(thetaAtEP);
00330 thePositionJacobian(4, 2) = 1;
00331
00332
00333
00334
00335 theMomentumJacobian(0, 0) = 1;
00336 theMomentumJacobian(1, 1) = 1;
00337 theMomentumJacobian(2, 2) = 1;
00338
00339 theMomentumJacobian(3, 2) = X*cos(phiAtEP) + Y*sin(phiAtEP);
00340 theMomentumJacobian(4, 1) = theMomentumJacobian(3,2)*
00341 (1 + 1/(tan(thetaAtEP)*tan(thetaAtEP)));
00342
00343 theMomentumJacobian(4, 2) = (X*sin(phiAtEP) - Y*cos(phiAtEP))/tan(thetaAtEP);
00344 theMomentumJacobian(5, 3) = 1;
00345
00346
00347 AlgebraicVector3 expansionPoint;
00348 expansionPoint[0] = thePredState.theState().globalPosition().x();
00349 expansionPoint[1] = thePredState.theState().globalPosition().y();
00350 expansionPoint[2] = thePredState.theState().globalPosition().z();
00351 AlgebraicVector4 momentumAtExpansionPoint;
00352 momentumAtExpansionPoint[0] = 1 / ptAtEP;
00353 momentumAtExpansionPoint[1] = thetaAtEP;
00354 momentumAtExpansionPoint[2] = phiAtEP;
00355 momentumAtExpansionPoint[3] = theExpandedParams[5];
00356
00357 theConstantTerm = AlgebraicVector6( theExpandedParams -
00358 thePositionJacobian * expansionPoint -
00359 theMomentumJacobian * momentumAtExpansionPoint );
00360 }
00361
00362 reco::TransientTrack ParticleKinematicLinearizedTrackState::track() const
00363 {
00364 throw VertexException(" ParticleKinematicLinearizedTrackState:: no TransientTrack to return");
00365 }
00366
00367
00368