ParticleKinematicLinearizedTrackState.cc

Go to the documentation of this file.

#include "RecoVertex/KinematicFitPrimitives/interface/ParticleKinematicLinearizedTrackState.h"
#include "RecoVertex/KinematicFitPrimitives/interface/KinematicRefittedTrackState.h"
#include "RecoVertex/KinematicFitPrimitives/interface/KinematicPerigeeConversions.h"

const AlgebraicVector6 & ParticleKinematicLinearizedTrackState::constantTerm() const
{
 if (!jacobiansAvailable) computeJacobians();
 return theConstantTerm;
}

const AlgebraicMatrix63 & ParticleKinematicLinearizedTrackState::positionJacobian() const
{
 if (!jacobiansAvailable) computeJacobians();
 return thePositionJacobian;
}

const AlgebraicMatrix64 & ParticleKinematicLinearizedTrackState::momentumJacobian() const
{
 if (!jacobiansAvailable)computeJacobians();
 return theMomentumJacobian;
}

const AlgebraicVector6 & ParticleKinematicLinearizedTrackState::parametersFromExpansion() const
{
 if (!jacobiansAvailable) computeJacobians();
 return theExpandedParams;
}

AlgebraicVector6 ParticleKinematicLinearizedTrackState::predictedStateParameters() const
{
 if(!jacobiansAvailable) computeJacobians();
 return thePredState.perigeeParameters().vector();
}
  
AlgebraicSymMatrix66  ParticleKinematicLinearizedTrackState::predictedStateWeight(int & error) const
{
  if(!jacobiansAvailable) computeJacobians();
  int i = 0;
  AlgebraicSymMatrix66 z = thePredState.perigeeError().weightMatrix(i);
  error = i;
  return z;  
//   return thePredState.perigeeError().weightMatrix(error);
}
  
AlgebraicSymMatrix66  ParticleKinematicLinearizedTrackState::predictedStateError() const
{
 if(!jacobiansAvailable) computeJacobians();
 return thePredState.perigeeError().covarianceMatrix();
} 
   
// ImpactPointMeasurement  ParticleKinematicLinearizedTrackState::impactPointMeasurement() const
// { throw VertexException(" ParticleKinematicLinearizedTrackState::impact point measurement is not implemented for kinematic classes!");}
  
TrackCharge  ParticleKinematicLinearizedTrackState::charge() const
{return part->initialState().particleCharge();}

RefCountedKinematicParticle  ParticleKinematicLinearizedTrackState::particle() const
{return part;}

bool  ParticleKinematicLinearizedTrackState::operator ==(LinearizedTrackState<6>& other)const
{
 const  ParticleKinematicLinearizedTrackState* otherP = 
    dynamic_cast<const  ParticleKinematicLinearizedTrackState*>(&other);
   if (otherP == nullptr) {
   throw VertexException(" ParticleKinematicLinearizedTrackState:: don't know how to compare myself to non-kinematic track state");
  }
  return (*(otherP->particle()) == *part);}

bool  ParticleKinematicLinearizedTrackState::hasError() const
{
 if (!jacobiansAvailable) computeJacobians();
 return thePredState.isValid();
}


// here we make a and b matrices of
// our measurement function expansion.
// marices will be almost the same as for 
// classical perigee, but bigger: 
// (6x3) and (6x4) respectivelly.
void  ParticleKinematicLinearizedTrackState::computeJacobians() const
{
 GlobalPoint paramPt(theLinPoint);
 thePredState = builder(part->currentState(), paramPt); 
//  bool valid = thePredState.isValid();
//  if (!valid) std::cout <<"Help!!!!!!!!! State is invalid\n";
//  if (!valid) return;
 if (std::abs(theCharge)<1e-5) {

//neutral track
  computeNeutralJacobians();
 }else{

//charged track
  computeChargedJacobians();
 } 
 jacobiansAvailable = true;
}

ReferenceCountingPointer<LinearizedTrackState<6> >
ParticleKinematicLinearizedTrackState::stateWithNewLinearizationPoint
                                               (const GlobalPoint & newLP) const
{
 RefCountedKinematicParticle pr = part;
 return new  ParticleKinematicLinearizedTrackState(newLP, pr);
}
                           
ParticleKinematicLinearizedTrackState::RefCountedRefittedTrackState
ParticleKinematicLinearizedTrackState::createRefittedTrackState(
    const GlobalPoint & vertexPosition,  const AlgebraicVector4 & vectorParameters,
    const AlgebraicSymMatrix77 & covarianceMatrix) const
{
 KinematicPerigeeConversions conversions;  
 KinematicState lst = conversions.kinematicState(vectorParameters, vertexPosition,
        charge(), covarianceMatrix, part->currentState().magneticField()); 
 RefCountedRefittedTrackState rst =  RefCountedRefittedTrackState(new KinematicRefittedTrackState(lst,vectorParameters));  
 return rst;
}                          
    
AlgebraicVector4  ParticleKinematicLinearizedTrackState::predictedStateMomentumParameters() const
{
 if(!jacobiansAvailable) computeJacobians();
 AlgebraicVector4 res;
 res[0] = thePredState.perigeeParameters().vector()(0);
 res[1] = thePredState.perigeeParameters().vector()(1);
 res[2] = thePredState.perigeeParameters().vector()(2);
 res[3] = thePredState.perigeeParameters().vector()(5);
 return res;
} 
 
AlgebraicSymMatrix44  ParticleKinematicLinearizedTrackState::predictedStateMomentumError() const
{ 
 int error;
 if(!jacobiansAvailable) computeJacobians();
 AlgebraicSymMatrix44 res;
 AlgebraicSymMatrix33 m3 = 
    thePredState.perigeeError().weightMatrix(error).Sub<AlgebraicSymMatrix33>(0,0);
 res.Place_at(m3,0,0);
 res(3,0) = thePredState.perigeeError().weightMatrix(error)(5,5);
 res(3,1) = thePredState.perigeeError().weightMatrix(error)(5,0);
 res(3,2) = thePredState.perigeeError().weightMatrix(error)(5,1);
 res(3,3) = thePredState.perigeeError().weightMatrix(error)(5,2);
 return res;
}                          
                           
double  ParticleKinematicLinearizedTrackState::weightInMixture() const
{return 1.;}


std::vector<ReferenceCountingPointer<LinearizedTrackState<6> > >
ParticleKinematicLinearizedTrackState::components()const
{
 std::vector<ReferenceCountingPointer<LinearizedTrackState<6> > > res;
 res.reserve(1);
 res.push_back(RefCountedLinearizedTrackState( 
            const_cast< ParticleKinematicLinearizedTrackState*>(this)));
 return res;
}


AlgebraicVector6 ParticleKinematicLinearizedTrackState::refittedParamFromEquation(
    const RefCountedRefittedTrackState & theRefittedState) const
{
  AlgebraicVectorM momentum = theRefittedState->momentumVector();
  if ((momentum(2)*predictedStateMomentumParameters()(2) <  0)&&(std::fabs(momentum(2))>M_PI/2) ) {
    if (predictedStateMomentumParameters()(2) < 0.) momentum(2)-= 2*M_PI;
    if (predictedStateMomentumParameters()(2) > 0.) momentum(2)+= 2*M_PI;
  }

  AlgebraicVector3 vertexPosition;
  vertexPosition(0) = theRefittedState->position().x();
  vertexPosition(1) = theRefittedState->position().y();
  vertexPosition(2) = theRefittedState->position().z();
  AlgebraicVector6 param = constantTerm() + 
               positionJacobian() * vertexPosition +
               momentumJacobian() * momentum;

  if (param(2) >  M_PI) param(2)-= 2*M_PI;
  if (param(2) < -M_PI) param(2)+= 2*M_PI;

  return param;
}


void ParticleKinematicLinearizedTrackState::checkParameters(AlgebraicVector6 & parameters) const
{
  if (parameters(2) >  M_PI) parameters(2)-= 2*M_PI;
  if (parameters(2) < -M_PI) parameters(2)+= 2*M_PI;
}


void ParticleKinematicLinearizedTrackState::computeChargedJacobians() const
{
 GlobalPoint paramPt(theLinPoint);
 
 double field  = part->currentState().magneticField()->inInverseGeV(thePredState.theState().globalPosition()).z();
 double signTC = -part->currentState().particleCharge();
 
 double thetaAtEP = thePredState.theState().globalMomentum().theta();
 double phiAtEP   = thePredState.theState().globalMomentum().phi();
 double ptAtEP = thePredState.theState().globalMomentum().perp();
 double transverseCurvatureAtEP = field / ptAtEP*signTC;

 double x_v = thePredState.theState().globalPosition().x();
 double y_v = thePredState.theState().globalPosition().y();
 double z_v = thePredState.theState().globalPosition().z();
 double X = x_v - paramPt.x() - sin(phiAtEP) / transverseCurvatureAtEP;
 double Y = y_v - paramPt.y() + cos(phiAtEP) / transverseCurvatureAtEP;
 double SS = X*X + Y*Y;
 double S = sqrt(SS);
 
// The track parameters at the expansion point
  theExpandedParams[0] = transverseCurvatureAtEP;
  theExpandedParams[1] = thetaAtEP;
  theExpandedParams[3] = 1/transverseCurvatureAtEP  - signTC * S;
  
  theExpandedParams[5] = part->currentState().mass();
  
  double phiFEP;
  if (std::abs(X)>std::abs(Y)) {
    double signX = (X>0.0? +1.0:-1.0);
    phiFEP = -signTC * signX*acos(signTC*Y/S);
  } else {
    phiFEP = asin(-signTC*X/S);
    if ((signTC*Y)<0.0)
      phiFEP = M_PI - phiFEP;
  }
  if (phiFEP>M_PI) phiFEP-= 2*M_PI;
  theExpandedParams[2] = phiFEP;
  theExpandedParams[4] = z_v - paramPt.z() - 
    (phiAtEP - theExpandedParams[2]) / tan(thetaAtEP)/transverseCurvatureAtEP;
        
  thePositionJacobian(2, 0) = - Y / (SS);
  thePositionJacobian(2, 1) = X / (SS);
  thePositionJacobian(3, 0) = - signTC * X / S;
  thePositionJacobian(3, 1) = - signTC * Y / S;
  thePositionJacobian(4, 0) = thePositionJacobian(2, 0)/tan(thetaAtEP)/transverseCurvatureAtEP;
  thePositionJacobian(4, 1) = thePositionJacobian(2, 1)/tan(thetaAtEP)/transverseCurvatureAtEP;
  thePositionJacobian(4, 2) = 1;
 
//debug code - to be removed later 
//   cout<<"parameters for momentum jacobian: X "<<X<<endl;
//   cout<<"parameters for momentum jacobian: Y "<<Y<<endl;
//   cout<<"parameters for momentum jacobian: SS "<<SS<<endl;
//   cout<<"parameters for momentum jacobian: PhiAtEP "<<phiAtEP<<endl;
//   cout<<"parameters for momentum jacobian: curv "<<transverseCurvatureAtEP<<endl;
//   cout<<"sin phi Atep "<<sin(phiAtEP)<<endl;
//   cout<<"cos phi at EP "<<cos(phiAtEP)<<endl;
//   cout<<"upper  part is "<<X*cos(phiAtEP) + Y*sin(phiAtEP) <<endl;  
//   cout<<"lower part is"<<SS*transverseCurvatureAtEP*transverseCurvatureAtEP<<endl;

  theMomentumJacobian(0, 0) = 1;
  theMomentumJacobian(1, 1) = 1;

  theMomentumJacobian(2, 0) = -
    (X*cos(phiAtEP) + Y*sin(phiAtEP))/
    (SS*transverseCurvatureAtEP*transverseCurvatureAtEP);

  theMomentumJacobian(2, 2) = (Y*cos(phiAtEP) - X*sin(phiAtEP)) / 
    (SS*transverseCurvatureAtEP);

  theMomentumJacobian(3, 0) = 
    (signTC * (Y*cos(phiAtEP) - X*sin(phiAtEP)) / S - 1)/
    (transverseCurvatureAtEP*transverseCurvatureAtEP);
  
  theMomentumJacobian(3, 2) = signTC *(X*cos(phiAtEP) + Y*sin(phiAtEP))/
    (S*transverseCurvatureAtEP);
  
  theMomentumJacobian(4, 0) = (phiAtEP - theExpandedParams(2)) /
    tan(thetaAtEP)/(transverseCurvatureAtEP*transverseCurvatureAtEP)+
    theMomentumJacobian(2, 0) / tan(thetaAtEP)/transverseCurvatureAtEP;

  theMomentumJacobian(4, 1) = (phiAtEP - theExpandedParams(2)) *
    (1 + 1/(tan(thetaAtEP)*tan(thetaAtEP)))/transverseCurvatureAtEP;

  theMomentumJacobian(4, 2) = (theMomentumJacobian(2, 2) - 1) / 
                tan(thetaAtEP)/transverseCurvatureAtEP;
                
                    
  theMomentumJacobian(5, 3) = 1;
  
// And finally the residuals:
  AlgebraicVector3 expansionPoint;
  expansionPoint[0] = thePredState.theState().globalPosition().x();
  expansionPoint[1] = thePredState.theState().globalPosition().y();
  expansionPoint[2] = thePredState.theState().globalPosition().z(); 
  AlgebraicVector4 momentumAtExpansionPoint;
  momentumAtExpansionPoint[0] = transverseCurvatureAtEP;  // Transverse Curv
  momentumAtExpansionPoint[1] = thetaAtEP;
  momentumAtExpansionPoint[2] = phiAtEP; 
  momentumAtExpansionPoint[3] = theExpandedParams[5];

  theConstantTerm = AlgebraicVector6( theExpandedParams -
          thePositionJacobian * expansionPoint -
          theMomentumJacobian * momentumAtExpansionPoint );
}
 
 
void ParticleKinematicLinearizedTrackState::computeNeutralJacobians() const
{
 GlobalPoint paramPt(theLinPoint);
 double thetaAtEP = thePredState.theState().globalMomentum().theta();
 double phiAtEP   = thePredState.theState().globalMomentum().phi();
 double ptAtEP = thePredState.theState().globalMomentum().perp();


 double x_v = thePredState.theState().globalPosition().x();
 double y_v = thePredState.theState().globalPosition().y();
 double z_v = thePredState.theState().globalPosition().z();
 double X = x_v - paramPt.x(); 
 double Y = y_v - paramPt.y();
  
// The track parameters at the expansion point
  theExpandedParams[0] = 1 / ptAtEP;
  theExpandedParams[1] = thetaAtEP;
  theExpandedParams[2] = phiAtEP;
  theExpandedParams[3] = X*sin(phiAtEP) - Y*cos(phiAtEP);
  theExpandedParams[4] = z_v - paramPt.z() - 
    (X*cos(phiAtEP) + Y*sin(phiAtEP)) / tan(thetaAtEP);
  theExpandedParams[5] = part->currentState().mass();
  
// The Jacobian: (all at the expansion point)
// [i,j]
// i = 0: rho = 1/pt , 1: theta, 2: phi_p, 3: epsilon, 4: z_p
// j = 0: x_v, 1: y_v, 2: z_v
 thePositionJacobian(3, 0) =   sin(phiAtEP);
 thePositionJacobian(3, 1) = - cos(phiAtEP);
 thePositionJacobian(4, 0) = - cos(phiAtEP)/tan(thetaAtEP);
 thePositionJacobian(4, 1) = - sin(phiAtEP)/tan(thetaAtEP);
 thePositionJacobian(4, 2) = 1;

// [i,j]
// i = 0: rho = 1/pt , 1: theta, 2: phi_p, 3: epsilon, 4: z_p
// j = 0: rho = 1/pt , 1: theta, 2: phi_v
 theMomentumJacobian(0, 0) = 1;
 theMomentumJacobian(1, 1) = 1;
 theMomentumJacobian(2, 2) = 1;

 theMomentumJacobian(3, 2) = X*cos(phiAtEP) + Y*sin(phiAtEP); 
 theMomentumJacobian(4, 1) = theMomentumJacobian(3,2)*
    (1 + 1/(tan(thetaAtEP)*tan(thetaAtEP)));

 theMomentumJacobian(4, 2) = (X*sin(phiAtEP) - Y*cos(phiAtEP))/tan(thetaAtEP);
 theMomentumJacobian(5, 3) = 1;
 
// And finally the residuals:
 AlgebraicVector3 expansionPoint;
 expansionPoint[0] = thePredState.theState().globalPosition().x();
 expansionPoint[1] = thePredState.theState().globalPosition().y();
 expansionPoint[2] = thePredState.theState().globalPosition().z(); 
 AlgebraicVector4 momentumAtExpansionPoint;
 momentumAtExpansionPoint[0] = 1 / ptAtEP;
 momentumAtExpansionPoint[1] = thetaAtEP;
 momentumAtExpansionPoint[2] = phiAtEP; 
 momentumAtExpansionPoint[3] = theExpandedParams[5];

 theConstantTerm = AlgebraicVector6( theExpandedParams -
               thePositionJacobian * expansionPoint -
           theMomentumJacobian * momentumAtExpansionPoint );           
}
     
reco::TransientTrack ParticleKinematicLinearizedTrackState::track() const
{
  throw VertexException(" ParticleKinematicLinearizedTrackState:: no TransientTrack to return");
}