CMS 3D CMS Logo

/data/refman/pasoursint/CMSSW_6_1_2_SLHC2/src/RecoVertex/KinematicFitPrimitives/src/ParticleKinematicLinearizedTrackState.cc

Go to the documentation of this file.
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 //   return thePredState.perigeeError().weightMatrix(error);
00043 }
00044   
00045 AlgebraicSymMatrix66  ParticleKinematicLinearizedTrackState::predictedStateError() const
00046 {
00047  if(!jacobiansAvailable) computeJacobians();
00048  return thePredState.perigeeError().covarianceMatrix();
00049 } 
00050    
00051 // ImpactPointMeasurement  ParticleKinematicLinearizedTrackState::impactPointMeasurement() const
00052 // { throw VertexException(" ParticleKinematicLinearizedTrackState::impact point measurement is not implemented for kinematic classes!");}
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 // here we make a and b matrices of
00077 // our measurement function expansion.
00078 // marices will be almost the same as for 
00079 // classical perigee, but bigger: 
00080 // (6x3) and (6x4) respectivelly.
00081 void  ParticleKinematicLinearizedTrackState::computeJacobians() const
00082 {
00083  GlobalPoint paramPt(theLinPoint);
00084  thePredState = builder(part->currentState(), paramPt); 
00085 //  bool valid = thePredState.isValid();
00086 //  if (!valid) std::cout <<"Help!!!!!!!!! State is invalid\n";
00087 //  if (!valid) return;
00088  if (std::abs(theCharge)<1e-5) {
00089 
00090 //neutral track
00091   computeNeutralJacobians();
00092  }else{
00093 
00094 //charged track
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 // The track parameters at the expansion point
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 //debug code - to be removed later 
00242 //   cout<<"parameters for momentum jacobian: X "<<X<<endl;
00243 //   cout<<"parameters for momentum jacobian: Y "<<Y<<endl;
00244 //   cout<<"parameters for momentum jacobian: SS "<<SS<<endl;
00245 //   cout<<"parameters for momentum jacobian: PhiAtEP "<<phiAtEP<<endl;
00246 //   cout<<"parameters for momentum jacobian: curv "<<transverseCurvatureAtEP<<endl;
00247 //   cout<<"sin phi Atep "<<sin(phiAtEP)<<endl;
00248 //   cout<<"cos phi at EP "<<cos(phiAtEP)<<endl;
00249 //   cout<<"upper  part is "<<X*cos(phiAtEP) + Y*sin(phiAtEP) <<endl;  
00250 //   cout<<"lower part is"<<SS*transverseCurvatureAtEP*transverseCurvatureAtEP<<endl;
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 // And finally the residuals:
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;  // Transverse Curv
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 // The track parameters at the expansion point
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 // The Jacobian: (all at the expansion point)
00323 // [i,j]
00324 // i = 0: rho = 1/pt , 1: theta, 2: phi_p, 3: epsilon, 4: z_p
00325 // j = 0: x_v, 1: y_v, 2: z_v
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 // [i,j]
00333 // i = 0: rho = 1/pt , 1: theta, 2: phi_p, 3: epsilon, 4: z_p
00334 // j = 0: rho = 1/pt , 1: theta, 2: phi_v
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 // And finally the residuals:
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