Back to home page

Project CMSSW displayed by LXR

 
 

    


File indexing completed on 2021-02-14 12:46:10

0001 //  Author     : Gero Flucke (based on code by Edmund Widl replacing ORCA's TkReferenceTrack)
0002 //  date       : 2006/09/17
0003 //  last update: $Date: 2012/12/25 16:42:04 $
0004 //  by         : $Author: innocent $
0005 
0006 #include <memory>
0007 #include <limits>
0008 #include <cmath>
0009 #include <cstdlib>
0010 
0011 #include "Alignment/ReferenceTrajectories/interface/ReferenceTrajectory.h"
0012 
0013 #include "DataFormats/GeometrySurface/interface/Surface.h"
0014 #include "DataFormats/GeometrySurface/interface/Plane.h"
0015 
0016 #include "FWCore/MessageLogger/interface/MessageLogger.h"
0017 
0018 #include "DataFormats/CLHEP/interface/AlgebraicObjects.h"
0019 #include "DataFormats/GeometrySurface/interface/LocalError.h"
0020 #include "DataFormats/GeometryVector/interface/LocalPoint.h"
0021 #include "Geometry/CommonDetUnit/interface/TrackerGeomDet.h"
0022 
0023 #include "DataFormats/TrajectoryState/interface/LocalTrajectoryParameters.h"
0024 #include "DataFormats/TrackingRecHit/interface/KfComponentsHolder.h"
0025 
0026 #include "TrackingTools/AnalyticalJacobians/interface/AnalyticalCurvilinearJacobian.h"
0027 #include "TrackingTools/AnalyticalJacobians/interface/JacobianLocalToCurvilinear.h"
0028 #include "TrackingTools/AnalyticalJacobians/interface/JacobianCurvilinearToLocal.h"
0029 
0030 #include "TrackingTools/GeomPropagators/interface/AnalyticalPropagator.h"
0031 #include "TrackPropagation/RungeKutta/interface/defaultRKPropagator.h"
0032 
0033 #include "TrackingTools/TrajectoryState/interface/TrajectoryStateOnSurface.h"
0034 #include "TrackingTools/TrajectoryParametrization/interface/GlobalTrajectoryParameters.h"
0035 
0036 #include "TrackingTools/MaterialEffects/interface/MultipleScatteringUpdator.h"
0037 #include "TrackingTools/MaterialEffects/interface/EnergyLossUpdator.h"
0038 #include "TrackingTools/MaterialEffects/interface/CombinedMaterialEffectsUpdator.h"
0039 #include <TrackingTools/PatternTools/interface/TSCPBuilderNoMaterial.h>
0040 #include <TrackingTools/PatternTools/interface/TSCBLBuilderNoMaterial.h>
0041 #include "TrackingTools/TrajectoryState/interface/TrajectoryStateClosestToPoint.h"
0042 #include "TrackingTools/TrajectoryState/interface/TrajectoryStateClosestToBeamLine.h"
0043 
0044 #include "MagneticField/Engine/interface/MagneticField.h"
0045 
0046 #include "Alignment/ReferenceTrajectories/interface/BeamSpotTransientTrackingRecHit.h"
0047 #include "Alignment/ReferenceTrajectories/interface/BeamSpotGeomDet.h"
0048 
0049 //__________________________________________________________________________________
0050 using namespace gbl;
0051 
0052 ReferenceTrajectory::ReferenceTrajectory(const TrajectoryStateOnSurface &refTsos,
0053                                          const TransientTrackingRecHit::ConstRecHitContainer &recHits,
0054                                          const MagneticField *magField,
0055                                          const reco::BeamSpot &beamSpot,
0056                                          const ReferenceTrajectoryBase::Config &config)
0057     : ReferenceTrajectoryBase(
0058           (config.materialEffects >= brokenLinesCoarse) ? 1 : refTsos.localParameters().mixedFormatVector().kSize,
0059           (config.useBeamSpot) ? recHits.size() + 1 : recHits.size(),
0060           (config.materialEffects >= brokenLinesCoarse)
0061               ? 2 * ((config.useBeamSpot) ? recHits.size() + 1 : recHits.size())
0062               : ((config.materialEffects == breakPoints)
0063                      ? 2 * ((config.useBeamSpot) ? recHits.size() + 1 : recHits.size()) - 2
0064                      : 0),
0065           (config.materialEffects >= brokenLinesCoarse)
0066               ? 2 * ((config.useBeamSpot) ? recHits.size() + 1 : recHits.size()) - 4
0067               : ((config.materialEffects == breakPoints)
0068                      ? 2 * ((config.useBeamSpot) ? recHits.size() + 1 : recHits.size()) - 2
0069                      : 0)),
0070       mass_(config.mass),
0071       materialEffects_(config.materialEffects),
0072       propDir_(config.propDir),
0073       useBeamSpot_(config.useBeamSpot),
0074       includeAPEs_(config.includeAPEs),
0075       allowZeroMaterial_(config.allowZeroMaterial) {
0076   // no check against magField == 0
0077   theParameters = asHepVector<5>(refTsos.localParameters().mixedFormatVector());
0078 
0079   if (config.hitsAreReverse) {
0080     TransientTrackingRecHit::ConstRecHitContainer fwdRecHits;
0081     fwdRecHits.reserve(recHits.size());
0082     for (TransientTrackingRecHit::ConstRecHitContainer::const_reverse_iterator it = recHits.rbegin();
0083          it != recHits.rend();
0084          ++it) {
0085       fwdRecHits.push_back(*it);
0086     }
0087     theValidityFlag = this->construct(refTsos, fwdRecHits, magField, beamSpot);
0088   } else {
0089     theValidityFlag = this->construct(refTsos, recHits, magField, beamSpot);
0090   }
0091 }
0092 
0093 //__________________________________________________________________________________
0094 
0095 ReferenceTrajectory::ReferenceTrajectory(unsigned int nPar,
0096                                          unsigned int nHits,
0097                                          const ReferenceTrajectoryBase::Config &config)
0098     : ReferenceTrajectoryBase((config.materialEffects >= brokenLinesCoarse) ? 1 : nPar,
0099                               nHits,
0100                               (config.materialEffects >= brokenLinesCoarse)
0101                                   ? 2 * nHits
0102                                   : ((config.materialEffects == breakPoints) ? 2 * nHits - 2 : 0),
0103                               (config.materialEffects >= brokenLinesCoarse)
0104                                   ? 2 * nHits - 4
0105                                   : ((config.materialEffects == breakPoints) ? 2 * nHits - 2 : 0)),
0106       mass_(config.mass),
0107       materialEffects_(config.materialEffects),
0108       propDir_(config.propDir),
0109       useBeamSpot_(config.useBeamSpot),
0110       includeAPEs_(config.includeAPEs),
0111       allowZeroMaterial_(config.allowZeroMaterial) {}
0112 
0113 //__________________________________________________________________________________
0114 
0115 bool ReferenceTrajectory::construct(const TrajectoryStateOnSurface &refTsos,
0116                                     const TransientTrackingRecHit::ConstRecHitContainer &recHits,
0117                                     const MagneticField *magField,
0118                                     const reco::BeamSpot &beamSpot) {
0119   TrajectoryStateOnSurface theRefTsos = refTsos;
0120 
0121   const SurfaceSide surfaceSide = this->surfaceSide(propDir_);
0122   // auto_ptr to avoid memory leaks in case of not reaching delete at end of method:
0123   std::unique_ptr<MaterialEffectsUpdator> aMaterialEffectsUpdator(this->createUpdator(materialEffects_, mass_));
0124   if (!aMaterialEffectsUpdator.get())
0125     return false;  // empty auto_ptr
0126 
0127   AlgebraicMatrix fullJacobian(theParameters.num_row(), theParameters.num_row());
0128   std::vector<AlgebraicMatrix> allJacobians;
0129   allJacobians.reserve(theNumberOfHits);
0130 
0131   TransientTrackingRecHit::ConstRecHitPointer previousHitPtr;
0132   TrajectoryStateOnSurface previousTsos;
0133   AlgebraicSymMatrix previousChangeInCurvature(theParameters.num_row(), 1);
0134   std::vector<AlgebraicSymMatrix> allCurvatureChanges;
0135   allCurvatureChanges.reserve(theNumberOfHits);
0136 
0137   const LocalTrajectoryError zeroErrors(0., 0., 0., 0., 0.);
0138 
0139   std::vector<AlgebraicMatrix> allProjections;
0140   allProjections.reserve(theNumberOfHits);
0141   std::vector<AlgebraicSymMatrix> allDeltaParameterCovs;
0142   allDeltaParameterCovs.reserve(theNumberOfHits);
0143 
0144   // CHK
0145   std::vector<AlgebraicMatrix> allLocalToCurv;
0146   allLocalToCurv.reserve(theNumberOfHits);
0147   std::vector<double> allSteps;
0148   allSteps.reserve(theNumberOfHits);
0149   std::vector<AlgebraicMatrix> allCurvlinJacobians;
0150   allCurvlinJacobians.reserve(theNumberOfHits);
0151 
0152   AlgebraicMatrix firstCurvlinJacobian(5, 5, 1);
0153 
0154   unsigned int iRow = 0;
0155 
0156   theNomField = magField->nominalValue();  // nominal magnetic field in kGauss
0157   // local storage vector of all rechits (including rechit for beam spot in case it is used)
0158   TransientTrackingRecHit::ConstRecHitContainer allRecHits;
0159 
0160   if (useBeamSpot_ && propDir_ == alongMomentum) {
0161     GlobalPoint bs(beamSpot.x0(), beamSpot.y0(), beamSpot.z0());
0162 
0163     TrajectoryStateClosestToBeamLine tsctbl(TSCBLBuilderNoMaterial()(*(refTsos.freeState()), beamSpot));
0164     if (!tsctbl.isValid()) {
0165       edm::LogError("Alignment") << "@SUB=ReferenceTrajectory::construct"
0166                                  << "TrajectoryStateClostestToBeamLine invalid. Skip track.";
0167       return false;
0168     }
0169 
0170     FreeTrajectoryState pcaFts = tsctbl.trackStateAtPCA();
0171     GlobalVector bd(beamSpot.dxdz(), beamSpot.dydz(), 1.0);
0172 
0173     //propagation FIXME: Should use same propagator everywhere...
0174     AnalyticalPropagator propagator(magField);
0175     std::pair<TrajectoryStateOnSurface, double> tsosWithPath = propagator.propagateWithPath(pcaFts, refTsos.surface());
0176 
0177     if (!tsosWithPath.first.isValid())
0178       return false;
0179 
0180     GlobalVector momDir(pcaFts.momentum());
0181     GlobalVector perpDir(bd.cross(momDir));
0182     Plane::RotationType rotation(perpDir, bd);
0183 
0184     BeamSpotGeomDet *bsGeom = new BeamSpotGeomDet(Plane::build(bs, rotation));
0185 
0186     // There is also a constructor taking the magentic field. Use this one instead?
0187     theRefTsos = TrajectoryStateOnSurface(pcaFts, bsGeom->surface());
0188 
0189     TransientTrackingRecHit::ConstRecHitPointer bsRecHit(
0190         new BeamSpotTransientTrackingRecHit(beamSpot, bsGeom, theRefTsos.freeState()->momentum().phi()));
0191     allRecHits.push_back(bsRecHit);
0192   }
0193 
0194   // copy all rechits to the local storage vector
0195   TransientTrackingRecHit::ConstRecHitContainer::const_iterator itRecHit;
0196   for (itRecHit = recHits.begin(); itRecHit != recHits.end(); ++itRecHit) {
0197     const TransientTrackingRecHit::ConstRecHitPointer &hitPtr = *itRecHit;
0198     allRecHits.push_back(hitPtr);
0199   }
0200 
0201   for (itRecHit = allRecHits.begin(); itRecHit != allRecHits.end(); ++itRecHit) {
0202     const TransientTrackingRecHit::ConstRecHitPointer &hitPtr = *itRecHit;
0203     theRecHits.push_back(hitPtr);
0204 
0205     if (0 == iRow) {
0206       // compute the derivatives of the reference-track's parameters w.r.t. the initial ones
0207       // derivative of the initial reference-track parameters w.r.t. themselves is of course the identity
0208       fullJacobian = AlgebraicMatrix(theParameters.num_row(), theParameters.num_row(), 1);
0209       allJacobians.push_back(fullJacobian);
0210       theTsosVec.push_back(theRefTsos);
0211       const JacobianLocalToCurvilinear startTrafo(hitPtr->det()->surface(), theRefTsos.localParameters(), *magField);
0212       const AlgebraicMatrix localToCurvilinear = asHepMatrix<5>(startTrafo.jacobian());
0213       if (materialEffects_ <= breakPoints) {
0214         theInnerTrajectoryToCurvilinear = asHepMatrix<5>(startTrafo.jacobian());
0215         theInnerLocalToTrajectory = AlgebraicMatrix(5, 5, 1);
0216       }
0217       allLocalToCurv.push_back(localToCurvilinear);
0218       allSteps.push_back(0.);
0219       allCurvlinJacobians.push_back(firstCurvlinJacobian);
0220 
0221     } else {
0222       AlgebraicMatrix nextJacobian;
0223       AlgebraicMatrix nextCurvlinJacobian;
0224       double nextStep = 0.;
0225       TrajectoryStateOnSurface nextTsos;
0226 
0227       if (!this->propagate(previousHitPtr->det()->surface(),
0228                            previousTsos,
0229                            hitPtr->det()->surface(),
0230                            nextTsos,
0231                            nextJacobian,
0232                            nextCurvlinJacobian,
0233                            nextStep,
0234                            magField)) {
0235         return false;  // stop if problem...// no delete aMaterialEffectsUpdator needed
0236       }
0237 
0238       allJacobians.push_back(nextJacobian);
0239       fullJacobian = nextJacobian * previousChangeInCurvature * fullJacobian;
0240       theTsosVec.push_back(nextTsos);
0241 
0242       const JacobianLocalToCurvilinear startTrafo(hitPtr->det()->surface(), nextTsos.localParameters(), *magField);
0243       const AlgebraicMatrix localToCurvilinear = asHepMatrix<5>(startTrafo.jacobian());
0244       allLocalToCurv.push_back(localToCurvilinear);
0245       if (nextStep == 0.) {
0246         edm::LogError("Alignment") << "@SUB=ReferenceTrajectory::construct"
0247                                    << "step 0. from id " << previousHitPtr->geographicalId() << " to "
0248                                    << hitPtr->det()->geographicalId() << ".";
0249         // brokenLinesFine will not work, brokenLinesCoarse combines close by layers
0250         if (materialEffects_ == brokenLinesFine) {
0251           edm::LogError("Alignment") << "@SUB=ReferenceTrajectory::construct"
0252                                      << "Skip track.";
0253           return false;
0254         }
0255       }
0256       allSteps.push_back(nextStep);
0257       allCurvlinJacobians.push_back(nextCurvlinJacobian);
0258     }
0259 
0260     // take material effects into account. since trajectory-state is constructed with errors equal zero,
0261     // the updated state contains only the uncertainties due to interactions in the current layer.
0262     const TrajectoryStateOnSurface tmpTsos(
0263         theTsosVec.back().localParameters(), zeroErrors, theTsosVec.back().surface(), magField, surfaceSide);
0264     const TrajectoryStateOnSurface updatedTsos = aMaterialEffectsUpdator->updateState(tmpTsos, propDir_);
0265 
0266     if (!updatedTsos.isValid())
0267       return false;  // no delete aMaterialEffectsUpdator needed
0268 
0269     if (theTsosVec.back().localParameters().charge()) {
0270       previousChangeInCurvature[0][0] = updatedTsos.localParameters().signedInverseMomentum() /
0271                                         theTsosVec.back().localParameters().signedInverseMomentum();
0272     }
0273 
0274     // get multiple-scattering covariance-matrix
0275     allDeltaParameterCovs.push_back(asHepMatrix<5>(updatedTsos.localError().matrix()));
0276     allCurvatureChanges.push_back(previousChangeInCurvature);
0277 
0278     // projection-matrix tsos-parameters -> measurement-coordinates
0279     allProjections.push_back(this->getHitProjectionMatrix(hitPtr));
0280     // set start-parameters for next propagation. trajectory-state without error
0281     //  - no error propagation needed here.
0282     previousHitPtr = hitPtr;
0283     previousTsos = TrajectoryStateOnSurface(updatedTsos.globalParameters(), updatedTsos.surface(), surfaceSide);
0284 
0285     if (materialEffects_ < brokenLinesCoarse) {
0286       this->fillDerivatives(allProjections.back(), fullJacobian, iRow);
0287     }
0288 
0289     AlgebraicVector mixedLocalParams = asHepVector<5>(theTsosVec.back().localParameters().mixedFormatVector());
0290     this->fillTrajectoryPositions(allProjections.back(), mixedLocalParams, iRow);
0291     if (useRecHit(hitPtr))
0292       this->fillMeasurementAndError(hitPtr, iRow, updatedTsos);
0293 
0294     iRow += nMeasPerHit;
0295   }  // end of loop on hits
0296 
0297   bool msOK = true;
0298   switch (materialEffects_) {
0299     case none:
0300       break;
0301     case multipleScattering:
0302     case energyLoss:
0303     case combined:
0304       msOK = this->addMaterialEffectsCov(allJacobians, allProjections, allCurvatureChanges, allDeltaParameterCovs);
0305       break;
0306     case breakPoints:
0307       msOK = this->addMaterialEffectsBp(
0308           allJacobians, allProjections, allCurvatureChanges, allDeltaParameterCovs, allLocalToCurv);
0309       break;
0310     case brokenLinesCoarse:
0311       msOK = this->addMaterialEffectsBrl(
0312           allProjections, allDeltaParameterCovs, allLocalToCurv, allSteps, refTsos.globalParameters());
0313       break;
0314     case brokenLinesFine:
0315       msOK = this->addMaterialEffectsBrl(allCurvlinJacobians,
0316                                          allProjections,
0317                                          allCurvatureChanges,
0318                                          allDeltaParameterCovs,
0319                                          allLocalToCurv,
0320                                          refTsos.globalParameters());
0321       break;
0322     case localGBL:
0323       msOK = this->addMaterialEffectsLocalGbl(allJacobians, allProjections, allCurvatureChanges, allDeltaParameterCovs);
0324       break;
0325     case curvlinGBL:
0326       msOK = this->addMaterialEffectsCurvlinGbl(
0327           allCurvlinJacobians, allProjections, allCurvatureChanges, allDeltaParameterCovs, allLocalToCurv);
0328   }
0329   if (!msOK)
0330     return false;
0331 
0332   if (refTsos.hasError()) {
0333     AlgebraicSymMatrix parameterCov = asHepMatrix<5>(refTsos.localError().matrix());
0334     AlgebraicMatrix parDeriv;
0335     if (theNumberOfVirtualPars > 0) {
0336       parDeriv = theDerivatives.sub(1, nMeasPerHit * allJacobians.size(), 1, theParameters.num_row());
0337     } else {
0338       parDeriv = theDerivatives;
0339     }
0340     theTrajectoryPositionCov = parameterCov.similarity(parDeriv);
0341   } else {
0342     theTrajectoryPositionCov = AlgebraicSymMatrix(theDerivatives.num_row(), 1);
0343   }
0344 
0345   return true;
0346 }
0347 
0348 //__________________________________________________________________________________
0349 
0350 MaterialEffectsUpdator *ReferenceTrajectory::createUpdator(MaterialEffects materialEffects, double mass) const {
0351   switch (materialEffects) {
0352       // MultipleScatteringUpdator doesn't change the trajectory-state
0353       // during update and can therefore be used if material effects should be ignored:
0354     case none:
0355     case multipleScattering:
0356       return new MultipleScatteringUpdator(mass);
0357     case energyLoss:
0358       return new EnergyLossUpdator(mass);
0359     case combined:
0360       return new CombinedMaterialEffectsUpdator(mass);
0361     case breakPoints:
0362       return new CombinedMaterialEffectsUpdator(mass);
0363     case brokenLinesCoarse:
0364     case brokenLinesFine:
0365     case localGBL:
0366     case curvlinGBL:
0367       return new CombinedMaterialEffectsUpdator(mass);
0368   }
0369 
0370   return nullptr;
0371 }
0372 
0373 //__________________________________________________________________________________
0374 
0375 bool ReferenceTrajectory::propagate(const Plane &previousSurface,
0376                                     const TrajectoryStateOnSurface &previousTsos,
0377                                     const Plane &newSurface,
0378                                     TrajectoryStateOnSurface &newTsos,
0379                                     AlgebraicMatrix &newJacobian,
0380                                     AlgebraicMatrix &newCurvlinJacobian,
0381                                     double &nextStep,
0382                                     const MagneticField *magField) const {
0383   // propagate to next layer
0384   /** From TrackingTools/ GeomPropagators/ interface/ AnalyticalPropagator.h
0385    * NB: this propagator assumes constant, non-zero magnetic field parallel to the z-axis!
0386    */
0387   //AnalyticalPropagator aPropagator(magField, propDir_);
0388   // Hard coded RungeKutta instead Analytical (avoid bias in TEC), but
0389   // work around TrackPropagation/RungeKutta/interface/RKTestPropagator.h and
0390   // http://www.parashift.com/c++-faq-lite/strange-inheritance.html#faq-23.9
0391   defaultRKPropagator::Product rkprod(magField, propDir_);  //double tolerance = 5.e-5)
0392   Propagator &aPropagator = rkprod.propagator;
0393   const std::pair<TrajectoryStateOnSurface, double> tsosWithPath =
0394       aPropagator.propagateWithPath(previousTsos, newSurface);
0395 
0396   // stop if propagation wasn't successful
0397   if (!tsosWithPath.first.isValid())
0398     return false;
0399 
0400   nextStep = tsosWithPath.second;
0401   // calculate derivative of reference-track parameters on the actual layer w.r.t. the ones
0402   // on the previous layer (both in global coordinates)
0403   const AnalyticalCurvilinearJacobian aJacobian(previousTsos.globalParameters(),
0404                                                 tsosWithPath.first.globalPosition(),
0405                                                 tsosWithPath.first.globalMomentum(),
0406                                                 tsosWithPath.second);
0407   const AlgebraicMatrix curvilinearJacobian = asHepMatrix<5, 5>(aJacobian.jacobian());
0408 
0409   // jacobian of the track parameters on the previous layer for local->global transformation
0410   const JacobianLocalToCurvilinear startTrafo(previousSurface, previousTsos.localParameters(), *magField);
0411   const AlgebraicMatrix localToCurvilinear = asHepMatrix<5>(startTrafo.jacobian());
0412 
0413   // jacobian of the track parameters on the actual layer for global->local transformation
0414   const JacobianCurvilinearToLocal endTrafo(newSurface, tsosWithPath.first.localParameters(), *magField);
0415   const AlgebraicMatrix curvilinearToLocal = asHepMatrix<5>(endTrafo.jacobian());
0416 
0417   // compute derivative of reference-track parameters on the actual layer w.r.t. the ones on
0418   // the previous layer (both in their local representation)
0419   newCurvlinJacobian = curvilinearJacobian;
0420   newJacobian = curvilinearToLocal * curvilinearJacobian * localToCurvilinear;
0421   newTsos = tsosWithPath.first;
0422 
0423   return true;
0424 }
0425 
0426 //__________________________________________________________________________________
0427 
0428 void ReferenceTrajectory::fillMeasurementAndError(const TransientTrackingRecHit::ConstRecHitPointer &hitPtr,
0429                                                   unsigned int iRow,
0430                                                   const TrajectoryStateOnSurface &updatedTsos) {
0431   // get the measurements and their errors, use information updated with tsos if improving
0432   // (GF: Also for measurements or only for errors or do the former not change?)
0433   // GF 10/2008: I doubt that it makes sense to update the hit with the tsos here:
0434   //             That is an analytical extrapolation and not the best guess of the real
0435   //             track state on the module, but the latter should be better to get the best
0436   //             hit uncertainty estimate!
0437 
0438   // FIXME FIXME  CLONE
0439   const auto &newHitPtr = hitPtr;
0440   //  TransientTrackingRecHit::ConstRecHitPointer newHitPtr(hitPtr->canImproveWithTrack() ?
0441   //                            hitPtr->clone(updatedTsos) : hitPtr);
0442 
0443   const LocalPoint localMeasurement = newHitPtr->localPosition();
0444   const LocalError localMeasurementCov = newHitPtr->localPositionError();  // CPE+APE
0445 
0446   theMeasurements[iRow] = localMeasurement.x();
0447   theMeasurements[iRow + 1] = localMeasurement.y();
0448   theMeasurementsCov[iRow][iRow] = localMeasurementCov.xx();
0449   theMeasurementsCov[iRow][iRow + 1] = localMeasurementCov.xy();
0450   theMeasurementsCov[iRow + 1][iRow + 1] = localMeasurementCov.yy();
0451 
0452   if (!includeAPEs_) {
0453     // subtract APEs (if existing) from covariance matrix
0454     auto det = static_cast<const TrackerGeomDet *>(newHitPtr->det());
0455     const auto localAPE = det->localAlignmentError();
0456     if (localAPE.valid()) {
0457       theMeasurementsCov[iRow][iRow] -= localAPE.xx();
0458       theMeasurementsCov[iRow][iRow + 1] -= localAPE.xy();
0459       theMeasurementsCov[iRow + 1][iRow + 1] -= localAPE.yy();
0460     }
0461   }
0462 }
0463 
0464 //__________________________________________________________________________________
0465 
0466 void ReferenceTrajectory::fillDerivatives(const AlgebraicMatrix &projection,
0467                                           const AlgebraicMatrix &fullJacobian,
0468                                           unsigned int iRow) {
0469   // derivatives of the local coordinates of the reference track w.r.t. to the inital track-parameters
0470   const AlgebraicMatrix projectedJacobian(projection * fullJacobian);
0471   for (int i = 0; i < parameters().num_row(); ++i) {
0472     for (int j = 0; j < projectedJacobian.num_row(); ++j) {
0473       theDerivatives[iRow + j][i] = projectedJacobian[j][i];
0474     }
0475   }
0476 }
0477 
0478 //__________________________________________________________________________________
0479 
0480 void ReferenceTrajectory::fillTrajectoryPositions(const AlgebraicMatrix &projection,
0481                                                   const AlgebraicVector &mixedLocalParams,
0482                                                   unsigned int iRow) {
0483   // get the local coordinates of the reference trajectory
0484   const AlgebraicVector localPosition(projection * mixedLocalParams);
0485   for (int i = 0; i < localPosition.num_row(); ++i) {
0486     theTrajectoryPositions[iRow + i] = localPosition[i];
0487   }
0488 }
0489 
0490 //__________________________________________________________________________________
0491 
0492 bool ReferenceTrajectory::addMaterialEffectsCov(const std::vector<AlgebraicMatrix> &allJacobians,
0493                                                 const std::vector<AlgebraicMatrix> &allProjections,
0494                                                 const std::vector<AlgebraicSymMatrix> &allCurvatureChanges,
0495                                                 const std::vector<AlgebraicSymMatrix> &allDeltaParameterCovs) {
0496   // the uncertainty due to multiple scattering is 'transferred' to the error matrix of the hits
0497 
0498   // GF: Needs update once hit dimension is not hardcoded as nMeasPerHit!
0499 
0500   AlgebraicSymMatrix materialEffectsCov(nMeasPerHit * allJacobians.size(), 0);
0501 
0502   // additional covariance-matrix of the measurements due to material-effects (single measurement)
0503   AlgebraicSymMatrix deltaMaterialEffectsCov;
0504 
0505   // additional covariance-matrix of the parameters due to material-effects
0506   AlgebraicSymMatrix paramMaterialEffectsCov(allDeltaParameterCovs[0]);  //initialization
0507   // error-propagation to state after energy loss
0508   //GFback  paramMaterialEffectsCov = paramMaterialEffectsCov.similarity(allCurvatureChanges[0]);
0509 
0510   AlgebraicMatrix tempParameterCov;
0511   AlgebraicMatrix tempMeasurementCov;
0512 
0513   for (unsigned int k = 1; k < allJacobians.size(); ++k) {
0514     // error-propagation to next layer
0515     paramMaterialEffectsCov = paramMaterialEffectsCov.similarity(allJacobians[k]);
0516 
0517     // get dependences for the measurements
0518     deltaMaterialEffectsCov = paramMaterialEffectsCov.similarity(allProjections[k]);
0519     materialEffectsCov[nMeasPerHit * k][nMeasPerHit * k] = deltaMaterialEffectsCov[0][0];
0520     materialEffectsCov[nMeasPerHit * k][nMeasPerHit * k + 1] = deltaMaterialEffectsCov[0][1];
0521     materialEffectsCov[nMeasPerHit * k + 1][nMeasPerHit * k] = deltaMaterialEffectsCov[1][0];
0522     materialEffectsCov[nMeasPerHit * k + 1][nMeasPerHit * k + 1] = deltaMaterialEffectsCov[1][1];
0523 
0524     // GFback add uncertainties for the following layers due to scattering at this layer
0525     paramMaterialEffectsCov += allDeltaParameterCovs[k];
0526     // end GFback
0527     tempParameterCov = paramMaterialEffectsCov;
0528 
0529     // compute "inter-layer-dependencies"
0530     for (unsigned int l = k + 1; l < allJacobians.size(); ++l) {
0531       tempParameterCov = allJacobians[l] * allCurvatureChanges[l] * tempParameterCov;
0532       tempMeasurementCov = allProjections[l] * tempParameterCov * allProjections[k].T();
0533 
0534       materialEffectsCov[nMeasPerHit * l][nMeasPerHit * k] = tempMeasurementCov[0][0];
0535       materialEffectsCov[nMeasPerHit * k][nMeasPerHit * l] = tempMeasurementCov[0][0];
0536 
0537       materialEffectsCov[nMeasPerHit * l][nMeasPerHit * k + 1] = tempMeasurementCov[0][1];
0538       materialEffectsCov[nMeasPerHit * k + 1][nMeasPerHit * l] = tempMeasurementCov[0][1];
0539 
0540       materialEffectsCov[nMeasPerHit * l + 1][nMeasPerHit * k] = tempMeasurementCov[1][0];
0541       materialEffectsCov[nMeasPerHit * k][nMeasPerHit * l + 1] = tempMeasurementCov[1][0];
0542 
0543       materialEffectsCov[nMeasPerHit * l + 1][nMeasPerHit * k + 1] = tempMeasurementCov[1][1];
0544       materialEffectsCov[nMeasPerHit * k + 1][nMeasPerHit * l + 1] = tempMeasurementCov[1][1];
0545     }
0546     // add uncertainties for the following layers due to scattering at this layer
0547     // GFback paramMaterialEffectsCov += allDeltaParameterCovs[k];
0548     // error-propagation to state after energy loss
0549     paramMaterialEffectsCov = paramMaterialEffectsCov.similarity(allCurvatureChanges[k]);
0550   }
0551   theMeasurementsCov += materialEffectsCov;
0552 
0553   return true;  // cannot fail
0554 }
0555 
0556 //__________________________________________________________________________________
0557 
0558 bool ReferenceTrajectory::addMaterialEffectsBp(const std::vector<AlgebraicMatrix> &allJacobians,
0559                                                const std::vector<AlgebraicMatrix> &allProjections,
0560                                                const std::vector<AlgebraicSymMatrix> &allCurvatureChanges,
0561                                                const std::vector<AlgebraicSymMatrix> &allDeltaParameterCovs,
0562                                                const std::vector<AlgebraicMatrix> &allLocalToCurv) {
0563   //CHK: add material effects using break points
0564   int offsetPar = theNumberOfPars;
0565   int offsetMeas = nMeasPerHit * allJacobians.size();
0566   int ierr = 0;
0567 
0568   AlgebraicMatrix tempJacobian;
0569   AlgebraicMatrix MSprojection(2, 5);
0570   MSprojection[0][1] = 1;
0571   MSprojection[1][2] = 1;
0572   AlgebraicSymMatrix tempMSCov;
0573   AlgebraicSymMatrix tempMSCovProj;
0574   AlgebraicMatrix tempMSJacProj;
0575 
0576   for (unsigned int k = 1; k < allJacobians.size(); ++k) {
0577     // CHK
0578     int kbp = k - 1;
0579     tempJacobian = allJacobians[k] * allCurvatureChanges[k];
0580     tempMSCov = allDeltaParameterCovs[k - 1].similarity(allLocalToCurv[k - 1]);
0581     tempMSCovProj = tempMSCov.similarity(MSprojection);
0582     theMeasurementsCov[offsetMeas + nMeasPerHit * kbp][offsetMeas + nMeasPerHit * kbp] = tempMSCovProj[0][0];
0583     theMeasurementsCov[offsetMeas + nMeasPerHit * kbp + 1][offsetMeas + nMeasPerHit * kbp + 1] = tempMSCovProj[1][1];
0584     theDerivatives[offsetMeas + nMeasPerHit * kbp][offsetPar + 2 * kbp] = 1.0;
0585     theDerivatives[offsetMeas + nMeasPerHit * kbp + 1][offsetPar + 2 * kbp + 1] = 1.0;
0586 
0587     tempMSJacProj = (allProjections[k] * (tempJacobian * allLocalToCurv[k - 1].inverse(ierr))) * MSprojection.T();
0588     if (ierr) {
0589       edm::LogError("Alignment") << "@SUB=ReferenceTrajectory::addMaterialEffectsBp"
0590                                  << "Inversion 1 for break points failed: " << ierr;
0591       return false;
0592     }
0593     theDerivatives[nMeasPerHit * k][offsetPar + 2 * kbp] = tempMSJacProj[0][0];
0594     theDerivatives[nMeasPerHit * k][offsetPar + 2 * kbp + 1] = tempMSJacProj[0][1];
0595     theDerivatives[nMeasPerHit * k + 1][offsetPar + 2 * kbp] = tempMSJacProj[1][0];
0596     theDerivatives[nMeasPerHit * k + 1][offsetPar + 2 * kbp + 1] = tempMSJacProj[1][1];
0597 
0598     for (unsigned int l = k + 1; l < allJacobians.size(); ++l) {
0599       // CHK
0600       int kbp = k - 1;
0601       tempJacobian = allJacobians[l] * allCurvatureChanges[l] * tempJacobian;
0602       tempMSJacProj = (allProjections[l] * (tempJacobian * allLocalToCurv[k - 1].inverse(ierr))) * MSprojection.T();
0603       if (ierr) {
0604         edm::LogError("Alignment") << "@SUB=ReferenceTrajectory::addMaterialEffectsBp"
0605                                    << "Inversion 2 for break points failed: " << ierr;
0606         return false;
0607       }
0608       theDerivatives[nMeasPerHit * l][offsetPar + 2 * kbp] = tempMSJacProj[0][0];
0609       theDerivatives[nMeasPerHit * l][offsetPar + 2 * kbp + 1] = tempMSJacProj[0][1];
0610       theDerivatives[nMeasPerHit * l + 1][offsetPar + 2 * kbp] = tempMSJacProj[1][0];
0611       theDerivatives[nMeasPerHit * l + 1][offsetPar + 2 * kbp + 1] = tempMSJacProj[1][1];
0612     }
0613   }
0614 
0615   return true;
0616 }
0617 
0618 //__________________________________________________________________________________
0619 
0620 bool ReferenceTrajectory::addMaterialEffectsBrl(const std::vector<AlgebraicMatrix> &allCurvlinJacobians,
0621                                                 const std::vector<AlgebraicMatrix> &allProjections,
0622                                                 const std::vector<AlgebraicSymMatrix> &allCurvatureChanges,
0623                                                 const std::vector<AlgebraicSymMatrix> &allDeltaParameterCovs,
0624                                                 const std::vector<AlgebraicMatrix> &allLocalToCurv,
0625                                                 const GlobalTrajectoryParameters &gtp) {
0626   //CHK: add material effects using broken lines
0627   //fine: use exact Jacobians, all detectors
0628   //broken lines: pair of offsets (u1,u2) = (xt,yt) (in curvilinear frame (q/p,lambda,phi,xt,yt)) at each layer
0629   //              scattering angles (alpha1,alpha2) = (cosLambda*dPhi, dLambda) (cosLambda cancels in Chi2)
0630   //              DU' = (dU'/dU)*DU + (dU'/dAlpha)*DAlpha + (dU'/dQbyp)*DQbyp (propagation of U)
0631   //                  = J*DU + S*DAlpha + d*DQbyp
0632   //           => DAlpha = S^-1 (DU' - J*DU - d*DQbyp)
0633 
0634   int offsetPar = theNumberOfPars;
0635   int offsetMeas = nMeasPerHit * allCurvlinJacobians.size();
0636   int ierr = 0;
0637 
0638   GlobalVector p = gtp.momentum();
0639   double cosLambda = sqrt((p.x() * p.x() + p.y() * p.y()) / (p.x() * p.x() + p.y() * p.y() + p.z() * p.z()));
0640 
0641   // transformations Curvilinear <-> BrokenLines
0642   AlgebraicMatrix QbypToCurv(5, 1);    // dCurv/dQbyp
0643   QbypToCurv[0][0] = 1.;               // dQbyp/dQbyp
0644   AlgebraicMatrix AngleToCurv(5, 2);   // dCurv/dAlpha
0645   AngleToCurv[1][1] = 1.;              // dlambda/dalpha2
0646   AngleToCurv[2][0] = 1. / cosLambda;  // dphi/dalpha1
0647   AlgebraicMatrix CurvToAngle(2, 5);   // dAlpha/dCurv
0648   CurvToAngle[1][1] = 1.;              // dalpha2/dlambda
0649   CurvToAngle[0][2] = cosLambda;       // dalpha1/dphi
0650   AlgebraicMatrix OffsetToCurv(5, 2);  // dCurv/dU
0651   OffsetToCurv[3][0] = 1.;             // dxt/du1
0652   OffsetToCurv[4][1] = 1.;             // dyt/du2
0653   AlgebraicMatrix CurvToOffset(2, 5);  // dU/dCurv
0654   CurvToOffset[0][3] = 1.;             // du1/dxt
0655   CurvToOffset[1][4] = 1.;             // du2/dyt
0656 
0657   // transformations  trajectory to components (Qbyp, U1, U2)
0658   AlgebraicMatrix TrajToQbyp(1, 5);
0659   TrajToQbyp[0][0] = 1.;
0660   AlgebraicMatrix TrajToOff1(2, 5);
0661   TrajToOff1[0][1] = 1.;
0662   TrajToOff1[1][2] = 1.;
0663   AlgebraicMatrix TrajToOff2(2, 5);
0664   TrajToOff2[0][3] = 1.;
0665   TrajToOff2[1][4] = 1.;
0666 
0667   AlgebraicMatrix JacOffsetToAngleC, JacQbypToAngleC;
0668   AlgebraicMatrix JacCurvToOffsetL, JacOffsetToOffsetL, JacAngleToOffsetL, JacQbypToOffsetL, JacOffsetToAngleL;
0669   AlgebraicMatrix JacCurvToOffsetN, JacOffsetToOffsetN, JacAngleToOffsetN, JacQbypToOffsetN, JacOffsetToAngleN;
0670 
0671   // transformation from trajectory to curvilinear parameters
0672 
0673   JacCurvToOffsetN = CurvToOffset * allCurvlinJacobians[1];  // (dU'/dCurv') * (dCurv'/dCurv) @ 2nd point
0674   JacOffsetToOffsetN = JacCurvToOffsetN * OffsetToCurv;  // J: (dU'/dU)     = (dU'/dCurv') * (dCurv'/dCurv) * (dCurv/dU)
0675   JacAngleToOffsetN =
0676       JacCurvToOffsetN * AngleToCurv;                // S: (dU'/dAlpha) = (dU'/dCurv') * (dCurv'/dCurv) * (dCurv/dAlpha)
0677   JacQbypToOffsetN = JacCurvToOffsetN * QbypToCurv;  // d: (dU'/dQbyp)  = (dU'/dCurv') * (dCurv'/dCurv) * (dCurv/dQbyp)
0678   JacOffsetToAngleN = JacAngleToOffsetN.inverse(ierr);  // W
0679   if (ierr) {
0680     edm::LogError("Alignment") << "@SUB=ReferenceTrajectory::addMaterialEffectsBrl"
0681                                << "Inversion 1 for fine broken lines failed: " << ierr;
0682     return false;
0683   }
0684   JacOffsetToAngleC = -(JacOffsetToAngleN * JacOffsetToOffsetN);  // (dAlpha/dU)
0685   JacQbypToAngleC = -(JacOffsetToAngleN * JacQbypToOffsetN);      // (dAlpha/dQbyp)
0686   // (dAlpha/dTraj) = (dAlpha/dQbyp) * (dQbyp/dTraj) + (dAlpha/dU1) * (dU1/dTraj) + (dAlpha/dU2) * (dU2/dTraj)
0687   AlgebraicMatrix JacTrajToAngle =
0688       JacQbypToAngleC * TrajToQbyp + JacOffsetToAngleC * TrajToOff1 + JacOffsetToAngleN * TrajToOff2;
0689   // (dCurv/dTraj) = (dCurv/dQbyp) * (dQbyp/dTraj) + (dCurv/dAlpha) * (dAlpha/dTraj) + (dCurv/dU) * (dU/dTraj)
0690   theInnerTrajectoryToCurvilinear = QbypToCurv * TrajToQbyp + AngleToCurv * JacTrajToAngle + OffsetToCurv * TrajToOff1;
0691   theInnerLocalToTrajectory = theInnerTrajectoryToCurvilinear.inverse(ierr) * allLocalToCurv[0];
0692   if (ierr) {
0693     edm::LogError("Alignment") << "@SUB=ReferenceTrajectory::addMaterialEffectsBrl"
0694                                << "Inversion 2 for fine broken lines failed: " << ierr;
0695     return false;
0696   }
0697 
0698   AlgebraicMatrix tempJacobian(allCurvatureChanges[0]);
0699   AlgebraicSymMatrix tempMSCov;
0700   AlgebraicSymMatrix tempMSCovProj;
0701   AlgebraicMatrix tempJacL, tempJacN;
0702   AlgebraicMatrix JacOffsetToMeas;
0703 
0704   // measurements from hits
0705   for (unsigned int k = 0; k < allCurvlinJacobians.size(); ++k) {
0706     //  (dMeas/dU) = (dMeas/dLoc) * (dLoc/dCurv) * (dCurv/dU)
0707     JacOffsetToMeas = (allProjections[k] * allLocalToCurv[k].inverse(ierr)) * OffsetToCurv;
0708     if (ierr) {
0709       edm::LogError("Alignment") << "@SUB=ReferenceTrajectory::addMaterialEffectsBrl"
0710                                  << "Inversion 3 for fine broken lines failed: " << ierr;
0711       return false;
0712     }
0713     theDerivatives[nMeasPerHit * k][offsetPar + 2 * k] = JacOffsetToMeas[0][0];
0714     theDerivatives[nMeasPerHit * k][offsetPar + 2 * k + 1] = JacOffsetToMeas[0][1];
0715     theDerivatives[nMeasPerHit * k + 1][offsetPar + 2 * k] = JacOffsetToMeas[1][0];
0716     theDerivatives[nMeasPerHit * k + 1][offsetPar + 2 * k + 1] = JacOffsetToMeas[1][1];
0717   }
0718 
0719   // measurement of MS kink
0720   for (unsigned int k = 1; k < allCurvlinJacobians.size() - 1; ++k) {
0721     // CHK
0722     int iMsMeas = k - 1;
0723     int l = k - 1;  // last hit
0724     int n = k + 1;  // next hit
0725 
0726     // amount of multiple scattering in layer k  (angular error perp to direction)
0727     tempMSCov = allDeltaParameterCovs[k].similarity(allLocalToCurv[k]);
0728     tempMSCovProj = tempMSCov.similarity(CurvToAngle);
0729     theMeasurementsCov[offsetMeas + nMeasPerHit * iMsMeas][offsetMeas + nMeasPerHit * iMsMeas] = tempMSCovProj[1][1];
0730     theMeasurementsCov[offsetMeas + nMeasPerHit * iMsMeas + 1][offsetMeas + nMeasPerHit * iMsMeas + 1] =
0731         tempMSCovProj[0][0];
0732 
0733     // transformation matices for offsets ( l <- k -> n )
0734     tempJacL = allCurvlinJacobians[k] * tempJacobian;
0735     JacCurvToOffsetL = CurvToOffset * tempJacL.inverse(ierr);  // (dU'/dCurv') * (dCurv'/dCurv) @ last point
0736 
0737     if (ierr) {
0738       edm::LogError("Alignment") << "@SUB=ReferenceTrajectory::addMaterialEffectsBrl"
0739                                  << "Inversion 4 for fine broken lines failed: " << ierr;
0740       return false;
0741     }
0742     JacOffsetToOffsetL =
0743         JacCurvToOffsetL * OffsetToCurv;  // J-: (dU'/dU)     = (dU'/dCurv') * (dCurv'/dCurv) * (dCurv/dU)
0744     JacAngleToOffsetL =
0745         JacCurvToOffsetL * AngleToCurv;  // S-: (dU'/dAlpha) = (dU'/dCurv') * (dCurv'/dCurv) * (dCurv/dAlpha)
0746     JacQbypToOffsetL =
0747         JacCurvToOffsetL * QbypToCurv;  // d-: (dU'/dQbyp)  = (dU'/dCurv') * (dCurv'/dCurv) * (dCurv/dQbyp)
0748     JacOffsetToAngleL = -JacAngleToOffsetL.inverse(ierr);  // W-
0749     if (ierr) {
0750       edm::LogError("Alignment") << "@SUB=ReferenceTrajectory::addMaterialEffectsBrl"
0751                                  << "Inversion 5 for fine broken lines failed: " << ierr;
0752       return false;
0753     }
0754     tempJacobian = tempJacobian * allCurvatureChanges[k];
0755     tempJacN = allCurvlinJacobians[n] * tempJacobian;
0756     JacCurvToOffsetN = CurvToOffset * tempJacN;  // (dU'/dCurv') * (dCurv'/dCurv) @ next point
0757     JacOffsetToOffsetN =
0758         JacCurvToOffsetN * OffsetToCurv;  // J+: (dU'/dU)     = (dU'/dCurv') * (dCurv'/dCurv) * (dCurv/dU)
0759     JacAngleToOffsetN =
0760         JacCurvToOffsetN * AngleToCurv;  // S+: (dU'/dAlpha) = (dU'/dCurv') * (dCurv'/dCurv) * (dCurv/dAlpha)
0761     JacQbypToOffsetN =
0762         JacCurvToOffsetN * QbypToCurv;  // d+: (dU'/dQbyp)  = (dU'/dCurv') * (dCurv'/dCurv) * (dCurv/dQbyp)
0763     JacOffsetToAngleN = JacAngleToOffsetN.inverse(ierr);  // W+
0764     if (ierr) {
0765       edm::LogError("Alignment") << "@SUB=ReferenceTrajectory::addMaterialEffectsBrl"
0766                                  << "Inversion 6 for fine broken lines failed: " << ierr;
0767       return false;
0768     }
0769     JacOffsetToAngleC = -(JacOffsetToAngleL * JacOffsetToOffsetL + JacOffsetToAngleN * JacOffsetToOffsetN);
0770     JacQbypToAngleC = -(JacOffsetToAngleL * JacQbypToOffsetL + JacOffsetToAngleN * JacQbypToOffsetN);
0771 
0772     // bending
0773     theDerivatives[offsetMeas + nMeasPerHit * iMsMeas][0] = JacQbypToAngleC[0][0];
0774     theDerivatives[offsetMeas + nMeasPerHit * iMsMeas + 1][0] = JacQbypToAngleC[1][0];
0775     // last layer
0776     theDerivatives[offsetMeas + nMeasPerHit * iMsMeas][offsetPar + 2 * l] = JacOffsetToAngleL[0][0];
0777     theDerivatives[offsetMeas + nMeasPerHit * iMsMeas][offsetPar + 2 * l + 1] = JacOffsetToAngleL[0][1];
0778     theDerivatives[offsetMeas + nMeasPerHit * iMsMeas + 1][offsetPar + 2 * l] = JacOffsetToAngleL[1][0];
0779     theDerivatives[offsetMeas + nMeasPerHit * iMsMeas + 1][offsetPar + 2 * l + 1] = JacOffsetToAngleL[1][1];
0780     // current layer
0781     theDerivatives[offsetMeas + nMeasPerHit * iMsMeas][offsetPar + 2 * k] = JacOffsetToAngleC[0][0];
0782     theDerivatives[offsetMeas + nMeasPerHit * iMsMeas][offsetPar + 2 * k + 1] = JacOffsetToAngleC[0][1];
0783     theDerivatives[offsetMeas + nMeasPerHit * iMsMeas + 1][offsetPar + 2 * k] = JacOffsetToAngleC[1][0];
0784     theDerivatives[offsetMeas + nMeasPerHit * iMsMeas + 1][offsetPar + 2 * k + 1] = JacOffsetToAngleC[1][1];
0785 
0786     // next layer
0787     theDerivatives[offsetMeas + nMeasPerHit * iMsMeas][offsetPar + 2 * n] = JacOffsetToAngleN[0][0];
0788     theDerivatives[offsetMeas + nMeasPerHit * iMsMeas][offsetPar + 2 * n + 1] = JacOffsetToAngleN[0][1];
0789     theDerivatives[offsetMeas + nMeasPerHit * iMsMeas + 1][offsetPar + 2 * n] = JacOffsetToAngleN[1][0];
0790     theDerivatives[offsetMeas + nMeasPerHit * iMsMeas + 1][offsetPar + 2 * n + 1] = JacOffsetToAngleN[1][1];
0791   }
0792 
0793   return true;
0794 }
0795 
0796 //__________________________________________________________________________________
0797 
0798 bool ReferenceTrajectory::addMaterialEffectsBrl(const std::vector<AlgebraicMatrix> &allProjections,
0799                                                 const std::vector<AlgebraicSymMatrix> &allDeltaParameterCovs,
0800                                                 const std::vector<AlgebraicMatrix> &allLocalToCurv,
0801                                                 const std::vector<double> &allSteps,
0802                                                 const GlobalTrajectoryParameters &gtp,
0803                                                 const double minStep) {
0804   //CHK: add material effects using broken lines
0805   //BrokenLinesCoarse: combine close by detectors,
0806   //                   use approximate Jacobians from Steps (limit Qbyp -> 0),
0807   //                   bending only in RPhi (B=(0,0,Bz)), no energy loss correction
0808 
0809   int offsetPar = theNumberOfPars;
0810   int offsetMeas = nMeasPerHit * allSteps.size();
0811   int ierr = 0;
0812 
0813   GlobalVector p = gtp.momentum();
0814   double cosLambda = sqrt((p.x() * p.x() + p.y() * p.y()) / (p.x() * p.x() + p.y() * p.y() + p.z() * p.z()));
0815   double bFac = -gtp.magneticFieldInInverseGeV(gtp.position()).mag();
0816 
0817   // transformation from trajectory to curvilinear parameters at refTsos
0818   double delta(1.0 / allSteps[1]);
0819   theInnerTrajectoryToCurvilinear[0][0] = 1;
0820   theInnerTrajectoryToCurvilinear[1][2] = -delta;
0821   theInnerTrajectoryToCurvilinear[1][4] = delta;
0822   theInnerTrajectoryToCurvilinear[2][0] = -0.5 * bFac / delta;
0823   theInnerTrajectoryToCurvilinear[2][1] = -delta / cosLambda;
0824   theInnerTrajectoryToCurvilinear[2][3] = delta / cosLambda;
0825   theInnerTrajectoryToCurvilinear[3][1] = 1;
0826   theInnerTrajectoryToCurvilinear[4][2] = 1;
0827   theInnerLocalToTrajectory = theInnerTrajectoryToCurvilinear.inverse(ierr) * allLocalToCurv[0];
0828   if (ierr) {
0829     edm::LogError("Alignment") << "@SUB=ReferenceTrajectory::addMaterialEffectsBrl"
0830                                << "Inversion 1 for coarse broken lines failed: " << ierr;
0831     return false;
0832   }
0833 
0834   AlgebraicMatrix CurvToAngle(2, 5);   // dAlpha/dCurv
0835   CurvToAngle[1][1] = 1.;              // dalpha2/dlambda
0836   CurvToAngle[0][2] = cosLambda;       // dalpha1/dphi
0837   AlgebraicMatrix OffsetToCurv(5, 2);  // dCurv/dU
0838   OffsetToCurv[3][0] = 1.;             // dxt/du1
0839   OffsetToCurv[4][1] = 1.;             // dyt/du2
0840 
0841   AlgebraicSymMatrix tempMSCov;
0842   AlgebraicSymMatrix tempMSCovProj;
0843   AlgebraicMatrix JacOffsetToMeas;
0844 
0845   // combine closeby detectors into single plane
0846   std::vector<unsigned int> first(allSteps.size());
0847   std::vector<unsigned int> last(allSteps.size());
0848   std::vector<unsigned int> plane(allSteps.size());
0849   std::vector<double> sPlane(allSteps.size());
0850   unsigned int nPlane = 0;
0851   double sTot = 0;
0852 
0853   for (unsigned int k = 1; k < allSteps.size(); ++k) {
0854     sTot += allSteps[k];
0855     if (fabs(allSteps[k]) > minStep) {
0856       nPlane += 1;
0857       first[nPlane] = k;
0858     }
0859     last[nPlane] = k;
0860     plane[k] = nPlane;
0861     sPlane[nPlane] += sTot;
0862   }
0863   if (nPlane < 2)
0864     return false;  // pathological cases: need at least 2 planes
0865 
0866   theNumberOfVirtualPars = 2 * (nPlane + 1);
0867   theNumberOfVirtualMeas = 2 * (nPlane - 1);  // unsigned underflow for nPlane == 0...
0868   for (unsigned int k = 0; k <= nPlane; ++k) {
0869     sPlane[k] /= (double)(last[k] - first[k] + 1);
0870   }
0871 
0872   // measurements from hits
0873   sTot = 0;
0874   for (unsigned int k = 0; k < allSteps.size(); ++k) {
0875     sTot += allSteps[k];
0876     //  (dMeas/dU) = (dMeas/dLoc) * (dLoc/dCurv) * (dCurv/dU)
0877     JacOffsetToMeas = (allProjections[k] * allLocalToCurv[k].inverse(ierr)) * OffsetToCurv;
0878     if (ierr) {
0879       edm::LogError("Alignment") << "@SUB=ReferenceTrajectory::addMaterialEffectsBrl"
0880                                  << "Inversion 2 for coarse broken lines failed: " << ierr;
0881       return false;
0882     }
0883 
0884     unsigned int iPlane = plane[k];
0885     if (last[iPlane] == first[iPlane]) {  // single plane
0886       theDerivatives[nMeasPerHit * k][offsetPar + 2 * iPlane] = JacOffsetToMeas[0][0];
0887       theDerivatives[nMeasPerHit * k][offsetPar + 2 * iPlane + 1] = JacOffsetToMeas[0][1];
0888       theDerivatives[nMeasPerHit * k + 1][offsetPar + 2 * iPlane] = JacOffsetToMeas[1][0];
0889       theDerivatives[nMeasPerHit * k + 1][offsetPar + 2 * iPlane + 1] = JacOffsetToMeas[1][1];
0890     } else {                // combined plane: (linear) interpolation
0891       unsigned int jPlane;  // neighbor plane for interpolation
0892       if (fabs(sTot) < fabs(sPlane[iPlane])) {
0893         jPlane = (iPlane > 0) ? iPlane - 1 : 1;
0894       } else {
0895         jPlane = (iPlane < nPlane) ? iPlane + 1 : nPlane - 1;
0896       }
0897       // interpolation weights
0898       double sDiff = sPlane[iPlane] - sPlane[jPlane];
0899       double iFrac = (sTot - sPlane[jPlane]) / sDiff;
0900       double jFrac = 1.0 - iFrac;
0901       theDerivatives[nMeasPerHit * k][offsetPar + 2 * iPlane] = JacOffsetToMeas[0][0] * iFrac;
0902       theDerivatives[nMeasPerHit * k][offsetPar + 2 * iPlane + 1] = JacOffsetToMeas[0][1] * iFrac;
0903       theDerivatives[nMeasPerHit * k + 1][offsetPar + 2 * iPlane] = JacOffsetToMeas[1][0] * iFrac;
0904       theDerivatives[nMeasPerHit * k + 1][offsetPar + 2 * iPlane + 1] = JacOffsetToMeas[1][1] * iFrac;
0905       theDerivatives[nMeasPerHit * k][offsetPar + 2 * jPlane] = JacOffsetToMeas[0][0] * jFrac;
0906       theDerivatives[nMeasPerHit * k][offsetPar + 2 * jPlane + 1] = JacOffsetToMeas[0][1] * jFrac;
0907       theDerivatives[nMeasPerHit * k + 1][offsetPar + 2 * jPlane] = JacOffsetToMeas[1][0] * jFrac;
0908       theDerivatives[nMeasPerHit * k + 1][offsetPar + 2 * jPlane + 1] = JacOffsetToMeas[1][1] * jFrac;
0909       // 2nd order neglected
0910       // theDerivatives[nMeasPerHit*k  ][                   0] = -0.5*bFac*sDiff*iFrac*sDiff*jFrac*cosLambda;
0911     }
0912   }
0913 
0914   // measurement of MS kink
0915   for (unsigned int i = 1; i < nPlane; ++i) {
0916     // CHK
0917     int iMsMeas = i - 1;
0918     int l = i - 1;  // last hit
0919     int n = i + 1;  // next hit
0920 
0921     // amount of multiple scattering in plane k
0922     for (unsigned int k = first[i]; k <= last[i]; ++k) {
0923       tempMSCov = allDeltaParameterCovs[k].similarity(allLocalToCurv[k]);
0924       tempMSCovProj = tempMSCov.similarity(CurvToAngle);
0925       theMeasurementsCov[offsetMeas + nMeasPerHit * iMsMeas][offsetMeas + nMeasPerHit * iMsMeas] += tempMSCovProj[0][0];
0926       theMeasurementsCov[offsetMeas + nMeasPerHit * iMsMeas + 1][offsetMeas + nMeasPerHit * iMsMeas + 1] +=
0927           tempMSCovProj[1][1];
0928     }
0929     // broken line measurements for layer k, correlations between both planes neglected
0930     double stepK = sPlane[i] - sPlane[l];
0931     double stepN = sPlane[n] - sPlane[i];
0932     double deltaK(1.0 / stepK);
0933     double deltaN(1.0 / stepN);
0934     // bending (only in RPhi)
0935     theDerivatives[offsetMeas + nMeasPerHit * iMsMeas][0] = -0.5 * bFac * (stepK + stepN) * cosLambda;
0936     // last layer
0937     theDerivatives[offsetMeas + nMeasPerHit * iMsMeas][offsetPar + 2 * l] = deltaK;
0938     theDerivatives[offsetMeas + nMeasPerHit * iMsMeas + 1][offsetPar + 2 * l + 1] = deltaK;
0939     // current layer
0940     theDerivatives[offsetMeas + nMeasPerHit * iMsMeas][offsetPar + 2 * i] = -(deltaK + deltaN);
0941     theDerivatives[offsetMeas + nMeasPerHit * iMsMeas + 1][offsetPar + 2 * i + 1] = -(deltaK + deltaN);
0942     // next layer
0943     theDerivatives[offsetMeas + nMeasPerHit * iMsMeas][offsetPar + 2 * n] = deltaN;
0944     theDerivatives[offsetMeas + nMeasPerHit * iMsMeas + 1][offsetPar + 2 * n + 1] = deltaN;
0945   }
0946 
0947   return true;
0948 }
0949 
0950 //__________________________________________________________________________________
0951 
0952 bool ReferenceTrajectory::addMaterialEffectsLocalGbl(const std::vector<AlgebraicMatrix> &allJacobians,
0953                                                      const std::vector<AlgebraicMatrix> &allProjections,
0954                                                      const std::vector<AlgebraicSymMatrix> &allCurvatureChanges,
0955                                                      const std::vector<AlgebraicSymMatrix> &allDeltaParameterCovs) {
0956   //CHK: add material effects using general broken lines, no initial kinks
0957   // local track parameters are defined in the TSO system
0958 
0959   const double minPrec = 1.0;  // minimum precision to use measurement (reject measurements in strip direction)
0960 
0961   AlgebraicMatrix OffsetToLocal(5, 2);  // dLocal/dU
0962   OffsetToLocal[3][0] = 1.;
0963   OffsetToLocal[4][1] = 1.;
0964   AlgebraicMatrix SlopeToLocal(5, 2);  // dLocal/dU'
0965   SlopeToLocal[1][0] = 1.;
0966   SlopeToLocal[2][1] = 1.;
0967 
0968   // GBL uses Eigen matrices as interface
0969   Eigen::Matrix2d covariance, scatPrecision, proLocalToMeas;
0970   Matrix5d jacPointToPoint;
0971   auto identity = Matrix5d::Identity();
0972   Eigen::Vector2d measurement, measPrecDiag;
0973   auto scatterer = Eigen::Vector2d::Zero();
0974 
0975   //bool initialKinks = (allCurvlinKinks.size()>0);
0976 
0977   // measurements and scatterers from hits
0978   unsigned int numHits = allJacobians.size();
0979   std::vector<GblPoint> GblPointList;
0980   GblPointList.reserve(numHits);
0981   for (unsigned int k = 0; k < numHits; ++k) {
0982     // GBL point to point jacobian
0983     clhep2eigen(allJacobians[k] * allCurvatureChanges[k], jacPointToPoint);
0984 
0985     // GBL point
0986     GblPoint aGblPoint(jacPointToPoint);
0987 
0988     // GBL projection from local to measurement system
0989     clhep2eigen(allProjections[k] * OffsetToLocal, proLocalToMeas);
0990 
0991     // GBL measurement (residuum to initial trajectory)
0992     clhep2eigen(theMeasurements.sub(2 * k + 1, 2 * k + 2) - theTrajectoryPositions.sub(2 * k + 1, 2 * k + 2),
0993                 measurement);
0994 
0995     // GBL measurement covariance matrix
0996     clhep2eigen(theMeasurementsCov.sub(2 * k + 1, 2 * k + 2), covariance);
0997 
0998     // GBL add measurement to point
0999     if (std::abs(covariance(0, 1)) < std::numeric_limits<double>::epsilon()) {
1000       // covariance matrix is diagonal, independent measurements
1001       for (unsigned int row = 0; row < 2; ++row) {
1002         measPrecDiag(row) = (0. < covariance(row, row) ? 1.0 / covariance(row, row) : 0.);
1003       }
1004       aGblPoint.addMeasurement(proLocalToMeas, measurement, measPrecDiag, minPrec);
1005     } else {
1006       // covariance matrix needs diagonalization
1007       aGblPoint.addMeasurement(proLocalToMeas, measurement, covariance.inverse(), minPrec);
1008     }
1009 
1010     // GBL multiple scattering (full matrix in local system)
1011     clhep2eigen(allDeltaParameterCovs[k].similarityT(SlopeToLocal), scatPrecision);
1012     if (!(scatPrecision.colPivHouseholderQr().isInvertible())) {
1013       if (!allowZeroMaterial_) {
1014         throw cms::Exception("Alignment") << "@SUB=ReferenceTrajectory::addMaterialEffectsLocalGbl"
1015                                           << "\nEncountered singular scatter covariance-matrix without allowing "
1016                                           << "for zero material.";
1017       }
1018     } else {
1019       // GBL add scatterer to point
1020       aGblPoint.addScatterer(scatterer, scatPrecision.inverse());
1021     }
1022     // add point to list
1023     GblPointList.push_back(aGblPoint);
1024   }
1025   // add list of points and transformation local to fit (=local) system at first hit
1026   theGblInput.push_back(std::make_pair(GblPointList, identity));
1027 
1028   return true;
1029 }
1030 
1031 //__________________________________________________________________________________
1032 
1033 bool ReferenceTrajectory::addMaterialEffectsCurvlinGbl(const std::vector<AlgebraicMatrix> &allCurvlinJacobians,
1034                                                        const std::vector<AlgebraicMatrix> &allProjections,
1035                                                        const std::vector<AlgebraicSymMatrix> &allCurvatureChanges,
1036                                                        const std::vector<AlgebraicSymMatrix> &allDeltaParameterCovs,
1037                                                        const std::vector<AlgebraicMatrix> &allLocalToCurv) {
1038   //CHK: add material effects using general broken lines
1039   // local track parameters are defined in the curvilinear system
1040 
1041   const double minPrec = 1.0;  // minimum precision to use measurement (reject measurements in strip direction)
1042   int ierr = 0;
1043 
1044   AlgebraicMatrix OffsetToCurv(5, 2);  // dCurv/dU
1045   OffsetToCurv[3][0] = 1.;             // dxt/du1
1046   OffsetToCurv[4][1] = 1.;             // dyt/du2
1047 
1048   AlgebraicMatrix JacOffsetToMeas, tempMSCov;
1049 
1050   // GBL uses Eigen matrices as interface
1051   Eigen::Matrix2d covariance, proLocalToMeas;
1052   Matrix5d jacPointToPoint, firstLocalToCurv;
1053   Eigen::Vector2d measurement, measPrecDiag, scatPrecDiag;
1054   auto scatterer = Eigen::Vector2d::Zero();
1055 
1056   // measurements and scatterers from hits
1057   unsigned int numHits = allCurvlinJacobians.size();
1058   std::vector<GblPoint> GblPointList;
1059   GblPointList.reserve(numHits);
1060   for (unsigned int k = 0; k < numHits; ++k) {
1061     //  (dMeas/dU) = (dMeas/dLoc) * (dLoc/dCurv) * (dCurv/dU)
1062     JacOffsetToMeas = (allProjections[k] * allLocalToCurv[k].inverse(ierr)) * OffsetToCurv;
1063     if (ierr) {
1064       edm::LogError("Alignment") << "@SUB=ReferenceTrajectory::addMaterialEffectsGbl"
1065                                  << "Inversion 1 for general broken lines failed: " << ierr;
1066       return false;
1067     }
1068 
1069     // GBL point to point jacobian
1070     clhep2eigen(allCurvlinJacobians[k] * allCurvatureChanges[k], jacPointToPoint);
1071 
1072     // GBL point
1073     GblPoint aGblPoint(jacPointToPoint);
1074 
1075     // GBL projection from local to measurement system
1076     clhep2eigen(JacOffsetToMeas, proLocalToMeas);
1077 
1078     // GBL measurement (residuum to initial trajectory)
1079     clhep2eigen(theMeasurements.sub(2 * k + 1, 2 * k + 2) - theTrajectoryPositions.sub(2 * k + 1, 2 * k + 2),
1080                 measurement);
1081 
1082     // GBL measurement covariance matrix
1083     clhep2eigen(theMeasurementsCov.sub(2 * k + 1, 2 * k + 2), covariance);
1084 
1085     // GBL add measurement to point
1086     if (std::abs(covariance(0, 1)) < std::numeric_limits<double>::epsilon()) {
1087       // covariance matrix is diagonal, independent measurements
1088       for (unsigned int row = 0; row < 2; ++row) {
1089         measPrecDiag(row) = (0. < covariance(row, row) ? 1.0 / covariance(row, row) : 0.);
1090       }
1091       aGblPoint.addMeasurement(proLocalToMeas, measurement, measPrecDiag, minPrec);
1092     } else {
1093       // covariance matrix needs diagonalization
1094       aGblPoint.addMeasurement(proLocalToMeas, measurement, covariance.inverse(), minPrec);
1095     }
1096 
1097     // GBL multiple scattering (diagonal matrix in curvilinear system)
1098     tempMSCov = allDeltaParameterCovs[k].similarity(allLocalToCurv[k]);
1099     for (unsigned int row = 0; row < 2; ++row) {
1100       scatPrecDiag(row) = 1.0 / tempMSCov[row + 1][row + 1];
1101     }
1102 
1103     // check for singularity
1104     bool singularCovariance{false};
1105     for (int row = 0; row < scatPrecDiag.rows(); ++row) {
1106       if (!(scatPrecDiag[row] < std::numeric_limits<double>::infinity())) {
1107         singularCovariance = true;
1108         break;
1109       }
1110     }
1111     if (singularCovariance && !allowZeroMaterial_) {
1112       throw cms::Exception("Alignment") << "@SUB=ReferenceTrajectory::addMaterialEffectsCurvlinGbl"
1113                                         << "\nEncountered singular scatter covariance-matrix without allowing "
1114                                         << "for zero material.";
1115     }
1116 
1117     // GBL add scatterer to point
1118     aGblPoint.addScatterer(scatterer, scatPrecDiag);
1119 
1120     // add point to list
1121     GblPointList.push_back(aGblPoint);
1122   }
1123   // add list of points and transformation local to fit (=curvilinear) system at first hit
1124   clhep2eigen(allLocalToCurv[0], firstLocalToCurv);
1125   theGblInput.push_back(std::make_pair(GblPointList, firstLocalToCurv));
1126 
1127   return true;
1128 }
1129 
1130 //__________________________________________________________________________________
1131 template <typename Derived>
1132 void ReferenceTrajectory::clhep2eigen(const AlgebraicVector &in, Eigen::MatrixBase<Derived> &out) {
1133   static_assert(Derived::ColsAtCompileTime == 1, "clhep2eigen: 'out' must be of vector type");
1134   for (int row = 0; row < in.num_row(); ++row) {
1135     out(row) = in[row];
1136   }
1137 }
1138 
1139 template <typename Derived>
1140 void ReferenceTrajectory::clhep2eigen(const AlgebraicMatrix &in, Eigen::MatrixBase<Derived> &out) {
1141   for (int row = 0; row < in.num_row(); ++row) {
1142     for (int col = 0; col < in.num_col(); ++col) {
1143       out(row, col) = in[row][col];
1144     }
1145   }
1146 }
1147 
1148 template <typename Derived>
1149 void ReferenceTrajectory::clhep2eigen(const AlgebraicSymMatrix &in, Eigen::MatrixBase<Derived> &out) {
1150   for (int row = 0; row < in.num_row(); ++row) {
1151     for (int col = 0; col < in.num_col(); ++col) {
1152       out(row, col) = in[row][col];
1153     }
1154   }
1155 }
1156 
1157 //__________________________________________________________________________________
1158 
1159 AlgebraicMatrix ReferenceTrajectory::getHitProjectionMatrix(
1160     const TransientTrackingRecHit::ConstRecHitPointer &hitPtr) const {
1161   if (this->useRecHit(hitPtr)) {
1162     // check which templated non-member function to call:
1163     switch (hitPtr->dimension()) {
1164       case 1:
1165         return getHitProjectionMatrixT<1>(hitPtr);
1166       case 2:
1167         return getHitProjectionMatrixT<2>(hitPtr);
1168       case 3:
1169         return getHitProjectionMatrixT<3>(hitPtr);
1170       case 4:
1171         return getHitProjectionMatrixT<4>(hitPtr);
1172       case 5:
1173         return getHitProjectionMatrixT<5>(hitPtr);
1174       default:
1175         throw cms::Exception("ReferenceTrajectory::getHitProjectionMatrix")
1176             << "Unexpected hit dimension: " << hitPtr->dimension() << "\n";
1177     }
1178   }
1179   // invalid or (to please compiler) unknown dimension
1180   return AlgebraicMatrix(2, 5, 0);  // get size from ???
1181 }
1182 
1183 //__________________________________________________________________________________
1184 
1185 template <unsigned int N>
1186 AlgebraicMatrix ReferenceTrajectory::getHitProjectionMatrixT(
1187     const TransientTrackingRecHit::ConstRecHitPointer &hitPtr) const {
1188   // define variables that will be used to setup the KfComponentsHolder
1189   // (their allocated memory is needed by 'hitPtr->getKfComponents(..)'
1190 
1191   ProjectMatrix<double, 5, N> pf;
1192   typename AlgebraicROOTObject<N>::Vector r, rMeas;
1193   typename AlgebraicROOTObject<N, N>::SymMatrix V, VMeas;
1194   // input for the holder - but dummy is OK here to just get the projection matrix:
1195   const AlgebraicVector5 dummyPars;
1196   const AlgebraicSymMatrix55 dummyErr;
1197 
1198   // setup the holder with the correct dimensions and get the values
1199   KfComponentsHolder holder;
1200   holder.setup<N>(&r, &V, &pf, &rMeas, &VMeas, dummyPars, dummyErr);
1201   hitPtr->getKfComponents(holder);
1202 
1203   return asHepMatrix<N, 5>(holder.projection<N>());
1204 }