GhostTrackFitter/src/GhostTrackPrediction.cc

0001 #include <cmath>
0002
0003 #include <Math/SMatrix.h>
0004 #include <Math/MatrixFunctions.h>
0005
0006 #include "DataFormats/GeometryVector/interface/GlobalPoint.h"
0007 #include "DataFormats/GeometryVector/interface/GlobalVector.h"
0008 #include "DataFormats/GeometryCommonDetAlgo/interface/GlobalError.h"
0009 #include "DataFormats/TrackReco/interface/Track.h"
0010
0011 #include "TrackingTools/TrajectoryParametrization/interface/CurvilinearTrajectoryParameters.h"
0012 #include "TrackingTools/TrajectoryParametrization/interface/GlobalTrajectoryParameters.h"
0013 #include "TrackingTools/TrajectoryState/interface/FreeTrajectoryState.h"
0014
0015 #include "RecoVertex/GhostTrackFitter/interface/GhostTrackPrediction.h"
0016
0017 using namespace reco;
0018
0019 namespace {
0020 #ifndef __clang__
0021   inline double sqr(double arg) { return arg * arg; }
0022 #endif
0023   typedef ROOT::Math::SMatrix<double, 3, 4> Matrix34;
0024   typedef ROOT::Math::SMatrix<double, 4, 5> Matrix45;
0025   typedef ROOT::Math::SMatrix<double, 5, 4> Matrix54;
0026   typedef ROOT::Math::SMatrix<double, 4, 6> Matrix46;
0027   typedef ROOT::Math::SMatrix<double, 6, 4> Matrix64;
0028   typedef ROOT::Math::SMatrix<double, 6, 6, ROOT::Math::MatRepSym<double, 6> > Matrix6S;
0029
0030   inline GhostTrackPrediction::Vector convert(const CurvilinearTrajectoryParameters &trajectory) {
0031     return GhostTrackPrediction::Vector(trajectory.yT() / std::cos(trajectory.lambda()),
0032                                         trajectory.xT(),
0033                                         std::tan(trajectory.lambda()),
0034                                         trajectory.phi());
0035   }
0036
0037   inline GhostTrackPrediction::Vector convert(const GlobalTrajectoryParameters &trajectory) {
0038     return convert(CurvilinearTrajectoryParameters(trajectory.position(), trajectory.momentum(), 0));
0039   }
0040
0041   inline GhostTrackPrediction::Error convert(const GhostTrackPrediction::Vector &pred,
0042                                              const CurvilinearTrajectoryError &error) {
0043     double rho2 = pred[2] * pred[2] + 1.;
0044     double rho = std::sqrt(rho2);
0045
0046     Matrix45 jacobian;
0047     jacobian(0, 1) = pred[0] * pred[2];
0048     jacobian(0, 4) = rho;
0049     jacobian(1, 3) = 1.;
0050     jacobian(2, 1) = rho2;
0051     jacobian(3, 2) = 1.;
0052
0053     return ROOT::Math::Similarity(jacobian, error.matrix());
0054   }
0055
0056 }  // namespace
0057
0058 void GhostTrackPrediction::init(const GlobalPoint &priorPosition,
0059                                 const GlobalError &priorError,
0060                                 const GlobalVector &direction,
0061                                 const GlobalError &directionError) {
0062   double perp2 = direction.perp2();
0063   GlobalVector dir = direction / std::sqrt(perp2);
0064   double tip = priorPosition.y() * dir.x() - priorPosition.x() * dir.y();
0065   double l = priorPosition.x() * dir.x() + priorPosition.y() * dir.y();
0066
0067   prediction_[0] = priorPosition.z() - dir.z() * l;
0068   prediction_[1] = tip;
0069   prediction_[2] = dir.z();
0070   prediction_[3] = dir.phi();
0071
0072   Matrix46 jacobian;
0073   jacobian(0, 0) = -dir.x() * dir.z();
0074   jacobian(1, 0) = -dir.y();
0075   jacobian(0, 1) = -dir.y() * dir.z();
0076   jacobian(1, 1) = dir.x();
0077   jacobian(0, 2) = 1.;
0078   jacobian(0, 3) = -dir.z() * priorPosition.x();
0079   jacobian(1, 3) = priorPosition.y();
0080   jacobian(3, 3) = -dir.y();
0081   jacobian(0, 4) = -dir.z() * priorPosition.y();
0082   jacobian(1, 4) = -priorPosition.x();
0083   jacobian(3, 4) = dir.x();
0084   jacobian(0, 5) = -l;
0085   jacobian(2, 5) = 1.;
0086
0087   Matrix6S origCov;
0088   origCov.Place_at(priorError.matrix(), 0, 0);
0089   origCov.Place_at(directionError.matrix() / perp2, 3, 3);
0090
0091   covariance_ = ROOT::Math::Similarity(jacobian, origCov);
0092 }
0093
0094 GhostTrackPrediction::GhostTrackPrediction(const GlobalPoint &priorPosition,
0095                                            const GlobalError &priorError,
0096                                            const GlobalVector &direction,
0097                                            double coneRadius) {
0098   double dTheta = std::cosh((double)direction.eta()) * coneRadius;
0099
0100   double r2 = direction.mag2();
0101   double r = std::sqrt(r2);
0102   double perp = direction.perp();
0103   double P = direction.x() / perp;
0104   double p = direction.y() / perp;
0105   double T = direction.z() / r;
0106   double t = perp / r;
0107   double h2 = dTheta * dTheta;
0108   double d2 = coneRadius * coneRadius;
0109
0110   GlobalError cov(r2 * (T * T * P * P * h2 + t * t * p * p * d2),
0111                   r2 * p * P * (T * T * h2 - t * t * d2),
0112                   r2 * (T * T * p * p * h2 + t * t * P * P * d2),
0113                   -r2 * t * T * P * h2,
0114                   -r2 * t * T * p * h2,
0115                   r2 * t * t * h2);
0116
0117   init(priorPosition, priorError, direction, cov);
0118 }
0119
0120 GhostTrackPrediction::GhostTrackPrediction(const GlobalTrajectoryParameters &trajectory,
0121                                            const CurvilinearTrajectoryError &error)
0122     : prediction_(convert(trajectory)), covariance_(convert(prediction_, error)) {}
0123
0124 GhostTrackPrediction::GhostTrackPrediction(const Track &track)
0125     : prediction_(convert(GlobalTrajectoryParameters(GlobalPoint(track.vx(), track.vy(), track.vz()),
0126                                                      GlobalVector(track.px(), track.py(), track.pz()),
0127                                                      0,
0128                                                      nullptr))),
0129       covariance_(convert(prediction_, track.covariance())) {}
0130
0131 GlobalError GhostTrackPrediction::positionError(double lambda) const {
0132   double x = std::cos(phi());
0133   double y = std::sin(phi());
0134
0135   Matrix34 jacobian;
0136   jacobian(0, 1) = -y;
0137   jacobian(0, 3) = -y * lambda - x * ip();
0138   jacobian(1, 1) = x;
0139   jacobian(1, 3) = x * lambda - y * ip();
0140   jacobian(2, 0) = 1.;
0141   jacobian(2, 2) = lambda;
0142
0143   return ROOT::Math::Similarity(jacobian, covariance());
0144 }
0145
0146 Matrix6S GhostTrackPrediction::cartesianError(double lambda) const {
0147   double x = std::cos(phi());
0148   double y = std::sin(phi());
0149
0150   Matrix64 jacobian;
0151   jacobian(0, 1) = -y;
0152   jacobian(0, 3) = -y * lambda - x * ip();
0153   jacobian(1, 1) = x;
0154   jacobian(1, 3) = x * lambda - y * ip();
0155   jacobian(2, 0) = 1.;
0156   jacobian(2, 2) = lambda;
0157   jacobian(3, 3) = -y;
0158   jacobian(4, 3) = x;
0159   jacobian(5, 2) = 1.;
0160
0161   return ROOT::Math::Similarity(jacobian, covariance());
0162 }
0163
0164 GlobalTrajectoryParameters GhostTrackPrediction::globalTrajectory(const MagneticField *fieldProvider) const {
0165   return GlobalTrajectoryParameters(origin(), direction(), 0, fieldProvider);
0166 }
0167
0168 CurvilinearTrajectoryError GhostTrackPrediction::curvilinearError() const {
0169   double rho2I = 1. / rho2();
0170   double rhoI = std::sqrt(rho2I);
0171
0172   Matrix54 jacobian;
0173   jacobian(1, 2) = rho2I;
0174   jacobian(2, 3) = 1.;
0175   jacobian(3, 1) = 1.;
0176   jacobian(4, 0) = rhoI;
0177   jacobian(4, 2) = -z() * rhoI * cotTheta() * rho2I;
0178
0179   AlgebraicSymMatrix55 err = Similarity(jacobian, covariance());
0180   err(0, 0) = 1.;
0181
0182   return CurvilinearTrajectoryError(err);
0183 }
0184
0185 FreeTrajectoryState GhostTrackPrediction::fts(const MagneticField *fieldProvider) const {
0186   return FreeTrajectoryState(globalTrajectory(fieldProvider), curvilinearError());
0187 }
0188
0189 Track GhostTrackPrediction::track(double ndof, double chi2) const {
0190   GlobalPoint origin = this->origin();
0191   GlobalVector dir = direction().unit();
0192
0193   Track::Point point(origin.x(), origin.y(), origin.z());
0194   Track::Vector vector(dir.x(), dir.y(), dir.z());
0195
0196   return Track(chi2, ndof, point, vector, 0, curvilinearError());
0197 }