GhostTrackFitter/src/TrackGhostTrackState.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/GeometrySurface/interface/Line.h"
0009
0010 #include "TrackingTools/TransientTrack/interface/TransientTrack.h"
0011 #include "TrackingTools/GeomPropagators/interface/AnalyticalImpactPointExtrapolator.h"
0012 #include "TrackingTools/GeomPropagators/interface/AnalyticalTrajectoryExtrapolatorToLine.h"
0013 #include "TrackingTools/TrajectoryState/interface/TrajectoryStateOnSurface.h"
0014 #include "TrackingTools/TrajectoryParametrization/interface/CartesianTrajectoryError.h"
0015
0016 #include "RecoVertex/GhostTrackFitter/interface/GhostTrackPrediction.h"
0017
0018 #include "RecoVertex/GhostTrackFitter/interface/TrackGhostTrackState.h"
0019
0020 using namespace reco;
0021
0022 namespace {
0023   inline double sqr(double arg) { return arg * arg; }
0024
0025   using namespace ROOT::Math;
0026
0027   typedef SVector<double, 3> Vector3;
0028
0029   typedef SMatrix<double, 3, 3, MatRepSym<double, 3> > Matrix3S;
0030   typedef SMatrix<double, 6, 6, MatRepSym<double, 6> > Matrix6S;
0031   typedef SMatrix<double, 3, 3> Matrix33;
0032   typedef SMatrix<double, 3, 6> Matrix36;
0033
0034   inline Vector3 conv(const GlobalVector &vec) {
0035     Vector3 result;
0036     result[0] = vec.x();
0037     result[1] = vec.y();
0038     result[2] = vec.z();
0039     return result;
0040   }
0041 }  // namespace
0042
0043 bool TrackGhostTrackState::linearize(const GhostTrackPrediction &pred, bool initial, double lambda) {
0044   AnalyticalTrajectoryExtrapolatorToLine extrap(track_.field());
0045
0046   GlobalPoint origin = pred.origin();
0047   GlobalVector direction = pred.direction();
0048
0049   if (isValid() && !initial) {
0050     GlobalPoint pca = origin + lambda_ * direction;
0051     Line line(pca, direction);
0052     tsos_ = extrap.extrapolate(tsos_, line);
0053   } else {
0054     GlobalPoint pca = origin + lambda * direction;
0055     Line line(pca, direction);
0056     tsos_ = extrap.extrapolate(track_.impactPointState(), line);
0057   }
0058
0059   if (!isValid())
0060     return false;
0061
0062   lambda_ = (tsos_.globalPosition() - origin) * direction / pred.rho2();
0063
0064   return true;
0065 }
0066
0067 bool TrackGhostTrackState::linearize(const GhostTrackPrediction &pred, double lambda) {
0068   AnalyticalImpactPointExtrapolator extrap(track_.field());
0069
0070   GlobalPoint point = pred.position(lambda);
0071
0072   tsos_ = extrap.extrapolate(track_.impactPointState(), point);
0073   if (!isValid())
0074     return false;
0075
0076   lambda_ = lambda;
0077
0078   return true;
0079 }
0080
0081 BasicGhostTrackState::Vertex TrackGhostTrackState::vertexStateOnGhostTrack(const GhostTrackPrediction &pred,
0082                                                                            bool withMeasurementError) const {
0083   using namespace ROOT::Math;
0084
0085   if (!isValid())
0086     return Vertex();
0087
0088   GlobalPoint origin = pred.origin();
0089   GlobalVector direction = pred.direction();
0090
0091   double rho2 = pred.rho2();
0092   double rho = std::sqrt(rho2);
0093   double lambda = (tsos_.globalPosition() - origin) * direction / rho2;
0094   GlobalPoint pos = origin + lambda * direction;
0095
0096   GlobalVector momentum = tsos_.globalMomentum();
0097   double mom = momentum.mag();
0098
0099   Vector3 b = conv(direction) / rho;
0100   Vector3 d = conv(momentum) / mom;
0101   double l = Dot(b, d);
0102   double g = 1. / (1. - sqr(l));
0103
0104   Vector3 ca = conv(pos - tsos_.globalPosition());
0105   Vector3 bd = b - l * d;
0106   b *= g;
0107
0108   Matrix33 pA = TensorProd(b, bd);
0109   Matrix33 pB = TensorProd(b, ca);
0110
0111   Matrix36 jacobian;
0112   jacobian.Place_at(-pA + Matrix33(SMatrixIdentity()), 0, 0);
0113   jacobian.Place_at(pB / rho, 0, 3);
0114   Matrix3S error = Similarity(jacobian, pred.cartesianError(lambda));
0115
0116   if (withMeasurementError) {
0117     jacobian.Place_at(pA, 0, 0);
0118     jacobian.Place_at(-pB / mom, 0, 3);
0119     error += Similarity(jacobian, tsos_.cartesianError().matrix());
0120   }
0121
0122   return Vertex(pos, error);
0123 }
0124
0125 BasicGhostTrackState::Vertex TrackGhostTrackState::vertexStateOnMeasurement(const GhostTrackPrediction &pred,
0126                                                                             bool withGhostTrackError) const {
0127   using namespace ROOT::Math;
0128
0129   if (!isValid())
0130     return Vertex();
0131
0132   GlobalPoint origin = pred.origin();
0133   GlobalVector direction = pred.direction();
0134
0135   double rho2 = pred.rho2();
0136   double rho = std::sqrt(rho2);
0137   double lambda = (tsos_.globalPosition() - origin) * direction / rho2;
0138   GlobalPoint pos = origin + lambda * direction;
0139
0140   GlobalVector momentum = tsos_.globalMomentum();
0141   double mom = momentum.mag();
0142
0143   Vector3 b = conv(direction) / rho;
0144   Vector3 d = conv(momentum) / mom;
0145   double l = Dot(b, d);
0146   double g = 1. / (1. - sqr(l));
0147
0148   Vector3 ca = conv(tsos_.globalPosition() - pos);
0149   Vector3 bd = l * b - d;
0150   d *= g;
0151
0152   Matrix33 pC = TensorProd(d, bd);
0153   Matrix33 pD = TensorProd(d, ca);
0154
0155   Matrix36 jacobian;
0156   jacobian.Place_at(pC + Matrix33(SMatrixIdentity()), 0, 0);
0157   jacobian.Place_at(pD / mom, 0, 3);
0158   Matrix3S error = Similarity(jacobian, tsos_.cartesianError().matrix());
0159
0160   if (withGhostTrackError) {
0161     jacobian.Place_at(-pC, 0, 0);
0162     jacobian.Place_at(-pD / rho, 0, 3);
0163     error += Similarity(jacobian, pred.cartesianError(lambda));
0164   }
0165
0166   return Vertex(tsos_.globalPosition(), error);
0167 }