| 1 |
#include "TrackingTools/TrajectoryState/interface/PerigeeConversions.h"
|
| 2 |
#include "TrackingTools/TrajectoryState/interface/TrajectoryStateClosestToPoint.h"
|
| 3 |
#include "MagneticField/Engine/interface/MagneticField.h"
|
| 4 |
#include <cmath>
|
| 5 |
|
| 6 |
PerigeeTrajectoryParameters PerigeeConversions::ftsToPerigeeParameters
|
| 7 |
(const FTS& originalFTS, const GlobalPoint& referencePoint, double & pt) const
|
| 8 |
|
| 9 |
{
|
| 10 |
GlobalVector impactDistance = originalFTS.position() - referencePoint;
|
| 11 |
|
| 12 |
pt = originalFTS.momentum().perp();
|
| 13 |
if (pt==0.) throw cms::Exception("PerigeeConversions", "Track with pt=0");
|
| 14 |
|
| 15 |
double theta = originalFTS.momentum().theta();
|
| 16 |
double phi = originalFTS.momentum().phi();
|
| 17 |
double field = originalFTS.parameters().magneticField().inInverseGeV(originalFTS.position()).z();
|
| 18 |
// if (field==0.) throw cms::Exception("PerigeeConversions", "Field is 0") << " at " << originalFTS.position() << "\n" ;
|
| 19 |
|
| 20 |
double positiveMomentumPhi = ( (phi>0) ? phi : (2*M_PI+phi) );
|
| 21 |
double positionPhi = impactDistance.phi();
|
| 22 |
double positivePositionPhi =
|
| 23 |
( (positionPhi>0) ? positionPhi : (2*M_PI+positionPhi) );
|
| 24 |
double phiDiff = positiveMomentumPhi - positivePositionPhi;
|
| 25 |
if (phiDiff<0.0) phiDiff+= (2*M_PI);
|
| 26 |
double signEpsilon = ( (phiDiff > M_PI) ? -1.0 : 1.0);
|
| 27 |
|
| 28 |
double epsilon = signEpsilon *
|
| 29 |
sqrt ( impactDistance.x()*impactDistance.x() +
|
| 30 |
impactDistance.y()*impactDistance.y() );
|
| 31 |
|
| 32 |
// The track parameters:
|
| 33 |
AlgebraicVector5 theTrackParameters;
|
| 34 |
|
| 35 |
double signTC = - originalFTS.charge();
|
| 36 |
bool isCharged = (signTC!=0) && (fabs(field)>1.e-10);
|
| 37 |
if (isCharged) {
|
| 38 |
theTrackParameters[0] = field / pt*signTC;
|
| 39 |
} else {
|
| 40 |
theTrackParameters[0] = 1 / pt;
|
| 41 |
}
|
| 42 |
theTrackParameters[1] = theta;
|
| 43 |
theTrackParameters[2] = phi;
|
| 44 |
theTrackParameters[3] = epsilon;
|
| 45 |
theTrackParameters[4] = impactDistance.z();
|
| 46 |
return PerigeeTrajectoryParameters(theTrackParameters, isCharged);
|
| 47 |
}
|
| 48 |
|
| 49 |
// PerigeeTrajectoryParameters PerigeeConversions::helixToPerigeeParameters
|
| 50 |
// (const reco::helix::Parameters & helixPar, const GlobalPoint& referencePoint) const
|
| 51 |
// {
|
| 52 |
// AlgebraicVector theTrackParameters = AlgebraicVector(5);
|
| 53 |
// double field = TrackingTools::FakeField::Field::inTesla(helixPar.vertex()).z() * 2.99792458e-3;
|
| 54 |
// theTrackParameters[0] = - field*helixPar.omega();
|
| 55 |
// theTrackParameters[1] = atan(1/helixPar.tanDip());
|
| 56 |
// theTrackParameters[2] = helixPar.phi0() - M_PI/2;
|
| 57 |
// theTrackParameters[3] = helixPar.d0();
|
| 58 |
// theTrackParameters[4] = helixPar.dz();
|
| 59 |
// return PerigeeTrajectoryParameters(theTrackParameters, helixPar.pt(), true);
|
| 60 |
// }
|
| 61 |
|
| 62 |
PerigeeTrajectoryError PerigeeConversions::ftsToPerigeeError
|
| 63 |
(const FTS& originalFTS) const
|
| 64 |
{
|
| 65 |
AlgebraicSymMatrix55 errorMatrix = originalFTS.curvilinearError().matrix();
|
| 66 |
AlgebraicMatrix55 curv2perigee = jacobianCurvilinear2Perigee(originalFTS);
|
| 67 |
return PerigeeTrajectoryError(ROOT::Math::Similarity(curv2perigee,errorMatrix));
|
| 68 |
}
|
| 69 |
|
| 70 |
// PerigeeTrajectoryError PerigeeConversions::helixToPerigeeError
|
| 71 |
// (const reco::helix::Parameters & helixPar,
|
| 72 |
// const reco::helix::Covariance & helixCov) const
|
| 73 |
// {
|
| 74 |
// //FIXME: verify that the order of the parameters are correct
|
| 75 |
// AlgebraicSymMatrix55 helixCovMatrix;
|
| 76 |
// helixCovMatrix(0,0) = helixCov(reco::helix::i_d0,reco::helix::i_d0);
|
| 77 |
// helixCovMatrix(1,1) = helixCov(reco::helix::i_phi0,reco::helix::i_phi0);
|
| 78 |
// helixCovMatrix(2,2) = helixCov(reco::helix::i_omega,reco::helix::i_omega);
|
| 79 |
// helixCovMatrix(3,3) = helixCov(reco::helix::i_dz,reco::helix::i_dz);
|
| 80 |
// helixCovMatrix(4,4) = helixCov(reco::helix::i_tanDip,reco::helix::i_tanDip);
|
| 81 |
//
|
| 82 |
// helixCovMatrix(0,1) = helixCov(reco::helix::i_d0,reco::helix::i_phi0);
|
| 83 |
// helixCovMatrix(0,2) = helixCov(reco::helix::i_d0,reco::helix::i_omega);
|
| 84 |
// helixCovMatrix(0,3) = helixCov(reco::helix::i_d0,reco::helix::i_dz);
|
| 85 |
// helixCovMatrix(0,4) = helixCov(reco::helix::i_d0,reco::helix::i_tanDip);
|
| 86 |
//
|
| 87 |
// helixCovMatrix(1,2) = helixCov(reco::helix::i_phi0,reco::helix::i_omega);
|
| 88 |
// helixCovMatrix(1,3) = helixCov(reco::helix::i_phi0,reco::helix::i_dz);
|
| 89 |
// helixCovMatrix(1,4) = helixCov(reco::helix::i_phi0,reco::helix::i_tanDip);
|
| 90 |
//
|
| 91 |
// helixCovMatrix(2,3) = helixCov(reco::helix::i_omega,reco::helix::i_dz);
|
| 92 |
// helixCovMatrix(2,4) = helixCov(reco::helix::i_omega,reco::helix::i_tanDip);
|
| 93 |
//
|
| 94 |
// helixCovMatrix(3,4) = helixCov(reco::helix::i_dz,reco::helix::i_tanDip);
|
| 95 |
//
|
| 96 |
// AlgebraicMatrix helix2perigee = jacobianHelix2Perigee(helixPar, helixCov);
|
| 97 |
// return PerigeeTrajectoryError(helixCovMatrix.similarity(helix2perigee));
|
| 98 |
// }
|
| 99 |
|
| 100 |
|
| 101 |
CurvilinearTrajectoryError PerigeeConversions::curvilinearError
|
| 102 |
(const PerigeeTrajectoryError& perigeeError, const GlobalTrajectoryParameters& gtp) const
|
| 103 |
{
|
| 104 |
AlgebraicMatrix55 perigee2curv = jacobianPerigee2Curvilinear(gtp);
|
| 105 |
return CurvilinearTrajectoryError(ROOT::Math::Similarity(perigee2curv, perigeeError.covarianceMatrix()));
|
| 106 |
}
|
| 107 |
|
| 108 |
GlobalPoint PerigeeConversions::positionFromPerigee
|
| 109 |
(const PerigeeTrajectoryParameters& parameters, const GlobalPoint& referencePoint) const
|
| 110 |
{
|
| 111 |
AlgebraicVector5 theVector = parameters.vector();
|
| 112 |
return GlobalPoint(theVector[3]*sin(theVector[2])+referencePoint.x(),
|
| 113 |
-theVector[3]*cos(theVector[2])+referencePoint.y(),
|
| 114 |
theVector[4]+referencePoint.z());
|
| 115 |
}
|
| 116 |
|
| 117 |
|
| 118 |
GlobalVector PerigeeConversions::momentumFromPerigee
|
| 119 |
(const PerigeeTrajectoryParameters& parameters, double pt, const GlobalPoint& referencePoint) const
|
| 120 |
{
|
| 121 |
return GlobalVector(cos(parameters.phi()) * pt,
|
| 122 |
sin(parameters.phi()) * pt,
|
| 123 |
pt / tan(parameters.theta()));
|
| 124 |
}
|
| 125 |
|
| 126 |
|
| 127 |
GlobalVector PerigeeConversions::momentumFromPerigee
|
| 128 |
(const AlgebraicVector3& momentum, const TrackCharge& charge,
|
| 129 |
const GlobalPoint& referencePoint, const MagneticField* field) const
|
| 130 |
{
|
| 131 |
double pt;
|
| 132 |
if (momentum[0]==0.) throw cms::Exception("PerigeeConversions", "Track with rho=0");
|
| 133 |
|
| 134 |
double bz = fabs(field->inInverseGeV(referencePoint).z());
|
| 135 |
if ( charge!=0 && bz>1.e-10 ) {
|
| 136 |
pt = std::abs(bz/momentum[0]);
|
| 137 |
if (pt<1.e-10) throw cms::Exception("PerigeeConversions", "pt is 0");
|
| 138 |
} else {
|
| 139 |
pt = 1 / momentum[0];
|
| 140 |
}
|
| 141 |
|
| 142 |
return GlobalVector(cos(momentum[2]) * pt,
|
| 143 |
sin(momentum[2]) * pt,
|
| 144 |
pt/tan(momentum[1]));
|
| 145 |
}
|
| 146 |
|
| 147 |
TrackCharge PerigeeConversions::chargeFromPerigee
|
| 148 |
(const PerigeeTrajectoryParameters& parameters) const
|
| 149 |
{
|
| 150 |
return parameters.charge();
|
| 151 |
}
|
| 152 |
|
| 153 |
TrajectoryStateClosestToPoint PerigeeConversions::trajectoryStateClosestToPoint
|
| 154 |
(const AlgebraicVector3& momentum, const GlobalPoint& referencePoint,
|
| 155 |
const TrackCharge& charge, const AlgebraicSymMatrix66& theCovarianceMatrix,
|
| 156 |
const MagneticField* field) const
|
| 157 |
{
|
| 158 |
AlgebraicMatrix66 param2cart = jacobianParameters2Cartesian
|
| 159 |
(momentum, referencePoint, charge, field);
|
| 160 |
CartesianTrajectoryError cartesianTrajErr(ROOT::Math::Similarity(param2cart, theCovarianceMatrix));
|
| 161 |
|
| 162 |
FTS theFTS(GlobalTrajectoryParameters(referencePoint,
|
| 163 |
momentumFromPerigee(momentum, charge, referencePoint, field), charge,
|
| 164 |
field), cartesianTrajErr);
|
| 165 |
|
| 166 |
return TrajectoryStateClosestToPoint(theFTS, referencePoint);
|
| 167 |
}
|
| 168 |
|
| 169 |
|
| 170 |
AlgebraicMatrix66
|
| 171 |
PerigeeConversions::jacobianParameters2Cartesian(
|
| 172 |
const AlgebraicVector3& momentum, const GlobalPoint& position,
|
| 173 |
const TrackCharge& charge, const MagneticField* field) const
|
| 174 |
{
|
| 175 |
if (momentum[0]==0.) throw cms::Exception("PerigeeConversions", "Track with rho=0");
|
| 176 |
double factor = 1.;
|
| 177 |
double bField = field->inInverseGeV(position).z();
|
| 178 |
if (charge!=0 && fabs(bField)>1.e-10) {
|
| 179 |
// if (bField==0.) throw cms::Exception("PerigeeConversions", "Field is 0");
|
| 180 |
factor = -bField*charge;
|
| 181 |
}
|
| 182 |
AlgebraicMatrix66 frameTransJ;
|
| 183 |
frameTransJ(0,0) = 1;
|
| 184 |
frameTransJ(1,1) = 1;
|
| 185 |
frameTransJ(2,2) = 1;
|
| 186 |
frameTransJ(3,3) = - factor * cos(momentum[2]) / (momentum[0]*momentum[0]);
|
| 187 |
frameTransJ(4,3) = - factor * sin(momentum[2]) / (momentum[0]*momentum[0]);
|
| 188 |
frameTransJ(5,3) = - factor / tan(momentum[1]) / (momentum[0]*momentum[0]);
|
| 189 |
|
| 190 |
frameTransJ(3,5) = - factor * sin(momentum[2]) / (momentum[0]);
|
| 191 |
frameTransJ(4,5) = factor * cos(momentum[2]) / (momentum[0]);
|
| 192 |
frameTransJ(5,4) = - factor / (momentum[0]*sin(momentum[1])*sin(momentum[1]));
|
| 193 |
|
| 194 |
return frameTransJ;
|
| 195 |
}
|
| 196 |
|
| 197 |
|
| 198 |
AlgebraicMatrix55
|
| 199 |
PerigeeConversions::jacobianCurvilinear2Perigee(const FreeTrajectoryState& fts) const {
|
| 200 |
|
| 201 |
GlobalVector p = fts.momentum();
|
| 202 |
|
| 203 |
GlobalVector Z = GlobalVector(0.,0.,1.);
|
| 204 |
GlobalVector T = p.unit();
|
| 205 |
GlobalVector U = Z.cross(T).unit();;
|
| 206 |
GlobalVector V = T.cross(U);
|
| 207 |
|
| 208 |
GlobalVector I = GlobalVector(-p.x(), -p.y(), 0.); //opposite to track dir.
|
| 209 |
I = I.unit();
|
| 210 |
GlobalVector J(-I.y(), I.x(),0.); //counterclockwise rotation
|
| 211 |
GlobalVector K(Z);
|
| 212 |
GlobalPoint x = fts.position();
|
| 213 |
GlobalVector B = fts.parameters().magneticField().inInverseGeV(x);
|
| 214 |
GlobalVector H = B.unit();
|
| 215 |
GlobalVector HxT = H.cross(T);
|
| 216 |
GlobalVector N = HxT.unit();
|
| 217 |
double alpha = HxT.mag();
|
| 218 |
double qbp = fts.signedInverseMomentum();
|
| 219 |
double Q = -B.mag() * qbp;
|
| 220 |
double alphaQ = alpha * Q;
|
| 221 |
|
| 222 |
double lambda = 0.5 * M_PI - p.theta();
|
| 223 |
double coslambda = cos(lambda), tanlambda = tan(lambda);
|
| 224 |
|
| 225 |
double TI = T.dot(I);
|
| 226 |
double NU = N.dot(U);
|
| 227 |
double NV = N.dot(V);
|
| 228 |
double UI = U.dot(I);
|
| 229 |
double VI = V.dot(I);
|
| 230 |
double UJ = U.dot(J);
|
| 231 |
double VJ = V.dot(J);
|
| 232 |
double UK = U.dot(K);
|
| 233 |
double VK = V.dot(K);
|
| 234 |
|
| 235 |
AlgebraicMatrix55 jac;
|
| 236 |
|
| 237 |
if( fabs(fts.transverseCurvature())<1.e-10 ) {
|
| 238 |
jac(0,0) = 1/coslambda;
|
| 239 |
jac(0,1) = tanlambda/coslambda/fts.parameters().momentum().mag();
|
| 240 |
}else{
|
| 241 |
double Bz = B.z();
|
| 242 |
jac(0,0) = -Bz/coslambda;
|
| 243 |
jac(0,1) = -Bz * tanlambda/coslambda*qbp;
|
| 244 |
jac(1,3) = alphaQ * NV * UI/TI;
|
| 245 |
jac(1,4) = alphaQ * NV * VI/TI;
|
| 246 |
jac(0,3) = -jac(0,1) * jac(1,3);
|
| 247 |
jac(0,4) = -jac(0,1) * jac(1,4);
|
| 248 |
jac(2,3) = -alphaQ/coslambda * NU * UI/TI;
|
| 249 |
jac(2,4) = -alphaQ/coslambda * NU * VI/TI;
|
| 250 |
}
|
| 251 |
jac(1,1) = -1.;
|
| 252 |
jac(2,2) = 1.;
|
| 253 |
jac(3,3) = VK/TI;
|
| 254 |
jac(3,4) = -UK/TI;
|
| 255 |
jac(4,3) = -VJ/TI;
|
| 256 |
jac(4,4) = UJ/TI;
|
| 257 |
|
| 258 |
return jac;
|
| 259 |
|
| 260 |
}
|
| 261 |
|
| 262 |
|
| 263 |
|
| 264 |
AlgebraicMatrix55
|
| 265 |
PerigeeConversions::jacobianPerigee2Curvilinear(const GlobalTrajectoryParameters& gtp) const {
|
| 266 |
|
| 267 |
GlobalVector p = gtp.momentum();
|
| 268 |
|
| 269 |
GlobalVector Z = GlobalVector(0.f,0.f,1.f);
|
| 270 |
GlobalVector T = p.unit();
|
| 271 |
GlobalVector U = Z.cross(T).unit();;
|
| 272 |
GlobalVector V = T.cross(U);
|
| 273 |
|
| 274 |
GlobalVector I = GlobalVector(-p.x(), -p.y(), 0.f); //opposite to track dir.
|
| 275 |
I = I.unit();
|
| 276 |
GlobalVector J(-I.y(), I.x(),0.f); //counterclockwise rotation
|
| 277 |
GlobalVector K(Z);
|
| 278 |
GlobalPoint x = gtp.position();
|
| 279 |
GlobalVector B = gtp.magneticField().inInverseGeV(x);
|
| 280 |
GlobalVector H = B.unit();
|
| 281 |
GlobalVector HxT = H.cross(T);
|
| 282 |
GlobalVector N = HxT.unit();
|
| 283 |
double alpha = HxT.mag();
|
| 284 |
double qbp = gtp.signedInverseMomentum();
|
| 285 |
double Q = -B.mag() * qbp;
|
| 286 |
double alphaQ = alpha * Q;
|
| 287 |
|
| 288 |
double lambda = 0.5 * M_PI - p.theta();
|
| 289 |
double coslambda = cos(lambda), sinlambda = sin(lambda);
|
| 290 |
double mqbpt = -1./coslambda * qbp;
|
| 291 |
|
| 292 |
double TJ = T.dot(J);
|
| 293 |
double TK = T.dot(K);
|
| 294 |
double NU = N.dot(U);
|
| 295 |
double NV = N.dot(V);
|
| 296 |
double UJ = U.dot(J);
|
| 297 |
double VJ = V.dot(J);
|
| 298 |
double UK = U.dot(K);
|
| 299 |
double VK = V.dot(K);
|
| 300 |
|
| 301 |
AlgebraicMatrix55 jac;
|
| 302 |
|
| 303 |
if( fabs(gtp.transverseCurvature())<1.e-10f ) {
|
| 304 |
jac(0,0) = coslambda;
|
| 305 |
jac(0,1) = sinlambda/coslambda/gtp.momentum().mag();
|
| 306 |
}else{
|
| 307 |
jac(0,0) = -coslambda/B.z();
|
| 308 |
jac(0,1) = -sinlambda * mqbpt;
|
| 309 |
jac(1,3) = -alphaQ * NV * TJ;
|
| 310 |
jac(1,4) = -alphaQ * NV * TK;
|
| 311 |
jac(2,3) = -alphaQ/coslambda * NU * TJ;
|
| 312 |
jac(2,4) = -alphaQ/coslambda * NU * TK;
|
| 313 |
}
|
| 314 |
jac(1,1) = -1.;
|
| 315 |
jac(2,2) = 1.;
|
| 316 |
jac(3,3) = UJ;
|
| 317 |
jac(3,4) = UK;
|
| 318 |
jac(4,3) = VJ;
|
| 319 |
jac(4,4) = VK;
|
| 320 |
|
| 321 |
return jac;
|
| 322 |
}
|
| 323 |
|
| 324 |
// AlgebraicMatrix
|
| 325 |
// PerigeeConversions::jacobianHelix2Perigee(const reco::helix::Parameters & helixPar,
|
| 326 |
// const reco::helix::Covariance & helixCov) const
|
| 327 |
// {
|
| 328 |
// AlgebraicMatrix55 jac;
|
| 329 |
//
|
| 330 |
// jac(3,0) = 1.;
|
| 331 |
// jac(2,1) = 1.;
|
| 332 |
// // jac(0,2) = - 1. / magField.inTesla(helixPar.vertex()).z() * 2.99792458e-3;
|
| 333 |
// jac(0,2) = - 1. / (TrackingTools::FakeField::Field::inTesla(helixPar.vertex()).z() * 2.99792458e-3);
|
| 334 |
// jac(4,3) = 1.;
|
| 335 |
// jac(1,4) = -(1. + helixPar.tanDip()*helixPar.tanDip());
|
| 336 |
// std::std::cout << jac;
|
| 337 |
// return jac;
|
| 338 |
// }
|
