TrajectoryState/src/BasicTrajectoryState.cc

0001 #include "TrackingTools/TrajectoryState/interface/BasicTrajectoryState.h"
0002 #include "TrackingTools/AnalyticalJacobians/interface/JacobianLocalToCurvilinear.h"
0003 #include "TrackingTools/AnalyticalJacobians/interface/JacobianCurvilinearToLocal.h"
0004 #include "TrackingTools/AnalyticalJacobians/interface/JacobianLocalToCartesian.h"
0005 #include "TrackingTools/AnalyticalJacobians/interface/JacobianCartesianToLocal.h"
0006 #include "MagneticField/Engine/interface/MagneticField.h"
0007 #include "FWCore/MessageLogger/interface/MessageLogger.h"
0008
0009 #include <cmath>
0010 #include <sstream>
0011
0012 #ifdef DO_BTSCount
0013 unsigned int BTSCount::maxReferences = 0;
0014 unsigned long long BTSCount::aveReferences = 0;
0015 unsigned long long BTSCount::toteReferences = 0;
0016
0017 BTSCount::~BTSCount() {
0018   maxReferences = std::max(referenceMax_, maxReferences);
0019   toteReferences++;
0020   aveReferences += referenceMax_;
0021   // if (referenceMax_>100) std::cout <<"BST with " << referenceMax_ << std::endl;
0022 }
0023
0024 #include <iostream>
0025 namespace {
0026
0027   struct Printer {
0028     ~Printer() {
0029       std::cout << "maxReferences of BTSCount = " << BTSCount::maxReferences << " "
0030                 << double(BTSCount::aveReferences) / double(BTSCount::toteReferences) << std::endl;
0031     }
0032   };
0033   Printer printer;
0034
0035 }  // namespace
0036 #endif
0037
0038 BasicTrajectoryState::~BasicTrajectoryState() {}
0039 BasicTrajectoryState::BasicTrajectoryState(const SurfaceType& aSurface)
0040     : theLocalError(InvalidError()),
0041       theLocalParameters(),
0042       theLocalParametersValid(false),
0043       theValid(false),
0044       theSurfaceSide(SurfaceSideDefinition::atCenterOfSurface),
0045       theSurfaceP(&aSurface),
0046       theWeight(1.) {}
0047
0048 namespace {
0049   inline FreeTrajectoryState makeFTS(const LocalTrajectoryParameters& par,
0050                                      const BasicTrajectoryState::SurfaceType& surface,
0051                                      const MagneticField* field) {
0052     GlobalPoint x = surface.toGlobal(par.position());
0053     GlobalVector p = surface.toGlobal(par.momentum());
0054     return FreeTrajectoryState(x, p, par.charge(), field);
0055   }
0056
0057   inline FreeTrajectoryState makeFTS(const LocalTrajectoryParameters& par,
0058                                      const BasicTrajectoryState::SurfaceType& surface,
0059                                      const MagneticField* field,
0060                                      GlobalVector fieldValue) {
0061     GlobalPoint x = surface.toGlobal(par.position());
0062     GlobalVector p = surface.toGlobal(par.momentum());
0063     return FreeTrajectoryState(x, p, par.charge(), field, fieldValue);
0064   }
0065
0066 }  // namespace
0067
0068 BasicTrajectoryState::BasicTrajectoryState(const LocalTrajectoryParameters& par,
0069                                            const LocalTrajectoryError& err,
0070                                            const SurfaceType& aSurface,
0071                                            const MagneticField* field,
0072                                            const SurfaceSide side)
0073     : theFreeState(makeFTS(par, aSurface, field)),
0074       theLocalError(err),
0075       theLocalParameters(par),
0076       theLocalParametersValid(true),
0077       theValid(true),
0078       theSurfaceSide(side),
0079       theSurfaceP(&aSurface),
0080       theWeight(1.) {}
0081
0082 void BasicTrajectoryState::notValid() {
0083   throw TrajectoryStateException("TrajectoryStateOnSurface is invalid and cannot return any parameters");
0084 }
0085
0086 namespace {
0087   void verifyLocalErr(LocalTrajectoryError const& err, const FreeTrajectoryState& state) {
0088     if UNLIKELY (!err.posDef())
0089       edm::LogWarning("BasicTrajectoryState") << "local error not pos-def\n"
0090                                               << err.matrix() << "\npos/mom/mf " << state.position() << ' '
0091                                               << state.momentum() << ' ' << state.parameters().magneticFieldInTesla();
0092   }
0093   void verifyCurvErr(CurvilinearTrajectoryError const& err, const FreeTrajectoryState& state) {
0094     if UNLIKELY (!err.posDef())
0095       edm::LogWarning("BasicTrajectoryState") << "curv error not pos-def\n"
0096                                               << err.matrix() << "\npos/mom/mf " << state.position() << ' '
0097                                               << state.momentum() << ' ' << state.parameters().magneticFieldInTesla();
0098   }
0099 }  // namespace
0100
0101 void BasicTrajectoryState::missingError(char const* where) const {
0102   std::stringstream form;
0103   form << "BasicTrajectoryState: attempt to access errors when none available " << where
0104        << ".\nfreestate pointer: " << theFreeState << "\nlocal error valid/values :" << theLocalError.valid() << "\n"
0105        << theLocalError.matrix();
0106
0107   edm::LogWarning("BasicTrajectoryState") << form.str();
0108
0109   // throw TrajectoryStateException(form.str());
0110 }
0111
0112 void BasicTrajectoryState::checkCurvilinError() const {
0113   if LIKELY (theFreeState.hasCurvilinearError())
0114     return;
0115
0116   if UNLIKELY (!theLocalParametersValid)
0117     createLocalParameters();
0118
0119   JacobianLocalToCurvilinear loc2Curv(surface(), localParameters(), globalParameters(), *magneticField());
0120   const AlgebraicMatrix55& jac = loc2Curv.jacobian();
0121   const AlgebraicSymMatrix55& cov = ROOT::Math::Similarity(jac, theLocalError.matrix());
0122
0123   theFreeState.setCurvilinearError(cov);
0124
0125   verifyLocalErr(theLocalError, theFreeState);
0126   verifyCurvErr(cov, theFreeState);
0127 }
0128
0129 // create local parameters from global
0130 void BasicTrajectoryState::createLocalParameters() const {
0131   LocalPoint x = surface().toLocal(theFreeState.position());
0132   LocalVector p = surface().toLocal(theFreeState.momentum());
0133   // believe p.z() never exactly equals 0.
0134   bool isCharged = theFreeState.charge() != 0;
0135   theLocalParameters = LocalTrajectoryParameters(isCharged ? theFreeState.signedInverseMomentum() : 1. / p.mag(),
0136                                                  p.x() / p.z(),
0137                                                  p.y() / p.z(),
0138                                                  x.x(),
0139                                                  x.y(),
0140                                                  p.z() > 0. ? 1. : -1.,
0141                                                  isCharged);
0142   theLocalParametersValid = true;
0143 }
0144
0145 void BasicTrajectoryState::createLocalError() const {
0146   if LIKELY (theFreeState.hasCurvilinearError())
0147     createLocalErrorFromCurvilinearError();
0148   else
0149     theLocalError = InvalidError();
0150 }
0151
0152 void BasicTrajectoryState::createLocalErrorFromCurvilinearError() const {
0153   JacobianCurvilinearToLocal curv2Loc(surface(), localParameters(), globalParameters(), *magneticField());
0154   const AlgebraicMatrix55& jac = curv2Loc.jacobian();
0155
0156   theLocalError = ROOT::Math::Similarity(jac, theFreeState.curvilinearError().matrix());
0157
0158   verifyCurvErr(theFreeState.curvilinearError(), theFreeState);
0159   verifyLocalErr(theLocalError, theFreeState);
0160 }
0161
0162 // update in place and in the   very same place
0163 void BasicTrajectoryState::update(const LocalTrajectoryParameters& p, const SurfaceSide side) {
0164   theLocalParameters = p;
0165   theSurfaceSide = side;
0166   theLocalError = InvalidError();
0167   theFreeState = makeFTS(p, surface(), magneticField(), theFreeState.parameters().magneticFieldInTesla());
0168
0169   theValid = true;
0170   theLocalParametersValid = true;
0171 }
0172
0173 void BasicTrajectoryState::update(const LocalTrajectoryParameters& p,
0174                                   const SurfaceType& aSurface,
0175                                   const MagneticField* field,
0176                                   const SurfaceSide side) {
0177   theLocalParameters = p;
0178   if (&aSurface != &*theSurfaceP)
0179     theSurfaceP.reset(&aSurface);
0180   theSurfaceSide = side;
0181   theWeight = 1.0;
0182   theLocalError = InvalidError();
0183   theFreeState = makeFTS(p, aSurface, field);
0184
0185   theValid = true;
0186   theLocalParametersValid = true;
0187 }
0188
0189 void BasicTrajectoryState::update(double weight,
0190                                   const LocalTrajectoryParameters& p,
0191                                   const LocalTrajectoryError& err,
0192                                   const SurfaceType& aSurface,
0193                                   const MagneticField* field,
0194                                   const SurfaceSide side) {
0195   theLocalParameters = p;
0196   theLocalError = err;
0197   if (&aSurface != &*theSurfaceP)
0198     theSurfaceP.reset(&aSurface);
0199   theSurfaceSide = side;
0200   theWeight = weight;
0201   theFreeState = makeFTS(p, aSurface, field);
0202
0203   theValid = true;
0204   theLocalParametersValid = true;
0205 }
0206
0207 void BasicTrajectoryState::update(const LocalTrajectoryParameters& p,
0208                                   const LocalTrajectoryError& err,
0209                                   const SurfaceSide side) {
0210   theLocalParameters = p;
0211   theLocalError = err;
0212   theSurfaceSide = side;
0213   theFreeState = theFreeState =
0214       makeFTS(p, surface(), magneticField(), theFreeState.parameters().magneticFieldInTesla());
0215
0216   theValid = true;
0217   theLocalParametersValid = true;
0218 }
0219
0220 void BasicTrajectoryState::rescaleError(double factor) {
0221   if UNLIKELY (!hasError())
0222     missingError(" trying to rescale");
0223   theFreeState.rescaleError(factor);
0224
0225   if (theLocalError.valid()) {
0226     //do it by hand if the free state is not around.
0227     bool zeroField = (magneticField()->nominalValue() == 0);
0228     if UNLIKELY (zeroField) {
0229       AlgebraicSymMatrix55 errors = theLocalError.matrix();
0230       //scale the 0 indexed covariance by the square root of the factor
0231       for (unsigned int i = 1; i != 5; ++i)
0232         errors(i, 0) *= factor;
0233       double factor_squared = factor * factor;
0234       //scale all others by the scaled factor
0235       for (unsigned int i = 1; i != 5; ++i)
0236         for (unsigned int j = i; j != 5; ++j)
0237           errors(i, j) *= factor_squared;
0238       //term 0,0 is not scaled at all
0239       theLocalError = LocalTrajectoryError(errors);
0240     } else
0241       theLocalError *= (factor * factor);
0242   }
0243 }
0244
0245 #include "TrackingTools/TrajectoryState/interface/TrajectoryStateOnSurface.h"
0246 BasicSingleTrajectoryState::Components const& BasicSingleTrajectoryState::components() const {
0247   edm::LogError("BasicSingleTrajectoryState") << "asking for componenets to a SingleTrajectoryState" << std::endl;
0248   assert(false);
0249 }