diff --git a/Analysis/HitAnalyzer/plugins/ResidualGlobalCorrectionMaker.cc b/Analysis/HitAnalyzer/plugins/ResidualGlobalCorrectionMaker.cc deleted file mode 100644 index 610d44534d242..0000000000000 --- a/Analysis/HitAnalyzer/plugins/ResidualGlobalCorrectionMaker.cc +++ /dev/null @@ -1,3282 +0,0 @@ -#include "ResidualGlobalCorrectionMakerBase.h" -#include "MagneticFieldOffset.h" -#include "DataFormats/MuonReco/interface/Muon.h" - -#include "DataFormats/Math/interface/deltaR.h" - - -class ResidualGlobalCorrectionMaker : public ResidualGlobalCorrectionMakerBase -{ -public: - explicit ResidualGlobalCorrectionMaker(const edm::ParameterSet &); - ~ResidualGlobalCorrectionMaker() {} - -// static void fillDescriptions(edm::ConfigurationDescriptions &descriptions); - -private: - - virtual void beginStream(edm::StreamID) override; - virtual void produce(edm::Event &, const edm::EventSetup &) override; - - edm::EDGetTokenT> inputAssoc_; - - float muonPt; - bool muonLoose; - bool muonMedium; - bool muonTight; - bool muonIsPF; - bool muonIsTracker; - bool muonIsGlobal; - bool muonIsStandalone; - bool muonInnerTrackBest; - - bool trackExtraAssoc; - -}; - - -ResidualGlobalCorrectionMaker::ResidualGlobalCorrectionMaker(const edm::ParameterSet &iConfig) : ResidualGlobalCorrectionMakerBase(iConfig) -{ - - inputAssoc_ = consumes>(edm::InputTag("muonReducedTrackExtras")); - -} - -void ResidualGlobalCorrectionMaker::beginStream(edm::StreamID streamid) -{ - ResidualGlobalCorrectionMakerBase::beginStream(streamid); - - if (fillTrackTree_) { - const int basketSize = 4*1024*1024; - - tree->Branch("trackPt", &trackPt, basketSize); - tree->Branch("trackPtErr", &trackPtErr, basketSize); - tree->Branch("trackEta", &trackEta, basketSize); - tree->Branch("trackPhi", &trackPhi, basketSize); - tree->Branch("trackCharge", &trackCharge, basketSize); - //workaround for older ROOT version inability to store std::array automatically - // tree->Branch("trackOrigParms", trackOrigParms.data(), "trackOrigParms[5]/F", basketSize); - // tree->Branch("trackOrigCov", trackOrigCov.data(), "trackOrigCov[25]/F", basketSize); - tree->Branch("trackParms", trackParms.data(), "trackParms[5]/F", basketSize); - tree->Branch("trackCov", trackCov.data(), "trackCov[25]/F", basketSize); - - tree->Branch("refParms_iter0", refParms_iter0.data(), "refParms_iter0[5]/F", basketSize); - tree->Branch("refCov_iter0", refCov_iter0.data(), "refCov_iter0[25]/F", basketSize); - // tree->Branch("refParms_iter2", refParms_iter2.data(), "refParms_iter2[5]/F", basketSize); - // tree->Branch("refCov_iter2", refCov_iter2.data(), "refCov_iter2[25]/F", basketSize); - - tree->Branch("refParms", refParms.data(), "refParms[5]/F", basketSize); - tree->Branch("refCov", refCov.data(), "refCov[25]/F", basketSize); - tree->Branch("genParms", genParms.data(), "genParms[5]/F", basketSize); - - tree->Branch("genPt", &genPt, basketSize); - tree->Branch("genEta", &genEta, basketSize); - tree->Branch("genPhi", &genPhi, basketSize); - tree->Branch("genCharge", &genCharge, basketSize); - - tree->Branch("genX", &genX, basketSize); - tree->Branch("genY", &genY, basketSize); - tree->Branch("genZ", &genZ, basketSize); - - tree->Branch("normalizedChi2", &normalizedChi2, basketSize); - - tree->Branch("nHits", &nHits, basketSize); - tree->Branch("nValidHits", &nValidHits, basketSize); - tree->Branch("nValidPixelHits", &nValidPixelHits, basketSize); - tree->Branch("nJacRef", &nJacRef, basketSize); - - tree->Branch("nValidHitsFinal", &nValidHitsFinal); - tree->Branch("nValidPixelHitsFinal", &nValidPixelHitsFinal); - - tree->Branch("jacrefv",jacrefv.data(),"jacrefv[nJacRef]/F", basketSize); - - - -// tree->Branch("dxpxb1", &dxpxb1); -// tree->Branch("dypxb1", &dypxb1); -// -// tree->Branch("dxttec9rphi", &dxttec9rphi); -// tree->Branch("dxttec9stereo", &dxttec9stereo); -// -// tree->Branch("dxttec4rphi", &dxttec4rphi); -// tree->Branch("dxttec4stereo", &dxttec4stereo); -// -// tree->Branch("dxttec4rphisimgen", &dxttec4rphisimgen); -// tree->Branch("dyttec4rphisimgen", &dyttec4rphisimgen); -// tree->Branch("dxttec4rphirecsim", &dxttec4rphirecsim); -// -// tree->Branch("dxttec9rphisimgen", &dxttec9rphisimgen); -// tree->Branch("dyttec9rphisimgen", &dyttec9rphisimgen); -// -// tree->Branch("simlocalxref", &simlocalxref); -// tree->Branch("simlocalyref", &simlocalyref); - - tree->Branch("hitidxv", &hitidxv); - tree->Branch("dxrecgen", &dxrecgen); - tree->Branch("dyrecgen", &dyrecgen); - tree->Branch("dxsimgen", &dxsimgen); - tree->Branch("dysimgen", &dysimgen); - tree->Branch("dxrecsim", &dxrecsim); - tree->Branch("dyrecsim", &dyrecsim); - tree->Branch("dxerr", &dxerr); - tree->Branch("dyerr", &dyerr); - - tree->Branch("clusterSize", &clusterSize); - tree->Branch("clusterSizeX", &clusterSizeX); - tree->Branch("clusterSizeY", &clusterSizeY); - tree->Branch("clusterCharge", &clusterCharge); - tree->Branch("clusterChargeBin", &clusterChargeBin); - tree->Branch("clusterOnEdge", &clusterOnEdge); - - tree->Branch("clusterProbXY", &clusterProbXY); - tree->Branch("clusterSN", &clusterSN); - - tree->Branch("dxreccluster", &dxreccluster); - tree->Branch("dyreccluster", &dyreccluster); - - tree->Branch("localqop", &localqop); - tree->Branch("localdxdz", &localdxdz); - tree->Branch("localdydz", &localdydz); - tree->Branch("localx", &localx); - tree->Branch("localy", &localy); - - tree->Branch("simtestz", &simtestz); - tree->Branch("simtestvz", &simtestvz); - tree->Branch("simtestrho", &simtestrho); - tree->Branch("simtestzlocalref", &simtestzlocalref); - tree->Branch("simtestdx", &simtestdx); - tree->Branch("simtestdxrec", &simtestdxrec); - tree->Branch("simtestdy", &simtestdy); - tree->Branch("simtestdyrec", &simtestdyrec); - tree->Branch("simtestdxprop", &simtestdxprop); - tree->Branch("simtestdyprop", &simtestdyprop); - tree->Branch("simtestdetid", &simtestdetid); - - tree->Branch("rx", &rx); - tree->Branch("ry", &ry); - - tree->Branch("deigx", &deigx); - tree->Branch("deigy", &deigy); - - tree->Branch("muonPt", &muonPt); - tree->Branch("muonLoose", &muonLoose); - tree->Branch("muonMedium", &muonMedium); - tree->Branch("muonTight", &muonTight); - - tree->Branch("muonIsPF", &muonIsPF); - tree->Branch("muonIsTracker", &muonIsTracker); - tree->Branch("muonIsGlobal", &muonIsGlobal); - tree->Branch("muonIsStandalone", &muonIsStandalone); - - tree->Branch("muonInnerTrackBest", &muonInnerTrackBest); - - tree->Branch("trackExtraAssoc", &trackExtraAssoc); - - - nJacRef = 0.; - } -} - - -// ------------ method called for each event ------------ -void ResidualGlobalCorrectionMaker::produce(edm::Event &iEvent, const edm::EventSetup &iSetup) -{ - - const bool dogen = fitFromGenParms_; - - using namespace edm; - - Handle trackOrigH; - iEvent.getByToken(inputTrackOrig_, trackOrigH); - - -// bool foundmodule = false; -// for (const reco::Track &track : *trackOrigH) { -// if (track.innerDetId() == 302055944) { -// foundmodule = true; -// break; -// } -// // for (auto it = track.recHitsBegin(); it != track.recHitsEnd(); ++it) { -// // if ((*it)->geographicalId().rawId() == 302055944) { -// // foundmodule = true; -// // break; -// // } -// // } -// // if (foundmodule) { -// // break; -// // } -// } -// if (!foundmodule) { -// // printf("not found, returning\n"); -// return; -// } - - - - // loop over gen particles - - edm::ESHandle globalGeometry; - iSetup.get().get(globalGeometry); - -// edm::ESHandle globalGeometry; -// iSetup.get().get("idealForDigi", globalGeometry); - - edm::ESHandle trackerTopology; - iSetup.get().get(trackerTopology); - -// ESHandle magfield; -// iSetup.get().get(magfield); -// auto field = magfield.product(); - - edm::ESHandle ttrh; - iSetup.get().get("WithAngleAndTemplate",ttrh); - -// ESHandle thePropagator; -// iSetup.get().get("RungeKuttaTrackerPropagator", thePropagator); -// iSetup.get().get("PropagatorWithMaterial", thePropagator); -// iSetup.get().get("PropagatorWithMaterialParabolicMf", thePropagator); -// iSetup.get().get("Geant4ePropagator", thePropagator); -// const MagneticField* field = thePropagator->magneticField(); - -// edm::ESHandle fit; -// iSetup.get().get("G4eFitterSmoother", fit); -// const KFTrajectoryFitter *kffit = dynamic_cast(fit.product()); -// const Propagator *thePropagator = kffit->propagator(); - - -// ESHandle theAnalyticPropagator; -// iSetup.get().get("PropagatorWithMaterial", theAnalyticPropagator); - - ESHandle fieldh; - iSetup.get().get("", fieldh); - std::unique_ptr fieldOffset = std::make_unique(&(*fieldh)); - - std::unique_ptr fPropagator = std::make_unique(alongMomentum, 0.105, fieldOffset.get(), 1.6, true, -1., true); - - const MagneticField* field = fPropagator->magneticField(); - -// Handle trackH; -// Handle trackH; -// iEvent.getByToken(inputTrack_, trackH); - - - -// Handle > indicesH; -// iEvent.getByToken(inputIndices_, indicesH); - -// Handle > trajH; -// iEvent.getByToken(inputTraj_, trajH); - - - - - Handle bsH; - iEvent.getByToken(inputBs_, bsH); - - - Handle> genPartCollection; - Handle> genPartBarcodes; - if (doGen_) { - iEvent.getByToken(GenParticlesToken_, genPartCollection); - iEvent.getByToken(genParticlesBarcodeToken_, genPartBarcodes); - } - -// Handle> tecSimHits; - std::vector>> simHits(inputSimHits_.size()); - edm::Handle> simTracks; - if (doSim_) { - for (unsigned int isimhit = 0; isimhit muons; - if (doMuons_) { - iEvent.getByToken(inputMuons_, muons); - } - - Handle> assoc; - if (doMuons_) { - iEvent.getByToken(inputAssoc_, assoc); - } - -// if (doSim_) { -// iEvent.getByToken(inputSimHits_, tecSimHits); -// } - -// const float mass = 0.105; -// const float maxDPhi = 1.6; -// PropagatorWithMaterial rPropagator(oppositeToMomentum, mass, field, maxDPhi, true, -1., false); -// PropagatorWithMaterial fPropagator(alongMomentum, mass, field, maxDPhi, true, -1., false); - -// std::unique_ptr fPropagator(static_cast(thePropagator->clone())); -// fPropagator->setPropagationDirection(alongMomentum); -// -// std::unique_ptr fAnalyticPropagator(static_cast(theAnalyticPropagator->clone())); -// fAnalyticPropagator->setPropagationDirection(alongMomentum); - - KFUpdator updator; - TkClonerImpl const& cloner = static_cast(ttrh.product())->cloner(); - - -// siStripClusterInfo_.initEvent(iSetup); - -// edm::ESHandle globalPositionRcd; -// iSetup.get().get(globalPositionRcd); -// -// printf("globalPositionRcd translation = %e, %e, %e\n", globalPositionRcd->m_align.front().translation().x(), -// globalPositionRcd->m_align.front().translation().y(), -// globalPositionRcd->m_align.front().translation().z()); -// std::cout << "globalPositionRcd rotation" << globalPositionRcd->m_align.front().rotation() << std::endl; - - // set up cylindrical surface for beam pipe -// const double ABe = 9.0121831; -// const double ZBe = 4.; -// const double K = 0.307075*1e-3; -// const double dr = 0.08; -// // const double xibeampipe = 0.5*K*dr*ZBe/ABe; -// const double xibeampipe = 0.*0.5*K*dr*ZBe/ABe; - - - - -// auto beampipe = Cylinder::build(Surface::PositionType(0.,0.,0.), Surface::RotationType(), 2.94); -// beampipe->setMediumProperties(MediumProperties(0., xibeampipe)); - -// std::cout << "xi beampipe: " << xibeampipe << std::endl; - -// const GeomDet *testdet = nullptr; -// //debugging -// for (const GeomDet* det : globalGeometry->detUnits()) { -// if (!det) { -// continue; -// } -// -// if (det->subDetector() == GeomDetEnumerators::TEC) { -// const DetId& detid = det->geographicalId(); -// // TECDetId detid(det->geographicalId()); -// // layer = -1 * (detid.side() == 1) * detid.wheel() + (detid.side() == 2) * detid.wheel(); -// unsigned int side = trackerTopology->tecSide(detid); -// unsigned int wheel = trackerTopology->tecWheel(detid); -// int layer = -1 * (side == 1) * wheel + (side == 2) * wheel; -// bool stereo = trackerTopology->isStereo(det->geographicalId()); -// -// if (layer == -9) { -// testdet = det; -// break; -// -// } -// } -// -// -// -// } -// -// if (testdet) { -// const GlobalPoint center = testdet->surface().toGlobal(LocalPoint(1.,0.)); -// -// const GlobalVector centerv(center.x(), center.y(), center.z()); -// const GlobalVector dir = centerv/centerv.mag(); -// const double sintheta = dir.perp(); -// const GlobalVector mom = (100000./sintheta)*dir; -// const GlobalPoint pos(0.,0.,0.); -// -// FreeTrajectoryState ftsplus(pos, mom, 1., field); -// FreeTrajectoryState ftsminus(pos, mom, -1., field); -// -// const TrajectoryStateOnSurface tsosplus = fPropagator->propagate(ftsplus, testdet->surface()); -// const TrajectoryStateOnSurface tsosminus = fPropagator->propagate(ftsminus, testdet->surface()); -// -// std::cout << "global target" << std::endl; -// std::cout << center << std::endl; -// -// std::cout << "momentum" << std::endl; -// std::cout << mom << std::endl; -// -// std::cout << "tsosplus local:" << std::endl; -// std::cout << tsosplus.localPosition() << std::endl; -// std::cout << "tsosminus local:" << std::endl; -// std::cout << tsosminus.localPosition() << std::endl; -// -// std::cout << "delta local" << std::endl; -// std::cout << tsosplus.localPosition() - tsosminus.localPosition() << std::endl; -// -// } - - - simtestz = -99.; - simtestvz = -99.; - simtestrho = -99.; - simtestzlocalref = -99.; - simtestdx = -99.; - simtestdxrec = -99.; - simtestdy = -99.; - simtestdyrec = -99.; - simtestdxprop = -99.; - simtestdyprop = -99.; - simtestdetid = 0; - - if (false) { - - //sim hit debugging - const reco::GenParticle* genmuon = nullptr; - for (const reco::GenParticle& genPart : *genPartCollection) { - if (genPart.status()==1 && std::abs(genPart.pdgId()) == 13) { - genmuon = &genPart; - break; - } - } - - if (genmuon) { - genPt = genmuon->pt(); - genCharge = genmuon->charge(); - genEta = genmuon->eta(); - genPhi = genmuon->phi(); - - simtestvz = genmuon->vertex().z(); - - auto const& refpoint = genmuon->vertex(); - auto const& trackmom = genmuon->momentum(); - const GlobalPoint refpos(refpoint.x(), refpoint.y(), refpoint.z()); - const GlobalVector refmom(trackmom.x(), trackmom.y(), trackmom.z()); - // const GlobalTrajectoryParameters refglobal(refpos, refmom, genmuon->charge(), field); - - // std::cout << "gen ref state" << std::endl; - // std::cout << refpos << std::endl; - // std::cout << refmom << std::endl; - // std::cout << genpart->charge() << std::endl; - - //zero uncertainty on generated parameters - // AlgebraicSymMatrix55 nullerr; - // const CurvilinearTrajectoryError referr(nullerr); - - const FreeTrajectoryState fts = FreeTrajectoryState(refpos, refmom, genmuon->charge(), field); - - std::cout << "gen muon charge: " << genmuon->charge() << std::endl; - - TrajectoryStateOnSurface tsos; - - std::vector simhitsflat; - for (auto const& simhith : simHits) { - for (const PSimHit& simHit : *simhith) { - simhitsflat.push_back(&simHit); - } - } - - auto simhitcompare = [&](const PSimHit *hit0, const PSimHit *hit1) { - return globalGeometry->idToDet(hit0->detUnitId())->surface().toGlobal(hit0->localPosition()).mag() < globalGeometry->idToDet(hit1->detUnitId())->surface().toGlobal(hit1->localPosition()).mag(); - }; - - std::sort(simhitsflat.begin(), simhitsflat.end(), simhitcompare); - - unsigned int ihit = 0; - for (auto const& simHitp : simhitsflat) { - auto const &simHit = *simHitp; - if (std::abs(simHit.particleType()) != 13) { - continue; - } - -// if (std::abs(simHit.localPosition().z()) > 1e-9) { -// continue; -// } - -// for (auto const& simhith : simHits) { -// for (const PSimHit& simHit : *simhith) { - - const GeomDet *detectorG = globalGeometry->idToDet(simHit.detUnitId()); - - bool isbarrel = detectorG->subDetector() == GeomDetEnumerators::PixelBarrel || detectorG->subDetector() == GeomDetEnumerators::TIB || detectorG->subDetector() == GeomDetEnumerators::TOB; - - float absz = std::abs(detectorG->surface().toGlobal(LocalVector(0.,0.,1.)).z()); - - bool idealdisk = absz == 1.; - bool idealbarrel = absz<1e-9; - - // idealdisk = false; - // idealbarrel = false; - - bool isstereo = trackerTopology->isStereo(simHit.detUnitId()); - - std::cout << "isbarrel: " << isbarrel << " idealbarrel: " << idealbarrel << " idealdisk: " << idealdisk << "stereo: " << isstereo << " globalpos: " << detectorG->surface().position() << std::endl; - - LocalPoint proplocal(0.,0.,0.); - -// auto const propresult = fPropagator->geometricalPropagator().propagateWithPath(fts, detectorG->surface()); -// if (propresult.first.isValid()) { -// proplocal = propresult.first.localPosition(); -// } - - if (!tsos.isValid()) { - tsos = fPropagator->geometricalPropagator().propagate(fts, detectorG->surface()); - } - else { - tsos = fPropagator->geometricalPropagator().propagate(tsos, detectorG->surface()); - } - - if (tsos.isValid()) { - proplocal = tsos.localPosition(); - } - - - - - // Vector3d refprop; - - // LocalTrajectoryParameters - Point3DBase reflocal(0, 0., 0.); - - simtestz = detectorG->surface().position().z(); - simtestrho = detectorG->surface().position().perp(); - - - GlobalPoint refglobal; - - auto const simhitglobal = detectorG->surface().toGlobal(Point3DBase(simHit.localPosition().x(), - simHit.localPosition().y(), - simHit.localPosition().z())); - - const Vector3d Msim(simhitglobal.x(), simhitglobal.y(), simhitglobal.z()); - - auto const propglobal = detectorG->surface().toGlobal(Point3DBase(proplocal.x(), - proplocal.y(), - proplocal.z())); - - - const Vector3d Mprop(propglobal.x(), propglobal.y(), propglobal.z()); - - Vector3d M(genmuon->vertex().x(), - genmuon->vertex().y(), - genmuon->vertex().z()); - - Vector3d P(genmuon->momentum().x(), - genmuon->momentum().y(), - genmuon->momentum().z()); - - - - - // if (true) { - for (unsigned int iref=0; iref<1; ++iref) { - const double zs = detectorG->surface().position().z(); - - const Vector3d T0 = P.normalized(); - - // const Vector3d T0 = P.normalized(); - - const Vector3d H(0.,0.,1.); - - const double rho = fts.transverseCurvature(); - - double s; - - if (idealdisk) { - s = (zs - M[2])/T0[2]; - } - else if (idealbarrel) { - HelixBarrelPlaneCrossingByCircle crossing(GlobalPoint(M[0],M[1],M[2]), GlobalVector(P[0],P[1],P[2]), rho); - s = crossing.pathLength(detectorG->surface()).second; - } - else { - HelixArbitraryPlaneCrossing crossing(Basic3DVector(M[0],M[1],M[2]), Basic3DVector(P[0],P[1],P[2]), rho); - s = crossing.pathLength(detectorG->surface()).second; - // s = propresult.second; - } - - const Vector3d HcrossT = H.cross(T0); - const double alpha = HcrossT.norm(); - const Vector3d N0 = HcrossT.normalized(); - - const double gamma = T0[2]; - const double q = genmuon->charge(); - const double Q = -3.8*2.99792458e-3*q/P.norm(); - const double theta = Q*s; - - const Vector3d dM = gamma*(theta-std::sin(theta))/Q*H + std::sin(theta)/Q*T0 + alpha*(1.-std::cos(theta))/Q*N0; - M = M + dM; - const Vector3d dT = gamma*(1.-std::cos(theta))*H + std::cos(theta)*T0 + alpha*std::sin(theta)*N0; - const Vector3d T = T0 + dT; - const double pmag = P.norm(); - P = pmag*T; - - refglobal = GlobalPoint(M[0], M[1], M[2]); - reflocal = detectorG->surface().toLocal(Point3DBase(M[0], M[1], M[2])); - simtestzlocalref = reflocal.z(); - - const Vector3d xhat = Vector3d(0.,0.,1.).cross(M).normalized(); - -// const LocalVector localxhat = LocalVector(1.,0.,0.); -// const GlobalVector globalxhat = detectorG->surface().toGlobal(localxhat); -// const Vector3d xhat(globalxhat.x(), globalxhat.y(), globalxhat.z()); - - const double dx = xhat.dot(Msim-M); - const double dxrec = xhat.dot(Mprop - Msim); - - const Vector3d yhat = Vector3d(0.,0.,1.).cross(xhat).normalized(); - - const double dy = yhat.dot(Msim-M); - const double dyrec = yhat.dot(Mprop - Msim); - - simtestdx = dx; -// simtestdxrec = dxrec; - simtestdxrec = proplocal.x() - simHit.localPosition().x(); - simtestdxprop = xhat.dot(Mprop-M); - - simtestdy = dy; -// simtestdyrec = dyrec; - simtestdyrec = proplocal.y() - simHit.localPosition().y(); - simtestdyprop = yhat.dot(Mprop-M); - - simtestdetid = detectorG->geographicalId().rawId(); - - if (idealdisk) { - break; - } - - // refprop = M; - - // const Vector3d Mprop(updtsosnomat.globalPosition().x(), - // updtsosnomat.globalPosition().y(), - // updtsosnomat.globalPosition().z()); - } - - tree->Fill(); - // else { - // const TrajectoryStateOnSurface propresult = fAnalyticPropagator->geometricalPropagator().propagate(fts, detectorG->surface()); - // if (propresult.isValid()) { - // reflocal = propresult.localPosition(); - // // refprop << propresult.globalPosition().x(), propresult.globalPosition().y(), propresult.globalPosition().z(); - // } - // // else { - // // refprop << 0., 0., 0.; - // // } - // } - - - // const LocalPoint reflocal = detectorG->surface().toLocal(GlobalPoint(refprop[0], refprop[1], refprop[2])); - - - - const LocalPoint simlocal = simHit.localPosition(); - -// std::cout << "isbarrel: " << isbarrel << " idealbarrel: " << idealbarrel << " idealdisk: " << idealdisk << "stereo: " << isstereo << " globalpos: " << detectorG->surface().position() << std::endl; - std::cout << "detid: " << simHit.detUnitId() << std::endl; - std::cout << "local z to global: " << detectorG->surface().toGlobal(LocalVector(0.,0.,1.)) << std::endl; - std::cout << "ref : " << reflocal << std::endl; - std::cout << "proplocal: " << proplocal << std::endl; - std::cout << "simlocal: " << simlocal << std::endl; - std::cout << "sim entry point: " << simHit.entryPoint() << std::endl; - std::cout << "sim exit point: " << simHit.exitPoint() << std::endl; - std::cout << "refglobal: " << refglobal << std::endl; - std::cout << "propglobal: " << propglobal << std::endl; - std::cout << "simglobal: " << simhitglobal << std::endl; - std::cout << "sim-ref : " << simlocal - reflocal << std::endl; - std::cout << "sim-ref (global): " << simhitglobal - refglobal << std::endl; - std::cout << "prop-ref: " << proplocal - reflocal << std::endl; - std::cout << "sim-prop: " << simlocal - proplocal << std::endl; - std::cout << "sim-prop (global): " << simhitglobal - propglobal << std::endl; - - - std::cout << "simtestdx: " << simtestdx << std::endl; - std::cout << "simtestdy: " << simtestdy << std::endl; - - std::cout << "simtestdxrec: " << simtestdxrec << std::endl; - std::cout << "simtestdyrec: " << simtestdyrec << std::endl; - - std::cout << "simtestdxprop: " << simtestdxprop << std::endl; - std::cout << "simtestdyprop: " << simtestdyprop << std::endl; - -// assert(std::abs(simtestdy)<0.1e-4); - -// assert(simHit.entryPoint().z()*simHit.exitPoint().z() < 0.); - -// assert(std::abs(simlocal.z())<1e-4); - -// assert(std::abs(simtestdxrec) < 40e-4); -// assert(simtestdxprop > -950e-4); - - ++ihit; - - // if (simHit.detUnitId() == preciseHit->geographicalId()) { - // dxsimgen.push_back(simHit.localPosition().x() - updtsos.localPosition().x()); - // dysimgen.push_back(simHit.localPosition().y() - updtsos.localPosition().y()); - // - // dxrecsim.push_back(preciseHit->localPosition().x() - simHit.localPosition().x()); - // dyrecsim.push_back(-99.); - // - // simvalid = true; - // break; - // } -// } - } - - - } - return; - - } - -// TkClonerImpl hitCloner; -// TKCloner const* cloner = static_cast(builder)->cloner() -// TrajectoryStateCombiner combiner; - - run = iEvent.run(); - lumi = iEvent.luminosityBlock(); - event = iEvent.id().event(); - -// for (const reco::Track &track : *trackOrigH) { - for (unsigned int itrack = 0; itrack < trackOrigH->size(); ++itrack) { - const reco::Track &track = (*trackOrigH)[itrack]; - const reco::TrackRef trackref(trackOrigH, itrack); -// const Trajectory& traj = (*trajH)[itraj]; - -// const edm::Ref > trajref(trajH, j); -// const reco::Track& track = *(*trackH)[trajref]; -// const reco::Track& track = (*trackH)[itraj]; -// const reco::Track& trackOrig = (*trackOrigH)[(*indicesH)[j]]; - -// std::cout << "j " << j << " (*indicesH)[j] " << (*indicesH)[j] <(trackParms.data()) = Map(tkparms.Array()).cast(); - Map >(trackCov.data()).triangularView() = Map >(tkcov.Array()).cast().triangularView(); - -// std::cout << "track charge: " << track.charge() << " trackorig charge " << trackOrig.charge() << "inner state charge " << tms.back().updatedState().charge() << std::endl; - - const reco::GenParticle* genpart = nullptr; - - genPt = -99.; - genEta = -99.; - genPhi = -99.; - genCharge = -99; - genX = -99.; - genY = -99.; - genZ = -99.; - genParms.fill(0.); - - int genBarcode = -99; - - - if (doGen_) { -// bool isjpsi = false; -// for (std::vector::const_iterator g = genPartCollection->begin(); g != genPartCollection->end(); ++g) -// { -// if (std::abs(g->pdgId()) == 443) { -// isjpsi = true; -// break; -// } -// } - - float drmin = 0.1; - - for (std::vector::const_iterator g = genPartCollection->begin(); g != genPartCollection->end(); ++g) - { - if (g->status() != 1) { - continue; - } - if (std::abs(g->pdgId()) != 13) { - continue; - } - -// if (isjpsi && g->charge() != track.charge()) { -// continue; -// } - - float dR = deltaR(*g, track); - - if (dR < drmin) - { - drmin = dR; - - genpart = &(*g); - - genBarcode = (*genPartBarcodes)[g - genPartCollection->begin()]; - - genPt = g->pt(); - genEta = g->eta(); - genPhi = g->phi(); - genCharge = g->charge(); - - genX = g->vertex().x(); - genY = g->vertex().y(); - genZ = g->vertex().z(); - - auto const& vtx = g->vertex(); - auto const& myBeamSpot = bsH->position(vtx.z()); - - //q/|p| - genParms[0] = g->charge()/g->p(); - //lambda - genParms[1] = M_PI_2 - g->momentum().theta(); - //phi - genParms[2] = g->phi(); - //dxy - genParms[3] = (-(vtx.x() - myBeamSpot.x()) * g->py() + (vtx.y() - myBeamSpot.y()) * g->px()) / g->pt(); - //dsz - genParms[4] = (vtx.z() - myBeamSpot.z()) * g->pt() / g->p() - - ((vtx.x() - myBeamSpot.x()) * g->px() + (vtx.y() - myBeamSpot.y()) * g->py()) / g->pt() * g->pz() / g->p(); - } - else { - continue; - } - } - } - - int simtrackid = -99; - if (genpart != nullptr && doSim_) { - for (auto const& simTrack : *simTracks) { - if (simTrack.genpartIndex() == genBarcode) { - simtrackid = simTrack.trackId(); - break; - } - } - } - - - muonPt = -99.; - muonLoose = false; - muonMedium = false; - muonTight = false; - muonIsTracker = false; - muonIsGlobal = false; - muonIsStandalone = false; - muonInnerTrackBest = false; - trackExtraAssoc = false; - - if (doMuons_) { - for (auto const &muon : *muons) { - if (muon.innerTrack() == trackref) { - muonPt = muon.pt(); - muonLoose = muon.passed(reco::Muon::CutBasedIdLoose); - muonMedium = muon.passed(reco::Muon::CutBasedIdMedium); - muonTight = muon.passed(reco::Muon::CutBasedIdTight); - muonIsPF = muon.isPFMuon(); - muonIsTracker = muon.isTrackerMuon(); - muonIsGlobal = muon.isGlobalMuon(); - muonIsStandalone = muon.isStandAloneMuon(); - if (muon.muonBestTrack() == trackref) { - muonInnerTrackBest = true; - } - } - } - if (assoc->contains(track.extra().id())) { - const reco::TrackExtraRef &trackextraref = (*assoc)[track.extra()]; - if (trackextraref.isNonnull()) { - trackExtraAssoc = true; - } - } - } - - - -// PropagationDirection rpropdir = traj.direction(); -// PropagationDirection fpropdir = rpropdir == alongMomentum ? oppositeToMomentum : alongMomentum; - - //TODO properly handle the outside-in case - assert(track.seedDirection() == alongMomentum); - - //prepare hits - TransientTrackingRecHit::RecHitContainer hits; - hits.reserve(track.recHitsSize()); -// hits.reserve(track.recHitsSize()+1); -// hits.push_back(RecHitPointer(InvalidTrackingRecHit()); - -// printf("building hits\n"); - - std::set> hitlayers; - - for (auto it = track.recHitsBegin(); it != track.recHitsEnd(); ++it) { - const GeomDet* detectorG = globalGeometry->idToDet((*it)->geographicalId()); - const GluedGeomDet* detglued = dynamic_cast(detectorG); - - // split matched invalid hits - if (detglued != nullptr && !(*it)->isValid()) { - bool order = detglued->stereoDet()->surface().position().mag() > detglued->monoDet()->surface().position().mag(); - const GeomDetUnit* detinner = order ? detglued->monoDet() : detglued->stereoDet(); - const GeomDetUnit* detouter = order ? detglued->stereoDet() : detglued->monoDet(); - - hits.push_back(TrackingRecHit::RecHitPointer(new InvalidTrackingRecHit(*detinner, (*it)->type()))); - hits.push_back(TrackingRecHit::RecHitPointer(new InvalidTrackingRecHit(*detouter, (*it)->type()))); - -// auto const& layerinner = detidlayermap.at(detinner->geographicalId()); -// auto const& layerouter = detidlayermap.at(detouter->geographicalId()); - -// if (!hitlayers.count(layerinner)) -// hits.push_back(TrackingRecHit::RecHitPointer(new InvalidTrackingRecHit(*detinner, (*it)->type()))); -// hitlayers.insert(layerinner); -// if (!hitlayers.count(layerouter)) -// hits.push_back(TrackingRecHit::RecHitPointer(new InvalidTrackingRecHit(*detouter, (*it)->type()))); -// hitlayers.insert(layerouter); - } - else { - // apply hit quality criteria - const bool ispixel = GeomDetEnumerators::isTrackerPixel(detectorG->subDetector()); -// bool hitquality = true; - bool hitquality = false; - if (applyHitQuality_ && (*it)->isValid()) { - const TrackerSingleRecHit* tkhit = dynamic_cast(*it); - assert(tkhit != nullptr); - - if (ispixel) { - const SiPixelRecHit *pixhit = dynamic_cast(tkhit); - const SiPixelCluster& cluster = *tkhit->cluster_pixel(); - assert(pixhit != nullptr); - - hitquality = !pixhit->isOnEdge() && cluster.sizeX() > 1; -// hitquality = !pixhit->isOnEdge(); -// hitquality = cluster.sizeX() > 1; - -// hitquality = pixhit->hasFilledProb() && pixhit->clusterProbability(0) > 0.000125 && pixhit->qBin()>0 && pixhit->qBin()<4; - -// if (pixhit->hasFilledProb() && pixhit->clusterProbability(0) > 0.000125 && pixhit->qBin()>0 && pixhit->qBin()<4) { -// if (pixhit->hasFilledProb() && pixhit->clusterProbability(0) > 0.000125 && pixhit->qBin()>0 && pixhit->qBin()<3) { -// hitquality = true; -// } - } - else { - assert(tkhit->cluster_strip().isNonnull()); - const SiStripCluster& cluster = *tkhit->cluster_strip(); - const StripTopology* striptopology = dynamic_cast(&(detectorG->topology())); - assert(striptopology); - - const uint16_t firstStrip = cluster.firstStrip(); - const uint16_t lastStrip = cluster.firstStrip() + cluster.amplitudes().size() - 1; - const bool isOnEdge = firstStrip == 0 || lastStrip == (striptopology->nstrips() - 1); - -// if (isOnEdge) { -// std::cout << "strip hit isOnEdge" << std::endl; -// } - -// hitquality = !isOnEdge; - hitquality = true; - - - - -// SiStripClusterInfo clusterInfo = SiStripClusterInfo(cluster, iSetup, (*it)->geographicalId().rawId()); -// if (clusterInfo.signalOverNoise() > 12.) { -// hitquality = true; -// } -// hitquality = true; - } - - } - else { - hitquality = true; - } - -// std::cout << "detid: " << (*it)->geographicalId().rawId() << std::endl; - -// bool foundsim = false; -// for (auto const& simhith : simHits) { -// for (const PSimHit& simHit : *simhith) { -// if (std::abs(simHit.particleType()) == 13 && simHit.detUnitId() == (*it)->geographicalId()) { -// foundsim = true; -// break; -// } -// } -// if (foundsim) { -// break; -// } -// } -// -// if (!foundsim) { -// hitquality = false; -// } - -// for (auto const& simhith : simHits) { -// for (const PSimHit& simHit : *simhith) { -// if (simHit.detUnitId() == (*it)->geographicalId()) { -// // std::cout << "particle type: " << simHit.particleType() << std::endl; -// // std::cout << "track id: " << simHit.trackId() << std::endl; -// // std::cout << "entry point: " << simHit.entryPoint() << std::endl; -// // std::cout << "exit point: " << simHit.exitPoint() << std::endl; -// // std::cout << "local position: " << simHit.localPosition() << std::endl; -// if (std::abs(simHit.particleType()) == 13 && std::abs(simHit.localPosition().z()) > 1e-4) { -// std::cout << "anomalous simhit!" << std::endl; -// hitquality = false; -// } -// } -// } -// } - - - if (hitquality) { - hits.push_back((*it)->cloneForFit(*detectorG)); - } - else { - hits.push_back(TrackingRecHit::RecHitPointer(new InvalidTrackingRecHit(*detectorG, TrackingRecHit::inactive))); - } - - -// auto const &layer = detidlayermap.at((*it)->geographicalId()); -// -// if (!hitlayers.count(layer)) { -// if (hitquality) { -// hits.push_back((*it)->cloneForFit(*detectorG)); -// } -// else { -// hits.push_back(TrackingRecHit::RecHitPointer(new InvalidTrackingRecHit(*detectorG, TrackingRecHit::inactive))); -// } -// } -// hitlayers.insert(layer); - } - } - -// printf("done building hits\n"); - -// const unsigned int nhits = track.recHitsSize(); - const unsigned int nhits = hits.size(); - nHits = nhits; -// unsigned int npixhits = 0; - - unsigned int nvalid = 0; - unsigned int nvalidpixel = 0; - unsigned int nvalidalign2d = 0; - -// std::set> hitlayers; - - // count valid hits since this is needed to size the arrays - for (auto const& hit : hits) { -// auto const &layers = detidlayermap.at(hit->geographicalId()); -// if (hitlayers.count(layers)) { -// std::cout << "WARNING: multiple hits on the same layer!!!" << std::endl; -// std::cout << layers[0] << " " << layers[1] << " " << layers[2] << std::endl; -// } -// hitlayers.insert(layers); - - assert(hit->dimension()<=2); - if (hit->isValid()) { - nvalid += 1; - -// const uint32_t gluedid = trackerTopology->glued(hit->geographicalId()); -// const bool isglued = gluedid != 0; -// const DetId parmdetid = isglued ? DetId(gluedid) : hit->geographicalId(); -// const bool align2d = detidparms.count(std::make_pair(1, parmdetid)); -// const bool align2d = detidparms.count(std::make_pair(2, hit->geographicalId())); - - const bool align2d = detidparms.count(std::make_pair(1, hit->geographicalId())); -// - if (align2d) { - nvalidalign2d += 1; - } - if (GeomDetEnumerators::isTrackerPixel(hit->det()->subDetector())) { - nvalidpixel += 1; - } - } - } - -// //count valid hits since this is needed to size the arrays -// auto const& hitsbegin = track.recHitsBegin(); -// for (unsigned int ihit = 0; ihit < track.recHitsSize(); ++ihit) { -// auto const& hit = *(hitsbegin + ihit); -// if (hit->isValid() && hit->dimension()<=2) { -// nvalid += 1; -// -// const GeomDet *detectorG = globalGeometry->idToDet(hit->geographicalId()); -// if (hit->dimension()==2 && GeomDetEnumerators::isTrackerPixel(detectorG->subDetector())) { -// // if (hit->dimension()==2) { -// nvalidpixel += 1; -// } -// } -// } - - nValidHits = nvalid; - nValidPixelHits = nvalidpixel; - - nValidHitsFinal = 0; - nValidPixelHitsFinal = 0; - -// const unsigned int nstriphits = nhits-npixhits; -// const unsigned int nparsAlignment = nstriphits + 2*npixhits; -// const unsigned int nvalidstrip = nvalid - nvalidpixel; -// const unsigned int nparsAlignment = nvalidstrip + 2*nvalidpixel; - const unsigned int nparsAlignment = 2*nvalid + nvalidalign2d; -// const unsigned int nparsAlignment = 6*nvalid; - const unsigned int nparsBfield = nhits; - const unsigned int nparsEloss = nhits - 1; - const unsigned int npars = nparsAlignment + nparsBfield + nparsEloss; - -// const unsigned int nstateparms = 5*(nhits+1); - const unsigned int nstateparms = 3*(nhits+1) - 1; -// const unsigned int nstateparms = 3*nhits - 1; - const unsigned int nparmsfull = nstateparms + npars; - - - const unsigned int nstateparmspost = 5*(nhits+1); - -// std::cout << "nhits " << nhits << std::endl; -// std::cout << "nstateparms " << nstateparms << std::endl; -// std::cout << "nparmsfull " << nparmsfull << std::endl; -// std::cout << "nparmsfull " << nparmsfull << std::endl; -// std::cout << "nparmsfull " << nparmsfull << std::endl; -// std::cout << "nparmsfull " << nparmsfull << std::endl; -// std::cout << "nparmsfull " << nparmsfull << std::endl; - -// const unsigned int npropparms = 5*(nhits-1); -// const unsigned int nhitparms = 2*nhits; -// const unsigned int nmomparms = 3*(nhits-1); -// const unsigned int nposparms = 2*(nhits-1); -// constexpr unsigned int nrefparms = 5; - - - - //active double for autodiff gradients -// using Adouble = AutoDiffScalar; -// using AVectorXd = Matrix; -// //double double for autodiff hessians -// using AAdouble = AutoDiffScalar; - - - -// using AAXd = AANT; -// using AAdouble = AAXd; -// -// using AA2d = AANT; -// using AA3d = AANT; -// using AA4d = AANT; -// using AA12d = AANT; -// -// using ScalarConst = AANT; - -// using AConstd = AutoDiffScalar; -// using AConstd = AutoDiffScalar>; - - -// using VectorXAd = Matrix; -// using MatrixXAd = Matrix; - - //two position parameters and and one alignment parameter - using StripHitScalar = AANT;; - - using StripHit1DJacobian = Matrix; - - using StripHitVector = Matrix; - using StripHit2DCovariance = Matrix; - using StripHit2DJacobian = Matrix; - - - - //two hit dimensions and two alignment parameters - using PixelHit2DScalar = AANT; - using PixelHit2DVector = Matrix; - using PixelHit2DCovariance = Matrix; - using PixelHit2DJacobian = Matrix; - - - //2x5 state parameters, one bfield parameter, and one material parameter -// using MSScalar = AANT;; - using MSScalar = AANT; - using MSVector = Matrix; - using MSProjection = Matrix; - using MSJacobian = Matrix; - using MSCovariance = Matrix; - - using BSScalar = AANT; - -// using HitProjection = Matrix; -// using HitCovariance = Matrix; -// using HitVector = Matrix; - -// evector Vinv(nhits, HitCovarianceMatrix::Zero()); -// evector Hh(nhits, HitProjection::Zero()); -// evector dy0(nhits, HitVector::Zero()); -// evector dx(nhits, StateVector::Zero()); -// //initialize backpropagation indexing -// for (unsigned int i=0; i rxfull(nvalid, 2); - Matrix ryfull(nvalid, 2); - -// VectorXd gradfull = chisq.value().derivatives(); -// MatrixXd hessfull = MatrixXd::Zero(nparmsfull, nparmsfull); -// for (unsigned int i=0; iSetBranchAddress("globalidxv", globalidxv.data()); - } - -// TrajectoryStateOnSurface currtsos; - - - - VectorXd dxfull; - MatrixXd dxdparms; - VectorXd grad; - MatrixXd hess; - LDLT Cinvd; - - if (dogen && genpart==nullptr) { - continue; - } - -// if (dogen && genpart->eta()>-2.3) { -// continue; -// } - -// if (genpart==nullptr) { -// continue; -// } -// if (genpart->pt()>10.) { -// continue; -// } -// if (genpart->pt()<100.) { -// continue; -// } -// if (genpart->eta()>-2.3) { -// continue; -// } - - if (debugprintout_) { - std::cout << "initial reference point parameters:" << std::endl; - std::cout << track.parameters() << std::endl; - } - -// //prepare hits -// TransientTrackingRecHit::RecHitContainer hits; -// hits.reserve(track.recHitsSize()); -// for (auto it = track.recHitsBegin(); it != track.recHitsEnd(); ++it) { -// const GeomDet *detectorG = globalGeometry->idToDet((*it)->geographicalId()); -// hits.push_back((*it)->cloneForFit(*detectorG)); -// } - -// // fix mixed up clusters? -// for (unsigned int ihit=0; ihit<(hits.size()-1); ++ihit) { -// TrackerSingleRecHit* hit = const_cast(dynamic_cast(hits[ihit].get())); -// TrackerSingleRecHit* nexthit = const_cast(dynamic_cast(hits[ihit+1].get())); -// -// // const TrackingRecHitSingle* nexthit = hits[ihit+1]; -// -// if (!hit || !nexthit) { -// continue; -// } -// -// const DetId partnerid = trackerTopology->partnerDetId(hit->geographicalId()); -// // -// if (partnerid == nexthit->geographicalId()) { -// // std::cout << "swapping clusters" << std::endl; -// const OmniClusterRef::ClusterStripRef cluster = hit->cluster_strip(); -// const OmniClusterRef::ClusterStripRef nextcluster = nexthit->cluster_strip(); -// -// hit->setClusterStripRef(nextcluster); -// nexthit->setClusterStripRef(cluster); -// } -// -// } - -// if (genpart==nullptr) { -// continue; -// } -// -// if (genpart->eta()<-2.4 || genpart->eta()>-2.3) { -// continue; -// } - - - FreeTrajectoryState refFts; - - if (dogen) { -// if (true) { - - if (genpart==nullptr) { - continue; - } - //init from gen state - auto const& refpoint = genpart->vertex(); - auto const& trackmom = genpart->momentum(); - const GlobalPoint refpos(refpoint.x(), refpoint.y(), refpoint.z()); - const GlobalVector refmom(trackmom.x(), trackmom.y(), trackmom.z()); - const GlobalTrajectoryParameters refglobal(refpos, refmom, genpart->charge(), field); - -// std::cout << "gen ref state" << std::endl; -// std::cout << refpos << std::endl; -// std::cout << refmom << std::endl; -// std::cout << genpart->charge() << std::endl; - - //zero uncertainty on generated parameters -// AlgebraicSymMatrix55 nullerr; -// const CurvilinearTrajectoryError referr(nullerr); - - refFts = FreeTrajectoryState(refpos, refmom, genpart->charge(), field); -// refFts = FreeTrajectoryState(refglobal, referr); - } - else { - //init from track state - auto const& refpoint = track.referencePoint(); - auto const& trackmom = track.momentum(); - const GlobalPoint refpos(refpoint.x(), refpoint.y(), refpoint.z()); - const GlobalVector refmom(trackmom.x(), trackmom.y(), trackmom.z()); -// const GlobalTrajectoryParameters refglobal(refpos, refmom, track.charge(), field); -// const CurvilinearTrajectoryError referr(track.covariance()); - - //null uncertainty (tracking process noise sum only) -// AlgebraicSymMatrix55 nullerr; -// const CurvilinearTrajectoryError referr(nullerr); - - refFts = FreeTrajectoryState(refpos, refmom, track.charge(), field); -// refFts = FreeTrajectoryState(refglobal, referr); - } - -// std::vector> layerStates; - std::vector layerStates; - layerStates.reserve(nhits); - - bool valid = true; - - -// //do propagation and prepare states -// auto propresult = fPropagator->propagateWithPath(refFts, *hits.front()->surface()); -// if (!propresult.first.isValid()) { -// std::cout << "Abort: Propagation from reference point failed" << std::endl; -// continue; -// } -// layerStates.push_back(propresult); -// -// for (auto const& hit : hits) { -// propresult = fPropagator->propagateWithPath(layerStates.back().first, *hit->surface()); -// if (!propresult.first.isValid()) { -// std::cout << "Abort: Propagation failed" << std::endl; -// valid = false; -// break; -// } -// layerStates.push_back(propresult); -// } -// -// if (!valid) { -// continue; -// } - - - - //inflate errors -// refFts.rescaleError(100.); - - -// unsigned int ntotalhitdim = 0; -// unsigned int alignmentidx = 0; -// unsigned int bfieldidx = 0; -// unsigned int elossidx = 0; - -// constexpr unsigned int niters = 1; - constexpr unsigned int niters = 10; - - for (unsigned int iiter=0; iiter 0; -// const bool islikelihood = true; - - gradfull = VectorXd::Zero(nparmsfull); - hessfull = MatrixXd::Zero(nparmsfull, nparmsfull); - statejac = MatrixXd::Zero(nstateparmspost, nparmsfull); - - validdxeigjac = MatrixXd::Zero(2*nvalid, nstateparms); - - evector, 11> > dhessv; - if (islikelihood) { - dhessv.resize(nhits-1); - } - - double chisq0val = 0.; - - dxpxb1 = -99.; - dypxb1 = -99.; - dxttec9rphi = -99.; - dxttec9stereo = -99.; - dxttec4rphi = -99.; - dxttec4stereo = -99.; - - dxttec4rphisimgen = -99.; - dyttec4rphisimgen = -99.; - dxttec4rphirecsim = -99.; - - dxttec9rphisimgen = -99.; - dyttec9rphisimgen = -99.; - - simlocalxref = -99.; - simlocalyref = -99.; - - - - const uint32_t gluedid0 = trackerTopology->glued(hits[0]->det()->geographicalId()); - const bool isglued0 = gluedid0 != 0; - const DetId parmdetid0 = isglued0 ? DetId(gluedid0) : hits[0]->geographicalId(); - const unsigned int bfieldidx = detidparms.at(std::make_pair(6, parmdetid0)); - fieldOffset->setOffset(corparms_[bfieldidx]); - -// dhessv.reserve(nhits-1); - - unsigned int parmidx = 0; - unsigned int alignmentparmidx = 0; - unsigned int ivalidhit = 0; - - if (iiter > 0) { - //update current state from reference point state (errors not needed beyond first iteration) - JacobianCurvilinearToCartesian curv2cart(refFts.parameters()); - const AlgebraicMatrix65& jac = curv2cart.jacobian(); - const AlgebraicVector6 glob = refFts.parameters().vector(); - - auto const& dxlocal = dxstate.head<5>(); - const Matrix globupd = Map>(glob.Array()) + Map>(jac.Array())*dxlocal; - -// const Vector6d dglob = Map>(jac.Array())*dxlocal; -// -// std::cout << "iiter = " << iiter << std::endl; -// std::cout << "dxlocal " << dxlocal << std::endl; -// std::cout << "glob " << glob << std::endl; -// std::cout << "dglob " << dglob << std::endl; - - const GlobalPoint pos(globupd[0], globupd[1], globupd[2]); - const GlobalVector mom(globupd[3], globupd[4], globupd[5]); - const double charge = std::copysign(1., refFts.charge()/refFts.momentum().mag() + dxlocal[0]); -// std::cout << "before update: reffts:" << std::endl; -// std::cout << refFts.parameters().vector() << std::endl; -// std::cout << "charge " << refFts.charge() << std::endl; - refFts = FreeTrajectoryState(pos, mom, charge, field); -// std::cout << "after update: reffts:" << std::endl; -// std::cout << refFts.parameters().vector() << std::endl; -// std::cout << "charge " << refFts.charge() << std::endl; -// currentFts = refFts; - } - -// Matrix5d Hlm = Matrix5d::Identity(); -// currentFts = refFts; -// TrajectoryStateOnSurface currentTsos; - -// ; - -// auto const &surface0orig = *hits[0]->surface(); - auto const &surface0 = *surfacemap_.at(hits[0]->geographicalId()); -// printf("detid = %u, parmdetid = %u, old = %p, new = %p\n", hits[0]->geographicalId().rawId(), parmdetid0.rawId(), &surface0orig, &surface0); -// std::cout << "old xi = " << surface0orig.mediumProperties().xi() << " new xi = " << surface0.mediumProperties().xi() << " dxi = " << surface0.mediumProperties().xi() - surface0orig.mediumProperties().xi() << std::endl; - auto propresult = fPropagator->geometricalPropagator().propagateWithPath(refFts, surface0); -// auto propresult = fPropagator->geometricalPropagator().propagateWithPath(refFts, *hits[0]->surface()); -// auto propresult = fPropagator->geometricalPropagator().propagateWithPath(refFts, *beampipe); - if (!propresult.first.isValid()) { - std::cout << "Abort: Propagation of reference state Failed!" << std::endl; - valid = false; - break; - } - -// std::cout << "position on beampipe " << propresult.first.globalParameters().position() << std::endl; - - const Matrix FdFp = curv2localTransportJacobian(refFts, propresult, false); - - Matrix J = FdFp.block<2, 2>(3, 3); - // (du/dalphap)^-1 - Matrix Sinv = FdFp.block<2, 2>(3, 1).inverse(); - // du/dqopp - Matrix D = FdFp.block<2, 1>(3, 0); - // du/dBp - Matrix Bpref = FdFp.block<2, 1>(3, 5); - - constexpr unsigned int jacstateidxout = 0; - constexpr unsigned int jacstateidxin = 0; - - // qop_i - statejac(jacstateidxout, jacstateidxin + 2) = 1.; - // d(lambda, phi)_i/dqop_i - statejac.block<2, 1>(jacstateidxout + 1, jacstateidxin + 2) = -Sinv*D; - // d(lambda, phi)_i/(dxy, dsz) - statejac.block<2, 2>(jacstateidxout + 1, jacstateidxin) = -Sinv*J; - // d(lambda, phi)_i/du_(i+1) - statejac.block<2, 2>(jacstateidxout + 1, jacstateidxin + 3) = Sinv; - // d(lambda, phi) / dbeta - statejac.block<2, 1>(jacstateidxout + 1, nstateparms + parmidx) = -Sinv*Bpref; - // dxy - statejac(jacstateidxout + 3, jacstateidxin) = 1.; - // dsz - statejac(jacstateidxout + 4, jacstateidxin + 1) = 1.; - - if (bsConstraint_) { - // apply beamspot constraint - // TODO add residual corrections for beamspot parameters? - - constexpr unsigned int nlocalstate = 2; - constexpr unsigned int nlocalbs = 0; - constexpr unsigned int nlocalparms = nlocalbs; - - constexpr unsigned int nlocal = nlocalstate + nlocalparms; - - constexpr unsigned int localstateidx = 0; - constexpr unsigned int localparmidx = localstateidx + nlocalstate; - - constexpr unsigned int fullstateidx = 0; - const unsigned int fullparmidx = nstateparms + parmidx; - - JacobianCurvilinearToCartesian curv2cart(refFts.parameters()); - const AlgebraicMatrix65& jac = curv2cart.jacobian(); - - const double sigb1 = bsH->BeamWidthX(); - const double sigb2 = bsH->BeamWidthY(); - const double sigb3 = bsH->sigmaZ(); - const double dxdz = bsH->dxdz(); - const double dydz = bsH->dydz(); - const double x0 = bsH->x0(); - const double y0 = bsH->y0(); - const double z0 = bsH->z0(); - - - // covariance matrix of luminous region in global coordinates - // taken from https://github.com/cms-sw/cmssw/blob/abc1f17b230effd629c9565fb5d95e527abcb294/RecoVertex/BeamSpotProducer/src/FcnBeamSpotFitPV.cc#L63-L90 - - // FIXME xy correlation is not stored and assumed to be zero - const double corrb12 = 0.; - - const double varb1 = sigb1*sigb1; - const double varb2 = sigb2*sigb2; - const double varb3 = sigb3*sigb3; - - Matrix covBS = Matrix::Zero(); - // parametrisation: rotation (dx/dz, dy/dz); covxy - covBS(0,0) = varb1; - covBS(1,0) = covBS(0,1) = corrb12*sigb1*sigb2; - covBS(1,1) = varb2; - covBS(2,0) = covBS(0,2) = dxdz*(varb3-varb1)-dydz*covBS(1,0); - covBS(2,1) = covBS(1,2) = dydz*(varb3-varb2)-dxdz*covBS(1,0); - covBS(2,2) = varb3; - -// std::cout << "covBS:" << std::endl; -// std::cout << covBS << std::endl; - - Matrix du = Matrix::Zero(); - for (unsigned int j=0; j dbs0; - dbs0[0] = BSScalar(refFts.position().x() - x0); - dbs0[1] = BSScalar(refFts.position().y() - y0); - dbs0[2] = BSScalar(refFts.position().z() - z0); - -// std::cout << "dposition / d(qop, lambda, phi) (should be 0?):" << std::endl; -// std::cout << Map>(jac.Array()).topLeftCorner<3,3>() << std::endl; - - const Matrix jacpos = Map>(jac.Array()).topRightCorner<3,2>().cast(); - const Matrix covBSinv = covBS.inverse().cast(); - - const Matrix dbs = dbs0 + jacpos*du; - const BSScalar chisq = dbs.transpose()*covBSinv*dbs; - - chisq0val += chisq.value().value(); - - auto const& gradlocal = chisq.value().derivatives(); - //fill local hessian - Matrix hesslocal; - for (unsigned int j=0; j(fullstateidx) += gradlocal.head(); - gradfull.segment(fullparmidx) += gradlocal.segment(localparmidx); - - //fill global hessian (upper triangular blocks only) - hessfull.block(fullstateidx, fullstateidx) += hesslocal.topLeftCorner(); - hessfull.block(fullstateidx, fullparmidx) += hesslocal.topRightCorner(); - hessfull.block(fullparmidx, fullparmidx) += hesslocal.bottomRightCorner(); - - - } - - - Matrix FdFm = curv2localTransportJacobian(refFts, propresult, true); - - for (unsigned int ihit = 0; ihit < hits.size(); ++ihit) { -// std::cout << "ihit " << ihit << std::endl; - auto const& hit = hits[ihit]; - - const uint32_t gluedid = trackerTopology->glued(hit->det()->geographicalId()); - const bool isglued = gluedid != 0; - const DetId parmdetid = isglued ? DetId(gluedid) : hit->geographicalId(); - const GeomDet* parmDet = isglued ? globalGeometry->idToDet(parmdetid) : hit->det(); - const double xifraction = isglued ? hit->det()->surface().mediumProperties().xi()/parmDet->surface().mediumProperties().xi() : 1.; - -// const DetId partnerid = isglued ? trackerTopology->partnerDetId(hit->det()->geographicalId()) : DetId(); -// -// const bool isfront = ihit != (hits.size() - 1) && isglued && hits[ihit+1]->det()->geographicalId() == partnerid; -// const bool isback = ihit !=0 && isglued && hits[ihit-1]->det()->geographicalId() == partnerid; -// -// const double xifraction = isfront ? 0. : 1.; - - TrajectoryStateOnSurface updtsos = propresult.first; - - //apply measurement update if applicable -// std::cout << "constructing preciseHit" << std::endl; - auto const& preciseHit = hit->isValid() ? cloner.makeShared(hit, updtsos) : hit; - if (hit->isValid() && !preciseHit->isValid()) { - std::cout << "Abort: Failed updating hit" << std::endl; - valid = false; - break; - } - -// const uint32_t gluedid = trackerTopology->glued(preciseHit->det()->geographicalId()); -// const bool isglued = gluedid != 0; -// const DetId parmdetid = isglued ? DetId(gluedid) : preciseHit->geographicalId(); -// const bool align2d = detidparms.count(std::make_pair(1, parmdetid)); -// const GeomDet* parmDet = isglued ? globalGeometry->idToDet(parmdetid) : preciseHit->det(); - - const bool align2d = detidparms.count(std::make_pair(1, preciseHit->geographicalId())); - - - // compute convolution correction in local coordinates (BEFORE material effects are applied) -// const Matrix dxlocalconv = localPositionConvolution(updtsos); - - // curvilinear to local jacobian - JacobianCurvilinearToLocal curv2localm(updtsos.surface(), updtsos.localParameters(), *updtsos.magneticField()); - const AlgebraicMatrix55& curv2localjacm = curv2localm.jacobian(); -// const Matrix Hm = Map>(curv2localjacm.Array()); - - //energy loss jacobian - const Matrix EdE = materialEffectsJacobian(updtsos, fPropagator->materialEffectsUpdator()); -// const Matrix EdE = materialEffectsJacobianVar(updtsos, fPropagator->materialEffectsUpdator()); - -// const Matrix EdEVar = materialEffectsJacobianVar(updtsos, fPropagator->materialEffectsUpdator()); -// -// std::cout << "EdE:" << std::endl; -// std::cout << EdE << std::endl; -// std::cout << "EdEVar:" << std::endl; -// std::cout << EdEVar << std::endl; - - //process noise jacobians - const std::array, 5> dQs = processNoiseJacobians(updtsos, fPropagator->materialEffectsUpdator()); - - //TODO update code to allow doing this in one step with nominal update - //temporary tsos to extract process noise without loss of precision - TrajectoryStateOnSurface tmptsos(updtsos); - tmptsos.update(tmptsos.localParameters(), - LocalTrajectoryError(0.,0.,0.,0.,0.), - tmptsos.surface(), - tmptsos.magneticField(), - tmptsos.surfaceSide()); - - //apply the state update from the material effects - bool ok = fPropagator->materialEffectsUpdator().updateStateInPlace(tmptsos, alongMomentum); - if (!ok) { - std::cout << "Abort: material update failed" << std::endl; - valid = false; - break; - } - - const AlgebraicVector5 dxeloss = tmptsos.localParameters().vector() - updtsos.localParameters().vector(); - - // compute convolution effects -// const AlgebraicVector5 dlocalconv = localMSConvolution(updtsos, fPropagator->materialEffectsUpdator()); - -// const GlobalPoint updtsospos = updtsos.globalParameters().position(); -// std::cout << "before material update: " << updtsos.globalParameters().position() << " " << updtsos.globalParameters().momentum() << std::endl; - ok = fPropagator->materialEffectsUpdator().updateStateInPlace(updtsos, alongMomentum); - if (!ok) { - std::cout << "Abort: material update failed" << std::endl; - valid = false; - break; - } -// std::cout << "after material update: " << updtsos.globalParameters().position() << " " << updtsos.globalParameters().momentum() << std::endl; - - -// std::cout << "local parameters" << std::endl; -// std::cout << updtsos.localParameters().vector() << std::endl; -// std::cout << "dlocalconv" << std::endl; -// std::cout << dlocalconv << std::endl; -// -// // apply convolution effects -// const LocalTrajectoryParameters localupd(updtsos.localParameters().vector() + dlocalconv, -// updtsos.localParameters().pzSign()); -// updtsos.update(localupd, -// // updtsos.localError(), -// updtsos.surface(), -// updtsos.magneticField(), -// updtsos.surfaceSide()); - - //get the process noise matrix - AlgebraicMatrix55 const Qmat = tmptsos.localError().matrix(); - const Map>Q(Qmat.Array()); - std::cout<< "Q" << std::endl; - std::cout<< Q << std::endl; - - - // update state from previous iteration - //momentum kink residual - AlgebraicVector5 idx0(0., 0., 0., 0., 0.); - if (iiter==0) { - layerStates.push_back(updtsos); - } - else { - //current state from previous state on this layer - //save current parameters - TrajectoryStateOnSurface& oldtsos = layerStates[ihit]; - -// JacobianCurvilinearToLocal curv2localold(oldtsos.surface(), oldtsos.localParameters(), *oldtsos.magneticField()); -// const AlgebraicMatrix55& curv2localjacold = curv2localold.jacobian(); -// const Matrix Hold = Map>(curv2localjacold.Array()); - - const AlgebraicVector5 local = oldtsos.localParameters().vector(); - auto const& dxlocal = dxstate.segment<5>(5*(ihit+1)); - const Matrix localupd = Map>(local.Array()) + dxlocal; - AlgebraicVector5 localvecupd(localupd[0],localupd[1],localupd[2],localupd[3],localupd[4]); - - idx0 = localvecupd - updtsos.localParameters().vector(); - - const LocalTrajectoryParameters localparms(localvecupd, oldtsos.localParameters().pzSign()); - -// std::cout << "before update: oldtsos:" << std::endl; -// std::cout << oldtsos.localParameters().vector() << std::endl; - oldtsos.update(localparms, oldtsos.surface(), field, oldtsos.surfaceSide()); -// std::cout << "after update: oldtsos:" << std::endl; -// std::cout << oldtsos.localParameters().vector() << std::endl; - updtsos = oldtsos; - - } - - // curvilinear to local jacobian - JacobianCurvilinearToLocal curv2localp(updtsos.surface(), updtsos.localParameters(), *updtsos.magneticField()); - const AlgebraicMatrix55& curv2localjacp = curv2localp.jacobian(); - const Matrix Hp = Map>(curv2localjacp.Array()); - - - //FIXME take care of this elsewhere for the moment - const bool genconstraint = dogen && ihit==0; -// const bool genconstraint = false; - - if (ihit < (nhits-1)) { - - //momentum kink residual - const Vector5d dx0 = Map(idx0.Array()); - - - const uint32_t gluedidip1 = trackerTopology->glued(hits[ihit + 1]->det()->geographicalId()); - const bool isgluedip1 = gluedidip1 != 0; - const DetId parmdetidip1 = isgluedip1 ? DetId(gluedidip1) : hits[ihit + 1]->geographicalId(); - const unsigned int bfieldidx = detidparms.at(std::make_pair(6, parmdetidip1)); - fieldOffset->setOffset(corparms_[bfieldidx]); - -// if (ihit==0) { -// FreeTrajectoryState tmpfts(updtsospos, updtsos.globalParameters().momentum(), updtsos.charge(), field); -// propresult = fPropagator->geometricalPropagator().propagateWithPath(tmpfts, *hits[ihit+1]->surface()); -// } -// else { -// propresult = fPropagator->geometricalPropagator().propagateWithPath(updtsos, *hits[ihit+1]->surface()); -// } - - -// auto const &surfaceip1 = *hits[ihit+1]->surface(); - auto const &surfaceip1 = *surfacemap_.at(hits[ihit+1]->geographicalId()); - propresult = fPropagator->geometricalPropagator().propagateWithPath(updtsos, surfaceip1); -// propresult = fPropagator->geometricalPropagator().propagateWithPath(updtsos, *hits[ihit+1]->surface()); - if (!propresult.first.isValid()) { - std::cout << "Abort: Propagation Failed!" << std::endl; - valid = false; - break; - } - -// auto const propresultalt = thePropagator->propagateWithPath(updtsos, surfaceip1); -// -// if (propresultalt.first.isValid()) { -// std::cout << "nominal" << std::endl; -// // std::cout << propresult.first.localPosition() << std::endl; -// std::cout << propresult.first.globalMomentum().mag() << std::endl; -// std::cout << "g4e" << std::endl; -// // std::cout << propresultalt.first.isValid() << std::endl; -// std::cout << propresultalt.first.globalMomentum().mag() << std::endl; -// } - - - if (true) { -// if (false) { - //forward propagation jacobian (local to local) - const Matrix FdFp = localTransportJacobian(updtsos, propresult, false); - -// const Matrix FdFpAlt = localTransportJacobianAlt(updtsos, propresult, false); -// -// std::cout << "FdFp" << std::endl; -// std::cout << FdFp << std::endl; -// std::cout << "FdFpAlt" << std::endl; -// std::cout << FdFpAlt << std::endl; - -// const float finitedB = 1e-2; -// fieldOffset->setOffset(finitedB); -// -// auto const propresultalt = fPropagator->geometricalPropagator().propagateWithPath(updtsos, surfaceip1); -// -// fieldOffset->setOffset(0.); -// -// // JacobianCurvilinearToCartesian curv2cartdebug(propresult.first.parameters()); -// // const AlgebraicMatrix65& jac = curv2cart.jacobian(); -// -// JacobianCurvilinearToLocal curv2localdebug(propresult.first.surface(), propresult.first.localParameters(), *propresult.first.magneticField()); -// const AlgebraicMatrix55& curv2localjacdebug = curv2localdebug.jacobian(); -// const Matrix Hdebug = Map>(curv2localjacdebug.Array()); -// -// const AlgebraicVector5 dparms = (propresultalt.first.localParameters().vector() - propresult.first.localParameters().vector())/finitedB; -// -// const Vector5d jacnom = Hdebug*FdFp.block<5,1>(0, 5); -// -// std::cout << "qop0 = " << updtsos.signedInverseMomentum() << std::endl; -// std::cout << "nominal b field jacobian:" << std::endl; -// std::cout << jacnom << std::endl; -// std::cout << "finite diff b field jacobian:" << std::endl; -// std::cout << dparms << std::endl; - - - Matrix J = FdFp.block<2, 2>(3, 3); - // (du/dalphap)^-1 - Matrix Sinv = FdFp.block<2, 2>(3, 1).inverse(); - // du/dqopp - Matrix D = FdFp.block<2, 1>(3, 0); - // du/dBp - Matrix Bstate = FdFp.block<2, 1>(3, 5); - - const unsigned int jacstateidxout = 5*(ihit+1); - const unsigned int jacstateidxin = 3*(ihit+1); - - // qop_i - statejac(jacstateidxout, jacstateidxin + 2) = 1.; - // dalpha_i/dqop_i - statejac.block<2, 1>(jacstateidxout + 1, jacstateidxin + 2) = -Sinv*D; - // dalpha_i/du_i - statejac.block<2, 2>(jacstateidxout + 1, jacstateidxin) = -Sinv*J; - // dalpha_i/du_(i+1) - statejac.block<2, 2>(jacstateidxout + 1, jacstateidxin + 3) = Sinv; - // d(lambda, phi) / dbeta - statejac.block<2, 1>(jacstateidxout + 1, nstateparms + parmidx) = -Sinv*Bstate; - // xlocal_i - statejac(jacstateidxout + 3, jacstateidxin) = 1.; - // ylocal_i - statejac(jacstateidxout + 4, jacstateidxin + 1) = 1.; - -// std::cout << "FdFm" << std::endl; -// std::cout << FdFm << std::endl; -// std::cout << "FdFp" << std::endl; -// std::cout << FdFp << std::endl; - - constexpr unsigned int nlocalstate = 8; - constexpr unsigned int nlocalbfield = 3; - constexpr unsigned int nlocaleloss = 2; - constexpr unsigned int nlocalparms = nlocalbfield + nlocaleloss; - - constexpr unsigned int nlocal = nlocalstate + nlocalbfield + nlocaleloss; - - constexpr unsigned int localstateidx = 0; - // constexpr unsigned int localbfieldidx = localstateidx + nlocalstate; - // constexpr unsigned int localelossidx = localbfieldidx + nlocalbfield; - constexpr unsigned int localparmidx = localstateidx + nlocalstate; - - const unsigned int fullstateidx = 3*ihit; -// const unsigned int fullstateidx = 3*(ihit-1); - const unsigned int fullparmidx = (nstateparms + parmidx) - 2; - -// std::cout << "ihit = " << ihit << " nstateparms = " << nstateparms << " parmidx = " << parmidx << " fullparmidx = " << fullparmidx << std::endl; - - // individual pieces, now starting to cast to active scalars for autograd, - // as in eq (3) of https://doi.org/10.1016/j.cpc.2011.03.017 - // du/dum - Matrix Jm = FdFm.block<2, 2>(3, 3).cast(); - // (du/dalpham)^-1 - Matrix Sinvm = FdFm.block<2, 2>(3, 1).inverse().cast(); - // du/dqopm - Matrix Dm = FdFm.block<2, 1>(3, 0).cast(); - // du/dBm - Matrix Bm = FdFm.block<2, 1>(3, 5).cast(); - - // du/dup - Matrix Jp = FdFp.block<2, 2>(3, 3).cast(); - // (du/dalphap)^-1 - Matrix Sinvp = FdFp.block<2, 2>(3, 1).inverse().cast(); - // du/dqopp - Matrix Dp = FdFp.block<2, 1>(3, 0).cast(); - // du/dBp - Matrix Bp = FdFp.block<2, 1>(3, 5).cast(); - -// std::cout << "Jm" << std::endl; -// std::cout << Jm << std::endl; -// std::cout << "Sinvm" << std::endl; -// std::cout << Sinvm << std::endl; -// std::cout << "Dm" << std::endl; -// std::cout << Dm << std::endl; -// std::cout << "Bm" << std::endl; -// std::cout << Bm << std::endl; -// -// std::cout << "Jp" << std::endl; -// std::cout << Jp << std::endl; -// std::cout << "Sinvp" << std::endl; -// std::cout << Sinvp << std::endl; -// std::cout << "Dp" << std::endl; -// std::cout << Dp << std::endl; -// std::cout << "Bp" << std::endl; -// std::cout << Bp << std::endl; - - // energy loss jacobians - // const MSJacobian E = EdE.leftCols<5>().cast(); - // const MSVector dE = EdE.rightCols<1>().cast(); - - // fraction of material on this layer compared to glued layer if relevant -// double xifraction = isglued ? preciseHit->det()->surface().mediumProperties().xi()/parmDet->surface().mediumProperties().xi() : 1.; - -// std::cout << "xifraction: " << xifraction << std::endl; - - const MSScalar Eqop(EdE(0,0)); - const Matrix Ealpha = EdE.block<1, 2>(0, 1).cast(); -// const MSScalar dE(EdE(0,5)); - const MSScalar dE(xifraction*EdE(0,5)); -// (void)EdE; - - const MSScalar muE(dxeloss[0]); - -// std::cout<<"EdE" << std::endl; -// std::cout << EdE << std::endl; - - //energy loss inverse variance - MSScalar invSigmaE(1./Q(0,0)); - - // multiple scattering inverse covariance - Matrix Qinvms = Q.block<2,2>(1,1).inverse().cast(); - - // initialize active scalars for state parameters - Matrix dum = Matrix::Zero(); - //suppress gradients of reference point parameters when fitting with gen constraint - for (unsigned int j=0; j du = Matrix::Zero(); - for (unsigned int j=0; j dup = Matrix::Zero(); - for (unsigned int j=0; j()*dbeta; - } - else if (dogen && ihit==1) { - init_twice_active_var(dbetam, nlocal, localparmidx); - dum = Bpref.cast()*dbetam; - } - - //multiple scattering kink term - -// Matrix Halphalamphim = Hm.block<2,2>(1, 1).cast(); -// Matrix Halphaum = Hm.block<2,2>(1, 3).cast(); - -// Matrix Halphalamphip = Hp.block<2,2>(1, 1).cast(); -// Matrix Halphaup = Hp.block<2,2>(1, 3).cast(); - - const Matrix dalpha0 = dx0.segment<2>(1).cast(); - - const Matrix dlamphim = Sinvm*(dum - Jm*du - Dm*dqopm - Bm*dbeta); - const Matrix dlamphip = Sinvp*(dup - Jp*du - Dp*dqop - Bp*dbetap); - - const Matrix dalpham = dlamphim; - const Matrix dalphap = dlamphip; - - -// const Matrix dalpham = Sinvm*(dum - Jm*du - Dm*dqopm - Bm*dbeta); -// const Matrix dalphap = Sinvp*(dup - Jp*du - Dp*dqop - Bp*dbetap); -// const Matrix dalpham = Sinvm*(dum - Jm*du - Dm*dqopm); -// const Matrix dalphap = Sinvp*(dup - Jp*du - Dp*dqop); - - - const MSScalar deloss0(dx0[0]); - - - - -// const Matrix dms = dalpha0 + dalphap - dalpham; -// const MSScalar chisqms = dms.transpose()*Qinvms*dms; -// //energy loss term -// -// -// const MSScalar deloss = deloss0 + dqop - Eqop*dqopm - (Ealpha*dalpham)[0] - dE*dxi; -// const MSScalar chisqeloss = deloss*deloss*invSigmaE; -// -// const MSScalar chisq = chisqms + chisqeloss; - - - - -// const bool dolikelihood = false; -// - MSScalar chisq; - - if (!islikelihood) { - //standard chisquared contribution - - const Matrix dms = dalpha0 + dalphap - dalpham; - const MSScalar chisqms = dms.transpose()*Qinvms*dms; - //energy loss term - - - const MSScalar deloss = deloss0 + dqop - Eqop*dqopm - (Ealpha*dalpham)[0] - dE*dxi; -// const MSScalar deloss = deloss0 + dqop - dqopm - dE*dxi; - const MSScalar chisqeloss = deloss*invSigmaE*deloss; - - chisq = chisqms + chisqeloss; - - chisq0val += chisq.value().value(); - } - else { -// islikelihood = true; - //maximum likelihood contribution - const MSCovariance dQdqop = dQs[0].cast(); -// const MSCovariance dQddxdz = dQs[1].cast(); -// const MSCovariance dQddydz = dQs[2].cast(); -// const MSCovariance dQdxi = dQs[3].cast(); - -// const MSCovariance dQ = dqopm*dQdqop + dalpham[0]*dQddxdz + dalpham[1]*dQddydz + dxi*dQdxi; -// const MSCovariance dQ = 0.5*(dqop+dqopm)*dQdqop; - const MSCovariance dQ = dqopm*dQdqop; -// const MSCovariance dQ = 0.5*(dqop+dqopm)*dQdqop + 0.5*(dalpham[0] + dalphap[0])*dQddxdz + 0.5*(dalpham[1]+dalphap[1])*dQddydz + dxi*dQdxi; - - const Matrix Qmsnom = Q.block<2,2>(1,1).cast(); - const Matrix Qmsnominv = Qmsnom.inverse(); - const Matrix Qmsinv = Qmsnominv - Qmsnominv*dQ.block<2,2>(1,1)*Qmsnominv; - - -// const Matrix Qms = Q.block<2,2>(1,1).cast() + dQ.block<2,2>(1,1); -// const Matrix Qmsinv = Qms.inverse(); -// const MSScalar logdetQms = Eigen::log(Qms.determinant()); - - const Matrix dms = dalpha0 + dalphap - dalpham; - MSScalar chisqms = dms.transpose()*Qmsinv*dms; -// chisqms = chisqms + logdetQms; - - //energy loss term -// const MSScalar sigmaE = MSScalar(Q(0,0)) + dQ(0,0); -// const MSScalar sigmaEinv = 1./sigmaE; - - const MSScalar sigmaEnom = MSScalar(Q(0,0)); - const MSScalar sigmaEnominv = 1./sigmaEnom; - - const MSScalar sigmaEinv = sigmaEnominv - sigmaEnominv*dQ(0,0)*sigmaEnominv; - -// const MSScalar logsigmaE = Eigen::log(sigmaE); - - const MSScalar deloss = deloss0 + dqop - Eqop*dqopm - (Ealpha*dalpham)[0] - dE*dxi; - MSScalar chisqeloss = deloss*sigmaEinv*deloss; -// chisqeloss = chisqeloss + logsigmaE; - - chisq = chisqms + chisqeloss; - - //compute contributions to hessian matrix-vector derivative - for (unsigned int i=0; i dQmsinv; - for (unsigned int j=0; j<2; ++j) { - for (unsigned int k=0; k<2; ++k) { - dQmsinv(j,k) = MSScalar(Qmsinv(j,k).value().derivatives()[i]); - } - } - const MSScalar dSigmaEinv(sigmaEinv.value().derivatives()[i]); - - MSScalar dchisqms = dms.transpose()*dQmsinv*x*dms; -// dchisqms = 3.*dchisqms; - MSScalar dchisqeloss = deloss*deloss*dSigmaEinv*x; -// dchisqeloss = 3.*dchisqeloss; - const MSScalar dchisq = dchisqms + dchisqeloss; - - //TODO should this be 11x11 instead? - //TODO check additional factor of 2 - for (unsigned int j=0; j<8; ++j) { - for (unsigned int k=0; k<8; ++k) { - dhessv[ihit][i](j,k) = dchisq.derivatives()[j].derivatives()[k]; - } - } - - } - - } - - - -// const MSScalar chisq = chisqms; - - // std::cout << "chisq.value()" << std::endl; - // std::cout << chisq.value() << std::endl; - // std::cout << "chisq.value().derivatives()" << std::endl; - // std::cout << chisq.value().derivatives() << std::endl; - // std::cout << "chisq.derivatives()[0].derivatives()" << std::endl; - // std::cout << chisq.derivatives()[0].derivatives() << std::endl; - - - // const MSVector dms = dx0 + H*dx - E*Hprop*F*dxprev - E*Hprop*dF*dbeta - dE*dxi; - - - - - // MSScalar chisq; - // - // if (ihit==0 || ihit == (nhits-1)) { - // //standard fit - // const MSVector dms = dx0 + H*dx - E*Hprop*F*dxprev - E*Hprop*dF*dbeta - dE*dxi; - // chisq = dms.transpose()*Qinv*dms; - // } - // else { - // //maximum likelihood fit - // const MSVector dxprop = Hprop*F*dxprev; - // const MSCovariance dQdxprop0 = dQs[0].cast(); - // const MSCovariance dQdxprop1 = dQs[1].cast(); - // const MSCovariance dQdxprop2 = dQs[2].cast(); - // const MSCovariance dQdxi = dQs[3].cast(); - // - // const MSCovariance dQ = dxprop[0]*dQdxprop0 + dxprop[1]*dQdxprop1 + dxprop[2]*dQdxprop2 + dxi*dQdxi; - // - // // const MSCovariance dQdxprop0 = dQs[0].cast(); - // // const MSCovariance d2Qdxprop02 = dQs[1].cast(); - // // // - // // const MSCovariance dQ = dxprop[0]*dQdxprop0 + 0.5*dxprop[0]*dxprop[0]*d2Qdxprop02; - // - // // const Matrix Qms = iQ.topLeftCorner<3,3>().cast() + dQ.topLeftCorner<3,3>(); - // // Qinv.topLeftCorner<3,3>() = Qms.inverse(); - // const Matrix Qms = iQ.block<2,2>(1,1).cast() + dQ.block<2,2>(1,1); - // Qinv.block<2,2>(1,1) = Qms.inverse(); - // - // const MSScalar logdetQ = Eigen::log(Qms.determinant()); - // - // const MSVector dms = dx0 + H*dx - E*dxprop - E*Hprop*dF*dbeta - dE*dxi; - // chisq = dms.transpose()*Qinv*dms; - // chisq = chisq + logdetQ; - // - // } - - // MSCovariance Q = iQ.cast(); - - // const MSVector dxprop = Hprop*F*dxprev; - // const MSCovariance dQdxprop0 = dQs[0].cast(); - // const MSCovariance dQdxprop1 = dQs[1].cast(); - // const MSCovariance dQdxprop2 = dQs[2].cast(); - // const MSCovariance dQdxi = dQs[3].cast(); - // // - // const MSCovariance dQ = dxprop[0]*dQdxprop0 + dxprop[1]*dQdxprop1 + dxprop[2]*dQdxprop2 + dxi*dQdxi; - - // const MSVector dxprop = Hprop*F*dxprev; - // const MSCovariance dQdxprop0 = dQs[0].cast(); - // const MSCovariance d2Qdxprop02 = dQs[1].cast(); - // - // const MSCovariance dQ = dxprop[0]*dQdxprop0 + 0.5*dxprop[0]*dxprop[0]*d2Qdxprop02; - - // MSCovariance Qinv = MSCovariance::Zero(); - // Qinv(3,3) = MSScalar(1./epsxy/epsxy); - // Qinv(4,4) = MSScalar(1./epsxy/epsxy); - // Qinv.block<2,2>(1,1) = iQ.block<2,2>(1,1).inverse().cast(); - // const MSScalar Qelos = MSScalar(iQ(0,0)) + dQ(0,0); - // Qinv(0,0) = 1./Qelos; - // // const Matrix Qms = iQ.topLeftCorner<3,3>().cast() + dQ.topLeftCorner<3,3>(); - // // Qinv.topLeftCorner<3,3>() = Qms.inverse(); - // // const MSScalar logdetQ = Eigen::log(Qms.determinant()); - // const MSScalar logdetQ = Eigen::log(Qelos); - // // - // const MSVector dms = dx0 + H*dx - E*dxprop - E*Hprop*dF*dbeta - dE*dxi; - // MSScalar chisq = dms.transpose()*Qinv*dms; - // chisq = chisq + logdetQ; - - // const MSCovariance Qinvmod = Qinv - Qinv*dQ*Qinv; - // const MSScalar dlogdetQ = Eigen::log(1. + (Qinv*dQ).trace()); - // - // const MSVector dms = dx0 + H*dx - E*dxprop - E*Hprop*dF*dbeta - dE*dxi; - // MSScalar chisq = dms.transpose()*Qinvmod*dms; - // chisq = chisq + dlogdetQ; - - - auto const& gradlocal = chisq.value().derivatives(); - //fill local hessian - Matrix hesslocal; - for (unsigned int j=0; j(fullstateidx) += gradlocal.head(); - gradfull.segment(fullparmidx) += gradlocal.segment(localparmidx); - - //fill global hessian (upper triangular blocks only) - hessfull.block(fullstateidx, fullstateidx) += hesslocal.topLeftCorner(); - hessfull.block(fullstateidx, fullparmidx) += hesslocal.topRightCorner(); - hessfull.block(fullparmidx, fullparmidx) += hesslocal.bottomRightCorner(); - - const unsigned int bfieldglobalidx = detidparms.at(std::make_pair(6, parmdetid)); - globalidxv[parmidx] = bfieldglobalidx; - parmidx++; - - const unsigned int elossglobalidx = detidparms.at(std::make_pair(7, parmdetid)); - globalidxv[parmidx] = elossglobalidx; - parmidx++; - } - - //backwards propagation jacobian (local to local) to be used at the next layer - FdFm = localTransportJacobian(updtsos, propresult, true); - - } - else { - - // special case for last hit, assume zero scattering angle and nominal energy loss on last layer - Matrix J = FdFm.block<2, 2>(3, 3); - // (du/dalphap)^-1 - Matrix Sinv = FdFm.block<2, 2>(3, 1).inverse(); - // du/dqopp - Matrix D = FdFm.block<2, 1>(3, 0); - // du/dBp - Matrix Bstate = FdFm.block<2, 1>(3, 5); - - const unsigned int jacstateidxout = 5*(ihit+1); - const unsigned int jacstateidxin = 3*(ihit+1); - - // qop_i - //FIXME this should account for the energy loss, but only needed for outer momentum estimate which is not currently used - statejac(jacstateidxout, jacstateidxin - 1) = 1.; - - // dalpha_i/dqop_i - statejac.block<2, 1>(jacstateidxout + 1, jacstateidxin - 1) = -Sinv*D; - // dalpha_i/du_i - statejac.block<2, 2>(jacstateidxout + 1, jacstateidxin) = -Sinv*J; - // dalpha_i/du_(i-1) - statejac.block<2, 2>(jacstateidxout + 1, jacstateidxin - 3) = Sinv; - // d(lambda, phi) / dbeta - statejac.block<2, 1>(jacstateidxout + 1, nstateparms + parmidx) = -Sinv*Bstate; - // xlocal_i - statejac(jacstateidxout + 3, jacstateidxin) = 1.; - // ylocal_i - statejac(jacstateidxout + 4, jacstateidxin + 1) = 1.; - - - const unsigned int bfieldglobalidx = detidparms.at(std::make_pair(6, parmdetid)); - globalidxv[parmidx] = bfieldglobalidx; - parmidx++; - } - - if (preciseHit->isValid()) { - - auto fillAlignGrads = [&](auto Nalign) { - constexpr unsigned int nlocalstate = 2; - constexpr unsigned int localstateidx = 0; - constexpr unsigned int localalignmentidx = nlocalstate; - constexpr unsigned int localparmidx = localalignmentidx; - - // abusing implicit template argument to pass - // a template value via std::integral_constant - constexpr unsigned int nlocalalignment = Nalign(); - constexpr unsigned int nlocalparms = nlocalalignment; - constexpr unsigned int nlocal = nlocalstate + nlocalparms; - - using AlignScalar = AANT; - - //FIXME dirty hack to abuse state idx for reference point magnetic field - const unsigned int fullstateidx = genconstraint ? nstateparms : 3*(ihit+1); - // const unsigned int fullstateidx = 3*ihit; - const unsigned int fullparmidx = nstateparms + nparsBfield + nparsEloss + alignmentparmidx; - - const bool ispixel = GeomDetEnumerators::isTrackerPixel(preciseHit->det()->subDetector()); - - //TODO add hit validation stuff - //TODO add simhit stuff - -// const PSimHit *matchedsim = nullptr; -// for (auto const& simhith : simHits) { -// for (const PSimHit& simHit : *simhith) { -// if (std::abs(simHit.particleType()) == 13 && simHit.detUnitId() == preciseHit->geographicalId()) { -// matchedsim = &simHit; -// break; -// } -// } -// if (matchedsim) { -// break; -// } -// } - - -// Matrix Hu = Hp.bottomRightCorner<2,2>().cast(); - - Matrix dy0; - Matrix Vinv; - // rotation from module to strip coordinates -// Matrix R; - Matrix2d R; - if (preciseHit->dimension() == 1) { -// dy0[0] = AlignScalar(matchedsim->localPosition().x() - updtsos.localPosition().x()); - dy0[0] = AlignScalar(preciseHit->localPosition().x() - updtsos.localPosition().x()); - dy0[1] = AlignScalar(0.); - -// bool simvalid = false; -// for (auto const& simhith : simHits) { -// for (const PSimHit& simHit : *simhith) { -// if (simHit.detUnitId() == preciseHit->geographicalId()) { -// -// dy0[0] = AlignScalar(simHit.localPosition().x() - updtsos.localPosition().x()); -// -// simvalid = true; -// break; -// } -// } -// if (simvalid) { -// break; -// } -// } - - Vinv = Matrix::Zero(); - Vinv(0,0) = 1./preciseHit->localPositionError().xx(); - -// R = Matrix::Identity(); - R = Matrix2d::Identity(); - } - else { - // 2d hit - Matrix2d iV; - iV << preciseHit->localPositionError().xx(), preciseHit->localPositionError().xy(), - preciseHit->localPositionError().xy(), preciseHit->localPositionError().yy(); - if (ispixel) { - //take 2d hit as-is for pixels -// dy0[0] = AlignScalar(matchedsim->localPosition().x() - updtsos.localPosition().x()); -// dy0[1] = AlignScalar(matchedsim->localPosition().y() - updtsos.localPosition().y()); - dy0[0] = AlignScalar(preciseHit->localPosition().x() - updtsos.localPosition().x()); - dy0[1] = AlignScalar(preciseHit->localPosition().y() - updtsos.localPosition().y()); - - Vinv = iV.inverse().cast(); - //FIXME various temporary hacks; - -// dy0[1] = AlignScalar(0.); -// Vinv = Matrix::Zero(); -// Vinv(0,0) = 1./preciseHit->localPositionError().xx(); - -// if (GeomDetEnumerators::isEndcap(preciseHit->det()->subDetector())) { -// if (GeomDetEnumerators::isBarrel(preciseHit->det()->subDetector())) { -// PXBDetId detidtest(preciseHit->det()->geographicalId()); -// int layertest = detidtest.layer(); -// -// if (layertest > 1) { -// Vinv = Matrix::Zero(); -// } -// -// // Vinv = Matrix::Zero(); -// // dy0[0] = AlignScalar(0.); -// // dy0[1] = AlignScalar(0.); -// } - -// bool simvalid = false; -// for (auto const& simhith : simHits) { -// for (const PSimHit& simHit : *simhith) { -// if (simHit.detUnitId() == preciseHit->geographicalId()) { -// -// if (GeomDetEnumerators::isBarrel(preciseHit->det()->subDetector())) { -// dy0[0] = AlignScalar(simHit.localPosition().x() - updtsos.localPosition().x()); -// dy0[1] = AlignScalar(simHit.localPosition().y() - updtsos.localPosition().y()); -// } -// -// // dy0[1] = AlignScalar(0.); -// -// simvalid = true; -// break; -// } -// } -// if (simvalid) { -// break; -// } -// } - - -// R = Matrix::Identity(); - R = Matrix2d::Identity(); - } - else { - // diagonalize and take only smallest eigenvalue for 2d hits in strip wedge modules, - // since the constraint parallel to the strip is spurious - SelfAdjointEigenSolver eigensolver(iV); -// const Matrix2d& v = eigensolver.eigenvectors(); - R = eigensolver.eigenvectors().transpose(); - if (R(0,0) < 0.) { - R.row(0) *= -1.; - } - if (R(1,1) <0.) { - R.row(1) *= -1.; - } - - Matrix dy0local; -// dy0local[0] = matchedsim->localPosition().x() - updtsos.localPosition().x(); -// dy0local[1] = matchedsim->localPosition().y() - updtsos.localPosition().y(); - dy0local[0] = preciseHit->localPosition().x() - updtsos.localPosition().x(); - dy0local[1] = preciseHit->localPosition().y() - updtsos.localPosition().y(); - -// bool simvalid = false; -// for (auto const& simhith : simHits) { -// for (const PSimHit& simHit : *simhith) { -// if (simHit.detUnitId() == preciseHit->geographicalId()) { -// -// dy0local[0] = simHit.localPosition().x() - updtsos.localPosition().x(); -// dy0local[1] = simHit.localPosition().y() - updtsos.localPosition().y(); -// -// simvalid = true; -// break; -// } -// } -// if (simvalid) { -// break; -// } -// } - - const Matrix dy0eig = R*dy0local; - - //TODO deal properly with rotations (rotate back to module local coords?) - dy0[0] = AlignScalar(dy0eig[0]); - dy0[1] = AlignScalar(0.); - - Vinv = Matrix::Zero(); - Vinv(0,0) = AlignScalar(1./eigensolver.eigenvalues()[0]); - -// R = v.transpose().cast(); - - } - } - - rxfull.row(ivalidhit) = R.row(0).cast(); - ryfull.row(ivalidhit) = R.row(1).cast(); - - validdxeigjac.block<2,2>(2*ivalidhit, 3*(ihit+1)) = R*Hp.bottomRightCorner<2,2>(); - - const Matrix Ralign = R.cast(); - - Matrix dx = Matrix::Zero(); - AlignScalar dbeta(0.); - if (!genconstraint) { - for (unsigned int j=0; j()*dbeta; - } - - Matrix dalpha = Matrix::Zero(); - // order in which to use parameters, especially relevant in case nlocalalignment < 6 - constexpr std::array alphaidxs = {{5, 0, 1, 2, 3, 4}}; - for (unsigned int idim=0; idim A = Matrix::Zero(); - - - // dx/dx - A(0,0) = AlignScalar(1.); - // dy/dy - A(1,1) = AlignScalar(1.); - // dx/dz - A(0,2) = updtsos.localParameters().dxdz(); - // dy/dz - A(1,2) = updtsos.localParameters().dydz(); - // dx/dtheta_x - A(0,3) = -updtsos.localPosition().y()*updtsos.localParameters().dxdz(); - // dy/dtheta_x - A(1,3) = -updtsos.localPosition().y()*updtsos.localParameters().dydz(); - // dx/dtheta_y - A(0,4) = -updtsos.localPosition().x()*updtsos.localParameters().dxdz(); - // dy/dtheta_y - A(1,4) = -updtsos.localPosition().x()*updtsos.localParameters().dydz(); - // dx/dtheta_z - A(0,5) = -updtsos.localPosition().y(); - // dy/dtheta_z - A(1,5) = updtsos.localPosition().x(); - - -// std::cout << "strip local z shift gradient: " << (Ralign*A.col(2))[0].value().value() << std::endl; - - // rotation from alignment basis to module local coordinates -// Matrix A; -// if (isglued) { -// const GlobalVector modx = preciseHit->det()->surface().toGlobal(LocalVector(1.,0.,0.)); -// const GlobalVector mody = preciseHit->det()->surface().toGlobal(LocalVector(0.,1.,0.)); -// -// const GlobalVector gluedx = parmDet->surface().toGlobal(LocalVector(1.,0.,0.)); -// const GlobalVector gluedy = parmDet->surface().toGlobal(LocalVector(0.,1.,0.)); -// -// A(0,0) = AlignScalar(modx.dot(gluedx)); -// A(0,1) = AlignScalar(modx.dot(gluedy)); -// A(1,0) = AlignScalar(mody.dot(gluedx)); -// A(1,1) = AlignScalar(mody.dot(gluedy)); -// } -// else { -// A = Matrix::Identity(); -// } -// -// Matrix dh = dy0 - R*Hu*dx - R*A*dalpha; - - - double thetaincidence = std::asin(1./std::sqrt(std::pow(updtsos.localParameters().dxdz(),2) + std::pow(updtsos.localParameters().dydz(),2) + 1.)); - -// bool morehitquality = applyHitQuality_ ? thetaincidence > 0.25 : true; - bool morehitquality = true; - - if (morehitquality) { - nValidHitsFinal++; - if (ispixel) { - nValidPixelHitsFinal++; - } - } - else { - Vinv = Matrix::Zero(); - } - - Matrix dh = dy0 - Ralign*dx - Ralign*A*dalpha; - AlignScalar chisq = dh.transpose()*Vinv*dh; - - chisq0val += chisq.value().value(); - - auto const& gradlocal = chisq.value().derivatives(); - //fill local hessian - Matrix hesslocal; - for (unsigned int j=0; j gradloctest0; -// Matrix gradloctest1; -// Matrix gradloctest2; - -// std::cout << "nlocalalignment: " << nlocalalignment << " nlocal: " << nlocal << std::endl; -// std::cout << "gradlocal type: " << typeid(gradlocal).name() << std::endl; -// std::cout << "gradloctest0 type: " << typeid(gradloctest0).name() << std::endl; -// std::cout << "gradloctest1 type: " << typeid(gradloctest1).name() << std::endl; -// std::cout << "gradloctest2 type: " << typeid(gradloctest2).name() << std::endl; -// -// std::cout << "nhits: " << nhits << " nvalid: " << nvalid << " nvalidalign2d: " << nvalidalign2d << " ihit: " << ihit << std::endl; -// std::cout << "gradfull.size(): " << gradfull.size() << " nlocalstate: " << nlocalstate << " fullstateidx: " << fullstateidx << " nlocalparms: " << nlocalparms << " fullparmidx: " << fullparmidx << std::endl; - - // FIXME the templated block functions don't work here for some reason - //fill global gradient - gradfull.segment(fullstateidx) += gradlocal.head(nlocalstate); - gradfull.segment(fullparmidx) += gradlocal.segment(localparmidx, nlocalparms); - - //fill global hessian (upper triangular blocks only) - hessfull.block(fullstateidx, fullstateidx) += hesslocal.topLeftCorner(nlocalstate,nlocalstate); - hessfull.block(fullstateidx, fullparmidx) += hesslocal.topRightCorner(nlocalstate, nlocalparms); - hessfull.block(fullparmidx, fullparmidx) += hesslocal.bottomRightCorner(nlocalparms, nlocalparms); - - for (unsigned int idim=0; idimgeographicalId())); - globalidxv[nparsBfield + nparsEloss + alignmentparmidx] = xglobalidx; - alignmentparmidx++; - if (alphaidxs[idim]==0) { - hitidxv.push_back(xglobalidx); - } - } - - if (iiter == 0) { - - // fill hit validation information - Vector2d dyrecgenlocal; - dyrecgenlocal << dy0[0].value().value(), dy0[1].value().value(); - const Vector2d dyrecgeneig = R*dyrecgenlocal; - dxrecgen.push_back(dyrecgeneig[0]); - dyrecgen.push_back(dyrecgeneig[1]); - - dxerr.push_back(1./std::sqrt(Vinv(0,0).value().value())); - dyerr.push_back(1./std::sqrt(Vinv(1,1).value().value())); - - localqop.push_back(updtsos.localParameters().qbp()); - localdxdz.push_back(updtsos.localParameters().dxdz()); - localdydz.push_back(updtsos.localParameters().dydz()); - localx.push_back(updtsos.localPosition().x()); - localy.push_back(updtsos.localPosition().y()); - - - const TrackerSingleRecHit* tkhit = dynamic_cast(preciseHit.get()); - assert(tkhit != nullptr); - - if (ispixel) { - const SiPixelCluster& cluster = *tkhit->cluster_pixel(); - const SiPixelRecHit *pixhit = dynamic_cast(tkhit); - assert(pixhit != nullptr); - // std::cout << "pixel cluster sizeX = " << cluster.sizeX() <<" sizeY = " << cluster.sizeY() << std::endl; - clusterSize.push_back(cluster.size()); - clusterSizeX.push_back(cluster.sizeX()); - clusterSizeY.push_back(cluster.sizeY()); - clusterCharge.push_back(cluster.charge()); - clusterChargeBin.push_back(pixhit->qBin()); - clusterOnEdge.push_back(pixhit->isOnEdge()); - if (pixhit->hasFilledProb()) { - clusterProbXY.push_back(pixhit->clusterProbability(0)); - } - else { - clusterProbXY.push_back(2.); - } - - clusterSN.push_back(-99.); - } - else { - const SiStripCluster& cluster = *tkhit->cluster_strip(); - // siStripClusterInfo_.setCluster(cluster, preciseHit->geographicalId().rawId()); - SiStripClusterInfo clusterInfo = SiStripClusterInfo(cluster, iSetup, preciseHit->geographicalId().rawId()); - clusterSize.push_back(cluster.amplitudes().size()); - clusterSizeX.push_back(cluster.amplitudes().size()); - clusterSizeY.push_back(1); - clusterCharge.push_back(cluster.charge()); - clusterChargeBin.push_back(-99); - clusterOnEdge.push_back(-99); - clusterProbXY.push_back(-99.); - clusterSN.push_back(clusterInfo.signalOverNoise()); - } - - // if (ispixel) { - // const SiPixelCluster& cluster = *tkhit->cluster_pixel(); - // dxreccluster.push_back(cluster.x() - preciseHit->localPosition().x()); - // dyreccluster.push_back(cluster.y() - preciseHit->localPosition().y()); - // } - // else { - // dxreccluster.push_back(-99.); - // dyreccluster.push_back(-99.); - // } - - if (doSim_) { - bool simvalid = false; - for (auto const& simhith : simHits) { - for (const PSimHit& simHit : *simhith) { - if (simHit.detUnitId() == preciseHit->geographicalId()) { - - if (int(simHit.trackId()) != simtrackid) { - continue; - } - - // std::cout << "entry point: " << simHit.entryPoint() << std::endl; - // std::cout << "exit point: " << simHit.exitPoint() << std::endl; - // std::cout << "local position: " << simHit.localPosition() << std::endl; - // std::cout << "particle type: " << simHit.particleType() << std::endl; - // std::cout << "trackId: " << simHit.trackId() << std::endl; - // - // // if (simHit.entryPoint().z() * simHit.exitPoint().z() >=0.) { - // if (std::abs(simHit.localPosition().z()) > 1e-4) { - // std::cout << "anomalous simhit!" << std::endl; - // } - - // assert(simHit.entryPoint().z() * simHit.exitPoint().z() < 0.); - - Vector2d dysimgenlocal; - dysimgenlocal << simHit.localPosition().x() - updtsos.localPosition().x(), - simHit.localPosition().y() - updtsos.localPosition().y(); - const Vector2d dysimgeneig = R*dysimgenlocal; - dxsimgen.push_back(dysimgeneig[0]); - dysimgen.push_back(dysimgeneig[1]); - // dxsimgen.push_back(simHit.localPosition().x() - updtsos.localPosition().x()); - // dysimgen.push_back(simHit.localPosition().y() - updtsos.localPosition().y()); - - - Vector2d dyrecsimlocal; - dyrecsimlocal << preciseHit->localPosition().x() - simHit.localPosition().x(), - preciseHit->localPosition().y() - simHit.localPosition().y(); - const Vector2d dyrecsimeig = R*dyrecsimlocal; - dxrecsim.push_back(dyrecsimeig[0]); - dyrecsim.push_back(dyrecsimeig[1]); - - // dxrecsim.push_back(preciseHit->localPosition().x() - simHit.localPosition().x()); - // dyrecsim.push_back(preciseHit->localPosition().y() - simHit.localPosition().y()); - - simvalid = true; - break; - } - } - if (simvalid) { - break; - } - } - if (!simvalid) { - dxsimgen.push_back(-99.); - dysimgen.push_back(-99.); - dxrecsim.push_back(-99.); - dyrecsim.push_back(-99.); - } - } - } - - }; - - if (align2d) { - fillAlignGrads(std::integral_constant()); - } - else { - fillAlignGrads(std::integral_constant()); - } -// fillAlignGrads(std::integral_constant()); - - ivalidhit++; - - } - -// std::cout << "hit " << ihit << " isvalid " << preciseHit->isValid() << std::endl; -// std::cout << "global position: " << updtsos.globalParameters().position() << std::endl; - //hit information - //FIXME consolidate this special cases into templated function(s) -// if (preciseHit->isValid()) { - } - - if (!valid) { - break; - } - - assert(parmidx == (nparsBfield + nparsEloss)); - assert(alignmentparmidx == nparsAlignment); - - //fake constraint on reference point parameters - if (dogen) { -// if (false) { - for (unsigned int i=0; i<5; ++i) { - gradfull[i] = 0.; - hessfull.row(i) *= 0.; - hessfull.col(i) *= 0.; - hessfull(i,i) = 1e6; - } -// //b field from reference point not consistently used in this case -// gradfull[nstateparms] = 0.; -// hessfull.row(nstateparms) *= 0.; -// hessfull.col(nstateparms) *= 0.; - } - - //now do the expensive calculations and fill outputs - auto const& dchisqdx = gradfull.head(nstateparms); - auto const& dchisqdparms = gradfull.tail(npars); - - auto const& d2chisqdx2 = hessfull.topLeftCorner(nstateparms, nstateparms); - auto const& d2chisqdxdparms = hessfull.topRightCorner(nstateparms, npars); - auto const& d2chisqdparms2 = hessfull.bottomRightCorner(npars, npars); - -// std::cout << "dchisqdx" << std::endl; -// std::cout << dchisqdx << std::endl; -// std::cout << "d2chisqdx2 diagonal" << std::endl; -// std::cout << d2chisqdx2.diagonal() << std::endl; -// std::cout << "d2chisqdx2" << std::endl; -// std::cout << d2chisqdx2 << std::endl; -// -// auto const& eigenvalues = d2chisqdx2.eigenvalues(); -// std::cout << "d2chisqdx2 eigenvalues" << std::endl; -// std::cout << eigenvalues << std::endl; - -// auto const& Cinvd = d2chisqdx2.ldlt(); - Cinvd.compute(d2chisqdx2); - - - if (islikelihood) { - const MatrixXd Cfull = Cinvd.solve(MatrixXd::Identity(nstateparms,nstateparms)); - - // add ln det terms to gradient and hessian - // MatrixXd dhessfulli; - // MatrixXd dhessfullj; - // VectorXd dgradfull; - // TODO should this cover correction parameter part of the matrix as well? - for (unsigned int ihit=0; ihit<(nhits-1); ++ihit) { - constexpr unsigned int localstateidx = 0; - const unsigned int fullstateidx = 3*ihit; - - auto const& Cblock = Cfull.block<8,8>(fullstateidx, fullstateidx); - - // dhessfulli = MatrixXd::Zero(nstateparms, nstateparms); - // dhessfullj = MatrixXd::Zero(nstateparms, nstateparms); - - //TODO fill correction parameter block as well - for (unsigned int i=0; i<8; ++i) { - gradfull[fullstateidx + i] += (Cblock*dhessv[ihit][i]).trace(); - for (unsigned int j=0; j<8; ++j) { - hessfull(fullstateidx + i, fullstateidx + j) += (-Cblock*dhessv[ihit][j]*Cblock*dhessv[ihit][i]).trace(); - } - } - - } - - Cinvd.compute(d2chisqdx2); - - } - - dxfull = -Cinvd.solve(dchisqdx); - - dxstate = statejac.leftCols(nstateparms)*dxfull; - - const Matrix deltachisq = dchisqdx.transpose()*dxfull + 0.5*dxfull.transpose()*d2chisqdx2*dxfull; - - chisqval = chisq0val + deltachisq[0]; - - ndof = 3*(nhits - 1) + nvalid + nvalidalign2d - nstateparms; - -// std::cout << "iiter = " << iiter << ", deltachisq = " << deltachisq[0] << std::endl; - -// const Vector5d dxRef = dx.head<5>(); -// // const Vector5d dxRef = -Cinvd.solve(dchisqdx).head<5>(); -// const Matrix5d Cinner = Cinvd.solve(MatrixXd::Identity(nstateparms,nstateparms)).topLeftCorner<5,5>(); - -// dxdparms = -Cinvd.solve(d2chisqdxdparms).transpose(); -// -// grad = dchisqdparms + dxdparms*dchisqdx; -// hess = d2chisqdparms2 + 2.*dxdparms*d2chisqdxdparms + dxdparms*d2chisqdx2*dxdparms.transpose(); -// -// std::cout << "dxfull" << std::endl; -// std::cout << dxfull << std::endl; -// std::cout << "errsq" << std::endl; -// std::cout << Cinvd.solve(MatrixXd::Identity(nstateparms,nstateparms)).diagonal() << std::endl; - - const Vector5d dxRef = dxstate.head<5>(); - const Matrix5d Cinner = (statejac.leftCols(nstateparms)*Cinvd.solve(MatrixXd::Identity(nstateparms,nstateparms))*statejac.leftCols(nstateparms).transpose()).topLeftCorner<5,5>(); - -// const Matrix5d Cinner = Cinvd.solve(MatrixXd::Identity(nstateparms,nstateparms)).topLeftCorner<5,5>(); - - if (debugprintout_) { - std::cout<< "dxRef" << std::endl; - std::cout<< dxRef << std::endl; - } - - //fill output with corrected state and covariance at reference point - refParms.fill(0.); - refCov.fill(0.); -// const AlgebraicVector5& refVec = track.parameters(); - CurvilinearTrajectoryParameters curvparms(refFts.position(), refFts.momentum(), refFts.charge()); - const AlgebraicVector5& refVec = curvparms.vector(); - Map(refParms.data()) = (Map(refVec.Array()) + dxRef).cast(); - Map >(refCov.data()).triangularView() = (2.*Cinner).cast().triangularView(); - - if (iiter==0) { - refParms_iter0 = refParms; - refCov_iter0 = refCov; - } - - niter = iiter + 1; - edmval = -deltachisq[0]; - -// std::cout << "iiter = " << iiter << " edmval = " << edmval << std::endl; - - if (iiter > 1 && std::abs(deltachisq[0])<1e-3) { - break; - } - -// else if (iiter==2) { -// refParms_iter2 = refParms; -// refCov_iter2 = refCov; -// } - -// std::cout << "refParms" << std::endl; -// std::cout << Map(refParms.data()) << std::endl; - -// // gradv.clear(); -// jacrefv.clear(); -// -// // gradv.resize(npars,0.); -// jacrefv.resize(5*npars, 0.); -// -// nJacRef = 5*npars; -// // tree->SetBranchAddress("gradv", gradv.data()); -// tree->SetBranchAddress("jacrefv", jacrefv.data()); -// -// //eigen representation of the underlying vector storage -// // Map gradout(gradv.data(), npars); -// Map > jacrefout(jacrefv.data(), 5, npars); -// -// jacrefout = dxdparms.leftCols<5>().transpose().cast(); - - } - - if (!valid) { - continue; - } - - if (!nValidPixelHitsFinal) { - continue; - } - - auto const& dchisqdx = gradfull.head(nstateparms); - auto const& dchisqdparms = gradfull.tail(npars); - - auto const& d2chisqdx2 = hessfull.topLeftCorner(nstateparms, nstateparms); - auto const& d2chisqdxdparms = hessfull.topRightCorner(nstateparms, npars); - auto const& d2chisqdparms2 = hessfull.bottomRightCorner(npars, npars); - - dxdparms = -Cinvd.solve(d2chisqdxdparms).transpose(); - -// if (debugprintout_) { -// std::cout << "dxrefdparms" << std::endl; -// std::cout << dxdparms.leftCols<5>() << std::endl; -// } - - grad = dchisqdparms + dxdparms*dchisqdx; - //TODO check the simplification -// hess = d2chisqdparms2 + 2.*dxdparms*d2chisqdxdparms + dxdparms*d2chisqdx2*dxdparms.transpose(); - hess = d2chisqdparms2 + dxdparms*d2chisqdxdparms; - -// const Vector5d dxRef = dxfull.head<5>(); -// const Matrix5d Cinner = Cinvd.solve(MatrixXd::Identity(nstateparms,nstateparms)).topLeftCorner<5,5>(); - -// std::cout << "dchisqdparms.head<6>()" << std::endl; -// std::cout << dchisqdparms.head<6>() << std::endl; -// -// std::cout << "grad.head<6>()" << std::endl; -// std::cout << grad.head<6>() << std::endl; -// -// std::cout << "d2chisqdparms2.topLeftCorner<6, 6>():" << std::endl; -// std::cout << d2chisqdparms2.topLeftCorner<6, 6>() << std::endl; -// std::cout << "hess.topLeftCorner<6, 6>():" << std::endl; -// std::cout << hess.topLeftCorner<6, 6>() << std::endl; -// -// std::cout << "dchisqdparms.segment<6>(nparsBfield+nparsEloss)" << std::endl; -// std::cout << dchisqdparms.segment<6>(nparsBfield+nparsEloss) << std::endl; -// -// std::cout << "grad.segment<6>(nparsBfield+nparsEloss)" << std::endl; -// std::cout << grad.segment<6>(nparsBfield+nparsEloss) << std::endl; -// -// std::cout << "d2chisqdparms2.block<6, 6>(nparsBfield+nparsEloss, nparsBfield+nparsEloss):" << std::endl; -// std::cout << d2chisqdparms2.block<6, 6>(nparsBfield+nparsEloss, nparsBfield+nparsEloss) << std::endl; -// std::cout << "hess.block<6, 6>(nparsBfield+nparsEloss, nparsBfield+nparsEloss):" << std::endl; -// std::cout << hess.block<6, 6>(nparsBfield+nparsEloss, nparsBfield+nparsEloss) << std::endl; -// -// std::cout << "d2chisqdparms2.bottomRightCorner<6, 6>():" << std::endl; -// std::cout << d2chisqdparms2.bottomRightCorner<6, 6>() << std::endl; -// std::cout << "hess.bottomRightCorner<6, 6>():" << std::endl; -// std::cout << hess.bottomRightCorner<6, 6>() << std::endl; - - gradv.clear(); - jacrefv.clear(); - - gradv.resize(npars,0.); - jacrefv.resize(5*npars, 0.); - - nJacRef = 5*npars; - if (fillTrackTree_ && fillGrads_) { - tree->SetBranchAddress("gradv", gradv.data()); - } - if (fillTrackTree_) { - tree->SetBranchAddress("jacrefv", jacrefv.data()); - } - - //eigen representation of the underlying vector storage - Map gradout(gradv.data(), npars); - Map > jacrefout(jacrefv.data(), 5, npars); - -// jacrefout = dxdparms.leftCols<5>().transpose().cast(); - jacrefout = ( (dxdparms*statejac.leftCols(nstateparms).transpose()).leftCols<5>().transpose() + statejac.topRightCorner(5, npars) ).cast(); - - gradout = grad.cast(); - - - rx.resize(2*nvalid); - Map > rxout(rx.data(), nvalid, 2); - rxout = rxfull; -// std::cout << "rx:" << std::endl; -// for (auto elem : rx) { -// std::cout << elem << " "; -// } -// std::cout << std::endl; -// std::cout << rx << std::endl; - - ry.resize(2*nvalid); - Map > ryout(ry.data(), nvalid, 2); - ryout = ryfull; - - deigx.resize(nvalid); - deigy.resize(nvalid); - - validdxeig = validdxeigjac*dxfull; - - for (unsigned int ivalid = 0; ivalid < nvalid; ++ivalid) { - deigx[ivalid] = validdxeig[2*ivalid]; - deigy[ivalid] = validdxeig[2*ivalid + 1]; - } - -// unsigned int ivalid = 0; -// for (unsigned int ihit = 0; ihit < nhits; ++ihit) { -// auto const& hit = hits[ihit]; -// if (hit->isValid()) { -// // if (ihit < (nhits-1)) { -// // std::cout << "ihit = " << ihit << ", ivalid = " << ivalid << std::endl; -// // std::cout << "dxfull.segment<3>(3*(ihit+1)):" << std::endl; -// // std::cout << dxfull.segment<3>(3*(ihit+1)) << std::endl; -// // std::cout << "dxstate.segment<5>(5*(ihit+1))" << std::endl; -// // std::cout << dxstate.segment<5>(5*(ihit+1)) << std::endl; -// // } -// const unsigned int dxidx = 3*(ihit + 1); -// dlocalx[ivalid] = dxfull[dxidx]; -// dlocaly[ivalid] = dxfull[dxidx + 1]; -// ++ivalid; -// } -// } - - - float refPt = dogen ? genpart->pt() : std::abs(1./refParms[0])*std::sin(M_PI_2 - refParms[1]); - - gradmax = 0.; - for (unsigned int i=0; igradmax) { - gradmax = absval; - } - } - - - hessmax = 0.; - for (unsigned int i=0; ihessmax) { - hessmax = absval; - } - - } - - } - -// if (gradmax < 1e5 && refPt > 5.5) { -// //fill aggregrate gradient and hessian -// for (unsigned int i=0; ihessmax) { -// hessmax = absval; -// } -// -// const std::pair key = std::make_pair(std::min(iidx,jidx), std::max(iidx,jidx)); -// -// auto it = hessaggsparse.find(key); -// if (it==hessaggsparse.end()) { -// hessaggsparse[key] = hess(i,j); -// } -// else { -// it->second += hess(i,j); -// } -// } -// } -// } - - - if (debugprintout_) { - const Matrix5d Cinner = (statejac*Cinvd.solve(MatrixXd::Identity(nstateparms,nstateparms))*statejac.transpose()).topLeftCorner<5,5>(); - std::cout << "hess debug" << std::endl; - std::cout << "track parms" << std::endl; - std::cout << tkparms << std::endl; - // std::cout << "dxRef" << std::endl; - // std::cout << dxRef << std::endl; - std::cout << "original cov" << std::endl; - std::cout << track.covariance() << std::endl; - std::cout << "recomputed cov" << std::endl; - std::cout << 2.*Cinner << std::endl; - } - -// std::cout << "dxinner/dparms" << std::endl; -// std::cout << dxdparms.bottomRows<5>() << std::endl; -// std::cout << "grad" << std::endl; -// std::cout << grad << std::endl; -// std::cout << "hess diagonal" << std::endl; -// std::cout << hess.diagonal() << std::endl; -// std::cout << "hess0 diagonal" << std::endl; -// std::cout << d2chisqdparms2.diagonal() << std::endl; -// std::cout << "hess1 diagonal" << std::endl; -// std::cout << 2.*(dxdparms.transpose()*d2chisqdxdparms).diagonal() << std::endl; -// std::cout << "hess2 diagonal" << std::endl; -// std::cout << (dxdparms.transpose()*d2chisqdx2*dxdparms).diagonal() << std::endl; - - //fill packed hessian and indices - const unsigned int nsym = npars*(1+npars)/2; - hesspackedv.clear(); - hesspackedv.resize(nsym, 0.); - - nSym = nsym; - if (fillTrackTree_ && fillGrads_) { - tree->SetBranchAddress("hesspackedv", hesspackedv.data()); - } - - Map hesspacked(hesspackedv.data(), nsym); - const Map globalidx(globalidxv.data(), npars); - - unsigned int packedidx = 0; - for (unsigned int ipar = 0; ipar < npars; ++ipar) { - const unsigned int segmentsize = npars - ipar; - hesspacked.segment(packedidx, segmentsize) = hess.block<1, Dynamic>(ipar, ipar, 1, segmentsize).cast(); - packedidx += segmentsize; - } - - if (fillTrackTree_) { - tree->Fill(); - } - } -} - -DEFINE_FWK_MODULE(ResidualGlobalCorrectionMaker); diff --git a/Analysis/HitAnalyzer/plugins/ResidualGlobalCorrectionMakerBase.cc b/Analysis/HitAnalyzer/plugins/ResidualGlobalCorrectionMakerBase.cc index e3a1d7a204edb..358d43adb10f1 100644 --- a/Analysis/HitAnalyzer/plugins/ResidualGlobalCorrectionMakerBase.cc +++ b/Analysis/HitAnalyzer/plugins/ResidualGlobalCorrectionMakerBase.cc @@ -121,15 +121,21 @@ ResidualGlobalCorrectionMakerBase::ResidualGlobalCorrectionMakerBase(const edm:: fitFromGenParms_ = iConfig.getParameter("fitFromGenParms"); + fitFromSimParms_ = iConfig.getParameter("fitFromSimParms"); fillTrackTree_ = iConfig.getParameter("fillTrackTree"); fillGrads_ = iConfig.getParameter("fillGrads"); + fillJac_ = iConfig.getParameter("fillJac"); fillRunTree_ = iConfig.getParameter("fillRunTree"); doGen_ = iConfig.getParameter("doGen"); + requireGen_ = iConfig.getParameter("requireGen"); doSim_ = iConfig.getParameter("doSim"); bsConstraint_ = iConfig.getParameter("bsConstraint"); applyHitQuality_ = iConfig.getParameter("applyHitQuality"); doMuons_ = iConfig.getParameter("doMuons"); - corFile_ = iConfig.getParameter("corFile"); + doTrigger_ = iConfig.getParameter("doTrigger"); + doRes_ = iConfig.getParameter("doRes"); + useIdealGeometry_ = iConfig.getParameter("useIdealGeometry"); + corFiles_ = iConfig.getParameter>("corFiles"); inputBs_ = consumes(edm::InputTag("offlineBeamSpot")); @@ -138,7 +144,9 @@ ResidualGlobalCorrectionMakerBase::ResidualGlobalCorrectionMakerBase(const edm:: // GenParticlesToken_ = consumes>(edm::InputTag("genParticles")); // GenParticlesToken_ = consumes>(edm::InputTag("genParticles")); GenParticlesToken_ = consumes>(iConfig.getParameter("genParticles")); + genXyz0Token_ = consumes(edm::InputTag("genParticles","xyz0")); genEventInfoToken_ = consumes(edm::InputTag("generator")); + pileupSummaryToken_ = consumes>(iConfig.getParameter("pileupInfo")); } if (doSim_) { @@ -165,6 +173,12 @@ ResidualGlobalCorrectionMakerBase::ResidualGlobalCorrectionMakerBase(const edm:: inputMuons_ = consumes>(edm::InputTag(iConfig.getParameter("muons"))); } + if (doTrigger_) { + inputTriggerResults_ = consumes(edm::InputTag("TriggerResults","","HLT")); + triggers_ = iConfig.getParameter>("triggers"); + triggerDecisions_.assign(triggers_.size(), 0); + } + inputGeometry_ = consumes(edm::InputTag("geopro")); debugprintout_ = false; @@ -187,6 +201,7 @@ ResidualGlobalCorrectionMakerBase::ResidualGlobalCorrectionMakerBase(const edm:: // tree = new TTree("tree", "tree"); + outprefix = iConfig.getUntrackedParameter("outprefix", "globalcor"); } @@ -207,8 +222,11 @@ void ResidualGlobalCorrectionMakerBase::beginStream(edm::StreamID streamid) { if (fillTrackTree_ || fillRunTree_) { std::stringstream filenamestream; - filenamestream << "globalcor_" << streamid.value() << ".root"; +// filenamestream << "globalcor_" << streamid.value() << ".root"; + filenamestream << outprefix << "_" << streamid.value() << ".root"; + // std::cout << "file is: " << filenamestream.str().c_str() << std::endl; fout = new TFile(filenamestream.str().c_str(), "RECREATE"); + // std::cout << "filename is: " << fout->GetName() << std::endl; } // runtree = new TTree("runtree",""); @@ -225,6 +243,11 @@ void ResidualGlobalCorrectionMakerBase::beginStream(edm::StreamID streamid) // tree->Branch("globalidxv", globalidxv.data(), "globalidxv[nParms]/i", basketSize); tree->Branch("globalidxv", &globalidxvfinal, basketSize); + if (fillJac_) { + tree->Branch("gradchisqv", &gradchisqv); + } + + if (fillGrads_) { tree->Branch("gradv", gradv.data(), "gradv[nParms]/F", basketSize); tree->Branch("nSym", &nSym, basketSize); @@ -239,6 +262,7 @@ void ResidualGlobalCorrectionMakerBase::beginStream(edm::StreamID streamid) tree->Branch("event", &event); tree->Branch("edmval", &edmval); + tree->Branch("edmvalref", &edmvalref); tree->Branch("deltachisqval", &deltachisqval); tree->Branch("niter", &niter); @@ -247,9 +271,18 @@ void ResidualGlobalCorrectionMakerBase::beginStream(edm::StreamID streamid) tree->Branch("genweight", &genweight); + tree->Branch("Pileup_nPU", &Pileup_nPU); + tree->Branch("Pileup_nTrueInt", &Pileup_nTrueInt); + + tree->Branch("genl3d", &genl3d); + nParms = 0.; + for (std::size_t itrig = 0; itrig < triggers_.size(); ++itrig) { + tree->Branch(triggers_[itrig].c_str(), &triggerDecisions_[itrig]); + } + } } @@ -298,12 +331,14 @@ void ResidualGlobalCorrectionMakerBase::beginRun(edm::Run const& run, edm::EventSetup const& es) { - edm::ESHandle globalGeometry; - es.get().get(globalGeometry); + edm::ESHandle globalGeometryNominal; + es.get().get(globalGeometryNominal); edm::ESHandle globalGeometryIdeal; es.get().get("idealForDigi", globalGeometryIdeal); + const TrackingGeometry *globalGeometry = useIdealGeometry_ ? static_cast(globalGeometryIdeal.product()) : static_cast(globalGeometryNominal.product()); + edm::ESHandle trackerTopology; es.get().get(trackerTopology); @@ -318,8 +353,6 @@ ResidualGlobalCorrectionMakerBase::beginRun(edm::Run const& run, edm::EventSetup std::set > parmset; - - for (const GeomDet* det : globalGeometry->detUnits()) { if (!det) { continue; @@ -337,33 +370,67 @@ ResidualGlobalCorrectionMakerBase::beginRun(edm::Run const& run, edm::EventSetup const bool isglued = gluedid != 0; const DetId parmdetid = isglued ? DetId(gluedid) : det->geographicalId(); + const DetId aligndetid = alignGlued_ ? parmdetid : det->geographicalId(); + // const uint32_t gluedid = trackerTopology->glued(det->geographicalId()); // const bool isglued = gluedid != 0; // // const bool align2d = ispixel || isglued || isendcap; // const DetId parmdetid = isglued ? DetId(gluedid) : det->geographicalId(); // const bool align2d = ispixel || isendcap; + const bool align2d = ispixel || isendcap || (alignGlued_ && isglued); // const bool align2d = true; - const bool align2d = ispixel; +// const bool align2d = ispixel; // const bool align2d = false; // const bool align2d = isendcap && !ispixel; //always have parameters for local x alignment, in-plane rotation, bfield, and e-loss - parmset.emplace(0, det->geographicalId()); -// parmset.emplace(1, det->geographicalId()); +// parmset.emplace(0, det->geographicalId()); +// // parmset.emplace(1, det->geographicalId()); // parmset.emplace(2, det->geographicalId()); // parmset.emplace(3, det->geographicalId()); // parmset.emplace(4, det->geographicalId()); - parmset.emplace(5, det->geographicalId()); - +// parmset.emplace(5, det->geographicalId()); +// +// if (align2d) { +// //local y alignment parameters only for pixels and wedge modules +// parmset.emplace(1, det->geographicalId()); +// } + + parmset.emplace(0, aligndetid); + parmset.emplace(2, aligndetid); + parmset.emplace(3, aligndetid); + parmset.emplace(4, aligndetid); + parmset.emplace(5, aligndetid); + if (align2d) { - //local y alignment parameters only for pixels for now - parmset.emplace(1, det->geographicalId()); + //local y alignment parameters only for pixels and wedge modules + parmset.emplace(1, aligndetid); } + // bfield and material parameters are associated to glued detids where applicable parmset.emplace(6, parmdetid); parmset.emplace(7, parmdetid); + + if (doRes_) { + // hit resolution parameters are associated to individual modules + // local x/phi hit resolution + parmset.emplace(8, det->geographicalId()); + + // local y hit resolution (pixels only) + if (ispixel) { + parmset.emplace(9, det->geographicalId()); + } + + //MS resolution parameter associated to glued detids where applicable + parmset.emplace(10, parmdetid); + + //ionization resolution parameter associated to glued detids where applicable + parmset.emplace(11, parmdetid); + } + + } } @@ -379,6 +446,7 @@ ResidualGlobalCorrectionMakerBase::beginRun(edm::Run const& run, edm::EventSetup // parmset.emplace(8, bin); // } // } + if (detidparms.empty()) { @@ -394,24 +462,34 @@ ResidualGlobalCorrectionMakerBase::beginRun(edm::Run const& run, edm::EventSetup int subdet; int layer; int stereo; - float x; - float y; - float z; - float eta; - float phi; - float rho; - float xi; - float bx; - float by; - float bz; - float bradial; - float baxial; - float b0; - float b0trivial; - float dx; - float dy; - float dz; - float dtheta; + int glued; + double x; + double y; + double z; + double eta; + double phi; + double rho; + double xi; + double bx; + double by; + double bz; + double bradial; + double baxial; + double b0; + double b0trivial; + double dx; + double dy; + double dz; + double dtheta; + double nx; + double ny; + double nz; + double lxx; + double lxy; + double lxz; + double lyx; + double lyy; + double lyz; @@ -427,6 +505,7 @@ ResidualGlobalCorrectionMakerBase::beginRun(edm::Run const& run, edm::EventSetup runtree->Branch("subdet", &subdet); runtree->Branch("layer", &layer); runtree->Branch("stereo", &stereo); + runtree->Branch("glued", &glued); runtree->Branch("x", &x); runtree->Branch("y", &y); runtree->Branch("z", &z); @@ -445,6 +524,15 @@ ResidualGlobalCorrectionMakerBase::beginRun(edm::Run const& run, edm::EventSetup runtree->Branch("dy", &dy); runtree->Branch("dz", &dz); runtree->Branch("dtheta", &dtheta); + runtree->Branch("nx", &nx); + runtree->Branch("ny", &ny); + runtree->Branch("nz", &nz); + runtree->Branch("lxx", &lxx); + runtree->Branch("lxy", &lxy); + runtree->Branch("lxz", &lxz); + runtree->Branch("lyx", &lyx); + runtree->Branch("lyy", &lyy); + runtree->Branch("lyz", &lyz); } @@ -456,7 +544,7 @@ ResidualGlobalCorrectionMakerBase::beginRun(edm::Run const& run, edm::EventSetup iidx = globalidx; parmtype = key.first; - if (parmtype < 8) { + if (true) { const DetId& detid = key.second; const GeomDet* det = globalGeometry->idToDet(detid); @@ -489,6 +577,8 @@ ResidualGlobalCorrectionMakerBase::beginRun(edm::Run const& run, edm::EventSetup layer = 0; stereo = 0; + glued = !det->isLeaf(); + // int subdet = det->subDetector(); // float eta = det->surface().position().eta(); @@ -557,6 +647,30 @@ ResidualGlobalCorrectionMakerBase::beginRun(edm::Run const& run, edm::EventSetup rho = det->surface().position().perp(); xi = det->surface().mediumProperties().xi(); +// nx = det->surface().normalVector().x(); +// ny = det->surface().normalVector().y(); +// nz = det->surface().normalVector().z(); + + const LocalVector lx(1.,0.,0.); + const LocalVector ly(0.,1.,0.); + const LocalVector lz(0.,0.,1.); + + const GlobalVector J1 = det->surface().toGlobal(lx); + const GlobalVector K1 = det->surface().toGlobal(ly); + const GlobalVector I1 = det->surface().toGlobal(lz); + + lxx = J1.x(); + lxy = J1.y(); + lxz = J1.z(); + + lyx = K1.x(); + lyy = K1.y(); + lyz = K1.z(); + + nx = I1.x(); + ny = I1.y(); + nz = I1.z(); + auto const bfieldval = field->inTesla(det->surface().position()); bx = bfieldval.x(); by = bfieldval.y(); @@ -628,33 +742,51 @@ ResidualGlobalCorrectionMakerBase::beginRun(edm::Run const& run, edm::EventSetup } // load corrections from previous iteration if applicable + corparmsIncremental_.assign(corFiles_.size(), std::vector(parmset.size(), 0.)); corparms_.assign(parmset.size(), 0.); - - if (!corFile_.empty()) { - TFile *corfile = TFile::Open(corFile_.c_str()); + + for (unsigned int iiter = 0; iiter < corFiles_.size(); ++iiter) { + TFile *corfile = TFile::Open(corFiles_[iiter].c_str()); TTree *cortree = (TTree*)corfile->Get("parmtree"); - - unsigned int idx; + float val; - -// cortree->SetBranchAddress("idx", &idx); + cortree->SetBranchAddress("x", &val); - + const unsigned int nparms = cortree->GetEntries(); assert(nparms == parmset.size()); for (unsigned int iparm = 0; iparm < nparms; ++iparm) { cortree->GetEntry(iparm); - corparms_[iparm] = val; -// corparms_[idx] = val; + corparmsIncremental_[iiter][iparm] = val; + corparms_[iparm] += val; } + + corfile->Close(); } } surfacemap_.clear(); surfacemapD_.clear(); + surfacemapIdealD_.clear(); + rgluemap_.clear(); + + // fill map of ideal surfaces + for (const GeomDet* det : globalGeometryIdeal->detUnits()) { + if (!det) { + continue; + } + if (GeomDetEnumerators::isTracker(det->subDetector())) { + const Surface &surface = det->surface(); + + surfacemapIdealD_[det->geographicalId()] = surfaceToDouble(surface); + } + } + + // fill map of modified surfaces with results of previous iteration if applicable for (const GeomDet* det : globalGeometry->detUnits()) { +// for (const GeomDet* det : globalGeometryIdeal->detUnits()) { if (!det) { continue; } @@ -668,102 +800,117 @@ ResidualGlobalCorrectionMakerBase::beginRun(edm::Run const& run, edm::EventSetup const DetId parmdetid = isglued ? DetId(gluedid) : det->geographicalId(); const GeomDet* parmDet = isglued ? globalGeometry->idToDet(parmdetid) : det; - const double xifraction = isglued ? det->surface().mediumProperties().xi()/parmDet->surface().mediumProperties().xi() : 1.; - -// const bool align2d = ispixel; -// const bool align2d = false; - - const bool align2d = detidparms.count(std::make_pair(1, det->geographicalId())); + const DetId aligndetid = alignGlued_ ? parmdetid : det->geographicalId(); - - - - const unsigned int dxidx = detidparms.at(std::make_pair(0, det->geographicalId())); - const unsigned int dthetaidx = detidparms.at(std::make_pair(5, det->geographicalId())); -// const unsigned int dbidx = detidparms.at(std::make_pair(6, parmdetid)); - const unsigned int dxiidx = detidparms.at(std::make_pair(7, parmdetid)); - - // n.b. sign is flipped for alignment terms - const float dx = -corparms_[dxidx]; -// const float dy = corparms_[dyidx]; - const float dtheta = -corparms_[dthetaidx]; -// const float db = corparms_[dbdx]; - const float dxi = corparms_[dxiidx]; - - float dy = 0.; - if (align2d) { - const unsigned int dyidx = detidparms.at(std::make_pair(1, det->geographicalId())); - dy = -corparms_[dyidx]; - } + const bool align2d = detidparms.count(std::make_pair(1, aligndetid)); const Surface &surface = det->surface(); -// Point3DBase pos = surface.position(); -// -// //re-orthogonalize -// Matrix rot; -// rot << surface.rotation().xx(), surface.rotation().xy(), surface.rotation().xz(), -// surface.rotation().yx(), surface.rotation().yy(), surface.rotation().yz(), -// surface.rotation().zx(), surface.rotation().zy(), surface.rotation().zz(); -// -// // std::cout << "rot check pre:" << std::endl; -// // std::cout << rot.transpose()*rot << std::endl; -// -// for (unsigned int i = 0; i < 3; ++i) { -// for (unsigned int j = 0; j < i; ++j) { -// rot.row(i) = (rot.row(i) - (rot.row(i)*rot.row(j).transpose())[0]/rot.row(j).squaredNorm()*rot.row(j)).eval(); -// } -// } -// for (unsigned int i = 0; i < 3; ++i) { -// rot.row(i) = (rot.row(i)/rot.row(i).norm()).eval(); -// } -// -// // std::cout << "rot check post:" << std::endl; -// // std::cout << rot.transpose()*rot << std::endl; -// -// const TkRotation tkrot(rot(0,0), rot(0,1), rot(0,2), -// rot(1,0), rot(1,1), rot(1,2), -// rot(2,0), rot(2,1), rot(2,2)); -// -// -// GloballyPositioned surfaceD(pos, tkrot); GloballyPositioned surfaceD = surfaceToDouble(surface); - - ReferenceCountingPointer plane = Plane::build(det->surface()); -// std::shared_ptr plane = std::make_shared(det->surface()); - - //move, rotate and modify material - plane->move(plane->toGlobal(LocalVector(dx, dy, 0.))); - plane->rotate(Surface::RotationType(Surface::RotationType::BasicVector(plane->toGlobal(LocalVector(0.,0.,1.))), dtheta)); - - surfaceD.move(surfaceD.toGlobal(Vector3DBase(dx, dy, 0.))); - surfaceD.rotate(TkRotation(TkRotation::BasicVector(surfaceD.toGlobal(Vector3DBase(0.,0.,1.))), dtheta)); - -// Matrix rot2; -// rot2 << surfaceD.rotation().xx(), surfaceD.rotation().xy(), surfaceD.rotation().xz(), -// surfaceD.rotation().yx(), surfaceD.rotation().yy(), surfaceD.rotation().yz(), -// surfaceD.rotation().zx(), surfaceD.rotation().zy(), surfaceD.rotation().zz(); -// -// std::cout << "rot2 check:" << std::endl; -// std::cout << rot2.transpose()*rot2 << std::endl; - -// if (plane->mediumProperties().xi() + xifraction*dxi < 0.05*plane->mediumProperties().xi()) { -// std::cout << "xi value clipped!" << std::endl; -// } - - const float radLen = plane->mediumProperties().radLen(); - const float xi = std::max(plane->mediumProperties().xi() + xifraction*dxi, 0.05*plane->mediumProperties().xi()); -// const float xi = 0.05*plane->mediumProperties().xi(); - const MediumProperties mp(radLen, xi); -// plane->setMediumProperties(mp); - -// printf("in beginRun, detid = %u, parmdetid = %u, oldp = %p, newp = %p\n", det->geographicalId().rawId(), parmdetid.rawId(), &det->surface(), &(*plane)); -// std::cout << "oldxi = " << det->surface().mediumProperties().xi() << " newxi = " << plane->mediumProperties().xi() << " dxi = " << dxi << " xifraction = " << xifraction << std::endl; - -// surfacemap_[det->geographicalId()] = plane; - + Matrix Rglued = Matrix::Identity(); + + if (isglued) { + GloballyPositioned surfaceGlued = surfaceToDouble(parmDet->surface()); + + applyAlignment(surfaceGlued, parmdetid); + + if (false) { + auto const dpos = surfaceD.position() - surfaceGlued.position(); + const double gluedot = surfaceD.rotation().z()*surfaceGlued.rotation().z(); + const double gluecross = surfaceD.rotation().z().cross(surfaceGlued.rotation().z()).mag(); + + std::cout << "surface pos:\n" << surfaceD.position() << std::endl; + std::cout << "surfaceglued pos:\n" << surfaceGlued.position() << std::endl; + std::cout << "gluedot = " << gluedot <<" gluecross = " << gluecross << " dpos:\n" << dpos << std::endl; + } + + surfacemapD_[parmdetid] = surfaceGlued; + + // recreate plane using relative position and orientation from ideal geometry, enforcing that the plane is parallel to the glued one (but preserving the relative orientation of the local z axis in case they are flipped) + const Surface &surfaceGluedIdealPre = globalGeometryIdeal->idToDet(parmDet->geographicalId())->surface(); + const Surface &surfaceIdealPre = globalGeometryIdeal->idToDet(det->geographicalId())->surface(); + + const double zdot = surfaceGluedIdealPre.rotation().z()*surfaceIdealPre.rotation().z(); + const double zsign = std::copysign(1.0, zdot); + + const GloballyPositioned surfaceGluedIdeal = surfaceToDouble(surfaceGluedIdealPre); + const GloballyPositioned surfaceIdeal = surfaceToDouble(surfaceIdealPre, zsign*surfaceGluedIdeal.rotation().z()); + + if (false) { + // const GloballyPositioned surfaceIdealOrig = surfaceToDouble(surfaceIdealPre); + + std::cout << "zdot = " << zdot << std::endl; + } + + const Vector3DBase uxpre(surfaceIdeal.rotation().x()); + const Vector3DBase uypre(surfaceIdeal.rotation().y()); + + const Point3DBase poslocal = surfaceGluedIdeal.toLocal(surfaceIdeal.position()); + + const Vector3DBase uxlocal = surfaceGluedIdeal.toLocal(uxpre); + const Vector3DBase uylocal = surfaceGluedIdeal.toLocal(uypre); + + const Point3DBase posglobal = surfaceGlued.toGlobal(poslocal); + const Vector3DBase uxglobal = surfaceGlued.toGlobal(uxlocal); + const Vector3DBase uyglobal = surfaceGlued.toGlobal(uylocal); + auto const &uzglobal = zsign*surfaceGlued.rotation().z(); + + const TkRotation tkrot(uxglobal.x(), uxglobal.y(), uxglobal.z(), + uyglobal.x(), uyglobal.y(), uyglobal.z(), + uzglobal.x(), uzglobal.y(), uzglobal.z()); + + surfaceD = GloballyPositioned(posglobal, tkrot); + + if (false) { + const Point3DBase posrel = surfaceGlued.toLocal(surfaceD.position()); + + const Vector3DBase uxrel = surfaceGlued.toLocal(Vector3DBase(surfaceD.rotation().x())); + const Vector3DBase uyrel = surfaceGlued.toLocal(Vector3DBase(surfaceD.rotation().y())); + const Vector3DBase uzrel = surfaceGlued.toLocal(Vector3DBase(surfaceD.rotation().z())); + + const Point3DBase posrelideal = surfaceGluedIdeal.toLocal(surfaceIdeal.position()); + + const Vector3DBase uxrelideal = surfaceGluedIdeal.toLocal(Vector3DBase(surfaceIdeal.rotation().x())); + const Vector3DBase uyrelideal = surfaceGluedIdeal.toLocal(Vector3DBase(surfaceIdeal.rotation().y())); + const Vector3DBase uzrelideal = surfaceGluedIdeal.toLocal(Vector3DBase(surfaceIdeal.rotation().z())); + + std::cout << "posrel: " << posrel << std::endl; + std::cout << "uxrel: " << uxrel << std::endl; + std::cout << "uyrel: " << uyrel << std::endl; + std::cout << "uzrel: " << uzrel << std::endl; + + std::cout << "posrelideal: " << posrelideal << std::endl; + std::cout << "uxrelideal: " << uxrelideal << std::endl; + std::cout << "uyrelideal: " << uyrelideal << std::endl; + std::cout << "uzrelideal: " << uzrelideal << std::endl; + + } + +// const GloballyPositioned surfacemod(posglobal, tkrot); +// // +// std::cout << "surfaceD position:\n" << surfaceD.position() << "\nrotation:\n" << surfaceD.rotation() << std::endl; +// // +// std::cout << "surfacemod position:\n" << surfacemod.position() << "\nrotation:\n" << surfacemod.rotation() << std::endl; + + const Vector3DBase uxglued(surfaceGlued.rotation().x()); + const Vector3DBase uyglued(surfaceGlued.rotation().y()); + + const Vector3DBase lxalt = surfaceD.toLocal(uxglued); + const Vector3DBase lyalt = surfaceD.toLocal(uyglued); + + // rotation matrix (jacobian dlocal/dglued) + Rglued(0, 0) = lxalt.x(); + Rglued(0, 1) = lyalt.x(); + Rglued(1, 0) = lxalt.y(); + Rglued(1, 1) = lyalt.y(); + } + else { + applyAlignment(surfaceD, det->geographicalId()); + } + surfacemapD_[det->geographicalId()] = surfaceD; + rgluemap_[det->geographicalId()] = Rglued; } @@ -774,60 +921,188 @@ ResidualGlobalCorrectionMakerBase::beginRun(edm::Run const& run, edm::EventSetup } +// GloballyPositioned ResidualGlobalCorrectionMakerBase::surfaceToDouble(const Surface &surface) const { +// Point3DBase pos = surface.position(); +// //re-orthogonalize +// Matrix rot; +// rot << surface.rotation().xx(), surface.rotation().xy(), surface.rotation().xz(), +// surface.rotation().yx(), surface.rotation().yy(), surface.rotation().yz(), +// surface.rotation().zx(), surface.rotation().zy(), surface.rotation().zz(); +// +// // std::cout << "rot check pre:" << std::endl; +// // std::cout << rot.transpose()*rot << std::endl; +// +// for (unsigned int i = 0; i < 3; ++i) { +// for (unsigned int j = 0; j < i; ++j) { +// rot.row(i) = (rot.row(i) - (rot.row(i)*rot.row(j).transpose())[0]/rot.row(j).squaredNorm()*rot.row(j)).eval(); +// } +// } +// for (unsigned int i = 0; i < 3; ++i) { +// rot.row(i) = (rot.row(i)/rot.row(i).norm()).eval(); +// } +// +// // std::cout << "rot check post:" << std::endl; +// // std::cout << rot.transpose()*rot << std::endl; +// +// const TkRotation tkrot(rot(0,0), rot(0,1), rot(0,2), +// rot(1,0), rot(1,1), rot(1,2), +// rot(2,0), rot(2,1), rot(2,2)); +// +// +// return GloballyPositioned(pos, tkrot); +// } + + GloballyPositioned ResidualGlobalCorrectionMakerBase::surfaceToDouble(const Surface &surface) const { - Point3DBase pos = surface.position(); - //re-orthogonalize - Matrix rot; - rot << surface.rotation().xx(), surface.rotation().xy(), surface.rotation().xz(), - surface.rotation().yx(), surface.rotation().yy(), surface.rotation().yz(), - surface.rotation().zx(), surface.rotation().zy(), surface.rotation().zz(); - -// std::cout << "rot check pre:" << std::endl; -// std::cout << rot.transpose()*rot << std::endl; - - for (unsigned int i = 0; i < 3; ++i) { - for (unsigned int j = 0; j < i; ++j) { - rot.row(i) = (rot.row(i) - (rot.row(i)*rot.row(j).transpose())[0]/rot.row(j).squaredNorm()*rot.row(j)).eval(); - } - } - for (unsigned int i = 0; i < 3; ++i) { - rot.row(i) = (rot.row(i)/rot.row(i).norm()).eval(); - } - -// std::cout << "rot check post:" << std::endl; -// std::cout << rot.transpose()*rot << std::endl; - - const TkRotation tkrot(rot(0,0), rot(0,1), rot(0,2), - rot(1,0), rot(1,1), rot(1,2), - rot(2,0), rot(2,1), rot(2,2)); - + const Point3DBase pos = surface.position(); + + // reconstruct orthonormal basis + + auto const &gy = surface.rotation().y(); + auto const &gz = surface.rotation().z(); + + const Matrix vy(gy.x(), gy.y(), gy.z()); + const Matrix vz(gz.x(), gz.y(), gz.z()); + + const Matrix uz = vz.normalized(); + const Matrix ux = vy.cross(vz).normalized(); + const Matrix uy = uz.cross(ux); - return GloballyPositioned(pos, tkrot); + const TkRotation tkrot(ux[0], ux[1], ux[2], + uy[0], uy[1], uy[2], + uz[0], uz[1], uz[2]); + + const GloballyPositioned res(pos, tkrot); + + return res; + } -// ------------ method called when ending the processing of a run ------------ -/* -void -HitAnalyzer::endRun(edm::Run const&, edm::EventSetup const&) -{ +GloballyPositioned ResidualGlobalCorrectionMakerBase::surfaceToDouble(const Surface &surface, const Basic3DVector &gz) const { + const Point3DBase pos = surface.position(); + + // reconstruct orthonormal basis using user-provided z axis (assumed already normalized) + + auto const &gy = surface.rotation().y(); + + const Matrix vy(gy.x(), gy.y(), gy.z()); + const Matrix vz(gz.x(), gz.y(), gz.z()); + + const Matrix &uz = vz; + const Matrix ux = vy.cross(vz).normalized(); + const Matrix uy = uz.cross(ux); + + const TkRotation tkrot(ux[0], ux[1], ux[2], + uy[0], uy[1], uy[2], + uz[0], uz[1], uz[2]); + + const GloballyPositioned res(pos, tkrot); + + return res; + } -*/ -// ------------ method called when starting to processes a luminosity block ------------ -/* -void -HitAnalyzer::beginLuminosityBlock(edm::LuminosityBlock const&, edm::EventSetup const&) -{ +void ResidualGlobalCorrectionMakerBase::applyAlignment(GloballyPositioned &surface, const DetId &detid) const { + + using RotationT = GloballyPositioned::RotationType; + + const int idx = detidparms.at(std::make_pair(0, detid)); + const int idz = detidparms.at(std::make_pair(2, detid)); + const int idthetax = detidparms.at(std::make_pair(3, detid)); + const int idthetay = detidparms.at(std::make_pair(4, detid)); + const int idthetaz = detidparms.at(std::make_pair(5, detid)); + + int idy = -1; + auto const &dyiter = detidparms.find(std::make_pair(1, detid)); + if (dyiter != detidparms.end()) { + idy = dyiter->second; + } + + for (auto const &icorparms : corparmsIncremental_) { + const double dx = icorparms[idx]; + const double dy = idy >= 0 ? icorparms[idy] : 0.; + const double dz = icorparms[idz]; + const double dthetax = icorparms[idthetax]; + const double dthetay = icorparms[idthetay]; + const double dthetaz = icorparms[idthetaz]; + + const Vector3DBase dxlocal(dx, dy, dz); + const Vector3DBase dxglobal = surface.toGlobal(dxlocal); + + const RotationT rx(surface.rotation().x(), dthetax); + const RotationT ry(surface.rotation().y(), dthetay); + const RotationT rz(surface.rotation().x(), dthetaz); + + surface.move(dxglobal); + + // order is arbitrary since these angles are implicitly derived under an assumption of infinitesimal rotations. The ambiguity will be sorted out as part of the iteration process + surface.rotate(rx); + surface.rotate(ry); + surface.rotate(rz); + + + + + } + + } -*/ -// ------------ method called when ending the processing of a luminosity block ------------ -/* -void -HitAnalyzer::endLuminosityBlock(edm::LuminosityBlock const&, edm::EventSetup const&) -{ + +Matrix ResidualGlobalCorrectionMakerBase::globalToLocal(const Matrix &state, const GloballyPositioned &surface) const { + + const Point3DBase posprop(state[0], state[1], state[2]); + const Vector3DBase momprop(state[3], state[4], state[5]); + + const Point3DBase localpos = surface.toLocal(posprop); + const Vector3DBase localmom = surface.toLocal(momprop); + + const double signpz = std::copysign(1., localmom.z()); + + Matrix localparms; + localparms[0] = state[6]/state.segment<3>(3).norm(); + localparms[1] = localmom.x()/localmom.z(); + localparms[2] = localmom.y()/localmom.z(); + localparms[3] = localpos.x(); + localparms[4] = localpos.y(); + localparms[5] = signpz; + + return localparms; + +} + + +Matrix ResidualGlobalCorrectionMakerBase::localToGlobal(const Matrix &localstate, const GloballyPositioned &surface) const { + + const double qop = localstate[0]; + const double dxdz = localstate[1]; + const double dydz = localstate[2]; + const double lx = localstate[3]; + const double ly = localstate[4]; + const double signpz = localstate[5]; + + const double p = std::abs(1./qop); + const double charge = std::copysign(1.0, qop); + const double localmomfact = signpz/std::sqrt(1. + dxdz*dxdz + dydz*dydz); + + const Point3DBase localpos(lx, ly, 0.); + const Vector3DBase localmom(p*dxdz*localmomfact, p*dydz*localmomfact, p*localmomfact); + + const Point3DBase globalpos = surface.toGlobal(localpos); + const Vector3DBase globalmom = surface.toGlobal(localmom); + + Matrix globalstate; + globalstate[0] = globalpos.x(); + globalstate[1] = globalpos.y(); + globalstate[2] = globalpos.z(); + globalstate[3] = globalmom.x(); + globalstate[4] = globalmom.y(); + globalstate[5] = globalmom.z(); + globalstate[6] = charge; + + return globalstate; + } -*/ // ------------ method fills 'descriptions' with the allowed parameters for the module ------------ void ResidualGlobalCorrectionMakerBase::fillDescriptions(edm::ConfigurationDescriptions &descriptions) @@ -839,567 +1114,33 @@ void ResidualGlobalCorrectionMakerBase::fillDescriptions(edm::ConfigurationDescr descriptions.addDefault(desc); } -Matrix ResidualGlobalCorrectionMakerBase::bfieldJacobian(const GlobalTrajectoryParameters& globalSource, - const GlobalTrajectoryParameters& globalDest, - const double& s, - const GlobalVector& bfield) const { - - //analytic jacobian wrt magnitude of magnetic field - //TODO should we parameterize with respect to z-component instead? - //extending derivation from CMS NOTE 2006/001 - const Vector3d b(bfield.x(), - bfield.y(), - bfield.z()); - const double magb = b.norm(); - const Vector3d h = b.normalized(); - - const Vector3d p0(globalSource.momentum().x(), - globalSource.momentum().y(), - globalSource.momentum().z()); - const Vector3d p1(globalDest.momentum().x(), - globalDest.momentum().y(), - globalDest.momentum().z()); - const Vector3d M0(globalSource.position().x(), - globalSource.position().y(), - globalSource.position().z()); - const Vector3d T0 = p0.normalized(); - const Vector3d T = p1.normalized(); - const double p = p0.norm(); - const double q = globalSource.charge(); - const double qop = q/p; - - const Vector3d N0alpha = h.cross(T0); - const double alpha = N0alpha.norm(); - const Vector3d N0 = N0alpha.normalized(); - const double gamma = h.transpose()*T; - const Vector3d Z(0.,0.,1.); - const Vector3d U = Z.cross(T).normalized(); - const Vector3d V = T.cross(U); - - //this is printed from sympy.printing.cxxcode together with sympy.cse for automatic substitution of common expressions - auto const xf0 = h; - auto const xf1 = xf0[2]; - auto const xf2 = magb*qop; - auto const xf3 = s*xf2; - auto const xf4 = std::cos(xf3); - auto const xf5 = 1 - xf4; - auto const xf6 = gamma*xf5; - auto const xf7 = T0; - auto const xf8 = xf7[2]; - auto const xf9 = xf4*xf8; - auto const xf10 = N0; - auto const xf11 = xf10[2]; - auto const xf12 = std::sin(xf3); - auto const xf13 = alpha*xf12; - auto const xf14 = xf11*xf13; - auto const xf15 = -xf14 + xf9; - auto const xf16 = xf1*xf6 + xf15; - auto const xf17 = std::pow(qop, -2); - auto const xf18 = xf0[0]; - auto const xf19 = xf7[0]; - auto const xf20 = xf19*xf4; - auto const xf21 = xf10[0]; - auto const xf22 = xf13*xf21; - auto const xf23 = xf20 - xf22; - auto const xf24 = xf18*xf6 + xf23; - auto const xf25 = std::pow(xf24, 2); - auto const xf26 = xf0[1]; - auto const xf27 = xf7[1]; - auto const xf28 = xf27*xf4; - auto const xf29 = xf10[1]; - auto const xf30 = xf13*xf29; - auto const xf31 = xf28 - xf30; - auto const xf32 = xf26*xf6 + xf31; - auto const xf33 = std::pow(xf32, 2); - auto const xf34 = xf17*xf25 + xf17*xf33; - auto const xf35 = 1.0/xf34; - auto const xf36 = xf17*xf35; - auto const xf37 = 1.0/(std::pow(xf16, 2)*xf36 + 1); - auto const xf38 = qop*s; - auto const xf39 = xf12*xf8; - auto const xf40 = xf12*xf38; - auto const xf41 = gamma*xf1; - auto const xf42 = xf38*xf4; - auto const xf43 = alpha*xf11; - auto const xf44 = 1.0/std::fabs(qop); - auto const xf45 = std::pow(xf34, -1.0/2.0); - auto const xf46 = xf44*xf45; - auto const xf47 = xf19*xf40; - auto const xf48 = alpha*xf21; - auto const xf49 = xf42*xf48; - auto const xf50 = gamma*xf40; - auto const xf51 = xf18*xf50; - auto const xf52 = (1.0/2.0)*xf17; - auto const xf53 = xf24*xf52; - auto const xf54 = xf27*xf40; - auto const xf55 = alpha*xf29; - auto const xf56 = xf42*xf55; - auto const xf57 = xf26*xf50; - auto const xf58 = xf32*xf52; - auto const xf59 = xf16*xf44/std::pow(xf34, 3.0/2.0); - auto const xf60 = 1.0/magb; - auto const xf61 = s*xf4; - auto const xf62 = xf60*xf61; - auto const xf63 = 1.0/qop; - auto const xf64 = xf63/std::pow(magb, 2); - auto const xf65 = xf12*xf64; - auto const xf66 = xf62 - xf65; - auto const xf67 = -gamma*xf62 + gamma*xf65; - auto const xf68 = -xf12*xf3 + xf5; - auto const xf69 = xf64*xf68; - auto const xf70 = -xf16*(xf1*xf67 + xf43*xf69 + xf66*xf8) - xf24*(xf18*xf67 + xf19*xf66 + xf48*xf69) - xf32*(xf26*xf67 + xf27*xf66 + xf55*xf69); - auto const xf71 = xf12*xf2; - auto const xf72 = xf2*xf4; - auto const xf73 = xf19*xf71; - auto const xf74 = xf48*xf72; - auto const xf75 = gamma*xf71; - auto const xf76 = xf18*xf75; - auto const xf77 = xf27*xf71; - auto const xf78 = xf55*xf72; - auto const xf79 = xf26*xf75; - auto const xf80 = xf37*(xf46*(-xf2*xf39 + xf41*xf71 - xf43*xf72) + xf59*(-xf53*(-2*xf73 - 2*xf74 + 2*xf76) - xf58*(-2*xf77 - 2*xf78 + 2*xf79))); - auto const xf81 = xf24*xf36; - auto const xf82 = xf32*xf36; - auto const xf83 = xf81*(-xf77 - xf78 + xf79) - xf82*(-xf73 - xf74 + xf76); - auto const xf84 = U; - auto const xf85 = xf84[0]; - auto const xf86 = s*xf60; - auto const xf87 = xf5*xf64; - auto const xf88 = gamma*xf18; - auto const xf89 = xf12 - xf3; - auto const xf90 = xf64*xf89; - auto const xf91 = xf60*xf63; - auto const xf92 = xf91*(-xf38 + xf42); - auto const xf93 = -xf19*xf65 + xf20*xf86 - xf22*xf86 + xf48*xf87 + xf88*xf90 - xf88*xf92; - auto const xf94 = xf84[1]; - auto const xf95 = gamma*xf26; - auto const xf96 = -xf27*xf65 + xf28*xf86 - xf30*xf86 + xf55*xf87 + xf90*xf95 - xf92*xf95; - auto const xf97 = xf84[2]; - auto const xf98 = -xf14*xf86 + xf41*xf90 - xf41*xf92 + xf43*xf87 - xf65*xf8 + xf86*xf9; - auto const xf99 = xf91*(-xf2 + xf72); - auto const xf100 = xf23 - xf88*xf99; - auto const xf101 = xf31 - xf95*xf99; - auto const xf102 = xf15 - xf41*xf99; - auto const xf103 = xf100*xf85 + xf101*xf94 + xf102*xf97; - auto const xf104 = V; - auto const xf105 = xf104[0]; - auto const xf106 = xf104[1]; - auto const xf107 = xf104[2]; - auto const xf108 = xf100*xf105 + xf101*xf106 + xf102*xf107; - auto const xf109 = xf17*(((qop) > 0) - ((qop) < 0)); - auto const xf110 = magb*s; - auto const xf111 = xf110*xf12; - auto const xf112 = xf110*xf4; - auto const xf113 = std::pow(qop, -3); - auto const xf114 = xf111*xf19; - auto const xf115 = xf112*xf48; - auto const xf116 = xf111*xf88; - auto const xf117 = xf111*xf27; - auto const xf118 = xf112*xf55; - auto const xf119 = xf111*xf95; - auto const xf120 = xf61*xf63; - auto const xf121 = xf17*xf60; - auto const xf122 = xf12*xf121; - auto const xf123 = xf120 - xf122; - auto const xf124 = -gamma*xf120 + gamma*xf122; - auto const xf125 = xf121*xf68; - auto const xf126 = -xf16*(xf1*xf124 + xf123*xf8 + xf125*xf43) - xf24*(xf123*xf19 + xf124*xf18 + xf125*xf48) - xf32*(xf123*xf27 + xf124*xf26 + xf125*xf55); - auto const xf127 = xf35*xf44; - auto const xf128 = s*xf63; - auto const xf129 = xf121*xf5; - auto const xf130 = xf121*xf89; - auto const xf131 = xf91*(-xf110 + xf112); - auto const xf132 = -xf122*xf19 + xf128*xf20 - xf128*xf22 + xf129*xf48 + xf130*xf88 - xf131*xf88; - auto const xf133 = -xf122*xf27 + xf128*xf28 - xf128*xf30 + xf129*xf55 + xf130*xf95 - xf131*xf95; - auto const xf134 = -xf122*xf8 - xf128*xf14 + xf128*xf9 + xf129*xf43 + xf130*xf41 - xf131*xf41; - auto const dlamdB = xf37*(xf46*(-xf38*xf39 + xf40*xf41 - xf42*xf43) + xf59*(-xf53*(-2*xf47 - 2*xf49 + 2*xf51) - xf58*(-2*xf54 - 2*xf56 + 2*xf57))) + xf70*xf80; - auto const dphidB = xf70*xf83 + xf81*(-xf54 - xf56 + xf57) - xf82*(-xf47 - xf49 + xf51); - auto const dxtdB = xf103*xf70 + xf85*xf93 + xf94*xf96 + xf97*xf98; - auto const dytdB = xf105*xf93 + xf106*xf96 + xf107*xf98 + xf108*xf70; - auto const dlamdqop = xf126*xf80 + xf37*(-xf109*xf16*xf45 + xf46*(-xf110*xf39 + xf111*xf41 - xf112*xf43) + xf59*(xf113*xf25 + xf113*xf33 - xf53*(-2*xf114 - 2*xf115 + 2*xf116) - xf58*(-2*xf117 - 2*xf118 + 2*xf119))); - auto const dphidqop = xf126*xf83 + xf127*xf24*(-xf109*xf32 + xf44*(-xf117 - xf118 + xf119)) - xf127*xf32*(-xf109*xf24 + xf44*(-xf114 - xf115 + xf116)); - auto const dxtdqop = xf103*xf126 + xf132*xf85 + xf133*xf94 + xf134*xf97; - auto const dytdqop = xf105*xf132 + xf106*xf133 + xf107*xf134 + xf108*xf126; - - - Matrix dF; - dF[0] = 0.; - dF[1] = dlamdB; - dF[2] = dphidB; - dF[3] = dxtdB; - dF[4] = dytdB; - -// convert to tesla - dF *= 2.99792458e-3; - - Matrix Fqop; - Fqop[0] = 1.; - Fqop[1] = dlamdqop; - Fqop[2] = dphidqop; - Fqop[3] = dxtdqop; - Fqop[4] = dytdqop; -// // -// std::cout << "Fqop from sympy:" << std::endl; -// std::cout << Fqop << std::endl; - - return dF; -} - -Matrix ResidualGlobalCorrectionMakerBase::curvtransportJacobian(const GlobalTrajectoryParameters& globalSource, - const GlobalTrajectoryParameters& globalDest, - const double& s, - const GlobalVector& bfield) const { - - Matrix res; - const double qop0 = globalSource.signedInverseMomentum(); - const double limit = 5.; - const double cutCriterion = std::abs(s * qop0); +Matrix ResidualGlobalCorrectionMakerBase::hybrid2curvJacobianD(const Matrix &state, const MagneticField *field, double dBz) const { -// std::cout << "cutCriterion " << cutCriterion << std::endl; -// if (cutCriterion > limit) { - if (true) { -// if (false) { - - // large s limit, use CMSSW calculation which - // explicitly uses final position and momentum from propagation - -// std::cout << "computing jacobian from cmssw" << std::endl; - AnalyticalCurvilinearJacobian curv2curv; - curv2curv.computeFullJacobian(globalSource, globalDest.position(), globalDest.momentum(), bfield, s); - const AlgebraicMatrix55& curv2curvjac = curv2curv.jacobian(); - const Matrix F = Map>(curv2curvjac.Array()); - - res.topLeftCorner<5,5>() = F; - res(0,5) = 0.; - res.block<4, 1>(1,5) = qop0/bfield.mag()*F.block<4, 1>(1, 0); - } - else { - // small s limit, use sympy-based calculation which - // uses final position and momentum implicitly from path length + transport equations - -// std::cout << "computing jacobian from sympy" << std::endl; - - const double M0x = globalSource.position().x(); - const double M0y = globalSource.position().y(); - const double M0z = globalSource.position().z(); - - const double p0 = globalSource.momentum().mag(); - const GlobalVector W0 = globalSource.momentum()/p0; - const double W0x = W0.x(); - const double W0y = W0.y(); - const double W0z = W0.z(); - - const double B = bfield.mag(); - const GlobalVector H = bfield/B; - const double hx = H.x(); - const double hy = H.y(); - const double hz = H.z(); - - - const double x0 = B*s; - const double x1 = qop0*x0; - const double x2 = std::cos(x1); - const double x3 = std::pow(W0z, 2); - const double x4 = std::pow(W0x, 2); - const double x5 = std::pow(W0y, 2); - const double x6 = x4 + x5; - const double x7 = 1.0/x6; - const double x8 = std::pow(x3*x7 + 1, -1.0/2.0); - const double x9 = x2*x8; - const double x10 = std::sqrt(x6); - const double x11 = 1.0/x10; - const double x12 = W0z*x11; - const double x13 = x12*x9; - const double x14 = W0y*hx; - const double x15 = x11*x14; - const double x16 = W0x*hy; - const double x17 = x11*x16; - const double x18 = x15 - x17; - const double x19 = std::sin(x1); - const double x20 = x19*x8; - const double x21 = x18*x20; - const double x22 = x2 - 1; - const double x23 = hx*x8; - const double x24 = W0x*x11; - const double x25 = hy*x8; - const double x26 = W0y*x11; - const double x27 = hz*x8; - const double x28 = x12*x27 + x23*x24 + x25*x26; - const double x29 = x22*x28; - const double x30 = -hz*x29 + x13 - x21; - const double x31 = x26*x9; - const double x32 = x24*x27; - const double x33 = x12*x23 - x32; - const double x34 = x19*x33; - const double x35 = hy*x29; - const double x36 = x31 + x34 - x35; - const double x37 = x24*x9; - const double x38 = x12*x25; - const double x39 = x26*x27; - const double x40 = x38 - x39; - const double x41 = x19*x40; - const double x42 = -hx*x29 + x37 - x41; - const double x43 = std::pow(x36, 2) + std::pow(x42, 2); - const double x44 = 1.0/x43; - const double x45 = 1.0/(std::pow(x30, 2)*x44 + 1); - const double x46 = x12*x20; - const double x47 = -x15 + x17; - const double x48 = x47*x9; - const double x49 = hz*x28; - const double x50 = x19*x49; - const double x51 = std::pow(x43, -1.0/2.0); - const double x52 = x20*x26; - const double x53 = x0*x52; - const double x54 = x0*x2; - const double x55 = x33*x54; - const double x56 = hy*x28; - const double x57 = x0*x19; - const double x58 = x56*x57; - const double x59 = (1.0/2.0)*x36; - const double x60 = x20*x24; - const double x61 = x0*x60; - const double x62 = -x38 + x39; - const double x63 = x54*x62; - const double x64 = hx*x28; - const double x65 = x57*x64; - const double x66 = (1.0/2.0)*x42; - const double x67 = x30/std::pow(x43, 3.0/2.0); - const double x68 = x22*x8; - const double x69 = x18*x68; - const double x70 = B*qop0; - const double x71 = W0z*x24; - const double x72 = W0z*x26; - const double x73 = -M0x*x71 - M0y*x72 + M0z*(x11*x4 + x11*x5); - const double x74 = x10*x73; - const double x75 = x1 - x19; - const double x76 = x46 + x49*x75 + x69 + x70*x74; - const double x77 = 1.0/B; - const double x78 = x77/std::pow(qop0, 2); - const double x79 = x76*x78; - const double x80 = x0 - x54; - const double x81 = 1.0/qop0; - const double x82 = x77*x81; - const double x83 = x82*(B*x74 + x0*x13 - x0*x21 + x49*x80); - const double x84 = -M0x*x26 + M0y*x24; - const double x85 = W0z*x73; - const double x86 = W0x*x84 - W0y*x85; - const double x87 = B*x86; - const double x88 = qop0*x87 + x10*(-x22*x33 + x52 + x56*x75); - const double x89 = x11*x78; - const double x90 = x88*x89; - const double x91 = x10*(x0*x31 + x0*x34 + x56*x80) + x87; - const double x92 = x11*x82; - const double x93 = x91*x92; - const double x94 = W0x*x85 + W0y*x84; - const double x95 = B*x94; - const double x96 = -qop0*x95 + x10*(x22*x40 + x60 + x64*x75); - const double x97 = x89*x96; - const double x98 = x10*(x0*x37 - x0*x41 + x64*x80) - x95; - const double x99 = x92*x98; - const double x100 = -x30*(-x79 + x83) - x36*(-x90 + x93) - x42*(-x97 + x99); - const double x101 = x52*x70; - const double x102 = x2*x70; - const double x103 = x102*x33; - const double x104 = x19*x70; - const double x105 = x104*x56; - const double x106 = x60*x70; - const double x107 = x102*x62; - const double x108 = x104*x64; - const double x109 = x45*(x51*(-x46*x70 + x48*x70 + x50*x70) + x67*(-x59*(-2*x101 + 2*x103 + 2*x105) - x66*(-2*x106 + 2*x107 + 2*x108))); - const double x110 = W0z*x7; - const double x111 = W0x*x110; - const double x112 = W0y*x110; - const double x113 = -x111*x23 - x112*x25 + x27; - const double x114 = hz*x113; - const double x115 = x112*x9; - const double x116 = x111*x27 + x23; - const double x117 = x116*x19; - const double x118 = x113*x22; - const double x119 = hy*x118; - const double x120 = x111*x9; - const double x121 = x112*x27; - const double x122 = x19*(-x121 - x25); - const double x123 = hx*x118; - const double x124 = x114*x75 - x12*x69 + x20; - const double x125 = x30*x82; - const double x126 = x113*x75; - const double x127 = hx*x126 - x111*x20 + x22*(x121 + x25); - const double x128 = x42*x82; - const double x129 = 1 - x2; - const double x130 = hy*x126 - x112*x20 + x116*x129; - const double x131 = x36*x82; - const double x132 = -x124*x125 - x127*x128 - x130*x131; - const double x133 = W0x*hx; - const double x134 = x11*x133; - const double x135 = W0y*hy; - const double x136 = x11*x135; - const double x137 = -x23*x26 + x24*x25; - const double x138 = hz*x137; - const double x139 = x19*x39; - const double x140 = x137*x22; - const double x141 = hy*x140; - const double x142 = x19*x32; - const double x143 = hx*x140; - const double x144 = x138*x75 + x68*(x134 + x136); - const double x145 = x137*x75; - const double x146 = hy*x145 + x129*x39 + x60; - const double x147 = hx*x145 - x22*x32 - x52; - const double x148 = -x125*x144 - x128*x147 - x131*x146; - const double x149 = -x24*x36 + x26*x42; - const double x150 = -x10*x30 + x36*x72 + x42*x71; - const double x151 = qop0*s; - const double x152 = x151*x52; - const double x153 = x151*x2; - const double x154 = x153*x33; - const double x155 = x151*x19; - const double x156 = x155*x56; - const double x157 = x151*x60; - const double x158 = x153*x62; - const double x159 = x155*x64; - const double x160 = x81/std::pow(B, 2); - const double x161 = x160*x76; - const double x162 = x151 - x153; - const double x163 = x82*(qop0*x74 + x13*x151 - x151*x21 + x162*x49); - const double x164 = x11*x160; - const double x165 = x164*x88; - const double x166 = qop0*x86 + x10*(x151*x31 + x151*x34 + x162*x56); - const double x167 = x166*x92; - const double x168 = x164*x96; - const double x169 = -qop0*x94 + x10*(x151*x37 - x151*x41 + x162*x64); - const double x170 = x169*x92; - const double x171 = -x30*(-x161 + x163) - x36*(-x165 + x167) - x42*(-x168 + x170); - const double x172 = x42*x44; - const double x173 = -x31; - const double x174 = x44*(x173 - x34 + x35); - const double x175 = x172*(-x101 + x103 + x105) + x174*(-x106 + x107 + x108); - const double x176 = x22*(W0z*hz + x133 + x135); - const double x177 = W0y*x2 - hy*x176 + x19*(-W0x*hz + W0z*hx); - const double x178 = x3 + x6; - const double x179 = 1.0/x178; - const double x180 = W0x*x2 - hx*x176 + x19*(W0y*hz - W0z*hy); - const double x181 = x179*std::pow(x180, 2); - const double x182 = std::pow(x177, 2)*x179; - const double x183 = std::pow(x181 + x182, -1.0/2.0); - const double x184 = x183/std::sqrt(x178*x7); - const double x185 = x184*x7; - const double x186 = x177*x185; - const double x187 = x186*x96; - const double x188 = x180*x185; - const double x189 = x188*x88; - const double x190 = x188*x82; - const double x191 = x186*x82; - const double x192 = -x102 + x70; - const double x193 = x192*x56 + x31*x70 + x34*x70; - const double x194 = x180*x184; - const double x195 = x194*x92; - const double x196 = x192*x64 + x37*x70 - x41*x70; - const double x197 = x177*x184; - const double x198 = x197*x92; - const double x199 = x193*x195 - x196*x198; - const double x200 = x179*x183*(W0z*x2 - hz*x176 + x19*(-x14 + x16)); - const double x201 = x180*x200; - const double x202 = x177*x200; - const double x203 = x181*x183 + x182*x183; - const double x204 = x202*x82; - const double x205 = x201*x82; - const double x206 = x203*x82; - const double x207 = -x193*x204 - x196*x205 + x206*(x13*x70 + x192*x49 - x21*x70); - const double dqopdqop0 = 1; - const double dqopdlam0 = 0; - const double dqopdphi0 = 0; - const double dqopdxt0 = 0; - const double dqopdyt0 = 0; - const double dqopdB = 0; - const double dlamdqop0 = x100*x109 + x45*(x51*(-x0*x46 + x0*x48 + x0*x50) + x67*(-x59*(-2*x53 + 2*x55 + 2*x58) - x66*(-2*x61 + 2*x63 + 2*x65))); - const double dlamdlam0 = x109*x132 + x45*(x51*(-x114*x22 - x46*x47 + x9) + x67*(-x59*(-2*x115 + 2*x117 - 2*x119) - x66*(-2*x120 + 2*x122 - 2*x123))); - const double dlamdphi0 = x109*x148 + x45*(x51*(-x138*x22 + x20*(-x134 - x136)) + x67*(-x59*(2*x139 - 2*x141 + 2*x37) - x66*(2*x142 - 2*x143 - 2*x31))); - const double dlamdxt0 = x109*x149; - const double dlamdyt0 = x109*x150; - const double dlamdB = x109*x171 + x45*(x51*(-x151*x46 + x151*x48 + x151*x50) + x67*(-x59*(-2*x152 + 2*x154 + 2*x156) - x66*(-2*x157 + 2*x158 + 2*x159))); - const double dphidqop0 = x100*x175 + x172*(-x53 + x55 + x58) + x174*(-x61 + x63 + x65); - const double dphidlam0 = x132*x175 + x172*(-x115 + x117 - x119) + x174*(-x120 + x122 - x123); - const double dphidphi0 = x148*x175 + x172*(x139 - x141 + x37) + x174*(x142 - x143 + x173); - const double dphidxt0 = x149*x175; - const double dphidyt0 = x150*x175; - const double dphidB = x171*x175 + x172*(-x152 + x154 + x156) + x174*(-x157 + x158 + x159); - const double dxtdqop0 = x100*x199 + x187*x78 - x189*x78 + x190*x91 - x191*x98; - const double dxtdlam0 = -x127*x198 + x130*x195 + x132*x199; - const double dxtdphi0 = x146*x195 - x147*x198 + x148*x199; - const double dxtdxt0 = W0x*x188 + W0y*x186 + x149*x199; - const double dxtdyt0 = x111*x197 - x112*x194 + x150*x199; - const double dxtdB = x160*x187 - x160*x189 + x166*x190 - x169*x191 + x171*x199; - const double dytdqop0 = x100*x207 + x201*x97 - x201*x99 + x202*x90 - x202*x93 - x203*x79 + x203*x83; - const double dytdlam0 = x124*x206 - x127*x205 - x130*x204 + x132*x207; - const double dytdphi0 = x144*x206 - x146*x204 - x147*x205 + x148*x207; - const double dytdxt0 = x149*x207 + x201*x26 - x202*x24; - const double dytdyt0 = x10*x203 + x150*x207 + x201*x71 + x202*x72; - const double dytdB = -x161*x203 + x163*x203 + x165*x202 - x167*x202 + x168*x201 - x170*x201 + x171*x207; - - res(0,0) = dqopdqop0; - res(0,1) = dqopdlam0; - res(0,2) = dqopdphi0; - res(0,3) = dqopdxt0; - res(0,4) = dqopdyt0; - res(0,5) = dqopdB; - res(1,0) = dlamdqop0; - res(1,1) = dlamdlam0; - res(1,2) = dlamdphi0; - res(1,3) = dlamdxt0; - res(1,4) = dlamdyt0; - res(1,5) = dlamdB; - res(2,0) = dphidqop0; - res(2,1) = dphidlam0; - res(2,2) = dphidphi0; - res(2,3) = dphidxt0; - res(2,4) = dphidyt0; - res(2,5) = dphidB; - res(3,0) = dxtdqop0; - res(3,1) = dxtdlam0; - res(3,2) = dxtdphi0; - res(3,3) = dxtdxt0; - res(3,4) = dxtdyt0; - res(3,5) = dxtdB; - res(4,0) = dytdqop0; - res(4,1) = dytdlam0; - res(4,2) = dytdphi0; - res(4,3) = dytdxt0; - res(4,4) = dytdyt0; - res(4,5) = dytdB; - } - - // convert to tesla for B field gradient - res.col(5) *= 2.99792458e-3; + const GlobalPoint pos(state[0], state[1], state[2]); + const GlobalVector &bfield = field->inInverseGeV(pos); + const Matrix Bv(bfield.x(), bfield.y(), double(bfield.z()) + 2.99792458e-3*dBz); - return res; - -} - -Matrix ResidualGlobalCorrectionMakerBase::hybrid2curvJacobian(const FreeTrajectoryState &state) const { - const GlobalTrajectoryParameters &globalSource = state.parameters(); - const GlobalVector &bfield = state.parameters().magneticFieldInInverseGeV(); + const double q = state[6]; - CurvilinearTrajectoryParameters curvparms(globalSource.position(), globalSource.momentum(), globalSource.charge()); - - const double qop0 = globalSource.signedInverseMomentum(); - const double lam0 = curvparms.lambda(); - const double phi0 = curvparms.phi(); - - const double x0 = globalSource.position().x(); - const double y0 = globalSource.position().y(); - const double z0 = globalSource.position().z(); + const double qop0 = q/state.segment<3>(3).norm(); + const double lam0 = std::atan(state[5]/std::sqrt(state[3]*state[3] + state[4]*state[4])); + const double phi0 = std::atan2(state[4], state[3]); - const double p0 = globalSource.momentum().mag(); - const GlobalVector W0 = globalSource.momentum()/p0; - const double W0x = W0.x(); - const double W0y = W0.y(); - const double W0z = W0.z(); + const Matrix W0 = state.segment<3>(3).normalized(); + const double W0x = W0[0]; + const double W0y = W0[1]; + const double W0z = W0[2]; + + const double x0 = state[0]; + const double y0 = state[1]; + const double z0 = state[2]; - const double B = bfield.mag(); - const GlobalVector H = bfield/B; - const double hx = H.x(); - const double hy = H.y(); - const double hz = H.z(); + const double B = Bv.norm(); + const Matrix H = Bv.normalized(); + const double hx = H[0]; + const double hy = H[1]; + const double hz = H[2]; const double xf0 = std::cos(lam0); const double xf1 = std::pow(xf0, 2); @@ -1489,916 +1230,994 @@ Matrix ResidualGlobalCorrectionMakerBase::hybrid2curvJacobian(cons } -Matrix ResidualGlobalCorrectionMakerBase::hybrid2curvJacobianD(const Matrix &state, const MagneticField *field, double dBz) const { - - const GlobalPoint pos(state[0], state[1], state[2]); - const GlobalVector &bfield = field->inInverseGeV(pos); - const Matrix Bv(bfield.x(), bfield.y(), double(bfield.z()) + 2.99792458e-3*dBz); - - const double q = state[6]; - - const double qop0 = q/state.segment<3>(3).norm(); - const double lam0 = std::atan(state[5]/std::sqrt(state[3]*state[3] + state[4]*state[4])); - const double phi0 = std::atan2(state[4], state[3]); - - const Matrix W0 = state.segment<3>(3).normalized(); - const double W0x = W0[0]; - const double W0y = W0[1]; - const double W0z = W0[2]; +Matrix ResidualGlobalCorrectionMakerBase::pca2cart(const Matrix &statepca, const reco::BeamSpot &bs) const { + const double x0bs = bs.x0(); + const double y0bs = bs.y0(); + const double z0bs = bs.z0(); + const double dxdzbs = bs.dxdz(); + const double dydzbs = bs.dydz(); + + const Matrix bv(dxdzbs, dydzbs, 1.); + + const double qop0 = statepca[0]; + const double lam0 = statepca[1]; + const double phi0 = statepca[2]; + const double d0 = statepca[3]; + const double z0 = statepca[4]; + + const double p0 = std::abs(1./qop0); + const double q = std::copysign(1., qop0); + + const double px = p0*std::cos(lam0)*std::cos(phi0); + const double py = p0*std::cos(lam0)*std::sin(phi0); + const double pz = p0*std::sin(lam0); + + const Matrix mom(px, py, pz); + + // pca on beamline + Matrix pcabs; + pcabs[0] = x0bs + (z0 - z0bs)*dxdzbs; + pcabs[1] = y0bs + (z0 - z0bs)*dydzbs; + pcabs[2] = z0; + + const Matrix dv = bv.cross(mom).normalized()*d0; + + const Matrix xyz = pcabs + dv; + + Matrix res; + res.head<3>() = xyz; + res.segment<3>(3) = mom; + res[6] = q; + + return res; + +} + +Matrix ResidualGlobalCorrectionMakerBase::cart2pca(const Matrix &state, const reco::BeamSpot &bs) const { + + const double x0bs = bs.x0(); + const double y0bs = bs.y0(); + const double z0bs = bs.z0(); + const double dxdzbs = bs.dxdz(); + const double dydzbs = bs.dydz(); + + const Matrix xyz0bs(x0bs, y0bs, z0bs); + const Matrix bv(dxdzbs, dydzbs, 1.); + const Matrix bhat = bv.normalized(); + + const Matrix xyz0 = state.head<3>(); + const Matrix mom0 = state.segment<3>(3); + + // track reference point in beamspot local coordinates + const double vpll = (xyz0 - xyz0bs).dot(bhat); + + // point of closest approach on beamline + const Matrix pcabs = xyz0bs + vpll*bhat; + const double z0 = pcabs[2]; + + // vector from pca on beamline to track reference point + const Matrix dv = xyz0 - pcabs; + const double d0 = bhat.cross(mom0).normalized().dot(dv); + + const double q = state[6]; + + const double qop0 = q/state.segment<3>(3).norm(); + const double lam0 = std::atan(state[5]/std::sqrt(state[3]*state[3] + state[4]*state[4])); + const double phi0 = std::atan2(state[4], state[3]); + + Matrix res; + res[0] = qop0; + res[1] = lam0; + res[2] = phi0; + res[3] = d0; + res[4] = z0; + + return res; + +} + +Matrix ResidualGlobalCorrectionMakerBase::pca2curvJacobianD(const Matrix &state, const MagneticField *field, const reco::BeamSpot &bs, double dBz) const { + + const GlobalPoint pos(state[0], state[1], state[2]); + const GlobalVector &bfield = field->inInverseGeV(pos); + const Matrix Bv(bfield.x(), bfield.y(), double(bfield.z()) + 2.99792458e-3*dBz); + + const double x0bs = bs.x0(); + const double y0bs = bs.y0(); + const double z0bs = bs.z0(); + const double dxdzbs = bs.dxdz(); + const double dydzbs = bs.dydz(); + + const Matrix W0 = state.segment<3>(3).normalized(); + const double W0x = W0[0]; + const double W0y = W0[1]; + const double W0z = W0[2]; + + const Matrix statepca = cart2pca(state, bs); + const double qop0 = statepca[0]; + const double lam0 = statepca[1]; + const double phi0 = statepca[2]; + const double d0 = statepca[3]; + const double z0 = statepca[4]; - const double x0 = state[0]; - const double y0 = state[1]; - const double z0 = state[2]; - const double B = Bv.norm(); const Matrix H = Bv.normalized(); const double hx = H[0]; const double hy = H[1]; const double hz = H[2]; - - const double xf0 = std::cos(lam0); - const double xf1 = std::pow(xf0, 2); - const double xf2 = std::sqrt(xf1); - const double xf3 = 2*phi0; - const double xf4 = std::sin(xf3); - const double xf5 = std::cos(xf3); - const double xf6 = B*qop0; - const double xf7 = (1.0/4.0)*xf6*std::sqrt(2*std::cos(2*lam0) + 2); - const double xf8 = std::tan(lam0); - const double xf9 = std::sin(phi0); - const double xf10 = std::cos(phi0); - const double xf11 = std::sin(lam0); - const double xf12 = xf6*(hx*xf10*xf11 + hy*xf11*xf9 - hz*xf0); - const double xf13 = std::pow(W0x, 2) + std::pow(W0y, 2); - const double xf14 = std::pow(xf13, -1.0/2.0); - const double xf15 = W0x*xf9; - const double xf16 = W0y*xf10; - const double xf17 = xf1*(xf15 - xf16); - const double xf18 = W0x*W0z; - const double xf19 = W0y*W0z; - const double xf20 = xf0*xf10*xf18 + xf0*xf19*xf9 - xf11*xf13; - const double xf21 = xf0*xf20; + + const double xf0 = std::sin(lam0); + const double xf1 = std::sin(phi0); + const double xf2 = std::pow(xf1, 2); + const double xf3 = std::cos(lam0); + const double xf4 = std::pow(xf3, 2); + const double xf5 = xf2*xf4; + const double xf6 = std::cos(phi0); + const double xf7 = std::pow(xf6, 2); + const double xf8 = xf4*xf7; + const double xf9 = xf5 + xf8; + const double xf10 = 1.0/xf9; + const double xf11 = xf0*xf3; + const double xf12 = std::sqrt(xf4)*(-hx*xf1 + hy*xf6); + const double xf13 = xf12/xf3; + const double xf14 = B*d0; + const double xf15 = qop0*xf14; + const double xf16 = dxdzbs*xf1; + const double xf17 = dydzbs*xf6; + const double xf18 = xf16 - xf17; + const double xf19 = std::pow(dxdzbs, 2); + const double xf20 = std::pow(dydzbs, 2); + const double xf21 = dxdzbs*xf0; + const double xf22 = xf3*xf6; + const double xf23 = dydzbs*xf0; + const double xf24 = xf1*xf3; + const double xf25 = std::pow(-2*xf16*xf17*xf4 - xf19*xf8 + xf19 + xf20*xf4*xf7 - xf20*xf4 + xf20 - 2*xf21*xf22 - 2*xf23*xf24 + xf4, -1.0/2.0); + const double xf26 = xf18*xf25; + const double xf27 = xf15*xf26; + const double xf28 = -xf1*xf23 - xf21*xf6 + xf3; + const double xf29 = xf25*xf28; + const double xf30 = -dxdzbs*xf22 - dydzbs*xf24 - xf0; + const double xf31 = B*qop0; + const double xf32 = xf30*xf31; + const double xf33 = std::tan(lam0); + const double xf34 = hx*xf33*xf6 + hy*xf1*xf33 - hz; + const double xf35 = xf15*xf3; + const double xf36 = xf21 - xf22; + const double xf37 = xf14*xf36; + const double xf38 = z0 - z0bs; + const double xf39 = dydzbs*xf38 + y0bs; + const double xf40 = xf23 - xf24; + const double xf41 = std::pow(xf18, 2); + const double xf42 = std::pow(xf36, 2) + xf4*xf41 + std::pow(xf40, 2); + const double xf43 = std::sqrt(xf42); + const double xf44 = B*xf39*xf43 - xf37; + const double xf45 = 1.0/qop0; + const double xf46 = std::pow(W0x, 2); + const double xf47 = std::pow(W0y, 2); + const double xf48 = xf46 + xf47; + const double xf49 = std::pow(xf48, -1.0/2.0); + const double xf50 = W0x*xf49; + const double xf51 = 1.0/xf43; + const double xf52 = 1.0/B; + const double xf53 = xf51*xf52; + const double xf54 = xf50*xf53; + const double xf55 = xf45*xf54; + const double xf56 = xf14*xf40; + const double xf57 = dxdzbs*xf38 + x0bs; + const double xf58 = B*xf43; + const double xf59 = xf57*xf58; + const double xf60 = xf56 + xf59; + const double xf61 = W0y*xf49; + const double xf62 = xf53*xf61; + const double xf63 = xf45*xf62; + const double xf64 = std::pow(qop0, -2); + const double xf65 = qop0*xf56 + qop0*xf59; + const double xf66 = B*qop0*xf39*xf43 - qop0*xf37; + const double xf67 = W0x*xf1 - W0y*xf6; + const double xf68 = xf3*xf67; + const double xf69 = d0*xf49; + const double xf70 = dxdzbs*xf3; + const double xf71 = xf0*xf6; + const double xf72 = dydzbs*xf3; + const double xf73 = xf0*xf1; + const double xf74 = -xf11*xf41 + (1.0/2.0)*xf36*(2*xf70 + 2*xf71) + (1.0/2.0)*xf40*(2*xf72 + 2*xf73); + const double xf75 = B*qop0*xf39*xf51*xf74 - xf15*(xf70 + xf71); + const double xf76 = xf31*xf51; + const double xf77 = xf15*(xf72 + xf73) + xf57*xf74*xf76; + const double xf78 = -xf74; + const double xf79 = std::pow(xf42, -3.0/2.0); + const double xf80 = xf45*xf52*xf79; + const double xf81 = xf78*xf80; + const double xf82 = xf50*xf66; + const double xf83 = xf61*xf65; + const double xf84 = dxdzbs*xf6; + const double xf85 = dydzbs*xf1; + const double xf86 = (1.0/2.0)*xf18*xf4*(2*xf84 + 2*xf85) - xf22*xf40 + xf24*xf36; + const double xf87 = B*qop0*xf39*xf51*xf86 - xf15*xf24; + const double xf88 = B*qop0*xf51*xf57*xf86 - xf15*xf22; + const double xf89 = -xf86; + const double xf90 = xf80*xf89; + const double xf91 = xf36*xf51; + const double xf92 = xf40*xf51; + const double xf93 = xf30*xf49; + const double xf94 = W0z*xf50; + const double xf95 = xf45*xf53*xf94; + const double xf96 = W0z*xf61; + const double xf97 = xf45*xf53*xf96; + const double xf98 = xf46*xf49 + xf47*xf49; + const double xf99 = xf18*xf3; + const double xf100 = xf58*z0; + const double xf101 = qop0*xf100 + xf15*xf99; + const double xf102 = xf51*xf98; + const double xf103 = -W0x*W0z*xf22 - W0y*W0z*xf24 + xf0*xf48; + const double xf104 = xf65*xf94; + const double xf105 = xf66*xf96; const double dqopdqop0 = 1; const double dqopdlam0 = 0; const double dqopdphi0 = 0; - const double dqopdx0 = 0; - const double dqopdy0 = 0; + const double dqopdd0 = 0; const double dqopdz0 = 0; const double dlamdqop0 = 0; - const double dlamdlam0 = xf2/xf0; - const double dlamdphi0 = 0; - const double dlamdx0 = xf7*(hx*xf4 - hy*xf5 - hy); - const double dlamdy0 = xf7*(-hx*xf5 + hx - hy*xf4); - const double dlamdz0 = xf2*xf6*xf8*(hx*xf9 - hy*xf10); + const double dlamdlam0 = xf13*xf27 + (xf0*(xf11*xf2 + xf11*xf7)/std::pow(xf9, 3.0/2.0) + xf3/std::sqrt(xf9))/(std::pow(xf0, 2)*xf10 + 1); + const double dlamdphi0 = xf12*xf15*xf29; + const double dlamdd0 = 0; + const double dlamdz0 = xf13*xf32; const double dphidqop0 = 0; - const double dphidlam0 = 0; - const double dphidphi0 = 1; - const double dphidx0 = -xf10*xf12; - const double dphidy0 = -xf12*xf9; - const double dphidz0 = -xf12*xf8; - const double dxtdqop0 = 0; - const double dxtdlam0 = 0; - const double dxtdphi0 = 0; - const double dxtdx0 = -xf14*(W0y + xf10*xf17); - const double dxtdy0 = xf14*(W0x - xf17*xf9); - const double dxtdz0 = xf0*xf11*xf14*(-xf15 + xf16); - const double dytdqop0 = 0; - const double dytdlam0 = 0; - const double dytdphi0 = 0; - const double dytdx0 = xf14*(xf10*xf21 - xf18); - const double dytdy0 = xf14*(-xf19 + xf21*xf9); - const double dytdz0 = xf14*(xf11*xf20 + xf13); - Matrix res; + const double dphidlam0 = xf27*xf34; + const double dphidphi0 = xf10*xf5 + xf10*xf8 + xf29*xf34*xf35; + const double dphidd0 = 0; + const double dphidz0 = xf32*xf34; + const double dxtdqop0 = xf44*xf55 - xf54*xf64*xf66 - xf60*xf63 + xf62*xf64*xf65; + const double dxtdlam0 = xf26*xf68*xf69 + xf55*xf75 - xf63*xf77 + xf81*xf82 - xf81*xf83; + const double dxtdphi0 = xf29*xf4*xf67*xf69 + xf55*xf87 - xf63*xf88 + xf82*xf90 - xf83*xf90; + const double dxtdd0 = -xf50*xf91 - xf61*xf92; + const double dxtdz0 = -dxdzbs*xf61 + dydzbs*xf50 + xf68*xf93; + const double dytdqop0 = W0x*W0z*xf49*xf51*xf52*xf64*xf65 + W0y*W0z*xf49*xf51*xf52*xf64*xf66 - xf101*xf102*xf52*xf64 - xf44*xf97 + xf45*xf51*xf52*xf98*(xf100 + xf14*xf99) - xf60*xf95; + const double dytdlam0 = d0*xf103*xf18*xf25*xf49 + xf101*xf45*xf52*xf78*xf79*xf98 - xf104*xf81 - xf105*xf81 + xf45*xf51*xf52*xf98*(B*qop0*xf51*xf74*z0 - xf0*xf15*xf18) - xf75*xf97 - xf77*xf95; + const double dytdphi0 = d0*xf103*xf25*xf28*xf3*xf49 + xf101*xf45*xf52*xf79*xf89*xf98 - xf104*xf90 - xf105*xf90 + xf45*xf51*xf52*xf98*(xf35*(xf84 + xf85) + xf76*xf86*z0) - xf87*xf97 - xf88*xf95; + const double dytdd0 = xf102*xf99 + xf91*xf96 - xf92*xf94; + const double dytdz0 = -dxdzbs*xf94 - dydzbs*xf96 + xf103*xf93 + xf98; + Matrix res; res(0,0) = dqopdqop0; res(0,1) = dqopdlam0; res(0,2) = dqopdphi0; - res(0,3) = dqopdx0; - res(0,4) = dqopdy0; - res(0,5) = dqopdz0; + res(0,3) = dqopdd0; + res(0,4) = dqopdz0; res(1,0) = dlamdqop0; res(1,1) = dlamdlam0; res(1,2) = dlamdphi0; - res(1,3) = dlamdx0; - res(1,4) = dlamdy0; - res(1,5) = dlamdz0; + res(1,3) = dlamdd0; + res(1,4) = dlamdz0; res(2,0) = dphidqop0; res(2,1) = dphidlam0; res(2,2) = dphidphi0; - res(2,3) = dphidx0; - res(2,4) = dphidy0; - res(2,5) = dphidz0; + res(2,3) = dphidd0; + res(2,4) = dphidz0; res(3,0) = dxtdqop0; res(3,1) = dxtdlam0; res(3,2) = dxtdphi0; - res(3,3) = dxtdx0; - res(3,4) = dxtdy0; - res(3,5) = dxtdz0; + res(3,3) = dxtdd0; + res(3,4) = dxtdz0; res(4,0) = dytdqop0; res(4,1) = dytdlam0; res(4,2) = dytdphi0; - res(4,3) = dytdx0; - res(4,4) = dytdy0; - res(4,5) = dytdz0; - + res(4,3) = dytdd0; + res(4,4) = dytdz0; + return res; } -Matrix ResidualGlobalCorrectionMakerBase::hybrid2localTransportJacobian(const FreeTrajectoryState& start, - const std::pair& propresult) const { - - // hybrid to curvilinear - Matrix FdF = hybrid2curvTransportJacobian(start, propresult); - - // compute curvilinear to local jacobian at destination - const TrajectoryStateOnSurface &state1 = propresult.first; - JacobianCurvilinearToLocal curv2local(state1.surface(), state1.localParameters(), *state1.magneticField()); - const AlgebraicMatrix55& curv2localjac = curv2local.jacobian(); - const Matrix H1 = Map>(curv2localjac.Array()); - - return H1*FdF; - +Matrix ResidualGlobalCorrectionMakerBase::pca2cartJacobianD(const Matrix &state, const reco::BeamSpot &bs) const { + + const double x0bs = bs.x0(); + const double y0bs = bs.y0(); + const double z0bs = bs.z0(); + const double dxdzbs = bs.dxdz(); + const double dydzbs = bs.dydz(); + + const Matrix statepca = cart2pca(state, bs); + const double qop0 = statepca[0]; + const double lam0 = statepca[1]; + const double phi0 = statepca[2]; + const double d0 = statepca[3]; + const double z0 = statepca[4]; + + const double xf0 = std::sin(lam0); + const double xf1 = dxdzbs*xf0; + const double xf2 = std::cos(phi0); + const double xf3 = dydzbs*xf2; + const double xf4 = std::cos(lam0); + const double xf5 = std::pow(dxdzbs, 2); + const double xf6 = xf2*xf4; + const double xf7 = std::sin(phi0); + const double xf8 = dxdzbs*xf7; + const double xf9 = xf6*xf8; + const double xf10 = std::pow(xf2, 2); + const double xf11 = std::pow(dydzbs, 2); + const double xf12 = xf1 - xf6; + const double xf13 = xf4*xf7; + const double xf14 = dydzbs*xf0 - xf13; + const double xf15 = -xf3 + xf8; + const double xf16 = std::pow(xf12, 2) + std::pow(xf14, 2) + std::pow(xf15, 2)*std::pow(xf4, 2); + const double xf17 = d0/std::pow(xf16, 3.0/2.0); + const double xf18 = dxdzbs*xf2 + dydzbs*xf7; + const double xf19 = xf15*xf4; + const double xf20 = xf12*xf7 - xf14*xf2 + xf18*xf19; + const double xf21 = xf17*xf4; + const double xf22 = std::pow(xf16, -1.0/2.0); + const double xf23 = dxdzbs*xf4; + const double xf24 = std::pow(xf7, 2); + const double xf25 = xf0*xf11; + const double xf26 = 1.0/qop0; + const double xf27 = 1.0/std::fabs(qop0); + const double xf28 = xf27*xf6; + const double xf29 = xf0*xf27; + const double xf30 = xf13*xf27; + const double dxdqop0 = 0; + const double dxdlam0 = xf17*(std::pow(dydzbs, 3)*xf10*xf4 - dydzbs*xf10*xf4*xf5 + dydzbs*xf10*xf4 + dydzbs*xf4*xf5 + xf0*xf5*xf7 - xf1*xf3 - 2*xf11*xf9 - xf9); + const double dxdphi0 = xf21*(-xf14*xf20 - xf16*xf2); + const double dxdd0 = xf14*xf22; + const double dxdz0 = dxdzbs; + const double dydqop0 = 0; + const double dydlam0 = xf17*(-std::pow(dxdzbs, 3)*xf24*xf4 + dxdzbs*dydzbs*xf0*xf7 + dxdzbs*xf11*xf24*xf4 + 2*dydzbs*xf2*xf4*xf5*xf7 + dydzbs*xf2*xf4*xf7 - xf11*xf23 - xf2*xf25 - xf23*xf24); + const double dydphi0 = xf21*(xf12*xf20 - xf16*xf7); + const double dydd0 = -xf12*xf22; + const double dydz0 = dydzbs; + const double dzdqop0 = 0; + const double dzdlam0 = xf15*xf17*(dxdzbs*xf2*xf4 + dydzbs*xf4*xf7 - xf0*xf5 - xf25); + const double dzdphi0 = xf21*(-std::pow(xf15, 2)*xf4*(dxdzbs*xf6 + dydzbs*xf13 + xf0) + xf16*xf18); + const double dzdd0 = xf19*xf22; + const double dzdz0 = 1; + const double dpxdqop0 = -xf26*xf28; + const double dpxdlam0 = -xf2*xf29; + const double dpxdphi0 = -xf30; + const double dpxdd0 = 0; + const double dpxdz0 = 0; + const double dpydqop0 = -xf26*xf30; + const double dpydlam0 = -xf29*xf7; + const double dpydphi0 = xf28; + const double dpydd0 = 0; + const double dpydz0 = 0; + const double dpzdqop0 = -xf26*xf29; + const double dpzdlam0 = xf27*xf4; + const double dpzdphi0 = 0; + const double dpzdd0 = 0; + const double dpzdz0 = 0; + Matrix res; + res(0,0) = dxdqop0; + res(0,1) = dxdlam0; + res(0,2) = dxdphi0; + res(0,3) = dxdd0; + res(0,4) = dxdz0; + res(1,0) = dydqop0; + res(1,1) = dydlam0; + res(1,2) = dydphi0; + res(1,3) = dydd0; + res(1,4) = dydz0; + res(2,0) = dzdqop0; + res(2,1) = dzdlam0; + res(2,2) = dzdphi0; + res(2,3) = dzdd0; + res(2,4) = dzdz0; + res(3,0) = dpxdqop0; + res(3,1) = dpxdlam0; + res(3,2) = dpxdphi0; + res(3,3) = dpxdd0; + res(3,4) = dpxdz0; + res(4,0) = dpydqop0; + res(4,1) = dpydlam0; + res(4,2) = dpydphi0; + res(4,3) = dpydd0; + res(4,4) = dpydz0; + res(5,0) = dpzdqop0; + res(5,1) = dpzdlam0; + res(5,2) = dpzdphi0; + res(5,3) = dpzdd0; + res(5,4) = dpzdz0; + + return res; } -Matrix ResidualGlobalCorrectionMakerBase::hybrid2curvTransportJacobian(const FreeTrajectoryState& start, - const std::pair& propresult) const { - - const FreeTrajectoryState& end = *propresult.first.freeState(); - const TrajectoryStateOnSurface& proptsos = propresult.first; - - const FreeTrajectoryState& state0 = start; - const FreeTrajectoryState& state1 = end; - const GlobalVector& h = start.parameters().magneticFieldInInverseGeV(); -// const GlobalVector h = 0.5*(start.parameters().magneticFieldInInverseGeV() + end.parameters().magneticFieldInInverseGeV()); - const double s = propresult.second; - - const Matrix FdF = hybrid2curvTransportJacobian(state0.parameters(), state1.parameters(), s, h); - -// const Matrix FdFvar = hybrid2curvTransportJacobianVar(state0.parameters(), state1.parameters(), s, h); - -// std::cout << "FdF hybrid:" << std::endl; -// std::cout << FdF << std::endl; - -// std::cout << "FdF hybrid var:" << std::endl; -// std::cout << FdFvar << std::endl; +std::array, 2> ResidualGlobalCorrectionMakerBase::twoTrackPca2cart(const Matrix &statepca) const { + + const double qopa0 = statepca[0]; + const double lama0 = statepca[1]; + const double phia0 = statepca[2]; + const double qopb0 = statepca[3]; + const double lamb0 = statepca[4]; + const double phib0 = statepca[5]; + + const double d0 = statepca[6]; + + const Matrix xyz0 = statepca.tail<3>(); + + const double pa0 = std::abs(1./qopa0); + const double qa = std::copysign(1., qopa0); + + const double pax = pa0*std::cos(lama0)*std::cos(phia0); + const double pay = pa0*std::cos(lama0)*std::sin(phia0); + const double paz = pa0*std::sin(lama0); + + const double pb0 = std::abs(1./qopb0); + const double qb = std::copysign(1., qopb0); + + const double pbx = pb0*std::cos(lamb0)*std::cos(phib0); + const double pby = pb0*std::cos(lamb0)*std::sin(phib0); + const double pbz = pb0*std::sin(lamb0); + + const Matrix moma0(pax, pay, paz); + const Matrix momb0(pbx, pby, pbz); + + const Matrix dvhat = moma0.cross(momb0).normalized(); + const Matrix dv = d0*dvhat; + + const Matrix xyza0 = xyz0 - 0.5*dv; + const Matrix xyzb0 = xyz0 + 0.5*dv; + + std::array, 2> res; + Matrix &resa = res[0]; + Matrix &resb = res[1]; - return FdF; -// return FdFvar; + resa.head<3>() = xyza0; + resa.segment<3>(3) = moma0; + resa[6] = qa; + + resb.head<3>() = xyzb0; + resb.segment<3>(3) = momb0; + resb[6] = qb; + + return res; } -Matrix ResidualGlobalCorrectionMakerBase::hybrid2curvTransportJacobian(const GlobalTrajectoryParameters& globalSource, - const GlobalTrajectoryParameters& globalDest, - const double& s, - const GlobalVector& bfield) const { +Matrix ResidualGlobalCorrectionMakerBase::twoTrackCart2pca(const Matrix &state0, const Matrix &state1) const { - const double qop0 = globalSource.signedInverseMomentum(); + const Matrix xyza0 = state0.head<3>(); + const Matrix moma0 = state0.segment<3>(3); - - const double x0 = globalSource.position().x(); - const double y0 = globalSource.position().y(); - const double z0 = globalSource.position().z(); - - const double p0 = globalSource.momentum().mag(); - const GlobalVector W0 = globalSource.momentum()/p0; - const double W0x = W0.x(); - const double W0y = W0.y(); - const double W0z = W0.z(); - - const double B = bfield.mag(); - const GlobalVector H = bfield/B; - const double hx = H.x(); - const double hy = H.y(); - const double hz = H.z(); - - const double xf0 = B*s; - const double xf1 = qop0*xf0; - const double xf2 = std::cos(xf1); - const double xf3 = std::pow(W0z, 2); - const double xf4 = std::pow(W0x, 2) + std::pow(W0y, 2); - const double xf5 = 1.0/xf4; - const double xf6 = std::pow(xf3*xf5 + 1, -1.0/2.0); - const double xf7 = xf2*xf6; - const double xf8 = std::pow(xf4, -1.0/2.0); - const double xf9 = W0z*xf8; - const double xf10 = xf7*xf9; - const double xf11 = W0y*hx; - const double xf12 = xf11*xf8; - const double xf13 = W0x*hy; - const double xf14 = xf13*xf8; - const double xf15 = xf12 - xf14; - const double xf16 = std::sin(xf1); - const double xf17 = xf16*xf6; - const double xf18 = xf15*xf17; - const double xf19 = xf2 - 1; - const double xf20 = hx*xf6; - const double xf21 = xf20*xf8; - const double xf22 = hy*xf6; - const double xf23 = W0y*xf8; - const double xf24 = hz*xf6; - const double xf25 = xf24*xf8; - const double xf26 = W0x*xf21 + W0z*xf25 + xf22*xf23; - const double xf27 = xf19*xf26; - const double xf28 = hz*xf27; - const double xf29 = xf10 - xf18 - xf28; - const double xf30 = xf23*xf7; - const double xf31 = W0x*xf25; - const double xf32 = W0z*xf21 - xf31; - const double xf33 = xf16*xf32; - const double xf34 = hy*xf27; - const double xf35 = xf30 + xf33 - xf34; - const double xf36 = W0x*xf8; - const double xf37 = xf36*xf7; - const double xf38 = xf22*xf9; - const double xf39 = W0y*xf25; - const double xf40 = xf38 - xf39; - const double xf41 = xf16*xf40; - const double xf42 = hx*xf27; - const double xf43 = xf37 - xf41 - xf42; - const double xf44 = std::pow(xf35, 2) + std::pow(xf43, 2); - const double xf45 = 1.0/xf44; - const double xf46 = 1.0/(std::pow(xf29, 2)*xf45 + 1); - const double xf47 = xf17*xf9; - const double xf48 = -xf12 + xf14; - const double xf49 = xf48*xf7; - const double xf50 = hz*xf26; - const double xf51 = xf16*xf50; - const double xf52 = std::pow(xf44, -1.0/2.0); - const double xf53 = xf17*xf23; - const double xf54 = xf0*xf53; - const double xf55 = xf0*xf2; - const double xf56 = xf32*xf55; - const double xf57 = hy*xf26; - const double xf58 = xf0*xf16; - const double xf59 = xf57*xf58; - const double xf60 = (1.0/2.0)*xf35; - const double xf61 = xf17*xf36; - const double xf62 = xf0*xf61; - const double xf63 = -xf38 + xf39; - const double xf64 = xf55*xf63; - const double xf65 = hx*xf26; - const double xf66 = xf58*xf65; - const double xf67 = (1.0/2.0)*xf43; - const double xf68 = xf29/std::pow(xf44, 3.0/2.0); - const double xf69 = B*z0; - const double xf70 = xf19*xf6; - const double xf71 = xf15*xf70; - const double xf72 = xf1 - xf16; - const double xf73 = qop0*xf69 + xf47 + xf50*xf72 + xf71; - const double xf74 = 1.0/B; - const double xf75 = xf74/std::pow(qop0, 2); - const double xf76 = xf73*xf75; - const double xf77 = xf0 - xf55; - const double xf78 = 1.0/qop0; - const double xf79 = xf74*xf78; - const double xf80 = xf79*(xf0*xf10 - xf0*xf18 + xf50*xf77 + xf69); - const double xf81 = B*y0; - const double xf82 = qop0*xf81 - xf19*xf32 + xf53 + xf57*xf72; - const double xf83 = xf75*xf82; - const double xf84 = xf79*(xf0*xf30 + xf0*xf33 + xf57*xf77 + xf81); - const double xf85 = B*x0; - const double xf86 = qop0*xf85 + xf19*xf40 + xf61 + xf65*xf72; - const double xf87 = xf75*xf86; - const double xf88 = xf79*(xf0*xf37 - xf0*xf41 + xf65*xf77 + xf85); - const double xf89 = -xf29*(-xf76 + xf80) - xf35*(-xf83 + xf84) - xf43*(-xf87 + xf88); - const double xf90 = B*qop0; - const double xf91 = xf53*xf90; - const double xf92 = xf2*xf90; - const double xf93 = xf32*xf92; - const double xf94 = xf16*xf90; - const double xf95 = xf57*xf94; - const double xf96 = xf61*xf90; - const double xf97 = xf63*xf92; - const double xf98 = xf65*xf94; - const double xf99 = xf46*(xf52*(-xf47*xf90 + xf49*xf90 + xf51*xf90) + xf68*(-xf60*(-2*xf91 + 2*xf93 + 2*xf95) - xf67*(-2*xf96 + 2*xf97 + 2*xf98))); - const double xf100 = W0z*xf5; - const double xf101 = W0x*xf100; - const double xf102 = W0y*xf100; - const double xf103 = -xf101*xf20 - xf102*xf22 + xf24; - const double xf104 = hz*xf103; - const double xf105 = xf102*xf7; - const double xf106 = xf101*xf24 + xf20; - const double xf107 = xf106*xf16; - const double xf108 = xf103*xf19; - const double xf109 = hy*xf108; - const double xf110 = xf101*xf7; - const double xf111 = xf102*xf24; - const double xf112 = xf16*(-xf111 - xf22); - const double xf113 = hx*xf108; - const double xf114 = xf104*xf72 + xf17 - xf71*xf9; - const double xf115 = xf29*xf79; - const double xf116 = xf103*xf72; - const double xf117 = hx*xf116 - xf101*xf17 + xf19*(xf111 + xf22); - const double xf118 = xf43*xf79; - const double xf119 = 1 - xf2; - const double xf120 = hy*xf116 - xf102*xf17 + xf106*xf119; - const double xf121 = xf35*xf79; - const double xf122 = -xf114*xf115 - xf117*xf118 - xf120*xf121; - const double xf123 = W0x*hx; - const double xf124 = xf123*xf8; - const double xf125 = W0y*hy; - const double xf126 = xf125*xf8; - const double xf127 = -W0y*xf21 + xf22*xf36; - const double xf128 = hz*xf127; - const double xf129 = xf16*xf39; - const double xf130 = xf127*xf19; - const double xf131 = hy*xf130; - const double xf132 = xf16*xf31; - const double xf133 = hx*xf130; - const double xf134 = xf128*xf72 + xf70*(xf124 + xf126); - const double xf135 = xf127*xf72; - const double xf136 = hy*xf135 + xf119*xf39 + xf61; - const double xf137 = hx*xf135 - xf19*xf31 - xf53; - const double xf138 = -xf115*xf134 - xf118*xf137 - xf121*xf136; - const double xf139 = -xf37 + xf41 + xf42; - const double xf140 = -xf30; - const double xf141 = xf140 - xf33 + xf34; - const double xf142 = -xf10 + xf18 + xf28; - const double xf143 = qop0*s; - const double xf144 = xf143*xf53; - const double xf145 = xf143*xf2; - const double xf146 = xf145*xf32; - const double xf147 = xf143*xf16; - const double xf148 = xf147*xf57; - const double xf149 = xf143*xf61; - const double xf150 = xf145*xf63; - const double xf151 = xf147*xf65; - const double xf152 = xf78/std::pow(B, 2); - const double xf153 = xf152*xf73; - const double xf154 = xf143 - xf145; - const double xf155 = xf79*(qop0*z0 + xf10*xf143 - xf143*xf18 + xf154*xf50); - const double xf156 = xf152*xf82; - const double xf157 = xf79*(qop0*y0 + xf143*xf30 + xf143*xf33 + xf154*xf57); - const double xf158 = xf152*xf86; - const double xf159 = xf79*(qop0*x0 + xf143*xf37 - xf143*xf41 + xf154*xf65); - const double xf160 = -xf29*(-xf153 + xf155) - xf35*(-xf156 + xf157) - xf43*(-xf158 + xf159); - const double xf161 = xf43*xf45; - const double xf162 = xf141*xf45; - const double xf163 = xf161*(-xf91 + xf93 + xf95) + xf162*(-xf96 + xf97 + xf98); - const double xf164 = xf3 + xf4; - const double xf165 = 1.0/xf164; - const double xf166 = xf19*(W0z*hz + xf123 + xf125); - const double xf167 = W0x*xf2 - hx*xf166 + xf16*(W0y*hz - W0z*hy); - const double xf168 = xf165*std::pow(xf167, 2); - const double xf169 = W0y*xf2 - hy*xf166 + xf16*(-W0x*hz + W0z*hx); - const double xf170 = xf165*std::pow(xf169, 2); - const double xf171 = std::pow(xf168 + xf170, -1.0/2.0); - const double xf172 = xf169*xf171; - const double xf173 = xf8/std::sqrt(xf164*xf5); - const double xf174 = xf172*xf173; - const double xf175 = xf167*xf171; - const double xf176 = xf173*xf175; - const double xf177 = xf90 - xf92; - const double xf178 = xf177*xf57 + xf30*xf90 + xf33*xf90; - const double xf179 = xf176*xf79; - const double xf180 = xf177*xf65 + xf37*xf90 - xf41*xf90; - const double xf181 = xf174*xf79; - const double xf182 = xf178*xf179 - xf180*xf181; - const double xf183 = xf165*(W0z*xf2 - hz*xf166 + xf16*(-xf11 + xf13)); - const double xf184 = xf175*xf183; - const double xf185 = xf172*xf183; - const double xf186 = xf168*xf171 + xf170*xf171; - const double xf187 = xf185*xf79; - const double xf188 = xf184*xf79; - const double xf189 = xf186*xf79; - const double xf190 = -xf178*xf187 - xf180*xf188 + xf189*(xf10*xf90 + xf177*xf50 - xf18*xf90); + const Matrix xyzb0 = state1.head<3>(); + const Matrix momb0 = state1.segment<3>(3); - const double dqopdqop0 = 1; - const double dqopdlam0 = 0; - const double dqopdphi0 = 0; - const double dqopdx0 = 0; - const double dqopdy0 = 0; - const double dqopdz0 = 0; - const double dqopdB = 0; - const double dlamdqop0 = xf46*(xf52*(-xf0*xf47 + xf0*xf49 + xf0*xf51) + xf68*(-xf60*(-2*xf54 + 2*xf56 + 2*xf59) - xf67*(-2*xf62 + 2*xf64 + 2*xf66))) + xf89*xf99; - const double dlamdlam0 = xf122*xf99 + xf46*(xf52*(-xf104*xf19 - xf47*xf48 + xf7) + xf68*(-xf60*(-2*xf105 + 2*xf107 - 2*xf109) - xf67*(-2*xf110 + 2*xf112 - 2*xf113))); - const double dlamdphi0 = xf138*xf99 + xf46*(xf52*(-xf128*xf19 + xf17*(-xf124 - xf126)) + xf68*(-xf60*(2*xf129 - 2*xf131 + 2*xf37) - xf67*(2*xf132 - 2*xf133 - 2*xf30))); - const double dlamdx0 = xf139*xf99; - const double dlamdy0 = xf141*xf99; - const double dlamdz0 = xf142*xf99; - const double dlamdB = xf160*xf99 + xf46*(xf52*(-xf143*xf47 + xf143*xf49 + xf143*xf51) + xf68*(-xf60*(-2*xf144 + 2*xf146 + 2*xf148) - xf67*(-2*xf149 + 2*xf150 + 2*xf151))); - const double dphidqop0 = xf161*(-xf54 + xf56 + xf59) + xf162*(-xf62 + xf64 + xf66) + xf163*xf89; - const double dphidlam0 = xf122*xf163 + xf161*(-xf105 + xf107 - xf109) + xf162*(-xf110 + xf112 - xf113); - const double dphidphi0 = xf138*xf163 + xf161*(xf129 - xf131 + xf37) + xf162*(xf132 - xf133 + xf140); - const double dphidx0 = xf139*xf163; - const double dphidy0 = xf141*xf163; - const double dphidz0 = xf142*xf163; - const double dphidB = xf160*xf163 + xf161*(-xf144 + xf146 + xf148) + xf162*(-xf149 + xf150 + xf151); - const double dxtdqop0 = xf174*xf87 - xf174*xf88 - xf176*xf83 + xf176*xf84 + xf182*xf89; - const double dxtdlam0 = -xf117*xf181 + xf120*xf179 + xf122*xf182; - const double dxtdphi0 = xf136*xf179 - xf137*xf181 + xf138*xf182; - const double dxtdx0 = xf139*xf182 - xf174; - const double dxtdy0 = xf141*xf182 + xf176; - const double dxtdz0 = xf142*xf182; - const double dxtdB = -xf156*xf176 + xf157*xf176 + xf158*xf174 - xf159*xf174 + xf160*xf182; - const double dytdqop0 = xf184*xf87 - xf184*xf88 + xf185*xf83 - xf185*xf84 - xf186*xf76 + xf186*xf80 + xf190*xf89; - const double dytdlam0 = xf114*xf189 - xf117*xf188 - xf120*xf187 + xf122*xf190; - const double dytdphi0 = xf134*xf189 - xf136*xf187 - xf137*xf188 + xf138*xf190; - const double dytdx0 = xf139*xf190 - xf184; - const double dytdy0 = xf141*xf190 - xf185; - const double dytdz0 = xf142*xf190 + xf186; - const double dytdB = -xf153*xf186 + xf155*xf186 + xf156*xf185 - xf157*xf185 + xf158*xf184 - xf159*xf184 + xf160*xf190; - - Matrix res; + const Matrix xyz0 = 0.5*(xyza0 + xyzb0); + const Matrix dv = xyzb0 - xyza0; - res(0,0) = dqopdqop0; - res(0,1) = dqopdlam0; - res(0,2) = dqopdphi0; - res(0,3) = dqopdx0; - res(0,4) = dqopdy0; - res(0,5) = dqopdz0; - res(0,6) = dqopdB; - res(1,0) = dlamdqop0; - res(1,1) = dlamdlam0; - res(1,2) = dlamdphi0; - res(1,3) = dlamdx0; - res(1,4) = dlamdy0; - res(1,5) = dlamdz0; - res(1,6) = dlamdB; - res(2,0) = dphidqop0; - res(2,1) = dphidlam0; - res(2,2) = dphidphi0; - res(2,3) = dphidx0; - res(2,4) = dphidy0; - res(2,5) = dphidz0; - res(2,6) = dphidB; - res(3,0) = dxtdqop0; - res(3,1) = dxtdlam0; - res(3,2) = dxtdphi0; - res(3,3) = dxtdx0; - res(3,4) = dxtdy0; - res(3,5) = dxtdz0; - res(3,6) = dxtdB; - res(4,0) = dytdqop0; - res(4,1) = dytdlam0; - res(4,2) = dytdphi0; - res(4,3) = dytdx0; - res(4,4) = dytdy0; - res(4,5) = dytdz0; - res(4,6) = dytdB; + const Matrix dvhat = moma0.cross(momb0).normalized(); + + const double d0 = dvhat.dot(dv); + + const double qa = state0[6]; + + const double qopa0 = qa/state0.segment<3>(3).norm(); + const double lama0 = std::atan(state0[5]/std::sqrt(state0[3]*state0[3] + state0[4]*state0[4])); + const double phia0 = std::atan2(state0[4], state0[3]); + + const double qb = state1[6]; + + const double qopb0 = qb/state1.segment<3>(3).norm(); + const double lamb0 = std::atan(state1[5]/std::sqrt(state1[3]*state1[3] + state1[4]*state1[4])); + const double phib0 = std::atan2(state1[4], state1[3]); + + Matrix res; + res[0] = qopa0; + res[1] = lama0; + res[2] = phia0; + res[3] = qopb0; + res[4] = lamb0; + res[5] = phib0; + res[6] = d0; + res.tail<3>() = xyz0; - // convert to tesla for B field gradient - res.col(6) *= 2.99792458e-3; - return res; - } -Matrix ResidualGlobalCorrectionMakerBase::hybrid2curvTransportJacobianVar(const GlobalTrajectoryParameters& globalSource, - const GlobalTrajectoryParameters& globalDest, - const double& s, - const GlobalVector& bfield) const { +Matrix ResidualGlobalCorrectionMakerBase::twoTrackPca2curvJacobianD(const Matrix &state0, const Matrix &state1, const MagneticField *field, double dBz0, double dBz1) const { + + const GlobalPoint posa(state0[0], state0[1], state0[2]); + const GlobalVector &bfielda = field->inInverseGeV(posa); + const Matrix Bva(bfielda.x(), bfielda.y(), double(bfielda.z()) + 2.99792458e-3*dBz0); + + const GlobalPoint posb(state1[0], state1[1], state1[2]); + const GlobalVector &bfieldb = field->inInverseGeV(posb); + const Matrix Bvb(bfieldb.x(), bfieldb.y(), double(bfieldb.z()) + 2.99792458e-3*dBz1); + + const Matrix Wa0 = state0.segment<3>(3).normalized(); + const double Wa0x = Wa0[0]; + const double Wa0y = Wa0[1]; + const double Wa0z = Wa0[2]; + + const Matrix Wb0 = state1.segment<3>(3).normalized(); + const double Wb0x = Wb0[0]; + const double Wb0y = Wb0[1]; + const double Wb0z = Wb0[2]; + + const Matrix statepca = twoTrackCart2pca(state0, state1); + const double qopa0 = statepca[0]; + const double lama0 = statepca[1]; + const double phia0 = statepca[2]; + const double qopb0 = statepca[3]; + const double lamb0 = statepca[4]; + const double phib0 = statepca[5]; + const double d0 = statepca[6]; + const double x0 = statepca[7]; + const double y0 = statepca[8]; + const double z0 = statepca[9]; + + const double Ba = Bva.norm(); + const Matrix Ha = Bva.normalized(); + const double hax = Ha[0]; + const double hay = Ha[1]; + const double haz = Ha[2]; + + const double Bb = Bvb.norm(); + const Matrix Hb = Bvb.normalized(); + const double hbx = Hb[0]; + const double hby = Hb[1]; + const double hbz = Hb[2]; + + const double xf0 = std::sin(phib0); + const double xf1 = std::sin(lama0); + const double xf2 = std::cos(lamb0); + const double xf3 = xf1*xf2; + const double xf4 = xf0*xf3; + const double xf5 = std::sin(phia0); + const double xf6 = std::cos(lama0); + const double xf7 = std::sin(lamb0); + const double xf8 = xf6*xf7; + const double xf9 = xf5*xf8; + const double xf10 = xf4 - xf9; + const double xf11 = Ba*d0; + const double xf12 = xf10*xf11; + const double xf13 = std::pow(xf6, 2); + const double xf14 = std::pow(xf2, 2); + const double xf15 = phia0 - phib0; + const double xf16 = std::sin(xf15); + const double xf17 = std::pow(xf16, 2); + const double xf18 = std::cos(phib0); + const double xf19 = xf18*xf3; + const double xf20 = std::cos(phia0); + const double xf21 = xf20*xf8; + const double xf22 = xf19 - xf21; + const double xf23 = std::pow(xf10, 2) + xf13*xf14*xf17 + std::pow(xf22, 2); + const double xf24 = std::sqrt(xf23); + const double xf25 = 2*xf24; + const double xf26 = Ba*x0*xf25; + const double xf27 = 1.0/qopa0; + const double xf28 = 1.0/Ba; + const double xf29 = 1.0/xf24; + const double xf30 = (1.0/2.0)*xf29; + const double xf31 = xf28*xf30; + const double xf32 = xf27*xf31; + const double xf33 = xf32*(xf12 + xf26); + const double xf34 = qopa0*xf12 + qopa0*xf26; + const double xf35 = xf31/std::pow(qopa0, 2); + const double xf36 = xf34*xf35; + const double xf37 = xf20*xf6; + const double xf38 = xf11*xf22; + const double xf39 = xf32*(2*Ba*xf24*y0 - xf38); + const double xf40 = 2*Ba*qopa0*xf24*y0 - qopa0*xf38; + const double xf41 = xf35*xf40; + const double xf42 = xf5*xf6; + const double xf43 = -xf37*(xf33 - xf36) - xf42*(xf39 - xf41); + const double xf44 = Ba*qopa0; + const double xf45 = std::sqrt(xf13)*xf44*(-hax*xf5 + hay*xf20); + const double xf46 = xf45/xf6; + const double xf47 = std::pow(xf5, 2); + const double xf48 = xf13*xf47; + const double xf49 = std::pow(xf20, 2); + const double xf50 = xf13*xf49; + const double xf51 = xf48 + xf50; + const double xf52 = 1.0/xf51; + const double xf53 = xf1*xf6; + const double xf54 = d0*xf30; + const double xf55 = xf16*xf54; + const double xf56 = xf1*xf7; + const double xf57 = xf5*xf56; + const double xf58 = xf2*xf6; + const double xf59 = xf0*xf58; + const double xf60 = (1.0/2.0)*xf10; + const double xf61 = xf20*xf56; + const double xf62 = xf18*xf58; + const double xf63 = (1.0/2.0)*xf22; + const double xf64 = -xf1*xf14*xf17*xf6 + xf60*(2*xf57 + 2*xf59) + xf63*(2*xf61 + 2*xf62); + const double xf65 = (1.0/2.0)/std::pow(xf23, 3.0/2.0); + const double xf66 = -xf64*xf65; + const double xf67 = d0*xf16*xf58; + const double xf68 = -xf3*xf55 + xf66*xf67; + const double xf69 = xf57 + xf59; + const double xf70 = qopa0*xf11; + const double xf71 = 2*xf29; + const double xf72 = xf44*xf71; + const double xf73 = xf32*(x0*xf64*xf72 + xf69*xf70); + const double xf74 = xf27*xf28; + const double xf75 = xf66*xf74; + const double xf76 = xf34*xf75; + const double xf77 = xf61 + xf62; + const double xf78 = xf32*(2*Ba*qopa0*xf29*xf64*y0 - xf70*xf77); + const double xf79 = xf40*xf75; + const double xf80 = -xf1*xf68 - xf37*(xf73 + xf76) - xf42*(xf78 + xf79); + const double xf81 = std::cos(xf15); + const double xf82 = xf54*xf58*xf81; + const double xf83 = xf13*xf14*xf16*xf81; + const double xf84 = -xf10*xf20*xf6*xf7 + xf22*xf9 + xf83; + const double xf85 = -xf84; + const double xf86 = xf65*xf67; + const double xf87 = xf82 + xf85*xf86; + const double xf88 = xf32*(2*Ba*qopa0*x0*xf29*xf84 - xf21*xf70); + const double xf89 = xf65*xf74; + const double xf90 = xf85*xf89; + const double xf91 = xf34*xf90; + const double xf92 = xf32*(2*Ba*qopa0*xf29*xf84*y0 - xf70*xf9); + const double xf93 = xf40*xf90; + const double xf94 = -xf1*xf87 - xf37*(xf88 + xf91) - xf42*(xf92 + xf93); + const double xf95 = xf0*xf56; + const double xf96 = xf2*xf42; + const double xf97 = xf18*xf56; + const double xf98 = xf2*xf37; + const double xf99 = -xf13*xf17*xf2*xf7 + xf60*(-2*xf95 - 2*xf96) + xf63*(-2*xf97 - 2*xf98); + const double xf100 = -xf99; + const double xf101 = xf100*xf86 - xf55*xf8; + const double xf102 = xf100*xf89; + const double xf103 = xf102*xf34; + const double xf104 = -xf95 - xf96; + const double xf105 = xf32*(x0*xf72*xf99 + xf104*xf70); + const double xf106 = xf102*xf40; + const double xf107 = -xf97 - xf98; + const double xf108 = xf32*(2*Ba*qopa0*xf29*xf99*y0 - xf107*xf70); + const double xf109 = -xf1*xf101 - xf37*(xf103 + xf105) - xf42*(xf106 + xf108); + const double xf110 = -xf10*xf19 + xf22*xf4 + xf83; + const double xf111 = xf110*xf86 - xf82; + const double xf112 = -xf110; + const double xf113 = xf112*xf72; + const double xf114 = xf32*(x0*xf113 + xf19*xf70); + const double xf115 = xf110*xf89; + const double xf116 = xf115*xf34; + const double xf117 = xf32*(xf113*y0 + xf4*xf70); + const double xf118 = xf115*xf40; + const double xf119 = -xf1*xf111 - xf37*(xf114 + xf116) - xf42*(xf117 + xf118); + const double xf120 = xf16*xf30; + const double xf121 = xf29*xf60; + const double xf122 = -xf120*xf3*xf6 - xf121*xf37 + (1.0/2.0)*xf22*xf29*xf5*xf6; + const double xf123 = std::tan(lama0); + const double xf124 = xf44*(hax*xf123*xf20 + hay*xf123*xf5 - haz); + const double xf125 = std::pow(Wa0x, 2); + const double xf126 = std::pow(Wa0y, 2); + const double xf127 = xf125 + xf126; + const double xf128 = std::pow(xf127, -1.0/2.0); + const double xf129 = Wa0y*xf128; + const double xf130 = Wa0x*xf128; + const double xf131 = Wa0x*xf5 - Wa0y*xf20; + const double xf132 = xf128*xf131; + const double xf133 = xf132*xf6; + const double xf134 = xf29*xf63; + const double xf135 = xf13*xf132; + const double xf136 = Wa0z*xf130; + const double xf137 = Wa0z*xf129; + const double xf138 = -Wa0x*Wa0z*xf37 - Wa0y*Wa0z*xf42 + xf1*xf127; + const double xf139 = xf128*xf138; + const double xf140 = xf125*xf128 + xf126*xf128; + const double xf141 = xf120*xf58; + const double xf142 = -xf68; + const double xf143 = Bb*d0; + const double xf144 = qopb0*xf143; + const double xf145 = Bb*qopb0; + const double xf146 = xf145*xf71; + const double xf147 = 1.0/qopb0; + const double xf148 = 1.0/Bb; + const double xf149 = xf148*xf30; + const double xf150 = xf147*xf149; + const double xf151 = xf150*(xf144*xf77 + xf146*xf64*y0); + const double xf152 = xf143*xf22; + const double xf153 = Bb*xf25*y0; + const double xf154 = qopb0*xf152 + qopb0*xf153; + const double xf155 = xf147*xf148; + const double xf156 = xf155*xf66; + const double xf157 = xf154*xf156; + const double xf158 = xf0*xf2; + const double xf159 = xf150*(2*Bb*qopb0*x0*xf29*xf64 - xf144*xf69); + const double xf160 = xf10*xf143; + const double xf161 = 2*Bb*qopb0*x0*xf24 - qopb0*xf160; + const double xf162 = xf156*xf161; + const double xf163 = xf18*xf2; + const double xf164 = -xf142*xf7 - xf158*(xf151 + xf157) - xf163*(xf159 + xf162); + const double xf165 = std::sqrt(xf14)*xf145*(-hbx*xf0 + hby*xf18); + const double xf166 = xf165/xf2; + const double xf167 = -xf87; + const double xf168 = xf146*xf84; + const double xf169 = xf150*(xf144*xf9 + xf168*y0); + const double xf170 = xf155*xf65; + const double xf171 = xf170*xf85; + const double xf172 = xf154*xf171; + const double xf173 = xf150*(x0*xf168 + xf144*xf21); + const double xf174 = xf161*xf171; + const double xf175 = -xf158*(xf169 + xf172) - xf163*(xf173 + xf174) - xf167*xf7; + const double xf176 = xf150*(xf152 + xf153); + const double xf177 = xf149/std::pow(qopb0, 2); + const double xf178 = xf154*xf177; + const double xf179 = xf150*(2*Bb*x0*xf24 - xf160); + const double xf180 = xf161*xf177; + const double xf181 = -xf158*(xf176 - xf178) - xf163*(xf179 - xf180); + const double xf182 = std::pow(xf0, 2); + const double xf183 = xf14*xf182; + const double xf184 = std::pow(xf18, 2); + const double xf185 = xf14*xf184; + const double xf186 = xf183 + xf185; + const double xf187 = 1.0/xf186; + const double xf188 = xf2*xf7; + const double xf189 = -xf101; + const double xf190 = xf100*xf170; + const double xf191 = xf154*xf190; + const double xf192 = xf150*(xf107*xf144 + xf146*xf99*y0); + const double xf193 = xf161*xf190; + const double xf194 = xf150*(2*Bb*qopb0*x0*xf29*xf99 - xf104*xf144); + const double xf195 = -xf158*(xf191 + xf192) - xf163*(xf193 + xf194) - xf189*xf7; + const double xf196 = -xf111; + const double xf197 = xf150*(2*Bb*qopb0*xf112*xf29*y0 - xf144*xf4); + const double xf198 = xf110*xf170; + const double xf199 = xf154*xf198; + const double xf200 = xf150*(2*Bb*qopb0*x0*xf112*xf29 - xf144*xf19); + const double xf201 = xf161*xf198; + const double xf202 = -xf158*(xf197 + xf199) - xf163*(xf200 + xf201) - xf196*xf7; + const double xf203 = xf120*xf2*xf8 + xf121*xf163 - xf134*xf158; + const double xf204 = std::tan(lamb0); + const double xf205 = xf145*(hbx*xf18*xf204 + hby*xf0*xf204 - hbz); + const double xf206 = std::pow(Wb0x, 2); + const double xf207 = std::pow(Wb0y, 2); + const double xf208 = xf206 + xf207; + const double xf209 = std::pow(xf208, -1.0/2.0); + const double xf210 = Wb0x*xf209; + const double xf211 = Wb0y*xf209; + const double xf212 = xf209*(Wb0x*xf0 - Wb0y*xf18); + const double xf213 = xf2*xf212; + const double xf214 = xf14*xf212; + const double xf215 = Wb0z*xf211; + const double xf216 = Wb0z*xf210; + const double xf217 = xf206*xf209 + xf207*xf209; + const double xf218 = -Wb0x*Wb0z*xf163 - Wb0y*Wb0z*xf158 + xf208*xf7; + const double xf219 = xf209*xf218; + const double dqopadqopa0 = 1; + const double dqopadlama0 = 0; + const double dqopadphia0 = 0; + const double dqopadqopb0 = 0; + const double dqopadlamb0 = 0; + const double dqopadphib0 = 0; + const double dqopadd0 = 0; + const double dqopadx0 = 0; + const double dqopady0 = 0; + const double dqopadz0 = 0; + const double dlamadqopa0 = xf43*xf46; + const double dlamadlama0 = xf46*xf80 + (xf1*(xf47*xf53 + xf49*xf53)/std::pow(xf51, 3.0/2.0) + xf6/std::sqrt(xf51))/(std::pow(xf1, 2)*xf52 + 1); + const double dlamadphia0 = xf46*xf94; + const double dlamadqopb0 = 0; + const double dlamadlamb0 = xf109*xf46; + const double dlamadphib0 = xf119*xf46; + const double dlamadd0 = xf122*xf46; + const double dlamadx0 = -xf20*xf45; + const double dlamady0 = -xf45*xf5; + const double dlamadz0 = -xf1*xf46; + const double dphiadqopa0 = xf124*xf43; + const double dphiadlama0 = xf124*xf80; + const double dphiadphia0 = xf124*xf94 + xf48*xf52 + xf50*xf52; + const double dphiadqopb0 = 0; + const double dphiadlamb0 = xf109*xf124; + const double dphiadphib0 = xf119*xf124; + const double dphiadd0 = xf122*xf124; + const double dphiadx0 = -xf124*xf37; + const double dphiady0 = -xf124*xf42; + const double dphiadz0 = -xf1*xf124; + const double dxtadqopa0 = -xf129*xf33 + xf129*xf36 + xf130*xf39 - xf130*xf41 + xf133*xf43; + const double dxtadlama0 = -xf129*xf73 - xf129*xf76 + xf130*xf78 + xf130*xf79 + xf133*xf80; + const double dxtadphia0 = -xf129*xf88 - xf129*xf91 + xf130*xf92 + xf130*xf93 + xf133*xf94; + const double dxtadqopb0 = 0; + const double dxtadlamb0 = -xf103*xf129 - xf105*xf129 + xf106*xf130 + xf108*xf130 + xf109*xf133; + const double dxtadphib0 = -xf114*xf129 - xf116*xf129 + xf117*xf130 + xf118*xf130 + xf119*xf133; + const double dxtadd0 = -xf121*xf129 + xf122*xf128*xf131*xf6 - xf130*xf134; + const double dxtadx0 = -xf129 - xf135*xf20; + const double dxtady0 = xf130 - xf135*xf5; + const double dxtadz0 = -xf132*xf53; + const double dytadqopa0 = -xf136*xf33 + xf136*xf36 - xf137*xf39 + xf137*xf41 + xf139*xf43; + const double dytadlama0 = xf128*xf138*xf80 - xf136*xf73 - xf136*xf76 - xf137*xf78 - xf137*xf79 + xf140*xf68; + const double dytadphia0 = xf128*xf138*xf94 - xf136*xf88 - xf136*xf91 - xf137*xf92 - xf137*xf93 + xf140*xf87; + const double dytadqopb0 = 0; + const double dytadlamb0 = xf101*xf140 - xf103*xf136 - xf105*xf136 - xf106*xf137 - xf108*xf137 + xf109*xf128*xf138; + const double dytadphib0 = xf111*xf140 - xf114*xf136 - xf116*xf136 - xf117*xf137 - xf118*xf137 + xf119*xf128*xf138; + const double dytadd0 = -xf121*xf136 + xf122*xf139 + xf134*xf137 + xf140*xf141; + const double dytadx0 = -xf136 - xf139*xf37; + const double dytady0 = -xf137 - xf139*xf42; + const double dytadz0 = -xf1*xf139 + xf140; + const double dqopbdqopa0 = 0; + const double dqopbdlama0 = 0; + const double dqopbdphia0 = 0; + const double dqopbdqopb0 = 1; + const double dqopbdlamb0 = 0; + const double dqopbdphib0 = 0; + const double dqopbdd0 = 0; + const double dqopbdx0 = 0; + const double dqopbdy0 = 0; + const double dqopbdz0 = 0; + const double dlambdqopa0 = 0; + const double dlambdlama0 = xf164*xf166; + const double dlambdphia0 = xf166*xf175; + const double dlambdqopb0 = xf166*xf181; + const double dlambdlamb0 = xf166*xf195 + (xf2/std::sqrt(xf186) + xf7*(xf182*xf188 + xf184*xf188)/std::pow(xf186, 3.0/2.0))/(xf187*std::pow(xf7, 2) + 1); + const double dlambdphib0 = xf166*xf202; + const double dlambdd0 = xf166*xf203; + const double dlambdx0 = -xf165*xf18; + const double dlambdy0 = -xf0*xf165; + const double dlambdz0 = -xf166*xf7; + const double dphibdqopa0 = 0; + const double dphibdlama0 = xf164*xf205; + const double dphibdphia0 = xf175*xf205; + const double dphibdqopb0 = xf181*xf205; + const double dphibdlamb0 = xf195*xf205; + const double dphibdphib0 = xf183*xf187 + xf185*xf187 + xf202*xf205; + const double dphibdd0 = xf203*xf205; + const double dphibdx0 = -xf163*xf205; + const double dphibdy0 = -xf158*xf205; + const double dphibdz0 = -xf205*xf7; + const double dxtbdqopa0 = 0; + const double dxtbdlama0 = xf151*xf210 + xf157*xf210 - xf159*xf211 - xf162*xf211 + xf164*xf213; + const double dxtbdphia0 = xf169*xf210 + xf172*xf210 - xf173*xf211 - xf174*xf211 + xf175*xf213; + const double dxtbdqopb0 = xf176*xf210 - xf178*xf210 - xf179*xf211 + xf180*xf211 + xf181*xf213; + const double dxtbdlamb0 = xf191*xf210 + xf192*xf210 - xf193*xf211 - xf194*xf211 + xf195*xf213; + const double dxtbdphib0 = xf197*xf210 + xf199*xf210 - xf200*xf211 - xf201*xf211 + xf202*xf213; + const double dxtbdd0 = xf121*xf211 + xf134*xf210 + xf203*xf213; + const double dxtbdx0 = -xf18*xf214 - xf211; + const double dxtbdy0 = -xf0*xf214 + xf210; + const double dxtbdz0 = -xf188*xf212; + const double dytbdqopa0 = 0; + const double dytbdlama0 = xf142*xf217 - xf151*xf215 - xf157*xf215 - xf159*xf216 - xf162*xf216 + xf164*xf209*xf218; + const double dytbdphia0 = xf167*xf217 - xf169*xf215 - xf172*xf215 - xf173*xf216 - xf174*xf216 + xf175*xf209*xf218; + const double dytbdqopb0 = -xf176*xf215 + xf178*xf215 - xf179*xf216 + xf180*xf216 + xf181*xf219; + const double dytbdlamb0 = xf189*xf217 - xf191*xf215 - xf192*xf215 - xf193*xf216 - xf194*xf216 + xf195*xf209*xf218; + const double dytbdphib0 = xf196*xf217 - xf197*xf215 - xf199*xf215 - xf200*xf216 - xf201*xf216 + xf202*xf209*xf218; + const double dytbdd0 = xf121*xf216 - xf134*xf215 - xf141*xf217 + xf203*xf219; + const double dytbdx0 = -xf163*xf219 - xf216; + const double dytbdy0 = -xf158*xf219 - xf215; + const double dytbdz0 = xf217 - xf219*xf7; + Matrix res; + res(0,0) = dqopadqopa0; + res(0,1) = dqopadlama0; + res(0,2) = dqopadphia0; + res(0,3) = dqopadqopb0; + res(0,4) = dqopadlamb0; + res(0,5) = dqopadphib0; + res(0,6) = dqopadd0; + res(0,7) = dqopadx0; + res(0,8) = dqopady0; + res(0,9) = dqopadz0; + res(1,0) = dlamadqopa0; + res(1,1) = dlamadlama0; + res(1,2) = dlamadphia0; + res(1,3) = dlamadqopb0; + res(1,4) = dlamadlamb0; + res(1,5) = dlamadphib0; + res(1,6) = dlamadd0; + res(1,7) = dlamadx0; + res(1,8) = dlamady0; + res(1,9) = dlamadz0; + res(2,0) = dphiadqopa0; + res(2,1) = dphiadlama0; + res(2,2) = dphiadphia0; + res(2,3) = dphiadqopb0; + res(2,4) = dphiadlamb0; + res(2,5) = dphiadphib0; + res(2,6) = dphiadd0; + res(2,7) = dphiadx0; + res(2,8) = dphiady0; + res(2,9) = dphiadz0; + res(3,0) = dxtadqopa0; + res(3,1) = dxtadlama0; + res(3,2) = dxtadphia0; + res(3,3) = dxtadqopb0; + res(3,4) = dxtadlamb0; + res(3,5) = dxtadphib0; + res(3,6) = dxtadd0; + res(3,7) = dxtadx0; + res(3,8) = dxtady0; + res(3,9) = dxtadz0; + res(4,0) = dytadqopa0; + res(4,1) = dytadlama0; + res(4,2) = dytadphia0; + res(4,3) = dytadqopb0; + res(4,4) = dytadlamb0; + res(4,5) = dytadphib0; + res(4,6) = dytadd0; + res(4,7) = dytadx0; + res(4,8) = dytady0; + res(4,9) = dytadz0; + res(5,0) = dqopbdqopa0; + res(5,1) = dqopbdlama0; + res(5,2) = dqopbdphia0; + res(5,3) = dqopbdqopb0; + res(5,4) = dqopbdlamb0; + res(5,5) = dqopbdphib0; + res(5,6) = dqopbdd0; + res(5,7) = dqopbdx0; + res(5,8) = dqopbdy0; + res(5,9) = dqopbdz0; + res(6,0) = dlambdqopa0; + res(6,1) = dlambdlama0; + res(6,2) = dlambdphia0; + res(6,3) = dlambdqopb0; + res(6,4) = dlambdlamb0; + res(6,5) = dlambdphib0; + res(6,6) = dlambdd0; + res(6,7) = dlambdx0; + res(6,8) = dlambdy0; + res(6,9) = dlambdz0; + res(7,0) = dphibdqopa0; + res(7,1) = dphibdlama0; + res(7,2) = dphibdphia0; + res(7,3) = dphibdqopb0; + res(7,4) = dphibdlamb0; + res(7,5) = dphibdphib0; + res(7,6) = dphibdd0; + res(7,7) = dphibdx0; + res(7,8) = dphibdy0; + res(7,9) = dphibdz0; + res(8,0) = dxtbdqopa0; + res(8,1) = dxtbdlama0; + res(8,2) = dxtbdphia0; + res(8,3) = dxtbdqopb0; + res(8,4) = dxtbdlamb0; + res(8,5) = dxtbdphib0; + res(8,6) = dxtbdd0; + res(8,7) = dxtbdx0; + res(8,8) = dxtbdy0; + res(8,9) = dxtbdz0; + res(9,0) = dytbdqopa0; + res(9,1) = dytbdlama0; + res(9,2) = dytbdphia0; + res(9,3) = dytbdqopb0; + res(9,4) = dytbdlamb0; + res(9,5) = dytbdphib0; + res(9,6) = dytbdd0; + res(9,7) = dytbdx0; + res(9,8) = dytbdy0; + res(9,9) = dytbdz0; - const double qop0 = globalSource.signedInverseMomentum(); + return res; +} +Matrix ResidualGlobalCorrectionMakerBase::curv2localJacobianAltelossD(const Matrix &state, const MagneticField *field, const GloballyPositioned &surface, double dEdx, double mass, double dBz) const { + + + const GlobalPoint pos(state[0], state[1], state[2]); + const GlobalVector &bfield = field->inInverseGeV(pos); + const Matrix Bv(bfield.x(), bfield.y(), double(bfield.z()) + 2.99792458e-3*dBz); + + const double q = state[6]; + + const double qop0 = q/state.segment<3>(3).norm(); + const double lam0 = std::atan(state[5]/std::sqrt(state[3]*state[3] + state[4]*state[4])); + const double phi0 = std::atan2(state[4], state[3]); + + const Matrix W0 = state.segment<3>(3).normalized(); + const double W0x = W0[0]; + const double W0y = W0[1]; + const double W0z = W0[2]; - const double x0 = globalSource.position().x(); - const double y0 = globalSource.position().y(); - const double z0 = globalSource.position().z(); + const Vector3DBase lx(1.,0.,0.); + const Vector3DBase ly(0.,1.,0.); + const Vector3DBase lz(0.,0.,1.); + const Point3DBase l0(0., 0., 0.); -// const double p0 = globalSource.momentum().mag(); -// const GlobalVector W0 = globalSource.momentum()/p0; -// const double W0x = W0.x(); -// const double W0y = W0.y(); -// const double W0z = W0.z(); +// const Vector3DBase I1 = surface.toGlobal(lz); +// const Vector3DBase J1 = surface.toGlobal(lx); +// const Vector3DBase K1 = surface.toGlobal(ly); +// const Point3DBase r1 = surface.toGlobal(l0); - const double lam0 = M_PI_2 - globalSource.momentum().theta(); - const double phi0 = globalSource.momentum().phi(); + const Vector3DBase I1 = surface.toGlobal(lz); + const Vector3DBase J1 = surface.toGlobal(lx); + const Vector3DBase K1 = surface.toGlobal(ly); + const Point3DBase r1 = surface.toGlobal(l0); - const double B = bfield.mag(); - const GlobalVector H = bfield/B; - const double hx = H.x(); - const double hy = H.y(); - const double hz = H.z(); - - const double xf0 = std::sin(lam0); - const double xf1 = B*s; - const double xf2 = qop0*xf1; - const double xf3 = std::cos(xf2); - const double xf4 = xf0*xf3; - const double xf5 = std::sin(phi0); - const double xf6 = hx*xf5; - const double xf7 = std::cos(phi0); - const double xf8 = hy*xf7; - const double xf9 = xf6 - xf8; - const double xf10 = std::cos(lam0); - const double xf11 = std::sin(xf2); - const double xf12 = xf10*xf11; - const double xf13 = xf12*xf9; - const double xf14 = xf3 - 1; - const double xf15 = hz*xf0; - const double xf16 = hx*xf10; - const double xf17 = hy*xf10; - const double xf18 = xf15 + xf16*xf7 + xf17*xf5; - const double xf19 = xf14*xf18; - const double xf20 = hz*xf19; - const double xf21 = -xf13 - xf20 + xf4; - const double xf22 = xf10*xf3; - const double xf23 = xf22*xf5; - const double xf24 = hx*xf0; - const double xf25 = hz*xf10; - const double xf26 = xf25*xf7; - const double xf27 = xf24 - xf26; - const double xf28 = xf11*xf27; - const double xf29 = hy*xf19; - const double xf30 = xf23 + xf28 - xf29; - const double xf31 = xf22*xf7; - const double xf32 = hy*xf0; - const double xf33 = xf25*xf5; - const double xf34 = xf32 - xf33; - const double xf35 = xf11*xf34; - const double xf36 = hx*xf19; - const double xf37 = xf31 - xf35 - xf36; - const double xf38 = std::pow(xf37, 2); - const double xf39 = std::pow(xf30, 2) + xf38; - const double xf40 = 1.0/xf39; - const double xf41 = 1.0/(std::pow(xf21, 2)*xf40 + 1); - const double xf42 = xf0*xf11; - const double xf43 = -xf6 + xf8; - const double xf44 = xf22*xf43; - const double xf45 = hz*xf18; - const double xf46 = xf11*xf45; - const double xf47 = -xf1*xf42 + xf1*xf44 + xf1*xf46; - const double xf48 = std::pow(xf39, -1.0/2.0); - const double xf49 = xf12*xf5; - const double xf50 = xf1*xf49; - const double xf51 = 2*xf50; - const double xf52 = xf1*xf3; - const double xf53 = xf27*xf52; - const double xf54 = hy*xf18; - const double xf55 = xf1*xf11; - const double xf56 = xf54*xf55; - const double xf57 = 2*xf56; - const double xf58 = (1.0/2.0)*xf30; - const double xf59 = xf12*xf7; - const double xf60 = xf1*xf59; - const double xf61 = -xf32 + xf33; - const double xf62 = xf52*xf61; - const double xf63 = hx*xf18; - const double xf64 = xf55*xf63; - const double xf65 = -2*xf60 + 2*xf62 + 2*xf64; - const double xf66 = (1.0/2.0)*xf37; - const double xf67 = -xf65*xf66; - const double xf68 = xf21/std::pow(xf39, 3.0/2.0); - const double xf69 = B*z0; - const double xf70 = xf14*xf9; - const double xf71 = -xf11 + xf2; - const double xf72 = qop0*xf69 + xf10*xf70 + xf42 + xf45*xf71; - const double xf73 = 1.0/B; - const double xf74 = xf73/std::pow(qop0, 2); - const double xf75 = xf72*xf74; - const double xf76 = xf1 - xf52; - const double xf77 = 1.0/qop0; - const double xf78 = xf73*xf77; - const double xf79 = xf78*(-xf1*xf13 + xf1*xf4 + xf45*xf76 + xf69); - const double xf80 = B*y0; - const double xf81 = qop0*xf80 - xf14*xf27 + xf49 + xf54*xf71; - const double xf82 = xf74*xf81; - const double xf83 = xf78*(xf1*xf23 + xf1*xf28 + xf54*xf76 + xf80); - const double xf84 = B*x0; - const double xf85 = qop0*xf84 + xf14*xf34 + xf59 + xf63*xf71; - const double xf86 = xf74*xf85; - const double xf87 = xf78*(xf1*xf31 - xf1*xf35 + xf63*xf76 + xf84); - const double xf88 = -xf21*(-xf75 + xf79) - xf30*(-xf82 + xf83) - xf37*(-xf86 + xf87); - const double xf89 = B*qop0; - const double xf90 = -xf42*xf89 + xf44*xf89 + xf46*xf89; - const double xf91 = xf49*xf89; - const double xf92 = 2*xf91; - const double xf93 = xf3*xf89; - const double xf94 = xf27*xf93; - const double xf95 = xf11*xf89; - const double xf96 = xf54*xf95; - const double xf97 = 2*xf96; - const double xf98 = xf59*xf89; - const double xf99 = xf61*xf93; - const double xf100 = xf63*xf95; - const double xf101 = 2*xf100 - 2*xf98 + 2*xf99; - const double xf102 = -xf101*xf66; - const double xf103 = xf41*(xf48*xf90 + xf68*(xf102 - xf58*(-xf92 + 2*xf94 + xf97))); - const double xf104 = -xf24*xf7 + xf25 - xf32*xf5; - const double xf105 = hz*xf104; - const double xf106 = -xf105*xf14 + xf22 - xf42*xf43; - const double xf107 = xf4*xf5; - const double xf108 = 2*xf107; - const double xf109 = xf15*xf7; - const double xf110 = xf109 + xf16; - const double xf111 = xf11*xf110; - const double xf112 = xf104*xf14; - const double xf113 = hy*xf112; - const double xf114 = 2*xf113; - const double xf115 = xf4*xf7; - const double xf116 = xf15*xf5; - const double xf117 = xf11*(-xf116 - xf17); - const double xf118 = hx*xf112; - const double xf119 = -2*xf115 + 2*xf117 - 2*xf118; - const double xf120 = -xf119*xf66; - const double xf121 = -xf0*xf70 + xf105*xf71 + xf12; - const double xf122 = xf21*xf78; - const double xf123 = xf104*xf71; - const double xf124 = hx*xf123 + xf14*(xf116 + xf17) - xf42*xf7; - const double xf125 = xf37*xf78; - const double xf126 = 1 - xf3; - const double xf127 = hy*xf123 + xf110*xf126 - xf42*xf5; - const double xf128 = xf30*xf78; - const double xf129 = -xf121*xf122 - xf124*xf125 - xf127*xf128; - const double xf130 = hx*xf7; - const double xf131 = hy*xf5; - const double xf132 = -xf10*xf6 + xf10*xf8; - const double xf133 = hz*xf132; - const double xf134 = xf12*(-xf130 - xf131) - xf133*xf14; - const double xf135 = 2*xf31; - const double xf136 = xf11*xf33; - const double xf137 = 2*xf136; - const double xf138 = xf132*xf14; - const double xf139 = hy*xf138; - const double xf140 = 2*xf139; - const double xf141 = xf11*xf26; - const double xf142 = hx*xf138; - const double xf143 = 2*xf141 - 2*xf142 - 2*xf23; - const double xf144 = -xf143*xf66; - const double xf145 = xf10*xf14*(xf130 + xf131) + xf133*xf71; - const double xf146 = xf132*xf71; - const double xf147 = hy*xf146 + xf126*xf33 + xf59; - const double xf148 = hx*xf146 - xf14*xf26 - xf49; - const double xf149 = -xf122*xf145 - xf125*xf148 - xf128*xf147; - const double xf150 = -xf31; - const double xf151 = xf150 + xf35 + xf36; - const double xf152 = -xf23; - const double xf153 = xf152 - xf28 + xf29; - const double xf154 = xf13 + xf20 - xf4; - const double xf155 = qop0*s; - const double xf156 = -xf155*xf42 + xf155*xf44 + xf155*xf46; - const double xf157 = xf155*xf49; - const double xf158 = 2*xf157; - const double xf159 = xf155*xf3; - const double xf160 = xf159*xf27; - const double xf161 = xf11*xf155; - const double xf162 = xf161*xf54; - const double xf163 = 2*xf162; - const double xf164 = xf155*xf59; - const double xf165 = xf159*xf61; - const double xf166 = xf161*xf63; - const double xf167 = -2*xf164 + 2*xf165 + 2*xf166; - const double xf168 = -xf167*xf66; - const double xf169 = xf77/std::pow(B, 2); - const double xf170 = xf169*xf72; - const double xf171 = xf155 - xf159; - const double xf172 = xf78*(qop0*z0 - xf13*xf155 + xf155*xf4 + xf171*xf45); - const double xf173 = xf169*xf81; - const double xf174 = xf78*(qop0*y0 + xf155*xf23 + xf155*xf28 + xf171*xf54); - const double xf175 = xf169*xf85; - const double xf176 = xf78*(qop0*x0 + xf155*xf31 - xf155*xf35 + xf171*xf63); - const double xf177 = -xf21*(-xf170 + xf172) - xf30*(-xf173 + xf174) - xf37*(-xf175 + xf176); - const double xf178 = -xf50 + xf53 + xf56; - const double xf179 = xf37*xf40; - const double xf180 = -xf60 + xf62 + xf64; - const double xf181 = xf153*xf40; - const double xf182 = -xf91 + xf94 + xf96; - const double xf183 = xf100 - xf98 + xf99; - const double xf184 = xf179*xf182 + xf181*xf183; - const double xf185 = -xf107 + xf111 - xf113; - const double xf186 = -xf115 + xf117 - xf118; - const double xf187 = xf136 - xf139 + xf31; - const double xf188 = xf141 - xf142 + xf152; - const double xf189 = -xf157 + xf160 + xf162; - const double xf190 = -xf164 + xf165 + xf166; - const double xf191 = std::pow(xf153, 2) + xf38; - const double xf192 = std::pow(xf191, -1.0/2.0); - const double xf193 = xf153*xf192; - const double xf194 = xf192*xf37; - const double xf195 = -xf24 + xf26; - const double xf196 = xf195*xf52; - const double xf197 = xf196 + xf50 - xf56; - const double xf198 = xf192*xf78; - const double xf199 = xf198*xf85; - const double xf200 = xf198*xf81; - const double xf201 = std::pow(xf191, -3.0/2.0); - const double xf202 = (1.0/2.0)*xf153; - const double xf203 = -xf202*(2*xf196 + xf51 - xf57) + xf67; - const double xf204 = xf201*xf203; - const double xf205 = xf78*xf85; - const double xf206 = xf153*xf205; - const double xf207 = xf125*xf81; - const double xf208 = xf89 - xf93; - const double xf209 = xf208*xf54 + xf23*xf89 + xf28*xf89; - const double xf210 = xf194*xf78; - const double xf211 = xf208*xf63 + xf31*xf89 - xf35*xf89; - const double xf212 = xf193*xf78; - const double xf213 = xf195*xf93; - const double xf214 = xf213 + xf91 - xf96; - const double xf215 = xf102 - xf202*(2*xf213 + xf92 - xf97); - const double xf216 = xf201*xf215; - const double xf217 = xf153*xf216; - const double xf218 = xf183*xf200 + xf199*xf214 + xf205*xf217 + xf207*xf216 + xf209*xf210 + xf211*xf212; - const double xf219 = xf11*(-xf109 - xf16); - const double xf220 = xf107 + xf113 + xf219; - const double xf221 = xf120 - xf202*(xf108 + xf114 + 2*xf219); - const double xf222 = xf201*xf221; - const double xf223 = -xf136 + xf139 + xf150; - const double xf224 = xf144 - xf202*(-xf135 - xf137 + xf140); - const double xf225 = xf201*xf224; - const double xf226 = xf159*xf195; - const double xf227 = xf157 - xf162 + xf226; - const double xf228 = xf168 - xf202*(xf158 - xf163 + 2*xf226); - const double xf229 = xf201*xf228; - const double xf230 = xf194*xf21; - const double xf231 = xf193*xf21; - const double xf232 = xf210*xf85; - const double xf233 = xf212*xf81; - const double xf234 = xf122*xf192; - const double xf235 = xf234*xf85; - const double xf236 = xf234*xf81; - const double xf237 = xf192*xf38 - xf193*xf30; - const double xf238 = xf125*xf21*xf85; - const double xf239 = xf122*xf81; - const double xf240 = xf153*xf239; - const double xf241 = xf192*xf30; - const double xf242 = xf201*xf38; - const double xf243 = xf153*xf30; - const double xf244 = xf201*xf243; - const double xf245 = xf72*xf78; - const double xf246 = xf231*xf78; - const double xf247 = xf230*xf78; - const double xf248 = xf237*xf78; - const double xf249 = -xf183*xf235 + xf209*xf246 - xf211*xf247 + xf214*xf236 - xf216*xf238 + xf217*xf239 - xf232*xf90 + xf233*xf90 + xf245*(xf101*xf194 - xf182*xf193 - xf214*xf241 + xf215*xf242 - xf216*xf243) + xf248*(-xf13*xf89 + xf208*xf45 + xf4*xf89); - const double dqopdqop0 = 1; - const double dqopdlam0 = 0; - const double dqopdphi0 = 0; - const double dqopdx0 = 0; - const double dqopdy0 = 0; - const double dqopdz0 = 0; - const double dqopdB = 0; - const double dlamdqop0 = xf103*xf88 + xf41*(xf47*xf48 + xf68*(-xf58*(-xf51 + 2*xf53 + xf57) + xf67)); - const double dlamdlam0 = xf103*xf129 + xf41*(xf106*xf48 + xf68*(xf120 - xf58*(-xf108 + 2*xf111 - xf114))); - const double dlamdphi0 = xf103*xf149 + xf41*(xf134*xf48 + xf68*(xf144 - xf58*(xf135 + xf137 - xf140))); - const double dlamdx0 = xf103*xf151; - const double dlamdy0 = xf103*xf153; - const double dlamdz0 = xf103*xf154; - const double dlamdB = xf103*xf177 + xf41*(xf156*xf48 + xf68*(xf168 - xf58*(-xf158 + 2*xf160 + xf163))); - const double dphidqop0 = xf178*xf179 + xf180*xf181 + xf184*xf88; - const double dphidlam0 = xf129*xf184 + xf179*xf185 + xf181*xf186; - const double dphidphi0 = xf149*xf184 + xf179*xf187 + xf181*xf188; - const double dphidx0 = xf151*xf184; - const double dphidy0 = xf153*xf184; - const double dphidz0 = xf154*xf184; - const double dphidB = xf177*xf184 + xf179*xf189 + xf181*xf190; - const double dxtdqop0 = xf180*xf200 - xf193*xf86 + xf193*xf87 - xf194*xf82 + xf194*xf83 + xf197*xf199 + xf204*xf206 + xf204*xf207 + xf218*xf88; - const double dxtdlam0 = xf124*xf212 + xf127*xf210 + xf129*xf218 + xf186*xf200 + xf199*xf220 + xf206*xf222 + xf207*xf222; - const double dxtdphi0 = xf147*xf210 + xf148*xf212 + xf149*xf218 + xf188*xf200 + xf199*xf223 + xf206*xf225 + xf207*xf225; - const double dxtdx0 = xf151*xf218 + xf193; - const double dxtdy0 = xf153*xf218 + xf194; - const double dxtdz0 = xf154*xf218; - const double dxtdB = -xf173*xf194 + xf174*xf194 - xf175*xf193 + xf176*xf193 + xf177*xf218 + xf190*xf200 + xf199*xf227 + xf206*xf229 + xf207*xf229; - const double dytdqop0 = -xf180*xf235 + xf197*xf236 - xf204*xf238 + xf204*xf240 + xf230*xf86 - xf230*xf87 - xf231*xf82 + xf231*xf83 - xf232*xf47 + xf233*xf47 - xf237*xf75 + xf237*xf79 + xf245*(-xf178*xf193 + xf194*xf65 - xf197*xf241 + xf203*xf242 - xf203*xf244) + xf249*xf88; - const double dytdlam0 = -xf106*xf232 + xf106*xf233 + xf121*xf248 - xf124*xf247 + xf127*xf246 + xf129*xf249 - xf186*xf235 + xf220*xf236 - xf222*xf238 + xf222*xf240 + xf245*(xf119*xf194 - xf185*xf193 - xf220*xf241 + xf221*xf242 - xf221*xf244); - const double dytdphi0 = -xf134*xf232 + xf134*xf233 + xf145*xf248 + xf147*xf246 - xf148*xf247 + xf149*xf249 - xf188*xf235 + xf223*xf236 - xf225*xf238 + xf225*xf240 + xf245*(xf143*xf194 - xf187*xf193 - xf223*xf241 + xf224*xf242 - xf224*xf244); - const double dytdx0 = xf151*xf249 - xf230; - const double dytdy0 = xf153*xf249 + xf231; - const double dytdz0 = xf154*xf249 + xf237; - const double dytdB = -xf156*xf232 + xf156*xf233 - xf170*xf237 + xf172*xf237 - xf173*xf231 + xf174*xf231 + xf175*xf230 - xf176*xf230 + xf177*xf249 - xf190*xf235 + xf227*xf236 - xf229*xf238 + xf229*xf240 + xf245*(xf167*xf194 - xf189*xf193 - xf227*xf241 + xf228*xf242 - xf228*xf244); - Matrix res; - res(0,0) = dqopdqop0; - res(0,1) = dqopdlam0; - res(0,2) = dqopdphi0; - res(0,3) = dqopdx0; - res(0,4) = dqopdy0; - res(0,5) = dqopdz0; - res(0,6) = dqopdB; - res(1,0) = dlamdqop0; - res(1,1) = dlamdlam0; - res(1,2) = dlamdphi0; - res(1,3) = dlamdx0; - res(1,4) = dlamdy0; - res(1,5) = dlamdz0; - res(1,6) = dlamdB; - res(2,0) = dphidqop0; - res(2,1) = dphidlam0; - res(2,2) = dphidphi0; - res(2,3) = dphidx0; - res(2,4) = dphidy0; - res(2,5) = dphidz0; - res(2,6) = dphidB; - res(3,0) = dxtdqop0; - res(3,1) = dxtdlam0; - res(3,2) = dxtdphi0; - res(3,3) = dxtdx0; - res(3,4) = dxtdy0; - res(3,5) = dxtdz0; - res(3,6) = dxtdB; - res(4,0) = dytdqop0; - res(4,1) = dytdlam0; - res(4,2) = dytdphi0; - res(4,3) = dytdx0; - res(4,4) = dytdy0; - res(4,5) = dytdz0; - res(4,6) = dytdB; - - // convert to tesla for B field gradient - res.col(6) *= 2.99792458e-3; - - return res; - -} - - -Matrix ResidualGlobalCorrectionMakerBase::localTransportJacobian(const TrajectoryStateOnSurface &start, - const std::pair &propresult, - bool doReverse) const { - - const TrajectoryStateOnSurface& state0 = doReverse ? propresult.first : start; - const TrajectoryStateOnSurface& state1 = doReverse ? start : propresult.first; - const GlobalVector& h = start.globalParameters().magneticFieldInInverseGeV(); - const double s = doReverse ? -propresult.second : propresult.second; - - // compute local to curvilinear jacobian at source - JacobianLocalToCurvilinear local2curv(state0.surface(), state0.localParameters(), *state0.magneticField()); - const AlgebraicMatrix55& local2curvjac = local2curv.jacobian(); - const Matrix H0 = Map>(local2curvjac.Array()); - - // compute curvilinear to local jacobian at destination - JacobianCurvilinearToLocal curv2local(state1.surface(), state1.localParameters(), *state1.magneticField()); - const AlgebraicMatrix55& curv2localjac = curv2local.jacobian(); - const Matrix H1 = Map>(curv2localjac.Array()); - -// // compute transport jacobian wrt curvlinear parameters -// AnalyticalCurvilinearJacobian curv2curv; -// curv2curv.computeFullJacobian(state0.globalParameters(), state1.globalParameters().position(), state1.globalParameters().momentum(), h, s); -// const AlgebraicMatrix55& curv2curvjac = curv2curv.jacobian(); -// const Matrix F = Map>(curv2curvjac.Array()); -// -// std::cout << "qbp" << std::endl; -// std::cout << state0.localParameters().qbp() << std::endl; -// -// std::cout << "F" << std::endl; -// std::cout << F << std::endl; -// -// // compute transport jacobian wrt B field (magnitude) -// const Matrix dF = bfieldJacobian(state0.globalParameters(), state1.globalParameters(), s, h); -// -// std::cout << "dF" << std::endl; -// std::cout << dF << std::endl; -// -// Matrix dFalt; -// dFalt[0] = 0.; -// dFalt.tail<4>() = 2.99792458e-3*state0.localParameters().qbp()/h.mag()*F.block<4, 1>(1, 0); -// -// std::cout << "dFalt" << std::endl; -// std::cout << dFalt << std::endl; -// -// const Matrix FdFfull = curvtransportgrad(state0.globalParameters(), state1.globalParameters(), s, h); -// std::cout << "FdFfull" << std::endl; -// std::cout << FdFfull << std::endl; -// -// const Matrix FmIcmssw = F - Matrix::Identity(); -// std::cout << "FmIcmssw" << std::endl; -// std::cout << FmIcmssw << std::endl; -// -// const Matrix FmI = FdFfull.block<5,5>(0,0) - Matrix::Identity(); -// std::cout << "FmI" << std::endl; -// std::cout << FmI << std::endl; - - const Matrix FdF = curvtransportJacobian(state0.globalParameters(), state1.globalParameters(), s, h); - const Matrix F = FdF.topLeftCorner<5,5>(); - const Matrix dF = FdF.col(5); - - Matrix res; - res.leftCols<5>() = H1*F*H0; - res.rightCols<1>() = H1*dF; - return res; - -} - - -Matrix ResidualGlobalCorrectionMakerBase::curv2localJacobianAlt(const TrajectoryStateOnSurface &state) const { - - - const GlobalTrajectoryParameters &globalSource = state.globalParameters(); - const GlobalVector &bfield = state.globalParameters().magneticFieldInInverseGeV(); - - CurvilinearTrajectoryParameters curvparms(globalSource.position(), globalSource.momentum(), globalSource.charge()); - - const double qop0 = curvparms.Qbp(); - const double lam0 = curvparms.lambda(); - const double phi0 = curvparms.phi(); - const double xt0 = curvparms.xT(); - const double yt0 = curvparms.yT(); - - const double p0 = globalSource.momentum().mag(); - const GlobalVector W0 = globalSource.momentum()/p0; - const double W0x = W0.x(); - const double W0y = W0.y(); - const double W0z = W0.z(); - - const LocalVector lx(1.,0.,0.); - const LocalVector ly(0.,1.,0.); - const LocalVector lz(0.,0.,1.); - const LocalPoint l0(0., 0.); - - const GlobalVector I1 = state.surface().toGlobal(lz); - const GlobalVector J1 = state.surface().toGlobal(lx); - const GlobalVector K1 = state.surface().toGlobal(ly); - const GlobalPoint r1 = state.surface().toGlobal(l0); +// const Vector3DBase I1 = toGlobal(surface, lz); +// const Vector3DBase J1 = toGlobal(surface, lx); +// const Vector3DBase K1 = toGlobal(surface, ly); +// const Point3DBase r1 = toGlobal(surface, l0); const double Ix1 = I1.x(); const double Iy1 = I1.y(); @@ -2416,91 +2235,95 @@ Matrix ResidualGlobalCorrectionMakerBase::curv2localJacobianAlt(co const double ry1 = r1.y(); const double rz1 = r1.z(); - const double B = bfield.mag(); - const GlobalVector H = bfield/B; - const double hx = H.x(); - const double hy = H.y(); - const double hz = H.z(); - - const double x0 = std::sin(lam0); - const double x1 = Iz1*x0; - const double x2 = std::cos(lam0); - const double x3 = std::cos(phi0); - const double x4 = Ix1*x3; - const double x5 = std::sin(phi0); - const double x6 = Iy1*x5; - const double x7 = x1 + x2*x4 + x2*x6; - const double x8 = std::pow(x7, -2); - const double x9 = Iz1*Jx1; - const double x10 = Iz1*Jy1; - const double x11 = lam0 + phi0; - const double x12 = -phi0; - const double x13 = lam0 + x12; - const double x14 = std::pow(Ix1*std::cos(x11) + Ix1*std::cos(x13) + Iy1*std::sin(x11) - Iy1*std::sin(x13) + 2*x1, -2); - const double x15 = 2*Ix1; - const double x16 = Jy1*x15; - const double x17 = 2*Iy1; - const double x18 = Jx1*x17; - const double x19 = 2*lam0; - const double x20 = std::cos(x19); - const double x21 = phi0 + x19; - const double x22 = std::cos(x21); - const double x23 = std::sin(x21); - const double x24 = Ix1*Jz1; - const double x25 = Iy1*Jz1; - const double x26 = x12 + x19; - const double x27 = std::cos(x26); - const double x28 = std::sin(x26); - const double x29 = Ix1*W0y - Iy1*W0x; - const double x30 = hz*x2; - const double x31 = hx*x0 - x3*x30; - const double x32 = hy*x0 - x30*x5; - const double x33 = x2*(hx*x5 - hy*x3); - const double x34 = x2*x3; - const double x35 = x2*x5; - const double x36 = Jx1*x34 + Jy1*x35 + Jz1*x0; - const double x37 = -Ix1*x32 + Iy1*x31 - Iz1*x33; - const double x38 = std::pow(W0x, 2) + std::pow(W0y, 2); - const double x39 = std::pow(x38, -1.0/2.0); - const double x40 = B*qop0*x39/std::pow(x7, 3); - const double x41 = x40*(-x36*x37 + x7*(-Jx1*x32 + Jy1*x31 - Jz1*x33)); - const double x42 = Ix1*W0x*W0z + Iy1*W0y*W0z - Iz1*x38; - const double x43 = Iz1*Kx1; - const double x44 = Iz1*Ky1; - const double x45 = Ky1*x15; - const double x46 = Kx1*x17; - const double x47 = Ix1*Kz1; - const double x48 = Iy1*Kz1; - const double x49 = Kx1*x34 + Ky1*x35 + Kz1*x0; - const double x50 = x40*(-x37*x49 + x7*(-Kx1*x32 + Ky1*x31 - Kz1*x33)); - const double x51 = x39/x7; - const double x52 = x38*x7; - const double x53 = W0z*x7; - const double dqopdqop0 = 1; + const double B = Bv.norm(); + const Matrix H = Bv.normalized(); + const double hx = H[0]; + const double hy = H[1]; + const double hz = H[2]; + + const double x0 = std::pow(q, 2); + const double x1 = std::pow(x0, 3.0/2.0); + const double x2 = Ix1*W0y - Iy1*W0x; + const double x3 = std::pow(qop0, 2); + const double x4 = std::pow(W0x, 2) + std::pow(W0y, 2); + const double x5 = std::pow(x4, -1.0/2.0); + const double x6 = std::sin(lam0); + const double x7 = Iz1*x6; + const double x8 = std::cos(lam0); + const double x9 = std::cos(phi0); + const double x10 = Ix1*x9; + const double x11 = std::sin(phi0); + const double x12 = Iy1*x11; + const double x13 = x10*x8 + x12*x8 + x7; + const double x14 = x5/x13; + const double x15 = dEdx*q*x14*x3*std::sqrt(std::pow(mass, 2)*x3 + x0)/x1; + const double x16 = Ix1*W0x*W0z + Iy1*W0y*W0z - Iz1*x4; + const double x17 = std::pow(x13, -2); + const double x18 = Iz1*Jx1; + const double x19 = Iz1*Jy1; + const double x20 = lam0 + phi0; + const double x21 = -phi0; + const double x22 = lam0 + x21; + const double x23 = std::pow(Ix1*std::cos(x20) + Ix1*std::cos(x22) + Iy1*std::sin(x20) - Iy1*std::sin(x22) + 2*x7, -2); + const double x24 = 2*Ix1; + const double x25 = Jy1*x24; + const double x26 = 2*Iy1; + const double x27 = Jx1*x26; + const double x28 = 2*lam0; + const double x29 = std::cos(x28); + const double x30 = phi0 + x28; + const double x31 = std::cos(x30); + const double x32 = std::sin(x30); + const double x33 = Ix1*Jz1; + const double x34 = Iy1*Jz1; + const double x35 = x21 + x28; + const double x36 = std::cos(x35); + const double x37 = std::sin(x35); + const double x38 = x8*x9; + const double x39 = x11*x8; + const double x40 = Jx1*x38 + Jy1*x39 + Jz1*x6; + const double x41 = hz*x8; + const double x42 = hy*x6 - x11*x41; + const double x43 = x8*(hx*x11 - hy*x9); + const double x44 = hx*x6 - x41*x9; + const double x45 = -Ix1*x42 + Iy1*x44 - Iz1*x43; + const double x46 = B*qop0*x5/std::pow(x13, 3); + const double x47 = x46*(x13*(-Jx1*x42 + Jy1*x44 - Jz1*x43) - x40*x45); + const double x48 = Iz1*Kx1; + const double x49 = Iz1*Ky1; + const double x50 = Ky1*x24; + const double x51 = Kx1*x26; + const double x52 = Ix1*Kz1; + const double x53 = Iy1*Kz1; + const double x54 = Kx1*x38 + Ky1*x39 + Kz1*x6; + const double x55 = x46*(x13*(-Kx1*x42 + Ky1*x44 - Kz1*x43) - x45*x54); + const double x56 = x13*x4; + const double x57 = W0z*x13; + const double dqopdqop0 = x1*std::fabs(qop0)/(std::pow(q, 3)*qop0); const double dqopdlam0 = 0; const double dqopdphi0 = 0; - const double dqopdxt0 = 0; - const double dqopdyt0 = 0; + const double dqopdxt0 = -x15*x2; + const double dqopdyt0 = -x15*x16; const double ddxdzdqop0 = 0; - const double ddxdzdlam0 = x8*(Jz1*x4 + Jz1*x6 - x10*x5 - x3*x9); - const double ddxdzdphi0 = x14*(x10*x23 + x10*x28 + x16*x20 + x16 - x18*x20 - x18 - x22*x24 + x22*x9 - x23*x25 + x24*x27 - x25*x28 - x27*x9); - const double ddxdzdxt0 = x29*x41; - const double ddxdzdyt0 = x41*x42; + const double ddxdzdlam0 = x17*(Jz1*x10 + Jz1*x12 - x11*x19 - x18*x9); + const double ddxdzdphi0 = x23*(x18*x31 - x18*x36 + x19*x32 + x19*x37 + x25*x29 + x25 - x27*x29 - x27 - x31*x33 - x32*x34 + x33*x36 - x34*x37); + const double ddxdzdxt0 = x2*x47; + const double ddxdzdyt0 = x16*x47; const double ddydzdqop0 = 0; - const double ddydzdlam0 = x8*(Kz1*x4 + Kz1*x6 - x3*x43 - x44*x5); - const double ddydzdphi0 = x14*(x20*x45 - x20*x46 + x22*x43 - x22*x47 + x23*x44 - x23*x48 - x27*x43 + x27*x47 + x28*x44 - x28*x48 + x45 - x46); - const double ddydzdxt0 = x29*x50; - const double ddydzdyt0 = x42*x50; + const double ddydzdlam0 = x17*(Kz1*x10 + Kz1*x12 - x11*x49 - x48*x9); + const double ddydzdphi0 = x23*(x29*x50 - x29*x51 + x31*x48 - x31*x52 + x32*x49 - x32*x53 - x36*x48 + x36*x52 + x37*x49 - x37*x53 + x50 - x51); + const double ddydzdxt0 = x2*x55; + const double ddydzdyt0 = x16*x55; const double dxdqop0 = 0; const double dxdlam0 = 0; const double dxdphi0 = 0; - const double dxdxt0 = x51*(x29*x36 + x7*(-Jx1*W0y + Jy1*W0x)); - const double dxdyt0 = x51*(Jz1*x52 + x36*x42 - x53*(Jx1*W0x + Jy1*W0y)); + const double dxdxt0 = x14*(x13*(-Jx1*W0y + Jy1*W0x) + x2*x40); + const double dxdyt0 = x14*(Jz1*x56 + x16*x40 - x57*(Jx1*W0x + Jy1*W0y)); const double dydqop0 = 0; const double dydlam0 = 0; const double dydphi0 = 0; - const double dydxt0 = x51*(x29*x49 + x7*(-Kx1*W0y + Ky1*W0x)); - const double dydyt0 = x51*(Kz1*x52 + x42*x49 - x53*(Kx1*W0x + Ky1*W0y)); + const double dydxt0 = x14*(x13*(-Kx1*W0y + Ky1*W0x) + x2*x54); + const double dydyt0 = x14*(Kz1*x56 + x16*x54 - x57*(Kx1*W0x + Ky1*W0y)); Matrix res; res(0,0) = dqopdqop0; res(0,1) = dqopdlam0; @@ -2527,168 +2350,139 @@ Matrix ResidualGlobalCorrectionMakerBase::curv2localJacobianAlt(co res(4,2) = dydphi0; res(4,3) = dydxt0; res(4,4) = dydyt0; - return res; } -Matrix ResidualGlobalCorrectionMakerBase::curv2localJacobianAlteloss(const TrajectoryStateOnSurface &state, double dEdx, double mass) const { - - const GlobalTrajectoryParameters &globalSource = state.globalParameters(); - const GlobalVector &bfield = globalSource.magneticFieldInInverseGeV(); - - const Surface &surface = state.surface(); - - CurvilinearTrajectoryParameters curvparms(globalSource.position(), globalSource.momentum(), globalSource.charge()); +Matrix ResidualGlobalCorrectionMakerBase::curv2localhybridJacobianAltelossD(const Matrix &state, const MagneticField *field, const GloballyPositioned &surface, double dEdx, double mass, double dBz) const { + + + const GlobalPoint pos(state[0], state[1], state[2]); + const GlobalVector &bfield = field->inInverseGeV(pos); + const Matrix Bv(bfield.x(), bfield.y(), double(bfield.z()) + 2.99792458e-3*dBz); + + const double q = state[6]; + + const double qop0 = q/state.segment<3>(3).norm(); + const double lam0 = std::atan(state[5]/std::sqrt(state[3]*state[3] + state[4]*state[4])); + const double phi0 = std::atan2(state[4], state[3]); + + const Matrix W0 = state.segment<3>(3).normalized(); + const double W0x = W0[0]; + const double W0y = W0[1]; + const double W0z = W0[2]; + + const Vector3DBase lx(1.,0.,0.); + const Vector3DBase ly(0.,1.,0.); + const Vector3DBase lz(0.,0.,1.); + const Point3DBase l0(0., 0., 0.); + +// const Vector3DBase I1 = surface.toGlobal(lz); +// const Vector3DBase J1 = surface.toGlobal(lx); +// const Vector3DBase K1 = surface.toGlobal(ly); +// const Point3DBase r1 = surface.toGlobal(l0); + + const Vector3DBase I1 = surface.toGlobal(lz); + const Vector3DBase J1 = surface.toGlobal(lx); + const Vector3DBase K1 = surface.toGlobal(ly); + const Point3DBase r1 = surface.toGlobal(l0); + +// const Vector3DBase I1 = toGlobal(surface, lz); +// const Vector3DBase J1 = toGlobal(surface, lx); +// const Vector3DBase K1 = toGlobal(surface, ly); +// const Point3DBase r1 = toGlobal(surface, l0); - const double qop0 = curvparms.Qbp(); - const double lam0 = curvparms.lambda(); - const double phi0 = curvparms.phi(); - const double xt0 = curvparms.xT(); - const double yt0 = curvparms.yT(); - - const double q = globalSource.charge(); - - const double p0 = globalSource.momentum().mag(); - const GlobalVector W0 = globalSource.momentum()/p0; - const double W0x = W0.x(); - const double W0y = W0.y(); - const double W0z = W0.z(); - - const LocalVector lx(1.,0.,0.); - const LocalVector ly(0.,1.,0.); - const LocalVector lz(0.,0.,1.); - const LocalPoint l0(0., 0.); - - const GlobalVector I1 = surface.toGlobal(lz); - const GlobalVector J1 = surface.toGlobal(lx); - const GlobalVector K1 = surface.toGlobal(ly); - const GlobalPoint r1 = surface.toGlobal(l0); - const double Ix1 = I1.x(); const double Iy1 = I1.y(); const double Iz1 = I1.z(); - + const double Jx1 = J1.x(); const double Jy1 = J1.y(); const double Jz1 = J1.z(); - + const double Kx1 = K1.x(); const double Ky1 = K1.y(); const double Kz1 = K1.z(); - + const double rx1 = r1.x(); const double ry1 = r1.y(); const double rz1 = r1.z(); - - const double B = bfield.mag(); - const GlobalVector H = bfield/B; - const double hx = H.x(); - const double hy = H.y(); - const double hz = H.z(); - + + const double B = Bv.norm(); + const Matrix H = Bv.normalized(); + const double hx = H[0]; + const double hy = H[1]; + const double hz = H[2]; + const double x0 = std::pow(q, 2); - const double x1 = std::pow(x0, -3.0/2.0); - const double x2 = Iy1*W0x; - const double x3 = Ix1*W0y; + const double x1 = std::pow(x0, 3.0/2.0); + const double x2 = Ix1*W0y; + const double x3 = -Iy1*W0x + x2; const double x4 = std::pow(qop0, 2); const double x5 = std::pow(W0x, 2) + std::pow(W0y, 2); - const double x6 = std::pow(x5, -1.0/2.0); - const double x7 = std::sin(lam0); - const double x8 = Iz1*x7; - const double x9 = std::cos(lam0); - const double x10 = std::cos(phi0); - const double x11 = Ix1*x10; - const double x12 = std::sin(phi0); - const double x13 = Iy1*x12; - const double x14 = x11*x9 + x13*x9 + x8; - const double x15 = x6/x14; - const double x16 = dEdx*q*x1*x15*x4*std::sqrt(std::pow(mass, 2)*x4 + x0); - const double x17 = Ix1*W0x*W0z + Iy1*W0y*W0z - Iz1*x5; - const double x18 = std::pow(x14, -2); - const double x19 = Iz1*Jx1; - const double x20 = Iz1*Jy1; - const double x21 = lam0 + phi0; - const double x22 = -phi0; - const double x23 = lam0 + x22; - const double x24 = std::pow(Ix1*std::cos(x21) + Ix1*std::cos(x23) + Iy1*std::sin(x21) - Iy1*std::sin(x23) + 2*x8, -2); - const double x25 = 2*Ix1; - const double x26 = Jy1*x25; - const double x27 = 2*Iy1; - const double x28 = Jx1*x27; - const double x29 = 2*lam0; - const double x30 = std::cos(x29); - const double x31 = phi0 + x29; - const double x32 = std::cos(x31); - const double x33 = std::sin(x31); - const double x34 = Ix1*Jz1; - const double x35 = Iy1*Jz1; - const double x36 = x22 + x29; - const double x37 = std::cos(x36); - const double x38 = std::sin(x36); - const double x39 = -x2 + x3; - const double x40 = hz*x9; - const double x41 = hx*x7 - x10*x40; - const double x42 = hy*x7 - x12*x40; - const double x43 = x9*(hx*x12 - hy*x10); - const double x44 = x10*x9; - const double x45 = x12*x9; - const double x46 = Jx1*x44 + Jy1*x45 + Jz1*x7; - const double x47 = -Ix1*x42 + Iy1*x41 - Iz1*x43; - const double x48 = B*qop0*x6/std::pow(x14, 3); - const double x49 = x48*(x14*(-Jx1*x42 + Jy1*x41 - Jz1*x43) - x46*x47); - const double x50 = Iz1*Kx1; - const double x51 = Iz1*Ky1; - const double x52 = Ky1*x25; - const double x53 = Kx1*x27; - const double x54 = Ix1*Kz1; - const double x55 = Iy1*Kz1; - const double x56 = Kx1*x44 + Ky1*x45 + Kz1*x7; - const double x57 = x48*(x14*(-Kx1*x42 + Ky1*x41 - Kz1*x43) - x47*x56); - const double x58 = x14*x5; - const double x59 = W0z*x14; - const double dqopdqop0 = std::pow(q, 3)*x1*std::fabs(qop0)/qop0; + const double x6 = std::sin(lam0); + const double x7 = std::cos(phi0); + const double x8 = std::cos(lam0); + const double x9 = x7*x8; + const double x10 = std::sin(phi0); + const double x11 = x10*x8; + const double x12 = Ix1*x9 + Iy1*x11 + Iz1*x6; + const double x13 = 1/(x12*std::sqrt(x5)); + const double x14 = dEdx*q*x13*x4*std::sqrt(std::pow(mass, 2)*x4 + x0)/x1; + const double x15 = Ix1*W0x*W0z + Iy1*W0y*W0z - Iz1*x5; + const double x16 = 1.0/x8; + const double x17 = x16*std::sqrt(std::pow(x8, 2)); + const double x18 = hx*x10; + const double x19 = hy*x7; + const double x20 = B*qop0*x13; + const double x21 = x17*x20; + const double x22 = x16*x20*(hx*x6*x7 + hy*x10*x6 - hz*x8); + const double x23 = Jx1*x9 + Jy1*x11 + Jz1*x6; + const double x24 = x12*x5; + const double x25 = W0z*x12; + const double x26 = Kx1*x9 + Ky1*x11 + Kz1*x6; + const double dqopdqop0 = x1*std::fabs(qop0)/(std::pow(q, 3)*qop0); const double dqopdlam0 = 0; const double dqopdphi0 = 0; - const double dqopdxt0 = x16*(x2 - x3); - const double dqopdyt0 = -x16*x17; - const double ddxdzdqop0 = 0; - const double ddxdzdlam0 = x18*(Jz1*x11 + Jz1*x13 - x10*x19 - x12*x20); - const double ddxdzdphi0 = x24*(x19*x32 - x19*x37 + x20*x33 + x20*x38 + x26*x30 + x26 - x28*x30 - x28 - x32*x34 - x33*x35 + x34*x37 - x35*x38); - const double ddxdzdxt0 = x39*x49; - const double ddxdzdyt0 = x17*x49; - const double ddydzdqop0 = 0; - const double ddydzdlam0 = x18*(Kz1*x11 + Kz1*x13 - x10*x50 - x12*x51); - const double ddydzdphi0 = x24*(x30*x52 - x30*x53 + x32*x50 - x32*x54 + x33*x51 - x33*x55 - x37*x50 + x37*x54 + x38*x51 - x38*x55 + x52 - x53); - const double ddydzdxt0 = x39*x57; - const double ddydzdyt0 = x17*x57; + const double dqopdxt0 = -x14*x3; + const double dqopdyt0 = -x14*x15; + const double dlamdqop0 = 0; + const double dlamdlam0 = x17; + const double dlamdphi0 = 0; + const double dlamdxt0 = x21*(Ix1*W0y*hy*x7 + Iy1*W0x*hx*x10 - Iy1*W0x*x19 - x18*x2); + const double dlamdyt0 = -x15*x21*(x18 - x19); + const double dphidqop0 = 0; + const double dphidlam0 = 0; + const double dphidphi0 = 1; + const double dphidxt0 = x22*x3; + const double dphidyt0 = x15*x22; const double dxdqop0 = 0; const double dxdlam0 = 0; const double dxdphi0 = 0; - const double dxdxt0 = x15*(x14*(-Jx1*W0y + Jy1*W0x) + x39*x46); - const double dxdyt0 = x15*(Jz1*x58 + x17*x46 - x59*(Jx1*W0x + Jy1*W0y)); + const double dxdxt0 = x13*(x12*(-Jx1*W0y + Jy1*W0x) + x23*x3); + const double dxdyt0 = x13*(Jz1*x24 + x15*x23 - x25*(Jx1*W0x + Jy1*W0y)); const double dydqop0 = 0; const double dydlam0 = 0; const double dydphi0 = 0; - const double dydxt0 = x15*(x14*(-Kx1*W0y + Ky1*W0x) + x39*x56); - const double dydyt0 = x15*(Kz1*x58 + x17*x56 - x59*(Kx1*W0x + Ky1*W0y)); + const double dydxt0 = x13*(x12*(-Kx1*W0y + Ky1*W0x) + x26*x3); + const double dydyt0 = x13*(Kz1*x24 + x15*x26 - x25*(Kx1*W0x + Ky1*W0y)); Matrix res; res(0,0) = dqopdqop0; res(0,1) = dqopdlam0; res(0,2) = dqopdphi0; res(0,3) = dqopdxt0; res(0,4) = dqopdyt0; - res(1,0) = ddxdzdqop0; - res(1,1) = ddxdzdlam0; - res(1,2) = ddxdzdphi0; - res(1,3) = ddxdzdxt0; - res(1,4) = ddxdzdyt0; - res(2,0) = ddydzdqop0; - res(2,1) = ddydzdlam0; - res(2,2) = ddydzdphi0; - res(2,3) = ddydzdxt0; - res(2,4) = ddydzdyt0; + res(1,0) = dlamdqop0; + res(1,1) = dlamdlam0; + res(1,2) = dlamdphi0; + res(1,3) = dlamdxt0; + res(1,4) = dlamdyt0; + res(2,0) = dphidqop0; + res(2,1) = dphidlam0; + res(2,2) = dphidphi0; + res(2,3) = dphidxt0; + res(2,4) = dphidyt0; res(3,0) = dxdqop0; res(3,1) = dxdlam0; res(3,2) = dxdphi0; @@ -2700,1721 +2494,379 @@ Matrix ResidualGlobalCorrectionMakerBase::curv2localJacobianAltelo res(4,3) = dydxt0; res(4,4) = dydyt0; - + return res; - + } -// Point3DBase ResidualGlobalCorrectionMakerBase::toGlobal(const Surface &surface, const Point3DBase &lp) const { -// Point3DBase position = surface.position(); -// TkRotation rotation = surface.rotation(); -// -// return Point3DBase(rotation.multiplyInverse(lp.basicVector()) + position.basicVector()); -// -// } -// -// Vector3DBase ResidualGlobalCorrectionMakerBase::toGlobal(const Surface &surface, const Vector3DBase &lv) const { -// TkRotation rotation = surface.rotation(); -// -// return Vector3DBase(rotation.multiplyInverse(lv.basicVector())); -// } -// -// Point3DBase ResidualGlobalCorrectionMakerBase::toLocal(const Surface &surface, const Point3DBase &gp) const { -// Point3DBase position = surface.position(); -// TkRotation rotation = surface.rotation(); -// -// return Point3DBase(rotation*(gp.basicVector() - position.basicVector())); -// -// } -// -// Vector3DBase ResidualGlobalCorrectionMakerBase::toLocal(const Surface &surface, const Vector3DBase &gv) const { -// TkRotation rotation = surface.rotation(); -// -// return Vector3DBase(rotation*(gv.basicVector())); -// -// } +Matrix ResidualGlobalCorrectionMakerBase::curv2localJacobianAlignmentD(const Matrix &state, const MagneticField *field, const GloballyPositioned &surface, double dEdx, double mass, double dBz) const { -Matrix ResidualGlobalCorrectionMakerBase::curv2localJacobianAltelossD(const Matrix &state, const MagneticField *field, const GloballyPositioned &surface, double dEdx, double mass, double dBz) const { - - - const GlobalPoint pos(state[0], state[1], state[2]); + + const GlobalPoint pos(state[0], state[1], state[2]); const GlobalVector &bfield = field->inInverseGeV(pos); const Matrix Bv(bfield.x(), bfield.y(), double(bfield.z()) + 2.99792458e-3*dBz); - + const double q = state[6]; - + const double qop0 = q/state.segment<3>(3).norm(); const double lam0 = std::atan(state[5]/std::sqrt(state[3]*state[3] + state[4]*state[4])); const double phi0 = std::atan2(state[4], state[3]); - + + const double M0x = state[0]; + const double M0y = state[1]; + const double M0z = state[2]; + const Matrix W0 = state.segment<3>(3).normalized(); const double W0x = W0[0]; const double W0y = W0[1]; const double W0z = W0[2]; - + const Vector3DBase lx(1.,0.,0.); const Vector3DBase ly(0.,1.,0.); const Vector3DBase lz(0.,0.,1.); const Point3DBase l0(0., 0., 0.); - + // const Vector3DBase I1 = surface.toGlobal(lz); // const Vector3DBase J1 = surface.toGlobal(lx); -// const Vector3DBase K1 = surface.toGlobal(ly); +// const Vector3DBase K1 = surface.toGlobal(ly); // const Point3DBase r1 = surface.toGlobal(l0); - + const Vector3DBase I1 = surface.toGlobal(lz); const Vector3DBase J1 = surface.toGlobal(lx); - const Vector3DBase K1 = surface.toGlobal(ly); + const Vector3DBase K1 = surface.toGlobal(ly); const Point3DBase r1 = surface.toGlobal(l0); - + // const Vector3DBase I1 = toGlobal(surface, lz); // const Vector3DBase J1 = toGlobal(surface, lx); -// const Vector3DBase K1 = toGlobal(surface, ly); +// const Vector3DBase K1 = toGlobal(surface, ly); // const Point3DBase r1 = toGlobal(surface, l0); - + const double Ix1 = I1.x(); const double Iy1 = I1.y(); const double Iz1 = I1.z(); - + const double Jx1 = J1.x(); const double Jy1 = J1.y(); const double Jz1 = J1.z(); - + const double Kx1 = K1.x(); const double Ky1 = K1.y(); const double Kz1 = K1.z(); - + const double rx1 = r1.x(); const double ry1 = r1.y(); const double rz1 = r1.z(); - + const double B = Bv.norm(); const Matrix H = Bv.normalized(); const double hx = H[0]; const double hy = H[1]; const double hz = H[2]; - - const double x0 = std::pow(q, 2); - const double x1 = std::pow(x0, -3.0/2.0); - const double x2 = Iy1*W0x; - const double x3 = Ix1*W0y; - const double x4 = std::pow(qop0, 2); - const double x5 = std::pow(W0x, 2) + std::pow(W0y, 2); - const double x6 = std::pow(x5, -1.0/2.0); - const double x7 = std::sin(lam0); - const double x8 = Iz1*x7; - const double x9 = std::cos(lam0); - const double x10 = std::cos(phi0); - const double x11 = Ix1*x10; - const double x12 = std::sin(phi0); - const double x13 = Iy1*x12; - const double x14 = x11*x9 + x13*x9 + x8; - const double x15 = x6/x14; - const double x16 = dEdx*q*x1*x15*x4*std::sqrt(std::pow(mass, 2)*x4 + x0); - const double x17 = Ix1*W0x*W0z + Iy1*W0y*W0z - Iz1*x5; - const double x18 = std::pow(x14, -2); - const double x19 = Iz1*Jx1; - const double x20 = Iz1*Jy1; - const double x21 = lam0 + phi0; - const double x22 = -phi0; - const double x23 = lam0 + x22; - const double x24 = std::pow(Ix1*std::cos(x21) + Ix1*std::cos(x23) + Iy1*std::sin(x21) - Iy1*std::sin(x23) + 2*x8, -2); - const double x25 = 2*Ix1; - const double x26 = Jy1*x25; - const double x27 = 2*Iy1; - const double x28 = Jx1*x27; - const double x29 = 2*lam0; - const double x30 = std::cos(x29); - const double x31 = phi0 + x29; - const double x32 = std::cos(x31); - const double x33 = std::sin(x31); - const double x34 = Ix1*Jz1; - const double x35 = Iy1*Jz1; - const double x36 = x22 + x29; - const double x37 = std::cos(x36); - const double x38 = std::sin(x36); - const double x39 = -x2 + x3; - const double x40 = hz*x9; - const double x41 = hx*x7 - x10*x40; - const double x42 = hy*x7 - x12*x40; - const double x43 = x9*(hx*x12 - hy*x10); - const double x44 = x10*x9; - const double x45 = x12*x9; - const double x46 = Jx1*x44 + Jy1*x45 + Jz1*x7; - const double x47 = -Ix1*x42 + Iy1*x41 - Iz1*x43; - const double x48 = B*qop0*x6/std::pow(x14, 3); - const double x49 = x48*(x14*(-Jx1*x42 + Jy1*x41 - Jz1*x43) - x46*x47); - const double x50 = Iz1*Kx1; - const double x51 = Iz1*Ky1; - const double x52 = Ky1*x25; - const double x53 = Kx1*x27; - const double x54 = Ix1*Kz1; - const double x55 = Iy1*Kz1; - const double x56 = Kx1*x44 + Ky1*x45 + Kz1*x7; - const double x57 = x48*(x14*(-Kx1*x42 + Ky1*x41 - Kz1*x43) - x47*x56); - const double x58 = x14*x5; - const double x59 = W0z*x14; - const double dqopdqop0 = std::pow(q, 3)*x1*std::fabs(qop0)/qop0; + + const double x0 = 1.0/qop0; + const double x1 = std::pow(q, 2); + const double x2 = std::fabs(qop0)/std::pow(x1, 3.0/2.0); + const double x3 = Ix1*W0y - Iy1*W0x; + const double x4 = std::pow(W0x, 2); + const double x5 = std::pow(W0y, 2); + const double x6 = x4 + x5; + const double x7 = std::sqrt(x6); + const double x8 = 1.0/x7; + const double x9 = std::sin(lam0); + const double x10 = std::cos(lam0); + const double x11 = std::cos(phi0); + const double x12 = x10*x11; + const double x13 = std::sin(phi0); + const double x14 = x10*x13; + const double x15 = Ix1*x12 + Iy1*x14 + Iz1*x9; + const double x16 = 1.0/x15; + const double x17 = std::pow(qop0, 2); + const double x18 = 1.0/x17; + const double x19 = dEdx*q*x16*x17*x2*std::sqrt(std::pow(mass, 2) + x1*x18); + const double x20 = x19*x8; + const double x21 = W0x*W0z; + const double x22 = W0y*W0z; + const double x23 = Ix1*x21 + Iy1*x22 - Iz1*x6; + const double x24 = Ix1*Jx1 + Iy1*Jy1 + Iz1*Jz1; + const double x25 = Ix1*Kx1 + Iy1*Ky1 + Iz1*Kz1; + const double x26 = std::pow(Ix1, 2) + std::pow(Iy1, 2) + std::pow(Iz1, 2); + const double x27 = M0x*W0x; + const double x28 = M0y*W0y; + const double x29 = M0z*W0z + x27 + x28; + const double x30 = W0z*x27; + const double x31 = W0z*x28; + const double x32 = -M0z*x6 + x30 + x31; + const double x33 = W0z*x29 - rz1 - x32; + const double x34 = M0x*W0y; + const double x35 = M0y*W0x - x34; + const double x36 = x29*x6; + const double x37 = -x32; + const double x38 = -W0x*x36 + W0y*x35 + rx1*x6 + x21*x37; + const double x39 = -W0x*x35 + W0y*W0z*x37 - W0y*x36 + ry1*x6; + const double x40 = -Kx1*x38 - Ky1*x39 + Kz1*x33*x6; + const double x41 = 1.0/x6; + const double x42 = x19*x41; + const double x43 = Jx1*x38 + Jy1*x39 - Jz1*x33*x6; + const double x44 = x11*x9; + const double x45 = x13*x9; + const double x46 = Iz1*x10; + const double x47 = Ix1*x44 + Iy1*x45 - x46; + const double x48 = std::pow(x15, -2); + const double x49 = Jx1*x12 + Jy1*x14 + Jz1*x9; + const double x50 = x48*x49; + const double x51 = Ix1*x14 - Iy1*x12; + const double x52 = B*qop0; + const double x53 = x52*(-hy*x9 + hz*x10*x13); + const double x54 = x52*(hx*x9 - hz*x12); + const double x55 = x52*(-hx*x13 + hy*x11); + const double x56 = -Ix1*x53 - Iy1*x54 - x46*x55; + const double x57 = x16*(x16*(Jx1*x53 + Jy1*x54 + Jz1*x10*x55) + x50*x56); + const double x58 = x57*x8; + const double x59 = Kx1*x12 + Ky1*x14 + Kz1*x9; + const double x60 = x41*x57; + const double x61 = -x49; + const double x62 = x16*x59; + const double x63 = x48*x59; + const double x64 = x16*(x16*(Kx1*x53 + Ky1*x54 + Kz1*x10*x55) + x56*x63); + const double x65 = x64*x8; + const double x66 = x41*x64; + const double x67 = x29*x7; + const double x68 = B*x67; + const double x69 = M0y*W0x*x8 - x34*x8; + const double x70 = M0z*(x4*x8 + x5*x8) - x30*x8 - x31*x8; + const double x71 = B*(W0y*x69 + x21*x70); + const double x72 = 1.0/B; + const double x73 = x72*x8; + const double x74 = x0*x73; + const double x75 = x52*x67; + const double x76 = W0x*x75 - qop0*x71; + const double x77 = x18*x73; + const double x78 = x74*(W0x*x68 - x71) - x76*x77; + const double x79 = B*(W0x*x69 - x22*x70); + const double x80 = W0y*x75 + qop0*x79; + const double x81 = x74*(W0y*x68 + x79) - x77*x80; + const double x82 = W0x*x8; + const double x83 = W0y*x8; + const double x84 = x16*x49; + const double x85 = x21*x8; + const double x86 = x22*x8; + const double x87 = Jx1*Kx1 + Jy1*Ky1 + Jz1*Kz1; + const double x88 = -rx1 + x0*x72*x76*x8; + const double x89 = -ry1 + x0*x72*x8*x80; + const double x90 = W0z*x29 - rz1 + x7*x70; + const double x91 = Ix1*x88 + Iy1*x89 + Iz1*x90; + const double x92 = x41*x62; + const double dqopdqop0 = std::pow(q, 3)*x0*x2; const double dqopdlam0 = 0; const double dqopdphi0 = 0; - const double dqopdxt0 = x16*(x2 - x3); - const double dqopdyt0 = -x16*x17; + const double dqopdxt0 = -x20*x3; + const double dqopdyt0 = -x20*x23; + const double dqopdalpha_x = -x19*x24; + const double dqopdalpha_y = -x19*x25; + const double dqopdalpha_z = -x19*x26; + const double dqopdtheta_x = -x40*x42; + const double dqopdtheta_y = -x42*x43; + const double dqopdtheta_z = 0; const double ddxdzdqop0 = 0; - const double ddxdzdlam0 = x18*(Jz1*x11 + Jz1*x13 - x10*x19 - x12*x20); - const double ddxdzdphi0 = x24*(x19*x32 - x19*x37 + x20*x33 + x20*x38 + x26*x30 + x26 - x28*x30 - x28 - x32*x34 - x33*x35 + x34*x37 - x35*x38); - const double ddxdzdxt0 = x39*x49; - const double ddxdzdyt0 = x17*x49; + const double ddxdzdlam0 = x16*(-Jx1*x44 - Jy1*x45 + Jz1*x10) + x47*x50; + const double ddxdzdphi0 = x16*(-Jx1*x14 + Jy1*x10*x11) + x50*x51; + const double ddxdzdxt0 = x3*x58; + const double ddxdzdyt0 = x23*x58; + const double ddxdzdalpha_x = x24*x57; + const double ddxdzdalpha_y = x25*x57; + const double ddxdzdalpha_z = x26*x57; + const double ddxdzdtheta_x = x40*x60 + x50*x59; + const double ddxdzdtheta_y = -x15*x16 + x43*x60 + x50*x61; + const double ddxdzdtheta_z = x62; const double ddydzdqop0 = 0; - const double ddydzdlam0 = x18*(Kz1*x11 + Kz1*x13 - x10*x50 - x12*x51); - const double ddydzdphi0 = x24*(x30*x52 - x30*x53 + x32*x50 - x32*x54 + x33*x51 - x33*x55 - x37*x50 + x37*x54 + x38*x51 - x38*x55 + x52 - x53); - const double ddydzdxt0 = x39*x57; - const double ddydzdyt0 = x17*x57; - const double dxdqop0 = 0; + const double ddydzdlam0 = x16*(-Kx1*x44 - Ky1*x45 + Kz1*x10) + x47*x63; + const double ddydzdphi0 = x16*(-Kx1*x14 + Ky1*x10*x11) + x51*x63; + const double ddydzdxt0 = x3*x65; + const double ddydzdyt0 = x23*x65; + const double ddydzdalpha_x = x24*x64; + const double ddydzdalpha_y = x25*x64; + const double ddydzdalpha_z = x26*x64; + const double ddydzdtheta_x = x40*x66 + x48*std::pow(x59, 2) + 1; + const double ddydzdtheta_y = x43*x66 + x61*x63; + const double ddydzdtheta_z = x16*x61; + const double dxdqop0 = Jx1*x78 + Jy1*x81; const double dxdlam0 = 0; const double dxdphi0 = 0; - const double dxdxt0 = x15*(x14*(-Jx1*W0y + Jy1*W0x) + x39*x46); - const double dxdyt0 = x15*(Jz1*x58 + x17*x46 - x59*(Jx1*W0x + Jy1*W0y)); - const double dydqop0 = 0; + const double dxdxt0 = -Jx1*x83 + Jy1*x82 + x3*x8*x84; + const double dxdyt0 = -Jx1*x85 - Jy1*x86 + Jz1*x7 + x16*x23*x49*x8; + const double dxdalpha_x = -std::pow(Jx1, 2) - std::pow(Jy1, 2) - std::pow(Jz1, 2) + x16*x24*x49; + const double dxdalpha_y = x16*x25*x49 - x87; + const double dxdalpha_z = x16*x26*x49 - x24; + const double dxdtheta_x = x40*x41*x84; + const double dxdtheta_y = x16*x41*x43*x49 - x91; + const double dxdtheta_z = Kx1*x88 + Ky1*x89 + Kz1*x90; + const double dydqop0 = Kx1*x78 + Ky1*x81; const double dydlam0 = 0; const double dydphi0 = 0; - const double dydxt0 = x15*(x14*(-Kx1*W0y + Ky1*W0x) + x39*x56); - const double dydyt0 = x15*(Kz1*x58 + x17*x56 - x59*(Kx1*W0x + Ky1*W0y)); - Matrix res; + const double dydxt0 = -Kx1*x83 + Ky1*x82 + x3*x62*x8; + const double dydyt0 = -Kx1*x85 - Ky1*x86 + Kz1*x7 + x16*x23*x59*x8; + const double dydalpha_x = x16*x24*x59 - x87; + const double dydalpha_y = -std::pow(Kx1, 2) - std::pow(Ky1, 2) - std::pow(Kz1, 2) + x16*x25*x59; + const double dydalpha_z = x16*x26*x59 - x25; + const double dydtheta_x = x40*x92 + x91; + const double dydtheta_y = x43*x92; + const double dydtheta_z = -Jx1*x88 - Jy1*x89 - Jz1*x90; + Matrix res; res(0,0) = dqopdqop0; res(0,1) = dqopdlam0; res(0,2) = dqopdphi0; res(0,3) = dqopdxt0; res(0,4) = dqopdyt0; + res(0,5) = dqopdalpha_x; + res(0,6) = dqopdalpha_y; + res(0,7) = dqopdalpha_z; + res(0,8) = dqopdtheta_x; + res(0,9) = dqopdtheta_y; + res(0,10) = dqopdtheta_z; res(1,0) = ddxdzdqop0; res(1,1) = ddxdzdlam0; res(1,2) = ddxdzdphi0; res(1,3) = ddxdzdxt0; res(1,4) = ddxdzdyt0; + res(1,5) = ddxdzdalpha_x; + res(1,6) = ddxdzdalpha_y; + res(1,7) = ddxdzdalpha_z; + res(1,8) = ddxdzdtheta_x; + res(1,9) = ddxdzdtheta_y; + res(1,10) = ddxdzdtheta_z; res(2,0) = ddydzdqop0; res(2,1) = ddydzdlam0; res(2,2) = ddydzdphi0; res(2,3) = ddydzdxt0; res(2,4) = ddydzdyt0; + res(2,5) = ddydzdalpha_x; + res(2,6) = ddydzdalpha_y; + res(2,7) = ddydzdalpha_z; + res(2,8) = ddydzdtheta_x; + res(2,9) = ddydzdtheta_y; + res(2,10) = ddydzdtheta_z; res(3,0) = dxdqop0; res(3,1) = dxdlam0; res(3,2) = dxdphi0; res(3,3) = dxdxt0; res(3,4) = dxdyt0; + res(3,5) = dxdalpha_x; + res(3,6) = dxdalpha_y; + res(3,7) = dxdalpha_z; + res(3,8) = dxdtheta_x; + res(3,9) = dxdtheta_y; + res(3,10) = dxdtheta_z; res(4,0) = dydqop0; res(4,1) = dydlam0; res(4,2) = dydphi0; res(4,3) = dydxt0; res(4,4) = dydyt0; + res(4,5) = dydalpha_x; + res(4,6) = dydalpha_y; + res(4,7) = dydalpha_z; + res(4,8) = dydtheta_x; + res(4,9) = dydtheta_y; + res(4,10) = dydtheta_z; - - return res; - -} - -Matrix ResidualGlobalCorrectionMakerBase::curv2cartJacobianAlt(const FreeTrajectoryState &state) const { - - const GlobalTrajectoryParameters &globalSource = state.parameters(); - - CurvilinearTrajectoryParameters curvparms(globalSource.position(), globalSource.momentum(), globalSource.charge()); - const double qop0 = curvparms.Qbp(); - const double lam0 = curvparms.lambda(); - const double phi0 = curvparms.phi(); - const double xt0 = curvparms.xT(); - const double yt0 = curvparms.yT(); - - const double p0 = globalSource.momentum().mag(); - const GlobalVector W0 = globalSource.momentum()/p0; - const double W0x = W0.x(); - const double W0y = W0.y(); - const double W0z = W0.z(); - - const double x0 = std::sqrt(std::pow(W0x, 2) + std::pow(W0y, 2)); - const double x1 = 1.0/x0; - const double x2 = W0y*x1; - const double x3 = W0x*x1; - const double x4 = 1.0/qop0; - const double x5 = std::cos(phi0); - const double x6 = 1.0/std::fabs(qop0); - const double x7 = x6*std::cos(lam0); - const double x8 = x5*x7; - const double x9 = x6*std::sin(lam0); - const double x10 = std::sin(phi0); - const double x11 = x10*x7; - const double dxdqop0 = 0; - const double dxdlam0 = 0; - const double dxdphi0 = 0; - const double dxdxt0 = -x2; - const double dxdyt0 = -W0z*x3; - const double dydqop0 = 0; - const double dydlam0 = 0; - const double dydphi0 = 0; - const double dydxt0 = x3; - const double dydyt0 = -W0z*x2; - const double dzdqop0 = 0; - const double dzdlam0 = 0; - const double dzdphi0 = 0; - const double dzdxt0 = 0; - const double dzdyt0 = x0; - const double dpxdqop0 = -x4*x8; - const double dpxdlam0 = -x5*x9; - const double dpxdphi0 = -x11; - const double dpxdxt0 = 0; - const double dpxdyt0 = 0; - const double dpydqop0 = -x11*x4; - const double dpydlam0 = -x10*x9; - const double dpydphi0 = x8; - const double dpydxt0 = 0; - const double dpydyt0 = 0; - const double dpzdqop0 = -x4*x9; - const double dpzdlam0 = x7; - const double dpzdphi0 = 0; - const double dpzdxt0 = 0; - const double dpzdyt0 = 0; - Matrix res; - res(0,0) = dxdqop0; - res(0,1) = dxdlam0; - res(0,2) = dxdphi0; - res(0,3) = dxdxt0; - res(0,4) = dxdyt0; - res(1,0) = dydqop0; - res(1,1) = dydlam0; - res(1,2) = dydphi0; - res(1,3) = dydxt0; - res(1,4) = dydyt0; - res(2,0) = dzdqop0; - res(2,1) = dzdlam0; - res(2,2) = dzdphi0; - res(2,3) = dzdxt0; - res(2,4) = dzdyt0; - res(3,0) = dpxdqop0; - res(3,1) = dpxdlam0; - res(3,2) = dpxdphi0; - res(3,3) = dpxdxt0; - res(3,4) = dpxdyt0; - res(4,0) = dpydqop0; - res(4,1) = dpydlam0; - res(4,2) = dpydphi0; - res(4,3) = dpydxt0; - res(4,4) = dpydyt0; - res(5,0) = dpzdqop0; - res(5,1) = dpzdlam0; - res(5,2) = dpzdphi0; - res(5,3) = dpzdxt0; - res(5,4) = dpzdyt0; - - return res; - - -} - -Matrix ResidualGlobalCorrectionMakerBase::curv2cartJacobianAltD(const Matrix &state) const { - - const double charge = state[6]; - - const double qop0 = charge/state.segment<3>(3).norm(); - const double lam0 = std::atan(state[5]/std::sqrt(state[3]*state[3] + state[4]*state[4])); - const double phi0 = std::atan2(state[4], state[3]); - - const Matrix W0 = state.segment<3>(3).normalized(); - const double W0x = W0[0]; - const double W0y = W0[1]; - const double W0z = W0[2]; - - const double x0 = std::sqrt(std::pow(W0x, 2) + std::pow(W0y, 2)); - const double x1 = 1.0/x0; - const double x2 = W0y*x1; - const double x3 = W0x*x1; - const double x4 = 1.0/qop0; - const double x5 = std::cos(phi0); - const double x6 = 1.0/std::fabs(qop0); - const double x7 = x6*std::cos(lam0); - const double x8 = x5*x7; - const double x9 = x6*std::sin(lam0); - const double x10 = std::sin(phi0); - const double x11 = x10*x7; - const double dxdqop0 = 0; - const double dxdlam0 = 0; - const double dxdphi0 = 0; - const double dxdxt0 = -x2; - const double dxdyt0 = -W0z*x3; - const double dydqop0 = 0; - const double dydlam0 = 0; - const double dydphi0 = 0; - const double dydxt0 = x3; - const double dydyt0 = -W0z*x2; - const double dzdqop0 = 0; - const double dzdlam0 = 0; - const double dzdphi0 = 0; - const double dzdxt0 = 0; - const double dzdyt0 = x0; - const double dpxdqop0 = -x4*x8; - const double dpxdlam0 = -x5*x9; - const double dpxdphi0 = -x11; - const double dpxdxt0 = 0; - const double dpxdyt0 = 0; - const double dpydqop0 = -x11*x4; - const double dpydlam0 = -x10*x9; - const double dpydphi0 = x8; - const double dpydxt0 = 0; - const double dpydyt0 = 0; - const double dpzdqop0 = -x4*x9; - const double dpzdlam0 = x7; - const double dpzdphi0 = 0; - const double dpzdxt0 = 0; - const double dpzdyt0 = 0; - Matrix res; - res(0,0) = dxdqop0; - res(0,1) = dxdlam0; - res(0,2) = dxdphi0; - res(0,3) = dxdxt0; - res(0,4) = dxdyt0; - res(1,0) = dydqop0; - res(1,1) = dydlam0; - res(1,2) = dydphi0; - res(1,3) = dydxt0; - res(1,4) = dydyt0; - res(2,0) = dzdqop0; - res(2,1) = dzdlam0; - res(2,2) = dzdphi0; - res(2,3) = dzdxt0; - res(2,4) = dzdyt0; - res(3,0) = dpxdqop0; - res(3,1) = dpxdlam0; - res(3,2) = dpxdphi0; - res(3,3) = dpxdxt0; - res(3,4) = dpxdyt0; - res(4,0) = dpydqop0; - res(4,1) = dpydlam0; - res(4,2) = dpydphi0; - res(4,3) = dpydxt0; - res(4,4) = dpydyt0; - res(5,0) = dpzdqop0; - res(5,1) = dpzdlam0; - res(5,2) = dpzdphi0; - res(5,3) = dpzdxt0; - res(5,4) = dpzdyt0; - - return res; - - -} - -Matrix ResidualGlobalCorrectionMakerBase::localTransportJacobianAlt(const TrajectoryStateOnSurface &start, - const std::pair &propresult, - bool doReverse) const { - - const TrajectoryStateOnSurface& state0 = doReverse ? propresult.first : start; - const TrajectoryStateOnSurface& state1 = doReverse ? start : propresult.first; - const GlobalVector& bfield = start.globalParameters().magneticFieldInInverseGeV(); - const double s = doReverse ? -propresult.second : propresult.second; - - const double qop0 = state0.localParameters().qbp(); - const double dxdz0 = state0.localParameters().dxdz(); - const double dydz0 = state0.localParameters().dydz(); - const double x0 = state0.localParameters().position().x(); - const double y0 = state0.localParameters().position().y(); - - const double localpzsign = state0.localParameters().pzSign(); - - const LocalVector lx(1.,0.,0.); - const LocalVector ly(0.,1.,0.); - const LocalVector lz(0.,0.,1.); - const LocalPoint l0(0., 0.); - - const GlobalVector I0 = state0.surface().toGlobal(lz); - const GlobalVector J0 = state0.surface().toGlobal(lx); - const GlobalVector K0 = state0.surface().toGlobal(ly); - const GlobalPoint r0 = state0.surface().toGlobal(l0); - - const double Ix0 = I0.x(); - const double Iy0 = I0.y(); - const double Iz0 = I0.z(); - - const double Jx0 = J0.x(); - const double Jy0 = J0.y(); - const double Jz0 = J0.z(); - - const double Kx0 = K0.x(); - const double Ky0 = K0.y(); - const double Kz0 = K0.z(); - - const double rx0 = r0.x(); - const double ry0 = r0.y(); - const double rz0 = r0.z(); - - const GlobalVector I1 = state1.surface().toGlobal(lz); - const GlobalVector J1 = state1.surface().toGlobal(lx); - const GlobalVector K1 = state1.surface().toGlobal(ly); - const GlobalPoint r1 = state1.surface().toGlobal(l0); - - const double Ix1 = I1.x(); - const double Iy1 = I1.y(); - const double Iz1 = I1.z(); - - const double Jx1 = J1.x(); - const double Jy1 = J1.y(); - const double Jz1 = J1.z(); - - const double Kx1 = K1.x(); - const double Ky1 = K1.y(); - const double Kz1 = K1.z(); - - const double rx1 = r1.x(); - const double ry1 = r1.y(); - const double rz1 = r1.z(); - - const double B = bfield.mag(); - const GlobalVector H = bfield/B; - const double hx = H.x(); - const double hy = H.y(); - const double hz = H.z(); - - const double x1 = B*s; - const double x2 = qop0*x1; - const double x3 = std::cos(x2); - const double x4 = Ix0 + Jx0*dxdz0 + Kx0*dydz0; - const double x5 = x3*x4; - const double x6 = std::sin(x2); - const double x7 = Iy0 + Jy0*dxdz0 + Ky0*dydz0; - const double x8 = hz*x7; - const double x9 = Iz0 + Jz0*dxdz0 + Kz0*dydz0; - const double x10 = hy*x9; - const double x11 = -x10 + x8; - const double x12 = x3 - 1; - const double x13 = hx*x4 + hy*x7 + hz*x9; - const double x14 = x12*x13; - const double x15 = -hx*x14 + x11*x6 + x5; - const double x16 = std::pow(dxdz0, 2) + std::pow(dydz0, 2) + 1; - const double x17 = std::sqrt(x16); - const double x18 = 1.0/x17; - const double x19 = localpzsign*x18; - const double x20 = x15*x19; - const double x21 = x3*x7; - const double x22 = hx*x9 - hz*x4; - const double x23 = x22*x6; - const double x24 = -hy*x14 + x21 + x23; - const double x25 = x19*x24; - const double x26 = x3*x9; - const double x27 = hy*x4; - const double x28 = hx*x7; - const double x29 = x27 - x28; - const double x30 = -hz*x14 + x26 + x29*x6; - const double x31 = x19*x30; - const double x32 = Ix1*x20 + Iy1*x25 + Iz1*x31; - const double x33 = 1.0/x32; - const double x34 = x1*x6; - const double x35 = x1*x3; - const double x36 = x13*x34; - const double x37 = hx*x36 + x11*x35 - x34*x4; - const double x38 = Jx1*x19; - const double x39 = hy*x36 + x22*x35 - x34*x7; - const double x40 = Jy1*x19; - const double x41 = hz*x36 + x29*x35 - x34*x9; - const double x42 = Jz1*x19; - const double x43 = Ix1*x19; - const double x44 = Iy1*x19; - const double x45 = Iz1*x19; - const double x46 = -x37*x43 - x39*x44 - x41*x45; - const double x47 = std::pow(x32, -2); - const double x48 = x47*(Jx1*x20 + Jy1*x25 + Jz1*x31); - const double x49 = localpzsign*x6; - const double x50 = Jx0*x0 + Kx0*y0 + rx0; - const double x51 = B*qop0; - const double x52 = x17*x51; - const double x53 = x10 - x8; - const double x54 = localpzsign*x12; - const double x55 = x2 - x6; - const double x56 = localpzsign*x13; - const double x57 = hx*x56; - const double x58 = x4*x49 + x50*x52 + x53*x54 + x55*x57; - const double x59 = 1.0/B; - const double x60 = x18*x59; - const double x61 = x60/std::pow(qop0, 2); - const double x62 = localpzsign*x1; - const double x63 = B*x17; - const double x64 = x1*x49; - const double x65 = x1 - x35; - const double x66 = 1.0/qop0; - const double x67 = x60*x66; - const double x68 = -x58*x61 + x67*(x5*x62 + x50*x63 - x53*x64 + x57*x65); - const double x69 = Jy0*x0 + Ky0*y0 + ry0; - const double x70 = hy*x56; - const double x71 = -x22*x54 + x49*x7 + x52*x69 + x55*x70; - const double x72 = -x61*x71 + x67*(x21*x62 + x23*x62 + x63*x69 + x65*x70); - const double x73 = Jz0*x0 + Kz0*y0 + rz0; - const double x74 = -x27 + x28; - const double x75 = hz*x56; - const double x76 = x49*x9 + x52*x73 + x54*x74 + x55*x75; - const double x77 = -x61*x76 + x67*(x26*x62 + x63*x73 - x64*x74 + x65*x75); - const double x78 = -Ix1*x68 - Iy1*x72 - Iz1*x77; - const double x79 = x51*x6; - const double x80 = x3*x51; - const double x81 = x13*x79; - const double x82 = hx*x81 + x11*x80 - x4*x79; - const double x83 = hy*x81 + x22*x80 - x7*x79; - const double x84 = hz*x81 + x29*x80 - x79*x9; - const double x85 = -x43*x82 - x44*x83 - x45*x84; - const double x86 = x33*(x33*(x38*x82 + x40*x83 + x42*x84) + x48*x85); - const double x87 = Jy0*hz; - const double x88 = Jz0*hy; - const double x89 = Jx0*hx + Jy0*hy + Jz0*hz; - const double x90 = x12*x89; - const double x91 = Jx0*x3 - hx*x90 + x6*(x87 - x88); - const double x92 = -Jx0*hz + Jz0*hx; - const double x93 = Jy0*x3 - hy*x90 + x6*x92; - const double x94 = Jx0*hy; - const double x95 = Jy0*hx; - const double x96 = Jz0*x3 - hz*x90 + x6*(x94 - x95); - const double x97 = std::pow(x16, -3.0/2.0); - const double x98 = dxdz0*x97; - const double x99 = localpzsign*x98; - const double x100 = x15*x99; - const double x101 = x24*x99; - const double x102 = x30*x99; - const double x103 = Ix1*x100 + Iy1*x101 + Iz1*x102 - x43*x91 - x44*x93 - x45*x96; - const double x104 = x18*x51; - const double x105 = dxdz0*x104; - const double x106 = localpzsign*x55; - const double x107 = x106*x89; - const double x108 = x58*x66; - const double x109 = x59*x98; - const double x110 = -x108*x109 + x67*(Jx0*x49 + hx*x107 + x105*x50 + x54*(-x87 + x88)); - const double x111 = x109*x66; - const double x112 = -x111*x76 + x67*(Jz0*x49 + hz*x107 + x105*x73 + x54*(-x94 + x95)); - const double x113 = -x111*x71 + x67*(Jy0*x49 + hy*x107 + x105*x69 - x54*x92); - const double x114 = -Ix1*x110 - Iy1*x113 - Iz1*x112; - const double x115 = Ky0*hz; - const double x116 = Kz0*hy; - const double x117 = Kx0*hx + Ky0*hy + Kz0*hz; - const double x118 = x117*x12; - const double x119 = Kx0*x3 - hx*x118 + x6*(x115 - x116); - const double x120 = -Kx0*hz + Kz0*hx; - const double x121 = Ky0*x3 - hy*x118 + x120*x6; - const double x122 = Kx0*hy; - const double x123 = Ky0*hx; - const double x124 = Kz0*x3 - hz*x118 + x6*(x122 - x123); - const double x125 = dydz0*x97; - const double x126 = localpzsign*x125; - const double x127 = x126*x15; - const double x128 = x126*x24; - const double x129 = x126*x30; - const double x130 = Ix1*x127 + Iy1*x128 + Iz1*x129 - x119*x43 - x121*x44 - x124*x45; - const double x131 = dydz0*x104; - const double x132 = x106*x117; - const double x133 = x125*x59; - const double x134 = -x108*x133 + x67*(Kx0*x49 + hx*x132 + x131*x50 + x54*(-x115 + x116)); - const double x135 = x133*x66; - const double x136 = -x135*x76 + x67*(Kz0*x49 + hz*x132 + x131*x73 + x54*(-x122 + x123)); - const double x137 = -x135*x71 + x67*(Ky0*x49 + hy*x132 - x120*x54 + x131*x69); - const double x138 = -Ix1*x134 - Iy1*x137 - Iz1*x136; - const double x139 = -Ix1*Jx0 - Iy1*Jy0 - Iz1*Jz0; - const double x140 = -Ix1*Kx0 - Iy1*Ky0 - Iz1*Kz0; - const double x141 = qop0*s; - const double x142 = x141*x6; - const double x143 = x141*x3; - const double x144 = x13*x142; - const double x145 = hx*x144 + x11*x143 - x142*x4; - const double x146 = hy*x144 - x142*x7 + x143*x22; - const double x147 = hz*x144 - x142*x9 + x143*x29; - const double x148 = -x145*x43 - x146*x44 - x147*x45; - const double x149 = x18/std::pow(B, 2); - const double x150 = localpzsign*x141; - const double x151 = qop0*x17; - const double x152 = x141*x49; - const double x153 = x141 - x143; - const double x154 = -x108*x149 + x67*(x150*x5 + x151*x50 - x152*x53 + x153*x57); - const double x155 = x149*x66; - const double x156 = -x155*x71 + x67*(x150*x21 + x150*x23 + x151*x69 + x153*x70); - const double x157 = -x155*x76 + x67*(x150*x26 + x151*x73 - x152*x74 + x153*x75); - const double x158 = -Ix1*x154 - Iy1*x156 - Iz1*x157; - const double x159 = Kx1*x19; - const double x160 = Ky1*x19; - const double x161 = Kz1*x19; - const double x162 = x47*(Kx1*x20 + Ky1*x25 + Kz1*x31); - const double x163 = x33*(x162*x85 + x33*(x159*x82 + x160*x83 + x161*x84)); - const double x164 = localpzsign*x51; - const double x165 = x51 - x80; - const double x166 = x67*(x164*x21 + x164*x23 + x165*x70); - const double x167 = x49*x51; - const double x168 = x67*(x164*x5 + x165*x57 - x167*x53); - const double x169 = x67*(x164*x26 + x165*x75 - x167*x74); - const double x170 = x33*(Jx1*x168 + Jy1*x166 + Jz1*x169); - const double x171 = x33*(Kx1*x168 + Ky1*x166 + Kz1*x169); - const double dqopdqop0 = 1; - const double dqopddxdz0 = 0; - const double dqopddydz0 = 0; - const double dqopdx0 = 0; - const double dqopdy0 = 0; - const double dqopdB = 0; - const double ddxdzdqop0 = x33*(x37*x38 + x39*x40 + x41*x42) + x46*x48 + x78*x86; - const double ddxdzddxdz0 = x103*x48 + x114*x86 + x33*(-Jx1*x100 - Jy1*x101 - Jz1*x102 + x38*x91 + x40*x93 + x42*x96); - const double ddxdzddydz0 = x130*x48 + x138*x86 + x33*(-Jx1*x127 - Jy1*x128 - Jz1*x129 + x119*x38 + x121*x40 + x124*x42); - const double ddxdzdx0 = x139*x86; - const double ddxdzdy0 = x140*x86; - const double ddxdzdB = x148*x48 + x158*x86 + x33*(x145*x38 + x146*x40 + x147*x42); - const double ddydzdqop0 = x162*x46 + x163*x78 + x33*(x159*x37 + x160*x39 + x161*x41); - const double ddydzddxdz0 = x103*x162 + x114*x163 + x33*(-Kx1*x100 - Ky1*x101 - Kz1*x102 + x159*x91 + x160*x93 + x161*x96); - const double ddydzddydz0 = x130*x162 + x138*x163 + x33*(-Kx1*x127 - Ky1*x128 - Kz1*x129 + x119*x159 + x121*x160 + x124*x161); - const double ddydzdx0 = x139*x163; - const double ddydzdy0 = x140*x163; - const double ddydzdB = x148*x162 + x158*x163 + x33*(x145*x159 + x146*x160 + x147*x161); - const double dxdqop0 = Jx1*x68 + Jy1*x72 + Jz1*x77 + x170*x78; - const double dxddxdz0 = Jx1*x110 + Jy1*x113 + Jz1*x112 + x114*x170; - const double dxddydz0 = Jx1*x134 + Jy1*x137 + Jz1*x136 + x138*x170; - const double dxdx0 = Jx0*Jx1 + Jy0*Jy1 + Jz0*Jz1 + x139*x170; - const double dxdy0 = Jx1*Kx0 + Jy1*Ky0 + Jz1*Kz0 + x140*x170; - const double dxdB = Jx1*x154 + Jy1*x156 + Jz1*x157 + x158*x170; - const double dydqop0 = Kx1*x68 + Ky1*x72 + Kz1*x77 + x171*x78; - const double dyddxdz0 = Kx1*x110 + Ky1*x113 + Kz1*x112 + x114*x171; - const double dyddydz0 = Kx1*x134 + Ky1*x137 + Kz1*x136 + x138*x171; - const double dydx0 = Jx0*Kx1 + Jy0*Ky1 + Jz0*Kz1 + x139*x171; - const double dydy0 = Kx0*Kx1 + Ky0*Ky1 + Kz0*Kz1 + x140*x171; - const double dydB = Kx1*x154 + Ky1*x156 + Kz1*x157 + x158*x171; - Matrix res; - res(0,0) = dqopdqop0; - res(0,1) = dqopddxdz0; - res(0,2) = dqopddydz0; - res(0,3) = dqopdx0; - res(0,4) = dqopdy0; - res(0,5) = dqopdB; - res(1,0) = ddxdzdqop0; - res(1,1) = ddxdzddxdz0; - res(1,2) = ddxdzddydz0; - res(1,3) = ddxdzdx0; - res(1,4) = ddxdzdy0; - res(1,5) = ddxdzdB; - res(2,0) = ddydzdqop0; - res(2,1) = ddydzddxdz0; - res(2,2) = ddydzddydz0; - res(2,3) = ddydzdx0; - res(2,4) = ddydzdy0; - res(2,5) = ddydzdB; - res(3,0) = dxdqop0; - res(3,1) = dxddxdz0; - res(3,2) = dxddydz0; - res(3,3) = dxdx0; - res(3,4) = dxdy0; - res(3,5) = dxdB; - res(4,0) = dydqop0; - res(4,1) = dyddxdz0; - res(4,2) = dyddydz0; - res(4,3) = dydx0; - res(4,4) = dydy0; - res(4,5) = dydB; - - res.col(5) *= 2.99792458e-3; - - return res; - -} - -Matrix ResidualGlobalCorrectionMakerBase::curv2localTransportJacobian(const FreeTrajectoryState& start, - const std::pair& propresult, - bool doReverse) const { - - const FreeTrajectoryState& end = *propresult.first.freeState(); - const TrajectoryStateOnSurface& proptsos = propresult.first; - - const FreeTrajectoryState& state0 = doReverse ? end : start; - const FreeTrajectoryState& state1 = doReverse ? start : end; - const GlobalVector& h = start.parameters().magneticFieldInInverseGeV(); - const double s = doReverse ? -propresult.second : propresult.second; - -// // compute transport jacobian wrt curvlinear parameters -// AnalyticalCurvilinearJacobian curv2curv; -// curv2curv.computeFullJacobian(state0.parameters(), state1.parameters().position(), state1.parameters().momentum(), h, s); -// const AlgebraicMatrix55& curv2curvjac = curv2curv.jacobian(); -// const Matrix F = Map>(curv2curvjac.Array()); -// -// // compute transport jacobian wrt B field (magnitude) -// const Matrix dF = bfieldJacobian(state0.parameters(), state1.parameters(), s, h); - - const Matrix FdF = curvtransportJacobian(state0.parameters(), state1.parameters(), s, h); - const Matrix F = FdF.topLeftCorner<5,5>(); - const Matrix dF = FdF.col(5); - - Matrix res; - - if (doReverse) { - // compute local to curvilinear jacobian at source - JacobianLocalToCurvilinear local2curv(proptsos.surface(), proptsos.localParameters(), *proptsos.magneticField()); - const AlgebraicMatrix55& local2curvjac = local2curv.jacobian(); - const Matrix H0 = Map>(local2curvjac.Array()); - - res.leftCols<5>() = F*H0; - res.rightCols<1>() = dF; - } - else { - // compute curvilinear to local jacobian at destination - JacobianCurvilinearToLocal curv2local(proptsos.surface(), proptsos.localParameters(), *proptsos.magneticField()); - const AlgebraicMatrix55& curv2localjac = curv2local.jacobian(); - const Matrix H1 = Map>(curv2localjac.Array()); - - res.leftCols<5>() = H1*F; - res.rightCols<1>() = H1*dF; - } - - return res; - -} - -Matrix ResidualGlobalCorrectionMakerBase::curv2curvTransportJacobian(const FreeTrajectoryState& start, - const std::pair& propresult, - bool doReverse) const { - - const FreeTrajectoryState& end = *propresult.first.freeState(); - const TrajectoryStateOnSurface& proptsos = propresult.first; - - const FreeTrajectoryState& state0 = doReverse ? end : start; - const FreeTrajectoryState& state1 = doReverse ? start : end; - const GlobalVector& h = start.parameters().magneticFieldInInverseGeV(); -// const GlobalVector h = 0.5*(start.parameters().magneticFieldInInverseGeV() + end.parameters().magneticFieldInInverseGeV()); - const double s = doReverse ? -propresult.second : propresult.second; - -// // compute transport jacobian wrt curvlinear parameters -// AnalyticalCurvilinearJacobian curv2curv; -// curv2curv.computeFullJacobian(state0.parameters(), state1.parameters().position(), state1.parameters().momentum(), h, s); -// const AlgebraicMatrix55& curv2curvjac = curv2curv.jacobian(); -// const Matrix F = Map>(curv2curvjac.Array()); -// -// // compute transport jacobian wrt B field (magnitude) -// const Matrix dF = bfieldJacobian(state0.parameters(), state1.parameters(), s, h); - - const Matrix FdF = curvtransportJacobian(state0.parameters(), state1.parameters(), s, h); - - return FdF; -} - -AlgebraicVector5 ResidualGlobalCorrectionMakerBase::localMSConvolution(const TrajectoryStateOnSurface& tsos, const MaterialEffectsUpdator& updator) const { - - const LocalVector lx(1.,0.,0.); - const LocalVector ly(0.,1.,0.); - const GlobalVector J = tsos.surface().toGlobal(lx); - const GlobalVector K = tsos.surface().toGlobal(ly); - - const double Jx = J.x(); - const double Jy = J.y(); - const double Jz = J.z(); - - const double Kx = K.x(); - const double Ky = K.y(); - const double Kz = K.z(); - - const Matrix p0(tsos.globalMomentum().x(), tsos.globalMomentum().y(), tsos.globalMomentum().z()); - const Matrix W0 = p0.normalized(); - const Matrix zhat(0., 0., 1.); - - const Matrix U0 = zhat.cross(W0).normalized(); - const Matrix V0 = W0.cross(U0); - - const double Ux = U0[0]; - const double Uy = U0[1]; - - const double Vx = V0[0]; - const double Vy = V0[1]; - const double Vz = V0[2]; - - const double Wx = W0.x(); - const double Wy = W0.y(); - const double Wz = W0.z(); - - const double x0 = std::pow(Wx, 2) + std::pow(Wy, 2) + std::pow(Wz, 2); - const double x1 = std::pow(x0, -1.0/2.0); - const double x2 = Ux*Wx + Uy*Wy; - const double x3 = 1.0/x0; - const double x4 = 2*x3; - const double x5 = x2*x4; - const double x6 = 3*x3; - const double x7 = std::pow(Ux, 2) + std::pow(Uy, 2) - std::pow(x2, 2)*x6; - const double x8 = x3*(Jx*Wx + Jy*Wy + Jz*Wz); - const double x9 = Vx*Wx + Vy*Wy; - const double x10 = x4*x9; - const double x11 = std::pow(Vx, 2) + std::pow(Vy, 2) - x6*std::pow(x9, 2); - const double x12 = x3*(Kx*Wx + Ky*Wy + Kz*Wz); - const double d2dxdzdthetau2 = x1*(-x5*(Jx*Ux + Jy*Uy) - x7*x8); - const double d2dxdzdthetav2 = x1*(-x10*(Jx*Vx + Jy*Vy) - x11*x8); - const double d2dydzdthetau2 = x1*(-x12*x7 - x5*(Kx*Ux + Ky*Uy)); - const double d2dydzdthetav2 = x1*(-x10*(Kx*Vx + Ky*Vy) - x11*x12); - - - - const Surface& surface = tsos.surface(); - // - // - // Now get information on medium - // - const MediumProperties& mp = surface.mediumProperties(); - - // Momentum vector - LocalVector d = tsos.localMomentum(); - float p2 = d.mag2(); - d *= 1.f / sqrt(p2); - float xf = 1.f / std::abs(d.z()); // increase of path due to angle of incidence - // calculate general physics things - constexpr float amscon = 1.8496e-4; // (13.6MeV)**2 - const float m2 = updator.mass() * updator.mass(); // use mass hypothesis from constructor - float e2 = p2 + m2; - float beta2 = p2 / e2; - // calculate the multiple scattering angle - float radLen = mp.radLen() * xf; // effective rad. length - float sigt2 = 0.; // sigma(alpha)**2 - - // Calculated rms scattering angle squared. - float fact = 1.f + 0.038f * unsafe_logf<2>(radLen); - fact *= fact; - float a = fact / (beta2 * p2); - sigt2 = amscon * radLen * a; - - const double sigma2 = sigt2; - - AlgebraicVector5 res; - res[0] = 0.; - res[1] = 0.5*d2dxdzdthetau2*sigma2 + 0.5*d2dxdzdthetav2*sigma2; - res[2] = 0.5*d2dydzdthetau2*sigma2 + 0.5*d2dydzdthetav2*sigma2; - res[3] = 0.; - res[4] = 0.; - - return res; -} - -// Matrix ResidualGlobalCorrectionMakerBase::materialEffectsJacobian(const TrajectoryStateOnSurface& tsos, const MaterialEffectsUpdator& updator) { -// -// //jacobian of local parameters with respect to initial local parameters and material parameter xi -// //n.b this is the jacobian in LOCAL parameters (so E multiplies to the left of H s.t the total projection is E*Hprop*F) -// -// const double m2 = pow(updator.mass(), 2); // use mass hypothesis from constructor -// constexpr double emass = 0.511e-3; -// constexpr double poti = 16.e-9 * 10.75; // = 16 eV * Z**0.9, for Si Z=14 -// const double eplasma = 28.816e-9 * sqrt(2.33 * 0.498); // 28.816 eV * sqrt(rho*(Z/A)) for Si -// const double qop = tsos.localParameters().qbp(); -// const double dxdz = tsos.localParameters().dxdz(); -// const double dydz = tsos.localParameters().dydz(); -// const double xi = tsos.surface().mediumProperties().xi(); -// const double scale = 26.; -// -// //this is printed from sympy.printing.cxxcode together with sympy.cse for automatic substitution of common expressions -// const double x0 = (((qop) > 0) - ((qop) < 0)); -// const double x1 = std::pow(qop, 2); -// const double x2 = 1.0/(m2*x1 + 1); -// const double x3 = std::pow(x2, -1.0/2.0); -// const double x4 = std::sqrt(std::pow(dxdz, 2) + std::pow(dydz, 2) + 1); -// const double x5 = scale*x3*x4; -// const double x6 = x5*xi; -// const double x7 = x0/std::pow(-x6 + x0/qop, 2); -// const double x8 = scale*x3*x7*xi/x4; -// const double res_0 = x7*(m2*qop*x2*x6 + x0/x1); -// const double res_1 = dxdz*x8; -// const double res_2 = dydz*x8; -// const double res_3 = x5*x7; -// -// Matrix EdE = Matrix::Zero(); -// //jacobian of q/p wrt local state parameters -// EdE(0,0) = res_0; -// EdE(0,1) = res_1; -// EdE(0,2) = res_2; -// EdE(1,1) = 1.; -// EdE(2,2) = 1.; -// EdE(3,3) = 1.; -// EdE(4,4) = 1.; -// //derivative of q/p wrt xi -// EdE(0,5) = res_3; -// -// return EdE; -// } - - -// Matrix ResidualGlobalCorrectionMakerBase::materialEffectsJacobian(const TrajectoryStateOnSurface& tsos, const MaterialEffectsUpdator& updator) { -// -// //jacobian of local parameters with respect to initial local parameters and material parameter xi -// //n.b this is the jacobian in LOCAL parameters (so E multiplies to the left of H s.t the total projection is E*Hprop*F) -// -// const double m2 = pow(updator.mass(), 2); // use mass hypothesis from constructor -// constexpr double emass = 0.511e-3; -// constexpr double poti = 16.e-9 * 10.75; // = 16 eV * Z**0.9, for Si Z=14 -// const double eplasma = 28.816e-9 * sqrt(2.33 * 0.498); // 28.816 eV * sqrt(rho*(Z/A)) for Si -// const double qop = tsos.localParameters().qbp(); -// const double dxdz = tsos.localParameters().dxdz(); -// const double dydz = tsos.localParameters().dydz(); -// const double xi = tsos.surface().mediumProperties().xi(); -// -// //this is printed from sympy.printing.cxxcode together with sympy.cse for automatic substitution of common expressions -// const double x0 = (((qop) > 0) - ((qop) < 0)); -// const double x1 = std::pow(qop, 2); -// const double x2 = 1.0/x1; -// const double x3 = m2*x1 + 1; -// const double x4 = 1.0/x3; -// const double x5 = 1.0/poti; -// const double x6 = std::sqrt(std::pow(dxdz, 2) + std::pow(dydz, 2) + 1); -// const double x7 = x3*xi; -// const double x8 = -2*std::log(eplasma*x5) + std::log(x5*x6*x7) + std::log(2.0*emass*x2/(m2*std::pow(poti, 2))) + 1.2; -// const double x9 = x3*x8 - 1; -// const double x10 = std::pow(x4, -1.0/2.0); -// const double x11 = x10*x6; -// const double x12 = x11*x9; -// const double x13 = x12*xi; -// const double x14 = 1.0/qop; -// const double x15 = 2*m2*qop; -// const double x16 = x0/std::pow(x0*x14 - x13, 2); -// const double x17 = x10/x6; -// const double x18 = dxdz*x17; -// const double x19 = x9*xi; -// const double x20 = dydz*x17; -// const double res_0 = x16*(m2*qop*x13*x4 + x0*x2 + x11*xi*(x15*x8 + x3*(-2.0*x14 + x15*x4))); -// const double res_1 = x16*(x18*x19 + x18*x7); -// const double res_2 = x16*(x19*x20 + x20*x7); -// const double res_3 = x16*(x11*x3 + x12); -// -// -// Matrix EdE = Matrix::Zero(); -// //jacobian of q/p wrt local state parameters -// EdE(0,0) = res_0; -// EdE(0,1) = res_1; -// EdE(0,2) = res_2; -// EdE(1,1) = 1.; -// EdE(2,2) = 1.; -// EdE(3,3) = 1.; -// EdE(4,4) = 1.; -// //derivative of q/p wrt xi -// EdE(0,5) = res_3; -// -// return EdE; -// } - -Matrix ResidualGlobalCorrectionMakerBase::materialEffectsJacobian(const TrajectoryStateOnSurface& tsos, const MaterialEffectsUpdator& updator) { - - //jacobian of local parameters with respect to initial local parameters and material parameter xi - //n.b this is the jacobian in LOCAL parameters (so E multiplies to the left of H s.t the total projection is E*Hprop*F) - - const double m2 = pow(updator.mass(), 2); // use mass hypothesis from constructor - constexpr double emass = 0.511e-3; - constexpr double poti = 16.e-9 * 10.75; // = 16 eV * Z**0.9, for Si Z=14 - const double eplasma = 28.816e-9 * sqrt(2.33 * 0.498); // 28.816 eV * sqrt(rho*(Z/A)) for Si - const double qop = tsos.localParameters().qbp(); - const double dxdz = tsos.localParameters().dxdz(); - const double dydz = tsos.localParameters().dydz(); - const double xi = tsos.surface().mediumProperties().xi(); - - //this is printed from sympy.printing.cxxcode together with sympy.cse for automatic substitution of common expressions - const double x0 = (((qop) > 0) - ((qop) < 0)); - const double x1 = std::pow(qop, 2); - const double x2 = 1.0/x1; - const double x3 = x0*x2; - const double x4 = m2*x1 + 1; - const double x5 = 1.0/x4; - const double x6 = std::pow(poti, 2); - const double x7 = 1.0/x6; - const double x8 = std::fabs(qop); - const double x9 = std::pow(emass, 2); - const double x10 = x8*x9; - const double x11 = std::sqrt(x4); - const double x12 = 2*emass; - const double x13 = m2*x8 + x10 + x11*x12; - const double x14 = 1.0/x13; - const double x15 = x10*x14*x7; - const double x16 = 4*x2; - const double x17 = 2*std::log(eplasma/poti) - std::log(x15*x16) - 1; - const double x18 = x17*x4 + 2; - const double x19 = std::pow(x5, -1.0/2.0); - const double x20 = std::sqrt(std::pow(dxdz, 2) + std::pow(dydz, 2) + 1); - const double x21 = x19*x20*xi; - const double x22 = x18*x21; - const double x23 = m2*qop; - const double x24 = x0/std::pow(x22 + x0/qop, 2); - const double x25 = x18*x19*x24; - const double x26 = x25*xi/x20; - const double res_0 = x24*(-x21*(-1.0/4.0*x1*x13*x4*x6*(x10*x16*x7*(-m2*x0 - x0*x9 - x12*x23/x11)/std::pow(x13, 2) + 4*x14*x3*x7*x9 - 8*x15/std::pow(qop, 3))/(x8*x9) + 2*x17*x23) - x22*x23*x5 + x3); - const double res_1 = -dxdz*x26; - const double res_2 = -dydz*x26; - const double res_3 = -x20*x25; - - Matrix EdE = Matrix::Zero(); - //jacobian of q/p wrt local state parameters - EdE(0,0) = res_0; - EdE(0,1) = res_1; - EdE(0,2) = res_2; - EdE(1,1) = 1.; - EdE(2,2) = 1.; - EdE(3,3) = 1.; - EdE(4,4) = 1.; - //derivative of q/p wrt xi - EdE(0,5) = res_3; - - return EdE; -} - -Matrix ResidualGlobalCorrectionMakerBase::materialEffectsJacobianVar(const TrajectoryStateOnSurface& tsos, const MaterialEffectsUpdator& updator) { - - //jacobian of local parameters with respect to initial local parameters and material parameter xi - //n.b this is the jacobian in LOCAL parameters (so E multiplies to the left of H s.t the total projection is E*Hprop*F) - - const double m2 = pow(updator.mass(), 2); // use mass hypothesis from constructor - constexpr double emass = 0.511e-3; - constexpr double poti = 16.e-9 * 10.75; // = 16 eV * Z**0.9, for Si Z=14 - const double eplasma = 28.816e-9 * sqrt(2.33 * 0.498); // 28.816 eV * sqrt(rho*(Z/A)) for Si - const double qop = tsos.localParameters().qbp(); - const double dxdz = tsos.localParameters().dxdz(); - const double dydz = tsos.localParameters().dydz(); - const double xi = tsos.surface().mediumProperties().xi(); - - //this is printed from sympy.printing.cxxcode together with sympy.cse for automatic substitution of common expressions - const double x0 = std::pow(qop, 2); - const double x1 = 1.0/x0; - const double x2 = (((qop) > 0) - ((qop) < 0)); - const double x3 = m2*x0 + 1; - const double x4 = std::fabs(qop); - const double x5 = std::pow(emass, 2)*x4; - const double x6 = (x3*(2*std::log(eplasma/poti) - std::log(4*x1*x5/(std::pow(poti, 2)*(2*emass*std::sqrt(x3) + m2*x4 + x5))) - 1) + 2)*std::sqrt(std::pow(dxdz, 2) + std::pow(dydz, 2) + 1)/std::sqrt(1.0/x3); - const double x7 = std::pow(x6*xi + x2/qop, -2); - const double res_0 = x1*x7; - const double res_1 = 0; - const double res_2 = 0; - const double res_3 = -x2*x6*x7; - - Matrix EdE = Matrix::Zero(); - //jacobian of q/p wrt local state parameters - EdE(0,0) = res_0; - EdE(0,1) = res_1; - EdE(0,2) = res_2; - EdE(1,1) = 1.; - EdE(2,2) = 1.; - EdE(3,3) = 1.; - EdE(4,4) = 1.; - //derivative of q/p wrt xi - EdE(0,5) = res_3; - - return EdE; -} - - -std::array, 5> ResidualGlobalCorrectionMakerBase::processNoiseJacobians(const TrajectoryStateOnSurface& tsos, const MaterialEffectsUpdator& updator) const { - - //this is the variation of the process noise matrix with respect to the relevant local parameters (qop, dxdz, dydz) and the material parameters xi and radlen - - const double m2 = pow(updator.mass(), 2); // use mass hypothesis from constructor - constexpr double emass = 0.511e-3; - constexpr double logfact = 0.038; - constexpr double amscon = 1.8496e-4; // (13.6MeV)**2 - const double qop = tsos.localParameters().qbp(); - const double dxdz = tsos.localParameters().dxdz(); - const double dydz = tsos.localParameters().dydz(); - const double signpz = tsos.localParameters().pzSign(); - const double xi = tsos.surface().mediumProperties().xi(); - const double radLen = tsos.surface().mediumProperties().radLen(); - - const double x0 = std::pow(qop, 5); - const double x1 = std::pow(qop, 2); - const double x2 = 1.0/x1; - const double x3 = m2 + x2; - const double x4 = 1.0/x3; - const double x5 = x2*x4; - const double x6 = 1 - 1.0/2.0*x5; - const double x7 = std::pow(dxdz, 2); - const double x8 = std::pow(dydz, 2); - const double x9 = x7 + x8; - const double x10 = x9 + 1; - const double x11 = std::sqrt(x10); - const double x12 = x11*xi; - const double x13 = x12*x6; - const double x14 = std::pow(x3, 2); - const double x15 = std::pow(emass, 2); - const double x16 = 1.0/m2; - const double x17 = std::sqrt(x3); - const double x18 = emass*x16; - const double x19 = 2*x18; - const double x20 = x15*x16 + x17*x19 + 1; - const double x21 = 1.0/x20; - const double x22 = x19*x21; - const double x23 = x14*x22; - const double x24 = x18*x21; - const double x25 = 8*x24; - const double x26 = std::fabs(qop); - const double x27 = (((qop) > 0) - ((qop) < 0)); - const double x28 = 1.0/x27; - const double x29 = x28/x26; - const double x30 = x13*x29; - const double x31 = std::pow(qop, 4); - const double x32 = x3*x31; - const double x33 = 14*x24; - const double x34 = std::pow(x3, 3.0/2.0); - const double x35 = std::pow(qop, 3); - const double x36 = 1.0/x14; - const double x37 = std::pow(qop, 7)*x23*x29; - const double x38 = x37*x6; - const double x39 = x38*xi/x11; - const double x40 = std::pow(signpz, 2); - const double x41 = x40/x10; - const double x42 = x41*x7; - const double x43 = x41*x8; - const double x44 = x42 + x43; - const double x45 = 1.0/x44; - const double x46 = 1.0*x45; - const double x47 = std::pow(x10, 2); - const double x48 = 1.0/x47; - const double x49 = std::pow(signpz, 4); - const double x50 = x48*x49; - const double x51 = x46*x50; - const double x52 = x42*x46 + x51*x8; - const double x53 = amscon*radLen; - const double x54 = 1.0/x49; - const double x55 = std::pow(x10, 5.0/2.0)*x54; - const double x56 = x53*x55; - const double x57 = x52*x56; - const double x58 = radLen*x11; - const double x59 = x29*x58; - const double x60 = logfact*std::log(qop*x59) + 1; - const double x61 = std::pow(x60, 2); - const double x62 = x3*x35; - const double x63 = 1.0*x62; - const double x64 = x61*x63; - const double x65 = x29*x61; - const double x66 = x57*x65; - const double x67 = 2.0*x1; - const double x68 = amscon*x52; - const double x69 = logfact*x60; - const double x70 = x54*x69; - const double x71 = 1.0/qop; - const double x72 = 2.0*x3; - const double x73 = x31*x72*(-x58*x71 + x59); - const double x74 = x47*x70*x73; - const double x75 = std::pow(x10, 3.0/2.0); - const double x76 = x53*x75; - const double x77 = dxdz*x76; - const double x78 = x52*x77; - const double x79 = x0*x29*x72; - const double x80 = x70*x79; - const double x81 = x3*x65; - const double x82 = x0*x81; - const double x83 = 5.0*x54*x82; - const double x84 = 2.0*x45; - const double x85 = dxdz*x84; - const double x86 = std::pow(dxdz, 3); - const double x87 = x40*x48; - const double x88 = x84*x87; - const double x89 = dxdz*x8; - const double x90 = 4.0*x45*x49/std::pow(x10, 3); - const double x91 = 2*x41; - const double x92 = 2*x87; - const double x93 = 1.0/std::pow(x44, 2); - const double x94 = x93*(-dxdz*x91 + x86*x92 + x89*x92); - const double x95 = x50*x8; - const double x96 = 1.0*x82; - const double x97 = x56*x96; - const double x98 = dydz*x76; - const double x99 = x80*x98; - const double x100 = x83*x98; - const double x101 = dydz*x84; - const double x102 = std::pow(dydz, 3); - const double x103 = dydz*x7; - const double x104 = x93*(-dydz*x91 + x102*x92 + x103*x92); - const double x105 = x55*x68; - const double x106 = x69*x79; - const double x107 = x26*x27; - const double x108 = x107*x61; - const double x109 = 1.0/x40; - const double x110 = dxdz*dydz*x109; - const double x111 = x110*x76; - const double x112 = x1*x3; - const double x113 = x112*x61; - const double x114 = amscon*x110; - const double x115 = x108*x63; - const double x116 = x109*x115; - const double x117 = amscon*x109*x58; - const double x118 = x103*x117; - const double x119 = x107*x60; - const double x120 = 2.0*x62; - const double x121 = logfact*x119*x120; - const double x122 = 3.0*x108*x62; - const double x123 = x117*x89; - const double x124 = x114*x75; - const double x125 = x43*x46 + x51*x7; - const double x126 = x125*x56; - const double x127 = amscon*x125; - const double x128 = x125*x77; - const double x129 = x50*x7; - const double x130 = x127*x55; - const double x131 = -x5; - const double x132 = x131 + 1; - const double x133 = qop*x3; - const double x134 = x131 + 2; - const double x135 = x132*x29; - const double x136 = x134*x14; - const double x137 = x112*x29; - const double x138 = x136*x29; - const double x139 = x134*x29; - const double x140 = x1*x139; - const double x141 = qop*x22; - const double x142 = 4*x24; - const double x143 = 4.0*x61; - const double x144 = logfact*(x29 - x71); - const double x145 = x144*x60; - const double x146 = -8.0*qop*x145; - const double x147 = 20.0*x62; - const double x148 = 4.0*x112; - const double x149 = x144*(-x119*x2 + x144 + x60*x71); - const double x150 = x56*(-14.0*qop*x65 - 8.0*x113 + x143 + x145*x147 - x145*x148*x26*x28 + x146 + x147*x65 + x149*x79)/x9; - const double x151 = 6.0*x108; - const double delosdqop = std::pow(qop, 6)*x14*x30*x33 - x0*x13*x23 + x12*x37*(x4/x35 - x36/x0) - x25*x30*x32 + 4*x15*x30*x31*x34/(std::pow(m2, 2)*std::pow(x20, 2)); - const double delosddxdz = dxdz*x39; - const double delosddydz = dydz*x39; - const double delosdxi = x11*x38; - const double delosdradLen = 0; - const double dmsxxdqop = 5.0*x32*x66 - x57*x64 - x66*x67 + x68*x74; - const double dmsxxddxdz = x78*x80 + x78*x83 + x97*(x41*x85 + x42*x94 - x86*x88 - x89*x90 + x94*x95); - const double dmsxxddydz = x100*x52 + x52*x99 + x97*(x101*x50 - x102*x90 - x103*x88 + x104*x42 + x104*x95); - const double dmsxxdxi = 0; - const double dmsxxdradLen = x105*x106 + x105*x96; - const double dmsxydqop = x10*x114*x69*x73 + 3.0*x107*x111*x113 - 2.0*x108*x111 + x111*x64; - const double dmsxyddxdz = x116*x98 + x118*x121 + x118*x122; - const double dmsxyddydz = x116*x77 + x121*x123 + x122*x123; - const double dmsxydxi = 0; - const double dmsxydradLen = x115*x124 + x121*x124; - const double dmsyydqop = 5.0*x126*x31*x81 - x126*x64 - x126*x65*x67 + x127*x74; - const double dmsyyddxdz = x128*x80 + x128*x83 + x97*(x129*x94 + x43*x94 + x50*x85 - x8*x85*x87 - x86*x90); - const double dmsyyddydz = x100*x125 + x125*x99 + x97*(x101*x41 - x102*x88 - x103*x90 + x104*x129 + x104*x43); - const double dmsyydxi = 0; - const double dmsyydradLen = x106*x130 + x130*x96; - const double d2elosdqop2 = x12*x141*(x1*x138*x24*(x142*x5 + x2/x34 - 3/x17) + 14*x112*x135 - 2*x132*x133 + 4*x133*x134 - 28*x134*x137 - x134*x141*x34 + x135*x142*x17 - 8*x135 - 6*x136*x35 - x137*(-7*x5 + 3 + 4*x36/x31) + 21*x138*x31 - x139*x17*x25 + x140*x33*x34 + 2*x140*(3*m2 + 5*x2)); - const double d2msxxdqop2 = x150*(x43 + x7); - const double d2msxydqop2 = x111*(x107*x120*x149 + 6.0*x113 + x119*x144*x148 + x133*x151 - x143 + 12.0*x145*x62 + x146 - x151*x71); - const double d2msyydqop2 = x150*(x42 + x8); - - std::array, 5> res; - - Matrix &dQdqop = res[0]; - dQdqop = Matrix::Zero(); - dQdqop(0,0) = delosdqop; - dQdqop(1,1) = dmsxxdqop; - dQdqop(1,2) = dmsxydqop; - dQdqop(2,1) = dmsxydqop; - dQdqop(2,2) = dmsyydqop; - -// std::cout << "dQdqop" << std::endl; -// std::cout << dQdqop << std::endl; - -// Matrix &d2Qdqop2 = res[1]; -// d2Qdqop2 = Matrix::Zero(); -// d2Qdqop2(0,0) = d2elosdqop2; -// d2Qdqop2(1,1) = d2msxxdqop2; -// d2Qdqop2(1,2) = d2msxydqop2; -// d2Qdqop2(2,1) = d2msxydqop2; -// d2Qdqop2(2,2) = d2msyydqop2; - - Matrix &dQddxdz = res[1]; - dQddxdz = Matrix::Zero(); -// dQddxdz(0,0) = delosddxdz; -// dQddxdz(1,1) = dmsxxddxdz; -// dQddxdz(1,2) = dmsxyddxdz; -// dQddxdz(2,1) = dmsxyddxdz; -// dQddxdz(2,2) = dmsyyddxdz; - - Matrix &dQddydz = res[2]; - dQddydz = Matrix::Zero(); -// dQddydz(0,0) = delosddydz; -// dQddydz(1,1) = dmsxxddydz; -// dQddydz(1,2) = dmsxyddydz; -// dQddydz(2,1) = dmsxyddydz; -// dQddydz(2,2) = dmsyyddydz; - - Matrix &dQdxi = res[3]; - dQdxi = Matrix::Zero(); - dQdxi(0,0) = delosdxi; - dQdxi(1,1) = dmsxxdxi; - dQdxi(1,2) = dmsxydxi; - dQdxi(2,1) = dmsxydxi; - dQdxi(2,2) = dmsyydxi; - - Matrix &dQdradLen = res[4]; - dQdradLen = Matrix::Zero(); - dQdradLen(0,0) = delosdradLen; - dQdradLen(1,1) = dmsxxdradLen; - dQdradLen(1,2) = dmsxydradLen; - dQdradLen(2,1) = dmsxydradLen; - dQdradLen(2,2) = dmsyydradLen; - - return res; -} - -Matrix ResidualGlobalCorrectionMakerBase::localPositionConvolution(const TrajectoryStateOnSurface& tsos, const Matrix &curvcov) const { - - // curvilinear parameters - const CurvilinearTrajectoryParameters curv(tsos.globalPosition(), tsos.globalMomentum(), tsos.charge()); - const double qop = curv.Qbp(); - const double lam = curv.lambda(); - const double phi = curv.phi(); -// const double xt = curv.xT(); -// const double yt = curv.yT(); - const double xt = 0.; - const double yt = 0.; - - const Matrix p0(tsos.globalMomentum().x(), tsos.globalMomentum().y(), tsos.globalMomentum().z()); - const Matrix W0 = p0.normalized(); - const Matrix zhat(0., 0., 1.); - - const Matrix U0 = zhat.cross(W0).normalized(); - const Matrix V0 = W0.cross(U0); - -// std::cout << "U0" << std::endl; -// std::cout << U0 << std::endl; -// std::cout << "V0" << std::endl; -// std::cout << V0 << std::endl; - -// const Matrix x0alt = xt*U0 + yt*V0; -// std::cout << "global pos" << std::endl; -// std::cout << tsos.globalPosition() << std::endl; -// std::cout << "x0alt" << std::endl; -// std::cout << x0alt << std::endl; -// std::cout << "xt" << std::endl; -// std::cout << xt << std::endl; -// std::cout << "yt" << std::endl; -// std::cout << yt << std::endl; - - const LocalVector lx(1.,0.,0.); - const LocalVector ly(0.,1.,0.); - const LocalVector lz(0.,0.,1.); - const GlobalVector I = tsos.surface().toGlobal(lz); - const GlobalVector J = tsos.surface().toGlobal(lx); - const GlobalVector K = tsos.surface().toGlobal(ly); - - const LocalPoint l0(0., 0.); - const GlobalPoint r = tsos.surface().toGlobal(l0); - - const double Ux = U0[0]; - const double Uy = U0[1]; -// const double Uz = U0[2]; - - const double Vx = V0[0]; - const double Vy = V0[1]; - const double Vz = V0[2]; - - const double Ix = I.x(); - const double Iy = I.y(); - const double Iz = I.z(); - - const double Jx = J.x(); - const double Jy = J.y(); - const double Jz = J.z(); - - const double Kx = K.x(); - const double Ky = K.y(); - const double Kz = K.z(); - - const double rx = r.x(); - const double ry = r.y(); - const double rz = r.z(); - - const double pos0x = tsos.globalPosition().x(); - const double pos0y = tsos.globalPosition().y(); - const double pos0z = tsos.globalPosition().z(); - - //sympy stuff goes here - const double x0 = std::sin(lam); - const double x1 = Iz*x0; - const double x2 = std::cos(lam); - const double x3 = std::cos(phi); - const double x4 = Ix*x3; - const double x5 = x2*x4; - const double x6 = std::sin(phi); - const double x7 = Iy*x6; - const double x8 = x2*x7; - const double x9 = x5 + x8; - const double x10 = x1 + x9; - const double x11 = 1.0/x10; - const double x12 = Ix*rx; - const double x13 = Iy*ry; - const double x14 = Iz*rz; - const double x15 = Ix*pos0x; - const double x16 = Iy*pos0y; - const double x17 = Iz*pos0z; - const double x18 = Ux*xt; - const double x19 = Ix*x18; - const double x20 = Vx*yt; - const double x21 = Ix*x20; - const double x22 = Uy*xt; - const double x23 = Iy*x22; - const double x24 = Vy*yt; - const double x25 = Iy*x24; - const double x26 = Vz*yt; - const double x27 = Iz*x26; - const double x28 = x12 + x13 + x14 - x15 - x16 - x17 - x19 - x21 - x23 - x25 - x27; - const double x29 = pos0z + x26; - const double x30 = Iz*x2; - const double x31 = x0*x4; - const double x32 = x0*x7; - const double x33 = x30 - x31 - x32; - const double x34 = x2*x28; - const double x35 = x29*x33 + x34; - const double x36 = Jz*x11; - const double x37 = pos0x + x18 + x20; - const double x38 = x0*x28; - const double x39 = -x3*x38 + x33*x37; - const double x40 = Jx*x11; - const double x41 = pos0y + x22 + x24; - const double x42 = x33*x41 - x38*x6; - const double x43 = Jy*x11; - const double x44 = std::pow(x10, -2); - const double x45 = -x30 + x31 + x32; - const double x46 = x44*x45; - const double x47 = x10*x29 + x38; - const double x48 = Jz*x47; - const double x49 = x3*x34; - const double x50 = x10*x37 + x49; - const double x51 = Jx*x50; - const double x52 = x34*x6; - const double x53 = x10*x41 + x52; - const double x54 = Jy*x46; - const double x55 = -x5 - x8; - const double x56 = -x1 + x55; - const double x57 = -x12 - x13 - x14 + x15 + x16 + x17 + x19 + x21 + x23 + x25 + x27; - const double x58 = x2*x57; - const double x59 = x3*x58; - const double x60 = x37*x56 + x59; - const double x61 = x41*x56 + x58*x6; - const double x62 = x29*x56 - x38; - const double x63 = Jz*x46; - const double x64 = 2*x35; - const double x65 = Jx*x46; - const double x66 = 2*x39; - const double x67 = 2*x42; - const double x68 = std::pow(x10, -3); - const double x69 = x45*x68; - const double x70 = x69*(-2*x30 + 2*x31 + 2*x32); - const double x71 = Jy*x53; - const double x72 = x0*x6; - const double x73 = Ix*x72; - const double x74 = x0*x3; - const double x75 = Iy*x74; - const double x76 = x73 - x75; - const double x77 = x29*x36; - const double x78 = x2*x3; - const double x79 = Iy*x78; - const double x80 = x2*x6; - const double x81 = Ix*x80; - const double x82 = x79 - x81; - const double x83 = x29*x82; - const double x84 = x41*x76 + x57*x74; - const double x85 = x37*x76 - x57*x72; - const double x86 = x44*(-x73 + x75); - const double x87 = -x79 + x81; - const double x88 = x44*x87; - const double x89 = Jz*x88; - const double x90 = x41*x82 + x49; - const double x91 = -x52; - const double x92 = x37*x82 + x91; - const double x93 = Jx*x88; - const double x94 = Jy*x88; - const double x95 = -2*x79 + 2*x81; - const double x96 = x69*x95; - const double x97 = Ix*Ux; - const double x98 = Iy*Uy; - const double x99 = -x97 - x98; - const double x100 = x36*x99; - const double x101 = x0*x99; - const double x102 = x97 + x98; - const double x103 = Ux*x33 + x102*x74; - const double x104 = Uy*x33 + x102*x72; - const double x105 = x78*x99; - const double x106 = Ux*x10 + x105; - const double x107 = Uy*x10 + x80*x99; - const double x108 = Ix*Vx; - const double x109 = Iy*Vy; - const double x110 = Iz*Vz; - const double x111 = x108 + x109 + x110; - const double x112 = Vx*x33 + x111*x74; - const double x113 = Vy*x33 + x111*x72; - const double x114 = -x108 - x109 - x110; - const double x115 = x114*x2; - const double x116 = Vz*x33 + x115; - const double x117 = Vz*x10 + x0*x114; - const double x118 = x117*x46; - const double x119 = x115*x3; - const double x120 = Vx*x10 + x119; - const double x121 = Vy*x10 + x115*x6; - const double x122 = 2*x83; - const double x123 = x37*x55 + x59; - const double x124 = x41*x55 + x91; - const double x125 = x44*x9; - const double x126 = 2*x90; - const double x127 = 2*x92; - const double x128 = x68*x87*x95; - const double x129 = Ux*x82 + x102*x80; - const double x130 = Uy*x82 + x105; - const double x131 = Vz*x82; - const double x132 = Vx*x82 + x111*x80; - const double x133 = Vy*x82 + x119; - const double x134 = Kz*x11; - const double x135 = Kx*x11; - const double x136 = Ky*x11; - const double x137 = Kz*x47; - const double x138 = Kx*x46; - const double x139 = Ky*x46; - const double x140 = Kz*x46; - const double x141 = Kx*x50; - const double x142 = Ky*x53; - const double x143 = x134*x29; - const double x144 = Kz*x88; - const double x145 = Kx*x88; - const double x146 = Ky*x88; - const double x147 = x134*x99; - const double shat = x11*x28; - const double dvdqop = 0; - const double d2vdqopdqop = 0; - const double d2vdqopdlam = 0; - const double d2vdqopdphi = 0; - const double d2vdqopdxt = 0; - const double d2vdqopdyt = 0; - const double dvdlam = x35*x36 + x39*x40 + x42*x43 + x46*x48 + x46*x51 + x53*x54; - const double d2vdlamdlam = x36*x47 + x36*x62 + x40*x50 + x40*x60 + x43*x53 + x43*x61 + x48*x70 + x51*x70 + x54*x67 + x63*x64 + x65*x66 + x70*x71; - const double d2vdlamdphi = x35*x89 + x39*x93 + x40*x85 + x42*x94 + x43*x84 + x48*x86 + x48*x96 + x51*x86 + x51*x96 + x54*x90 + x63*x83 + x65*x92 + x71*x86 + x71*x96 + x76*x77; - const double d2vdlamdxt = x100*x2 + x101*x63 + x103*x40 + x104*x43 + x106*x65 + x107*x54; - const double d2vdlamdyt = Jz*x118 + x112*x40 + x113*x43 + x116*x36 + x120*x65 + x121*x54; - const double dvdphi = x40*x92 + x43*x90 + x48*x88 + x51*x88 + x53*x94 + x77*x82; - const double d2vdphidphi = x122*x89 + x123*x40 + x124*x43 + x125*x48 + x125*x51 + x125*x71 + x126*x94 + x127*x93 + x128*x48 + x128*x51 + x128*x71 + x55*x77; - const double d2vdphidxt = x101*x89 + x106*x93 + x107*x94 + x129*x40 + x130*x43; - const double d2vdphidyt = x117*x89 + x120*x93 + x121*x94 + x131*x36 + x132*x40 + x133*x43; - const double dvdxt = x0*x100 + x106*x40 + x107*x43; - const double d2vdxtdxt = 0; - const double d2vdxtdyt = 0; - const double dvdyt = x117*x36 + x120*x40 + x121*x43; - const double d2vdytdyt = 0; - const double dwdqop = 0; - const double d2wdqopdqop = 0; - const double d2wdqopdlam = 0; - const double d2wdqopdphi = 0; - const double d2wdqopdxt = 0; - const double d2wdqopdyt = 0; - const double dwdlam = x134*x35 + x135*x39 + x136*x42 + x137*x46 + x138*x50 + x139*x53; - const double d2wdlamdlam = x134*x47 + x134*x62 + x135*x50 + x135*x60 + x136*x53 + x136*x61 + x137*x70 + x138*x66 + x139*x67 + x140*x64 + x141*x70 + x142*x70; - const double d2wdlamdphi = x135*x85 + x136*x84 + x137*x86 + x137*x96 + x138*x92 + x139*x90 + x140*x83 + x141*x86 + x141*x96 + x142*x86 + x142*x96 + x143*x76 + x144*x35 + x145*x39 + x146*x42; - const double d2wdlamdxt = x101*x140 + x103*x135 + x104*x136 + x106*x138 + x107*x139 + x147*x2; - const double d2wdlamdyt = Kz*x118 + x112*x135 + x113*x136 + x116*x134 + x120*x138 + x121*x139; - const double dwdphi = x135*x92 + x136*x90 + x137*x88 + x143*x82 + x145*x50 + x146*x53; - const double d2wdphidphi = x122*x144 + x123*x135 + x124*x136 + x125*x137 + x125*x141 + x125*x142 + x126*x146 + x127*x145 + x128*x137 + x128*x141 + x128*x142 + x143*x55; - const double d2wdphidxt = x101*x144 + x106*x145 + x107*x146 + x129*x135 + x130*x136; - const double d2wdphidyt = x117*x144 + x120*x145 + x121*x146 + x131*x134 + x132*x135 + x133*x136; - const double dwdxt = x0*x147 + x106*x135 + x107*x136; - const double d2wdxtdxt = 0; - const double d2wdxtdyt = 0; - const double dwdyt = x117*x134 + x120*x135 + x121*x136; - const double d2wdytdyt = 0; - Matrix d2vdx2; - d2vdx2(0, 0) = d2vdqopdqop; - d2vdx2(0, 1) = d2vdqopdlam; - d2vdx2(0, 2) = d2vdqopdphi; - d2vdx2(0, 3) = d2vdqopdxt; - d2vdx2(0, 4) = d2vdqopdyt; - d2vdx2(1, 0) = d2vdqopdlam; - d2vdx2(1, 1) = d2vdlamdlam; - d2vdx2(1, 2) = d2vdlamdphi; - d2vdx2(1, 3) = d2vdlamdxt; - d2vdx2(1, 4) = d2vdlamdyt; - d2vdx2(2, 0) = d2vdqopdphi; - d2vdx2(2, 1) = d2vdlamdphi; - d2vdx2(2, 2) = d2vdphidphi; - d2vdx2(2, 3) = d2vdphidxt; - d2vdx2(2, 4) = d2vdphidyt; - d2vdx2(3, 0) = d2vdqopdxt; - d2vdx2(3, 1) = d2vdlamdxt; - d2vdx2(3, 2) = d2vdphidxt; - d2vdx2(3, 3) = d2vdxtdxt; - d2vdx2(3, 4) = d2vdxtdyt; - d2vdx2(4, 0) = d2vdqopdyt; - d2vdx2(4, 1) = d2vdlamdyt; - d2vdx2(4, 2) = d2vdphidyt; - d2vdx2(4, 3) = d2vdxtdyt; - d2vdx2(4, 4) = d2vdytdyt; - Matrix d2wdx2; - d2wdx2(0, 0) = d2wdqopdqop; - d2wdx2(0, 1) = d2wdqopdlam; - d2wdx2(0, 2) = d2wdqopdphi; - d2wdx2(0, 3) = d2wdqopdxt; - d2wdx2(0, 4) = d2wdqopdyt; - d2wdx2(1, 0) = d2wdqopdlam; - d2wdx2(1, 1) = d2wdlamdlam; - d2wdx2(1, 2) = d2wdlamdphi; - d2wdx2(1, 3) = d2wdlamdxt; - d2wdx2(1, 4) = d2wdlamdyt; - d2wdx2(2, 0) = d2wdqopdphi; - d2wdx2(2, 1) = d2wdlamdphi; - d2wdx2(2, 2) = d2wdphidphi; - d2wdx2(2, 3) = d2wdphidxt; - d2wdx2(2, 4) = d2wdphidyt; - d2wdx2(3, 0) = d2wdqopdxt; - d2wdx2(3, 1) = d2wdlamdxt; - d2wdx2(3, 2) = d2wdphidxt; - d2wdx2(3, 3) = d2wdxtdxt; - d2wdx2(3, 4) = d2wdxtdyt; - d2wdx2(4, 0) = d2wdqopdyt; - d2wdx2(4, 1) = d2wdlamdyt; - d2wdx2(4, 2) = d2wdphidyt; - d2wdx2(4, 3) = d2wdxtdyt; - d2wdx2(4, 4) = d2wdytdyt; - Matrix dvdx; - dvdx[0] = dvdqop; - dvdx[1] = dvdlam; - dvdx[2] = dvdphi; - dvdx[3] = dvdxt; - dvdx[4] = dvdyt; - Matrix dwdx; - dwdx[0] = dwdqop; - dwdx[1] = dwdlam; - dwdx[2] = dwdphi; - dwdx[3] = dwdxt; - dwdx[4] = dwdyt; - - - -// std::cout << "dvdx" << std::endl; -// std::cout << dvdx << std::endl; -// -// std::cout << "dwdx" << std::endl; -// std::cout << dwdx << std::endl; -// -// std::cout << "d2vdx2" << std::endl; -// std::cout << d2vdx2 << std::endl; -// -// std::cout << "d2wdx2" << std::endl; -// std::cout << d2wdx2 << std::endl; - -// std::cout << "shat" << std::endl; -// std::cout << shat << std::endl; - - // covariance matrix in curvilinear parameters -// const AlgebraicMatrix55 curvcovsmat = tsos.curvilinearError().matrix(); - - // map to eigen data structure -// const Map> curvcov(curvcovsmat.Array()); - -// std::cout << "curvcov" << std::endl; -// std::cout << curvcov << std::endl; - - // compute eigendecomposition - SelfAdjointEigenSolver> es; - es.compute(curvcov); - - // cov = VDV^(-1) -// auto const& sqrtD = es.eigenvalues().cwiseSqrt(); - auto const& D = es.eigenvalues(); - auto const& V = es.eigenvectors(); + return res; + +} + +Matrix ResidualGlobalCorrectionMakerBase::curv2cartJacobianAltD(const Matrix &state) const { - // compute second order correction to local positions -// Matrix res; -// res[0] = 0.5*sqrtD.transpose()*V.transpose()*d2vdx2*V*sqrtD; -// res[1] = 0.5*sqrtD.transpose()*V.transpose()*d2wdx2*V*sqrtD; + const double charge = state[6]; - Matrix res = Matrix::Zero(); - for (unsigned int i=0; i<5; ++i) { - res[0] += 0.5*D[i]*V.col(i).transpose()*d2vdx2*V.col(i); - res[1] += 0.5*D[i]*V.col(i).transpose()*d2wdx2*V.col(i); - } + const double qop0 = charge/state.segment<3>(3).norm(); + const double lam0 = std::atan(state[5]/std::sqrt(state[3]*state[3] + state[4]*state[4])); + const double phi0 = std::atan2(state[4], state[3]); -// res[0] = d2vdx2*curvc + const Matrix W0 = state.segment<3>(3).normalized(); + const double W0x = W0[0]; + const double W0y = W0[1]; + const double W0z = W0[2]; + + const double x0 = std::sqrt(std::pow(W0x, 2) + std::pow(W0y, 2)); + const double x1 = 1.0/x0; + const double x2 = W0y*x1; + const double x3 = W0x*x1; + const double x4 = 1.0/qop0; + const double x5 = std::cos(phi0); + const double x6 = 1.0/std::fabs(qop0); + const double x7 = x6*std::cos(lam0); + const double x8 = x5*x7; + const double x9 = x6*std::sin(lam0); + const double x10 = std::sin(phi0); + const double x11 = x10*x7; + const double dxdqop0 = 0; + const double dxdlam0 = 0; + const double dxdphi0 = 0; + const double dxdxt0 = -x2; + const double dxdyt0 = -W0z*x3; + const double dydqop0 = 0; + const double dydlam0 = 0; + const double dydphi0 = 0; + const double dydxt0 = x3; + const double dydyt0 = -W0z*x2; + const double dzdqop0 = 0; + const double dzdlam0 = 0; + const double dzdphi0 = 0; + const double dzdxt0 = 0; + const double dzdyt0 = x0; + const double dpxdqop0 = -x4*x8; + const double dpxdlam0 = -x5*x9; + const double dpxdphi0 = -x11; + const double dpxdxt0 = 0; + const double dpxdyt0 = 0; + const double dpydqop0 = -x11*x4; + const double dpydlam0 = -x10*x9; + const double dpydphi0 = x8; + const double dpydxt0 = 0; + const double dpydyt0 = 0; + const double dpzdqop0 = -x4*x9; + const double dpzdlam0 = x7; + const double dpzdphi0 = 0; + const double dpzdxt0 = 0; + const double dpzdyt0 = 0; + Matrix res; + res(0,0) = dxdqop0; + res(0,1) = dxdlam0; + res(0,2) = dxdphi0; + res(0,3) = dxdxt0; + res(0,4) = dxdyt0; + res(1,0) = dydqop0; + res(1,1) = dydlam0; + res(1,2) = dydphi0; + res(1,3) = dydxt0; + res(1,4) = dydyt0; + res(2,0) = dzdqop0; + res(2,1) = dzdlam0; + res(2,2) = dzdphi0; + res(2,3) = dzdxt0; + res(2,4) = dzdyt0; + res(3,0) = dpxdqop0; + res(3,1) = dpxdlam0; + res(3,2) = dpxdphi0; + res(3,3) = dpxdxt0; + res(3,4) = dpxdyt0; + res(4,0) = dpydqop0; + res(4,1) = dpydlam0; + res(4,2) = dpydphi0; + res(4,3) = dpydxt0; + res(4,4) = dpydyt0; + res(5,0) = dpzdqop0; + res(5,1) = dpzdlam0; + res(5,2) = dpzdphi0; + res(5,3) = dpzdxt0; + res(5,4) = dpzdyt0; return res; + } + Matrix ResidualGlobalCorrectionMakerBase::localPositionConvolutionD(const Matrix& state, const Matrix &curvcov, const GloballyPositioned &surface) const { const double q = state[6]; @@ -4751,200 +3203,26 @@ Matrix ResidualGlobalCorrectionMakerBase::localPositionConvolution dwdx[4] = dwdyt; + Matrix res; + res[0] = 0.5*(d2vdx2*curvcov).trace(); + res[1] = 0.5*(d2wdx2*curvcov).trace(); -// std::cout << "dvdx" << std::endl; -// std::cout << dvdx << std::endl; -// -// std::cout << "dwdx" << std::endl; -// std::cout << dwdx << std::endl; -// -// std::cout << "d2vdx2" << std::endl; -// std::cout << d2vdx2 << std::endl; -// -// std::cout << "d2wdx2" << std::endl; -// std::cout << d2wdx2 << std::endl; - -// std::cout << "shat" << std::endl; -// std::cout << shat << std::endl; - - // covariance matrix in curvilinear parameters -// const AlgebraicMatrix55 curvcovsmat = tsos.curvilinearError().matrix(); - - // map to eigen data structure -// const Map> curvcov(curvcovsmat.Array()); - -// std::cout << "curvcov" << std::endl; -// std::cout << curvcov << std::endl; - - // compute eigendecomposition - SelfAdjointEigenSolver> es; - es.compute(curvcov); - - // cov = VDV^(-1) -// auto const& sqrtD = es.eigenvalues().cwiseSqrt(); - auto const& D = es.eigenvalues(); - auto const& V = es.eigenvectors(); - - // compute second order correction to local positions -// Matrix res; -// res[0] = 0.5*sqrtD.transpose()*V.transpose()*d2vdx2*V*sqrtD; -// res[1] = 0.5*sqrtD.transpose()*V.transpose()*d2wdx2*V*sqrtD; - - Matrix res = Matrix::Zero(); - for (unsigned int i=0; i<5; ++i) { - res[0] += 0.5*D[i]*V.col(i).transpose()*d2vdx2*V.col(i); - res[1] += 0.5*D[i]*V.col(i).transpose()*d2wdx2*V.col(i); - } - -// res[0] = d2vdx2*curvc return res; } - -AlgebraicVector5 ResidualGlobalCorrectionMakerBase::update(const TrajectoryStateOnSurface& tsos, const TrackingRecHit& aRecHit) { - switch (aRecHit.dimension()) { - case 1: - return lupdate<1>(tsos, aRecHit); - case 2: - return lupdate<2>(tsos, aRecHit); - case 3: - return lupdate<3>(tsos, aRecHit); - case 4: - return lupdate<4>(tsos, aRecHit); - case 5: - return lupdate<5>(tsos, aRecHit); - } - return AlgebraicVector5(); -} - -#if 0 -Matrix ResidualGlobalCorrectionMakerBase::vertexToCurvilinearJacobian(const FreeTrajectoryState &state) const { - GlobalVector xt = state.momentum(); - GlobalVector yt(-xt.y(), xt.x(), 0.); - GlobalVector zt = xt.cross(yt); - const GlobalVector& pvec = state.momentum(); - double pt = pvec.perp(), p = pvec.mag(); - double px = pvec.x(), py = pvec.y(), pz = pvec.z(); - double pt2 = pt * pt, p2 = p * p, p3 = p * p * p; - - xt = xt.unit(); - if (fabs(pt) > 0) { - yt = yt.unit(); - zt = zt.unit(); - } - - Matrix R; - R(0, 0) = xt.x(); - R(0, 1) = xt.y(); - R(0, 2) = xt.z(); - R(1, 0) = yt.x(); - R(1, 1) = yt.y(); - R(1, 2) = yt.z(); - R(2, 0) = zt.x(); - R(2, 1) = zt.y(); - R(2, 2) = zt.z(); - - const Matrix dsdvtx = R.row(0); - - const MagneticField &bfield = state.parameters().magneticField(); - - const Vector3d b(bfield.x(), - bfield.y(), - bfield.z()); - const double magb = b.norm(); - const Vector3d h = b.normalized(); - - const Vector3d p0(state.momentum().x(), - state.momentum().y(), - state.momentum().z()); - const Vector3d M0(state.position().x(), - state.position().y(), - state.position().z()); - const Vector3d T0 = p0.normalized(); - const double p = p0.norm(); - const double q = state.charge(); - const double qop = q/p; - - const Vector3d N0alpha = h.cross(T0); - const double alpha = N0alpha.norm(); - const Vector3d N0 = N0alpha.normalized(); - const double gamma0 = h.transpose()*T0; - const Vector3d Z(0.,0.,1.); - const Vector3d U = Z.cross(T).normalized(); - const Vector3d V = T.cross(U); - - - - Matrix res = Matrix::Zero(); - - // d(dxt, dyt) / dvtx - res.bottomRows<2>() = R.bottomRows<2>(); - -} -#endif - -Matrix ResidualGlobalCorrectionMakerBase::cartesianToCartesianJacobian(const FreeTrajectoryState &state) const { - const GlobalVector &bfield = state.parameters().magneticFieldInInverseGeV(); - - const Vector3d b(bfield.x(), - bfield.y(), - bfield.z()); - const double magb = b.norm(); - const Vector3d h = b.normalized(); - - const Vector3d p0(state.momentum().x(), - state.momentum().y(), - state.momentum().z()); -// const Vector3d M0(state.position().x(), -// state.position().y(), -// state.position().z()); - const Vector3d T0 = p0.normalized(); - const double p = p0.norm(); - const double q = state.charge(); - const double qop = q/p; - - const Vector3d N0alpha = h.cross(T0); - const double alpha = N0alpha.norm(); - const Vector3d N0 = N0alpha.normalized(); -// const double gamma0 = h.transpose()*T0; -// const Vector3d Z(0.,0.,1.); -// const Vector3d U = Z.cross(T).normalized(); -// const Vector3d V = T.cross(U); - - Matrix res = Matrix::Identity(); - - //dp/dvtx - res.bottomLeftCorner<3, 3>() = -alpha*magb*qop*N0*p0.transpose(); - - res = Matrix::Zero(); - - res *= 0.; -// res *= -1.; - - return res; - - - -} - - -Matrix ResidualGlobalCorrectionMakerBase::massJacobianAlt(const FreeTrajectoryState &state0, const FreeTrajectoryState &state1, double dmass) const { +Matrix ResidualGlobalCorrectionMakerBase::massJacobianAltD(const Matrix &state0, const Matrix &state1, double dmass) const { const double mp = dmass; - CurvilinearTrajectoryParameters curvparms0(state0.position(), state0.momentum(), state0.charge()); - - const double qop0 = curvparms0.Qbp(); - const double lam0 = curvparms0.lambda(); - const double phi0 = curvparms0.phi(); - - CurvilinearTrajectoryParameters curvparms1(state1.position(), state1.momentum(), state1.charge()); - - const double qop1 = curvparms1.Qbp(); - const double lam1 = curvparms1.lambda(); - const double phi1 = curvparms1.phi(); + const double qop0 = state0[6]/state0.segment<3>(3).norm(); + const double lam0 = std::atan(state0[5]/std::sqrt(state0[3]*state0[3] + state0[4]*state0[4])); + const double phi0 = std::atan2(state0[4], state0[3]); + + const double qop1 = state1[6]/state1.segment<3>(3).norm(); + const double lam1 = std::atan(state1[5]/std::sqrt(state1[3]*state1[3] + state1[4]*state1[4])); + const double phi1 = std::atan2(state1[4], state1[3]); const double xf0 = std::pow(mp, 2); const double xf1 = std::sin(lam0); @@ -5004,7 +3282,7 @@ Matrix ResidualGlobalCorrectionMakerBase::massJacobianAlt(const Fr return res; } -Matrix ResidualGlobalCorrectionMakerBase::massJacobianAltD(const Matrix &state0, const Matrix &state1, double dmass) const { +Matrix ResidualGlobalCorrectionMakerBase::massHessianAltD(const Matrix &state0, const Matrix &state1, double dmass) const { const double mp = dmass; @@ -5023,112 +3301,226 @@ Matrix ResidualGlobalCorrectionMakerBase::massJacobianAltD(const M const double xf4 = 1.0/std::fabs(qop1); const double xf5 = xf3*xf4; const double xf6 = xf2*xf5; - const double xf7 = std::sin(phi0); - const double xf8 = std::sin(phi1); - const double xf9 = xf7*xf8; - const double xf10 = std::cos(lam0); - const double xf11 = std::cos(lam1); - const double xf12 = xf11*xf5; - const double xf13 = xf10*xf12; - const double xf14 = 2*xf13; - const double xf15 = std::cos(phi0); - const double xf16 = std::cos(phi1); - const double xf17 = xf15*xf16; - const double xf18 = std::pow(qop0, -2); - const double xf19 = std::sqrt(xf0 + xf18); - const double xf20 = std::pow(qop1, -2); - const double xf21 = std::sqrt(xf0 + xf20); - const double xf22 = std::sqrt(2*xf0 - 2*xf1*xf6 - xf14*xf17 - xf14*xf9 + 2*xf19*xf21); - const double xf23 = xf1*xf2; - const double xf24 = xf18*xf4*(((qop0) > 0) - ((qop0) < 0)); - const double xf25 = xf10*xf11; - const double xf26 = xf24*xf25; - const double xf27 = 1.0/xf22; - const double xf28 = xf10*xf6; - const double xf29 = xf1*xf12; - const double xf30 = xf13*xf16*xf7; - const double xf31 = xf13*xf15*xf8; - const double xf32 = xf20*xf3*(((qop1) > 0) - ((qop1) < 0)); - const double xf33 = xf25*xf32; - const double m = xf22; - const double dmdqop0 = xf27*(xf17*xf26 + xf23*xf24 + xf26*xf9 - xf21/(std::pow(qop0, 3)*xf19)); - const double dmdlam0 = xf27*(xf17*xf29 - xf28 + xf29*xf9); - const double dmdphi0 = xf27*(xf30 - xf31); - const double dmdqop1 = xf27*(xf17*xf33 + xf23*xf32 + xf33*xf9 - xf19/(std::pow(qop1, 3)*xf21)); - const double dmdlam1 = xf27*(xf17*xf28 + xf28*xf9 - xf29); - const double dmdphi1 = xf27*(-xf30 + xf31); - Matrix res; - res(0,0) = dmdqop0; - res(0,1) = dmdlam0; - res(0,2) = dmdphi0; - res(0,3) = dmdqop1; - res(0,4) = dmdlam1; - res(0,5) = dmdphi1; - -// std::cout << "massJacobianAlt m = " << m << std::endl; - - + const double xf7 = xf1*xf6; + const double xf8 = std::cos(lam0); + const double xf9 = std::cos(lam1); + const double xf10 = xf5*xf9; + const double xf11 = xf10*xf8; + const double xf12 = std::sin(phi0); + const double xf13 = std::sin(phi1); + const double xf14 = xf12*xf13; + const double xf15 = xf11*xf14; + const double xf16 = std::cos(phi0); + const double xf17 = std::cos(phi1); + const double xf18 = xf16*xf17; + const double xf19 = xf11*xf18; + const double xf20 = std::pow(qop0, -2); + const double xf21 = xf0 + xf20; + const double xf22 = std::sqrt(xf21); + const double xf23 = std::pow(qop1, -2); + const double xf24 = xf0 + xf23; + const double xf25 = std::sqrt(xf24); + const double xf26 = 2*xf0 - 2*xf15 - 2*xf19 + 2*xf22*xf25 - 2*xf7; + const double xf27 = std::pow(xf26, -1.0/2.0); + const double xf28 = xf1*xf2; + const double xf29 = 2*xf28; + const double xf30 = std::pow(qop0, -3); + const double xf31 = (((qop0) > 0) - ((qop0) < 0)); + const double xf32 = xf31*xf4; + const double xf33 = xf30*xf32; + const double xf34 = 2*xf33; + const double xf35 = xf8*xf9; + const double xf36 = xf14*xf35; + const double xf37 = xf18*xf35; + const double xf38 = 1.0/xf22; + const double xf39 = xf20*xf32; + const double xf40 = xf35*xf39; + const double xf41 = xf14*xf40 + xf18*xf40 - xf25*xf30*xf38 + xf28*xf39; + const double xf42 = -xf41; + const double xf43 = std::pow(xf26, -3.0/2.0); + const double xf44 = xf41*xf43; + const double xf45 = xf1*xf9; + const double xf46 = xf39*xf45; + const double xf47 = xf27*(-xf14*xf46 - xf18*xf46 + xf2*xf20*xf31*xf4*xf8); + const double xf48 = xf1*xf10; + const double xf49 = xf14*xf48 + xf18*xf48 - xf2*xf3*xf4*xf8; + const double xf50 = -xf49; + const double xf51 = xf12*xf17; + const double xf52 = -xf13*xf16*xf20*xf31*xf4*xf8*xf9 + xf40*xf51; + const double xf53 = -xf27*xf52; + const double xf54 = xf11*xf51 - xf13*xf16*xf3*xf4*xf8*xf9; + const double xf55 = -xf54; + const double xf56 = (((qop1) > 0) - ((qop1) < 0)); + const double xf57 = xf23*xf56; + const double xf58 = xf20*xf31*xf57; + const double xf59 = std::pow(qop1, -3); + const double xf60 = 1.0/xf25; + const double xf61 = xf27*(-xf28*xf58 + xf30*xf38*xf59*xf60 - xf36*xf58 - xf37*xf58); + const double xf62 = xf3*xf57; + const double xf63 = xf35*xf62; + const double xf64 = xf14*xf63 + xf18*xf63 - xf22*xf59*xf60 + xf28*xf62; + const double xf65 = -xf64; + const double xf66 = xf2*xf8; + const double xf67 = xf39*xf66; + const double xf68 = xf27*(xf1*xf20*xf31*xf4*xf9 - xf14*xf67 - xf18*xf67); + const double xf69 = xf6*xf8; + const double xf70 = -xf1*xf3*xf4*xf9 + xf14*xf69 + xf18*xf69; + const double xf71 = -xf70; + const double xf72 = xf27*xf52; + const double xf73 = xf43*xf49; + const double xf74 = xf15 + xf19; + const double xf75 = xf27*(xf7 + xf74); + const double xf76 = -xf1*xf13*xf16*xf3*xf4*xf9 + xf48*xf51; + const double xf77 = -xf27*xf76; + const double xf78 = xf45*xf62; + const double xf79 = xf27*(-xf14*xf78 - xf18*xf78 + xf2*xf23*xf3*xf56*xf8); + const double xf80 = xf27*(-xf11 - xf14*xf7 - xf18*xf7); + const double xf81 = xf27*xf76; + const double xf82 = xf43*xf54; + const double xf83 = xf27*xf74 + xf55*xf82; + const double xf84 = -xf13*xf16*xf23*xf3*xf56*xf8*xf9 + xf51*xf63; + const double xf85 = -xf27*xf84; + const double xf86 = -xf13*xf16*xf2*xf3*xf4*xf8 + xf51*xf69; + const double xf87 = -xf27*xf86; + const double xf88 = -xf27*xf74; + const double xf89 = xf43*xf64; + const double xf90 = xf3*xf56*xf59; + const double xf91 = 2*xf90; + const double xf92 = xf62*xf66; + const double xf93 = xf27*(xf1*xf23*xf3*xf56*xf9 - xf14*xf92 - xf18*xf92); + const double xf94 = xf27*xf84; + const double xf95 = xf43*xf70; + const double xf96 = xf27*xf86; + const double xf97 = xf43*xf55; + const double d2mdqop0dqop0 = xf27*(-xf29*xf33 - xf34*xf36 - xf34*xf37 + 3*xf25*xf38/std::pow(qop0, 4) - xf25/(std::pow(qop0, 6)*std::pow(xf21, 3.0/2.0))) + xf42*xf44; + const double d2mdqop0dlam0 = xf44*xf50 + xf47; + const double d2mdqop0dphi0 = xf44*xf55 + xf53; + const double d2mdqop0dqop1 = xf44*xf65 + xf61; + const double d2mdqop0dlam1 = xf44*xf71 + xf68; + const double d2mdqop0dphi1 = xf44*xf54 + xf72; + const double d2mdlam0dqop0 = xf42*xf73 + xf47; + const double d2mdlam0dlam0 = xf50*xf73 + xf75; + const double d2mdlam0dphi0 = xf55*xf73 + xf77; + const double d2mdlam0dqop1 = xf65*xf73 + xf79; + const double d2mdlam0dlam1 = xf71*xf73 + xf80; + const double d2mdlam0dphi1 = xf54*xf73 + xf81; + const double d2mdphi0dqop0 = xf42*xf82 + xf53; + const double d2mdphi0dlam0 = xf50*xf82 + xf77; + const double d2mdphi0dphi0 = xf83; + const double d2mdphi0dqop1 = xf65*xf82 + xf85; + const double d2mdphi0dlam1 = xf71*xf82 + xf87; + const double d2mdphi0dphi1 = xf43*std::pow(xf54, 2) + xf88; + const double d2mdqop1dqop0 = xf42*xf89 + xf61; + const double d2mdqop1dlam0 = xf50*xf89 + xf79; + const double d2mdqop1dphi0 = xf55*xf89 + xf85; + const double d2mdqop1dqop1 = xf27*(-xf29*xf90 - xf36*xf91 - xf37*xf91 + 3*xf22*xf60/std::pow(qop1, 4) - xf22/(std::pow(qop1, 6)*std::pow(xf24, 3.0/2.0))) + xf65*xf89; + const double d2mdqop1dlam1 = xf71*xf89 + xf93; + const double d2mdqop1dphi1 = xf64*xf82 + xf94; + const double d2mdlam1dqop0 = xf42*xf95 + xf68; + const double d2mdlam1dlam0 = xf50*xf95 + xf80; + const double d2mdlam1dphi0 = xf55*xf95 + xf87; + const double d2mdlam1dqop1 = xf65*xf95 + xf93; + const double d2mdlam1dlam1 = xf71*xf95 + xf75; + const double d2mdlam1dphi1 = xf70*xf82 + xf96; + const double d2mdphi1dqop0 = xf42*xf97 + xf72; + const double d2mdphi1dlam0 = xf50*xf97 + xf81; + const double d2mdphi1dphi0 = xf43*std::pow(xf55, 2) + xf88; + const double d2mdphi1dqop1 = xf65*xf97 + xf94; + const double d2mdphi1dlam1 = xf71*xf97 + xf96; + const double d2mdphi1dphi1 = xf83; + Matrix res; + res(0,0) = d2mdqop0dqop0; + res(0,1) = d2mdqop0dlam0; + res(0,2) = d2mdqop0dphi0; + res(0,3) = d2mdqop0dqop1; + res(0,4) = d2mdqop0dlam1; + res(0,5) = d2mdqop0dphi1; + res(1,0) = d2mdlam0dqop0; + res(1,1) = d2mdlam0dlam0; + res(1,2) = d2mdlam0dphi0; + res(1,3) = d2mdlam0dqop1; + res(1,4) = d2mdlam0dlam1; + res(1,5) = d2mdlam0dphi1; + res(2,0) = d2mdphi0dqop0; + res(2,1) = d2mdphi0dlam0; + res(2,2) = d2mdphi0dphi0; + res(2,3) = d2mdphi0dqop1; + res(2,4) = d2mdphi0dlam1; + res(2,5) = d2mdphi0dphi1; + res(3,0) = d2mdqop1dqop0; + res(3,1) = d2mdqop1dlam0; + res(3,2) = d2mdqop1dphi0; + res(3,3) = d2mdqop1dqop1; + res(3,4) = d2mdqop1dlam1; + res(3,5) = d2mdqop1dphi1; + res(4,0) = d2mdlam1dqop0; + res(4,1) = d2mdlam1dlam0; + res(4,2) = d2mdlam1dphi0; + res(4,3) = d2mdlam1dqop1; + res(4,4) = d2mdlam1dlam1; + res(4,5) = d2mdlam1dphi1; + res(5,0) = d2mdphi1dqop0; + res(5,1) = d2mdphi1dlam0; + res(5,2) = d2mdphi1dphi0; + res(5,3) = d2mdphi1dqop1; + res(5,4) = d2mdphi1dlam1; + res(5,5) = d2mdphi1dphi1; return res; } -Matrix ResidualGlobalCorrectionMakerBase::massJacobianInvSq(const FreeTrajectoryState &state0, const FreeTrajectoryState &state1, double dmass) const { +Matrix ResidualGlobalCorrectionMakerBase::massinvsqJacobianAltD(const Matrix &state0, const Matrix &state1, double dmass) const { const double mp = dmass; - CurvilinearTrajectoryParameters curvparms0(state0.position(), state0.momentum(), state0.charge()); - - const double qop0 = curvparms0.Qbp(); - const double lam0 = curvparms0.lambda(); - const double phi0 = curvparms0.phi(); - - CurvilinearTrajectoryParameters curvparms1(state1.position(), state1.momentum(), state1.charge()); - - const double qop1 = curvparms1.Qbp(); - const double lam1 = curvparms1.lambda(); - const double phi1 = curvparms1.phi(); + const double qop0 = state0[6]/state0.segment<3>(3).norm(); + const double lam0 = std::atan(state0[5]/std::sqrt(state0[3]*state0[3] + state0[4]*state0[4])); + const double phi0 = std::atan2(state0[4], state0[3]); + + const double qop1 = state1[6]/state1.segment<3>(3).norm(); + const double lam1 = std::atan(state1[5]/std::sqrt(state1[3]*state1[3] + state1[4]*state1[4])); + const double phi1 = std::atan2(state1[4], state1[3]); const double xf0 = std::sin(lam0); const double xf1 = std::sin(lam1); const double xf2 = xf0*xf1; const double xf3 = std::pow(qop0, -2); - const double xf4 = 2/std::fabs(qop1); - const double xf5 = xf3*xf4*(((qop0) > 0) - ((qop0) < 0)); - const double xf6 = std::sin(phi0); - const double xf7 = std::sin(phi1); - const double xf8 = xf6*xf7; - const double xf9 = std::cos(lam0); - const double xf10 = std::cos(lam1); - const double xf11 = xf10*xf9; - const double xf12 = xf11*xf5; - const double xf13 = std::cos(phi0); - const double xf14 = std::cos(phi1); - const double xf15 = xf13*xf14; - const double xf16 = std::pow(mp, 2); - const double xf17 = std::sqrt(xf16 + xf3); - const double xf18 = std::pow(qop1, -2); - const double xf19 = std::sqrt(xf16 + xf18); - const double xf20 = 2*xf19; + const double xf4 = 1.0/std::fabs(qop1); + const double xf5 = 2*xf4; + const double xf6 = xf3*xf5*(((qop0) > 0) - ((qop0) < 0)); + const double xf7 = std::sin(phi0); + const double xf8 = std::sin(phi1); + const double xf9 = xf7*xf8; + const double xf10 = std::cos(lam0); + const double xf11 = std::cos(lam1); + const double xf12 = xf10*xf11; + const double xf13 = xf12*xf6; + const double xf14 = std::cos(phi0); + const double xf15 = std::cos(phi1); + const double xf16 = xf14*xf15; + const double xf17 = std::pow(mp, 2); + const double xf18 = std::sqrt(xf17 + xf3); + const double xf19 = std::pow(qop1, -2); + const double xf20 = std::sqrt(xf17 + xf19); const double xf21 = 1.0/std::fabs(qop0); - const double xf22 = xf21*xf4; + const double xf22 = xf21*xf5; const double xf23 = xf1*xf22; - const double xf24 = xf10*xf22; - const double xf25 = xf24*xf9; - const double xf26 = 1.0/std::pow(-xf0*xf23 - xf15*xf25 + 2*xf16 + xf17*xf20 - xf25*xf8, 2); - const double xf27 = xf23*xf9; - const double xf28 = xf0*xf24; - const double xf29 = xf14*xf25*xf6; - const double xf30 = xf13*xf25*xf7; - const double xf31 = 2*xf18*xf21*(((qop1) > 0) - ((qop1) < 0)); - const double xf32 = xf11*xf31; - const double dminvsqdqop0 = xf26*(-xf12*xf15 - xf12*xf8 - xf2*xf5 + xf20/(std::pow(qop0, 3)*xf17)); - const double dminvsqdlam0 = xf26*(-xf15*xf28 + xf27 - xf28*xf8); - const double dminvsqdphi0 = xf26*(-xf29 + xf30); - const double dminvsqdqop1 = xf26*(-xf15*xf32 - xf2*xf31 - xf32*xf8 + 2*xf17/(std::pow(qop1, 3)*xf19)); - const double dminvsqdlam1 = xf26*(-xf15*xf27 - xf27*xf8 + xf28); - const double dminvsqdphi1 = xf26*(xf29 - xf30); + const double xf24 = xf11*xf22; + const double xf25 = xf10*xf24; + const double xf26 = 1.0/std::pow(-xf0*xf23 - xf16*xf25 + 2*xf17 + 2*xf18*xf20 - xf25*xf9, 2); + const double xf27 = xf0*xf24; + const double xf28 = -2*xf10*xf11*xf14*xf21*xf4*xf8 + xf15*xf25*xf7; + const double xf29 = 2*xf19*xf21*(((qop1) > 0) - ((qop1) < 0)); + const double xf30 = xf12*xf29; + const double xf31 = xf10*xf23; + const double dminvsqdqop0 = xf26*(-xf13*xf16 - xf13*xf9 - xf2*xf6 + 2*xf20/(std::pow(qop0, 3)*xf18)); + const double dminvsqdlam0 = xf26*(2*xf1*xf10*xf21*xf4 - xf16*xf27 - xf27*xf9); + const double dminvsqdphi0 = -xf26*xf28; + const double dminvsqdqop1 = xf26*(-xf16*xf30 - xf2*xf29 - xf30*xf9 + 2*xf18/(std::pow(qop1, 3)*xf20)); + const double dminvsqdlam1 = xf26*(2*xf0*xf11*xf21*xf4 - xf16*xf31 - xf31*xf9); + const double dminvsqdphi1 = xf26*xf28; Matrix res; res(0,0) = dminvsqdqop0; res(0,1) = dminvsqdlam0; @@ -5136,7 +3528,6 @@ Matrix ResidualGlobalCorrectionMakerBase::massJacobianInvSq(const res(0,3) = dminvsqdqop1; res(0,4) = dminvsqdlam1; res(0,5) = dminvsqdphi1; - // std::cout << "massJacobianAlt m = " << m << std::endl; @@ -5147,128 +3538,200 @@ Matrix ResidualGlobalCorrectionMakerBase::massJacobianInvSq(const return res; } - -Matrix ResidualGlobalCorrectionMakerBase::mrJacobian(const FreeTrajectoryState &state0, const FreeTrajectoryState &state1, double dmass) const { +Matrix ResidualGlobalCorrectionMakerBase::massinvsqHessianAltD(const Matrix &state0, const Matrix &state1, double dmass) const { const double mp = dmass; - CurvilinearTrajectoryParameters curvparms0(state0.position(), state0.momentum(), state0.charge()); - - const double qop0 = curvparms0.Qbp(); - const double lam0 = curvparms0.lambda(); - const double phi0 = curvparms0.phi(); - - CurvilinearTrajectoryParameters curvparms1(state1.position(), state1.momentum(), state1.charge()); - - const double qop1 = curvparms1.Qbp(); - const double lam1 = curvparms1.lambda(); - const double phi1 = curvparms1.phi(); + const double qop0 = state0[6]/state0.segment<3>(3).norm(); + const double lam0 = std::atan(state0[5]/std::sqrt(state0[3]*state0[3] + state0[4]*state0[4])); + const double phi0 = std::atan2(state0[4], state0[3]); + + const double qop1 = state1[6]/state1.segment<3>(3).norm(); + const double lam1 = std::atan(state1[5]/std::sqrt(state1[3]*state1[3] + state1[4]*state1[4])); + const double phi1 = std::atan2(state1[4], state1[3]); const double xf0 = std::sin(lam0); const double xf1 = std::sin(lam1); const double xf2 = xf0*xf1; - const double xf3 = std::pow(qop0, -2); - const double xf4 = 2/std::fabs(qop1); - const double xf5 = xf3*xf4*(((qop0) > 0) - ((qop0) < 0)); - const double xf6 = std::sin(phi0); - const double xf7 = std::sin(phi1); - const double xf8 = xf6*xf7; - const double xf9 = std::cos(lam0); + const double xf3 = std::pow(qop0, -3); + const double xf4 = 1.0/std::fabs(qop1); + const double xf5 = (((qop0) > 0) - ((qop0) < 0)); + const double xf6 = xf4*xf5; + const double xf7 = 4*xf3*xf6; + const double xf8 = std::cos(lam0); + const double xf9 = xf7*xf8; const double xf10 = std::cos(lam1); - const double xf11 = xf10*xf9; - const double xf12 = xf11*xf5; - const double xf13 = std::cos(phi0); - const double xf14 = std::cos(phi1); - const double xf15 = xf13*xf14; - const double xf16 = std::pow(mp, 2); - const double xf17 = std::sqrt(xf16 + xf3); - const double xf18 = std::pow(qop1, -2); - const double xf19 = std::sqrt(xf16 + xf18); - const double xf20 = 2*xf19; - const double xf21 = 1.0/std::fabs(qop0); - const double xf22 = xf21*xf4; - const double xf23 = xf1*xf22; - const double xf24 = xf10*xf22; - const double xf25 = xf24*xf9; - const double xf26 = std::pow(-xf0*xf23 - xf15*xf25 + 2*xf16 + xf17*xf20 - xf25*xf8, -2); - const double xf27 = xf23*xf9; - const double xf28 = xf0*xf24; - const double xf29 = xf14*xf25*xf6; - const double xf30 = xf13*xf25*xf7; - const double xf31 = 2*xf18*xf21*(((qop1) > 0) - ((qop1) < 0)); - const double xf32 = xf11*xf31; - const double dmrdqop0 = xf26*(-xf12*xf15 - xf12*xf8 - xf2*xf5 + xf20/(std::pow(qop0, 3)*xf17)); - const double dmrdlam0 = xf26*(-xf15*xf28 + xf27 - xf28*xf8); - const double dmrdphi0 = xf26*(-xf29 + xf30); - const double dmrdqop1 = xf26*(-xf15*xf32 - xf2*xf31 - xf32*xf8 + 2*xf17/(std::pow(qop1, 3)*xf19)); - const double dmrdlam1 = xf26*(-xf15*xf27 - xf27*xf8 + xf28); - const double dmrdphi1 = xf26*(xf29 - xf30); - Matrix res; - res(0,0) = dmrdqop0; - res(0,1) = dmrdlam0; - res(0,2) = dmrdphi0; - res(0,3) = dmrdqop1; - res(0,4) = dmrdlam1; - res(0,5) = dmrdphi1; - - -// std::cout << "massJacobianAlt m = " << m << std::endl; - - - - - - return res; -} - -Matrix ResidualGlobalCorrectionMakerBase::elossAdHocJacobian(const FreeTrajectoryState &state, double mass) const { - - const double qop0 = state.signedInverseMomentum(); - const double q = state.charge(); - - const double eloss = 0.01; // 10 MeV - - const double x0 = std::pow(qop0, 2); - const double x1 = std::pow(q, 2); - const double dqopdde = -eloss*q*x0*std::sqrt(std::pow(mass, 2) + x1/x0)*std::fabs(qop0)/std::pow(x1, 3.0/2.0); - const double dlamdde = 0; - const double dphidde = 0; - const double dxtdde = 0; - const double dytdde = 0; - Eigen::Matrix res; - res(0,0) = dqopdde; - res(1,0) = dlamdde; - res(2,0) = dphidde; - res(3,0) = dxtdde; - res(4,0) = dytdde; - - return res; - -} - -Matrix ResidualGlobalCorrectionMakerBase::elossAdHocJacobianD(const Matrix &state, double mass) const { - - const double q = state[6]; - const double qop0 = q/state.segment<3>(3).norm(); - - const double eloss = 0.01; // 10 MeV + const double xf11 = std::sin(phi0); + const double xf12 = std::sin(phi1); + const double xf13 = xf11*xf12; + const double xf14 = xf10*xf13; + const double xf15 = std::cos(phi0); + const double xf16 = std::cos(phi1); + const double xf17 = xf15*xf16; + const double xf18 = xf10*xf17; + const double xf19 = std::pow(mp, 2); + const double xf20 = std::pow(qop0, -2); + const double xf21 = xf19 + xf20; + const double xf22 = std::pow(qop1, -2); + const double xf23 = xf19 + xf22; + const double xf24 = std::sqrt(xf23); + const double xf25 = std::sqrt(xf21); + const double xf26 = 1.0/xf25; + const double xf27 = 1.0/std::fabs(qop0); + const double xf28 = xf27*xf4; + const double xf29 = xf1*xf28; + const double xf30 = 2*xf0; + const double xf31 = xf29*xf30; + const double xf32 = xf10*xf28; + const double xf33 = 2*xf8; + const double xf34 = xf32*xf33; + const double xf35 = xf13*xf34 + xf17*xf34; + const double xf36 = xf31 + xf35; + const double xf37 = 2*xf19 + 2*xf24*xf25 - xf36; + const double xf38 = 1.0/std::pow(xf37, 2); + const double xf39 = 4*xf0; + const double xf40 = xf20*xf6; + const double xf41 = xf1*xf40; + const double xf42 = 4*xf8; + const double xf43 = xf40*xf42; + const double xf44 = -xf14*xf43 - xf18*xf43 + 4*xf24*xf26*xf3 - xf39*xf41; + const double xf45 = xf1*xf30; + const double xf46 = xf33*xf40; + const double xf47 = 1.0/std::pow(xf37, 3); + const double xf48 = xf47*(-xf14*xf46 - xf18*xf46 + 2*xf24*xf26*xf3 - xf40*xf45); + const double xf49 = xf33*xf41; + const double xf50 = xf10*xf30; + const double xf51 = xf40*xf50; + const double xf52 = xf38*(xf13*xf51 + xf17*xf51 - xf49); + const double xf53 = xf32*xf39; + const double xf54 = 4*xf1*xf27*xf4*xf8 - xf13*xf53 - xf17*xf53; + const double xf55 = xf11*xf16; + const double xf56 = xf10*xf46; + const double xf57 = xf12*xf15; + const double xf58 = xf55*xf56 - xf56*xf57; + const double xf59 = xf38*xf58; + const double xf60 = -4*xf10*xf12*xf15*xf27*xf4*xf8 + xf32*xf42*xf55; + const double xf61 = -xf60; + const double xf62 = (((qop1) > 0) - ((qop1) < 0)); + const double xf63 = xf20*xf22*xf5*xf62; + const double xf64 = xf33*xf63; + const double xf65 = 1.0/xf24; + const double xf66 = std::pow(qop1, -3); + const double xf67 = xf38*(xf14*xf64 + xf18*xf64 - 2*xf26*xf3*xf65*xf66 + xf45*xf63); + const double xf68 = xf27*xf62; + const double xf69 = xf22*xf68; + const double xf70 = xf1*xf69; + const double xf71 = xf42*xf69; + const double xf72 = -xf14*xf71 - xf18*xf71 + 4*xf25*xf65*xf66 - xf39*xf70; + const double xf73 = xf38*(xf13*xf49 + xf17*xf49 - xf51); + const double xf74 = xf29*xf8; + const double xf75 = 4*xf74; + const double xf76 = 4*xf0*xf10*xf27*xf4 - xf13*xf75 - xf17*xf75; + const double xf77 = -xf38*xf58; + const double xf78 = xf30*xf32; + const double xf79 = xf47*(2*xf1*xf27*xf4*xf8 - xf13*xf78 - xf17*xf78); + const double xf80 = -xf36*xf38; + const double xf81 = xf55*xf78 - xf57*xf78; + const double xf82 = xf38*xf81; + const double xf83 = xf33*xf70; + const double xf84 = xf50*xf69; + const double xf85 = xf38*(xf13*xf84 + xf17*xf84 - xf83); + const double xf86 = xf38*(xf13*xf31 + xf17*xf31 + xf34); + const double xf87 = -xf38*xf81; + const double xf88 = -2*xf10*xf12*xf15*xf27*xf4*xf8 + xf34*xf55; + const double xf89 = -xf47*xf88; + const double xf90 = -xf35*xf38; + const double xf91 = xf33*xf69; + const double xf92 = xf10*xf91; + const double xf93 = xf55*xf92 - xf57*xf92; + const double xf94 = xf38*xf93; + const double xf95 = 2*xf74; + const double xf96 = xf55*xf95 - xf57*xf95; + const double xf97 = xf38*xf96; + const double xf98 = xf35*xf38; + const double xf99 = xf47*(-xf14*xf91 - xf18*xf91 + 2*xf25*xf65*xf66 - xf45*xf69); + const double xf100 = 4*xf66*xf68; + const double xf101 = xf100*xf8; + const double xf102 = xf38*(xf13*xf83 + xf17*xf83 - xf84); + const double xf103 = -xf38*xf93; + const double xf104 = xf47*(2*xf0*xf10*xf27*xf4 - xf13*xf95 - xf17*xf95); + const double xf105 = -xf38*xf96; + const double xf106 = xf47*xf88; + const double d2minvsqdqop0dqop0 = xf38*(xf14*xf9 + xf18*xf9 + xf2*xf7 - 6*xf24*xf26/std::pow(qop0, 4) + 2*xf24/(std::pow(qop0, 6)*std::pow(xf21, 3.0/2.0))) + xf44*xf48; + const double d2minvsqdqop0dlam0 = xf48*xf54 + xf52; + const double d2minvsqdqop0dphi0 = xf48*xf61 + xf59; + const double d2minvsqdqop0dqop1 = xf48*xf72 + xf67; + const double d2minvsqdqop0dlam1 = xf48*xf76 + xf73; + const double d2minvsqdqop0dphi1 = xf48*xf60 + xf77; + const double d2minvsqdlam0dqop0 = xf44*xf79 + xf52; + const double d2minvsqdlam0dlam0 = xf54*xf79 + xf80; + const double d2minvsqdlam0dphi0 = xf61*xf79 + xf82; + const double d2minvsqdlam0dqop1 = xf72*xf79 + xf85; + const double d2minvsqdlam0dlam1 = xf76*xf79 + xf86; + const double d2minvsqdlam0dphi1 = xf60*xf79 + xf87; + const double d2minvsqdphi0dqop0 = xf44*xf89 + xf59; + const double d2minvsqdphi0dlam0 = xf54*xf89 + xf82; + const double d2minvsqdphi0dphi0 = xf61*xf89 + xf90; + const double d2minvsqdphi0dqop1 = xf72*xf89 + xf94; + const double d2minvsqdphi0dlam1 = xf76*xf89 + xf97; + const double d2minvsqdphi0dphi1 = xf60*xf89 + xf98; + const double d2minvsqdqop1dqop0 = xf44*xf99 + xf67; + const double d2minvsqdqop1dlam0 = xf54*xf99 + xf85; + const double d2minvsqdqop1dphi0 = xf61*xf99 + xf94; + const double d2minvsqdqop1dqop1 = xf38*(xf100*xf2 + xf101*xf14 + xf101*xf18 - 6*xf25*xf65/std::pow(qop1, 4) + 2*xf25/(std::pow(qop1, 6)*std::pow(xf23, 3.0/2.0))) + xf72*xf99; + const double d2minvsqdqop1dlam1 = xf102 + xf76*xf99; + const double d2minvsqdqop1dphi1 = xf103 + xf60*xf99; + const double d2minvsqdlam1dqop0 = xf104*xf44 + xf73; + const double d2minvsqdlam1dlam0 = xf104*xf54 + xf86; + const double d2minvsqdlam1dphi0 = xf104*xf61 + xf97; + const double d2minvsqdlam1dqop1 = xf102 + xf104*xf72; + const double d2minvsqdlam1dlam1 = xf104*xf76 + xf80; + const double d2minvsqdlam1dphi1 = xf104*xf60 + xf105; + const double d2minvsqdphi1dqop0 = xf106*xf44 + xf77; + const double d2minvsqdphi1dlam0 = xf106*xf54 + xf87; + const double d2minvsqdphi1dphi0 = xf106*xf61 + xf98; + const double d2minvsqdphi1dqop1 = xf103 + xf106*xf72; + const double d2minvsqdphi1dlam1 = xf105 + xf106*xf76; + const double d2minvsqdphi1dphi1 = xf106*xf60 + xf90; + Matrix res; + res(0,0) = d2minvsqdqop0dqop0; + res(0,1) = d2minvsqdqop0dlam0; + res(0,2) = d2minvsqdqop0dphi0; + res(0,3) = d2minvsqdqop0dqop1; + res(0,4) = d2minvsqdqop0dlam1; + res(0,5) = d2minvsqdqop0dphi1; + res(1,0) = d2minvsqdlam0dqop0; + res(1,1) = d2minvsqdlam0dlam0; + res(1,2) = d2minvsqdlam0dphi0; + res(1,3) = d2minvsqdlam0dqop1; + res(1,4) = d2minvsqdlam0dlam1; + res(1,5) = d2minvsqdlam0dphi1; + res(2,0) = d2minvsqdphi0dqop0; + res(2,1) = d2minvsqdphi0dlam0; + res(2,2) = d2minvsqdphi0dphi0; + res(2,3) = d2minvsqdphi0dqop1; + res(2,4) = d2minvsqdphi0dlam1; + res(2,5) = d2minvsqdphi0dphi1; + res(3,0) = d2minvsqdqop1dqop0; + res(3,1) = d2minvsqdqop1dlam0; + res(3,2) = d2minvsqdqop1dphi0; + res(3,3) = d2minvsqdqop1dqop1; + res(3,4) = d2minvsqdqop1dlam1; + res(3,5) = d2minvsqdqop1dphi1; + res(4,0) = d2minvsqdlam1dqop0; + res(4,1) = d2minvsqdlam1dlam0; + res(4,2) = d2minvsqdlam1dphi0; + res(4,3) = d2minvsqdlam1dqop1; + res(4,4) = d2minvsqdlam1dlam1; + res(4,5) = d2minvsqdlam1dphi1; + res(5,0) = d2minvsqdphi1dqop0; + res(5,1) = d2minvsqdphi1dlam0; + res(5,2) = d2minvsqdphi1dphi0; + res(5,3) = d2minvsqdphi1dqop1; + res(5,4) = d2minvsqdphi1dlam1; + res(5,5) = d2minvsqdphi1dphi1; - const double x0 = std::pow(qop0, 2); - const double x1 = std::pow(q, 2); - const double dqopdde = -eloss*q*x0*std::sqrt(std::pow(mass, 2) + x1/x0)*std::fabs(qop0)/std::pow(x1, 3.0/2.0); - const double dlamdde = 0; - const double dphidde = 0; - const double dxtdde = 0; - const double dytdde = 0; - Eigen::Matrix res; - res(0,0) = dqopdde; - res(1,0) = dlamdde; - res(2,0) = dphidde; - res(3,0) = dxtdde; - res(4,0) = dytdde; - return res; - } //define this as a plug-in diff --git a/Analysis/HitAnalyzer/plugins/ResidualGlobalCorrectionMakerBase.h b/Analysis/HitAnalyzer/plugins/ResidualGlobalCorrectionMakerBase.h index f032d5811654d..922a5c444c897 100644 --- a/Analysis/HitAnalyzer/plugins/ResidualGlobalCorrectionMakerBase.h +++ b/Analysis/HitAnalyzer/plugins/ResidualGlobalCorrectionMakerBase.h @@ -81,8 +81,10 @@ #include "SimDataFormats/Track/interface/SimTrack.h" +#include "DataFormats/Common/interface/TriggerResults.h" +#include "DataFormats/Provenance/interface/ParameterSetID.h" - +#include "SimDataFormats/PileupSummaryInfo/interface/PileupSummaryInfo.h" // #include "../interface/OffsetMagneticField.h" // #include "../interface/ParmInfo.h" @@ -123,19 +125,6 @@ typedef MatrixXd AlignmentJacobianMatrix; typedef MatrixXd TransportJacobianMatrix; typedef MatrixXd ELossJacobianMatrix; - -// struct ParmInfo { -// int parmtype; -// int subdet; -// int layer; -// float x; -// float y; -// float z; -// float eta; -// float phi; -// float rho; -// }; - // // class declaration // @@ -183,83 +172,45 @@ class ResidualGlobalCorrectionMakerBase : public edm::stream::EDProducer<> virtual void endStream() override; virtual void beginRun(edm::Run const&, edm::EventSetup const&) override; - //virtual void endRun(edm::Run const&, edm::EventSetup const&) override; - //virtual void beginLuminosityBlock(edm::LuminosityBlock const&, edm::EventSetup const&) override; - //virtual void endLuminosityBlock(edm::LuminosityBlock const&, edm::EventSetup const&) override; GloballyPositioned surfaceToDouble(const Surface &surface) const; - - Matrix localTransportJacobian(const TrajectoryStateOnSurface& start, - const std::pair& propresult, - bool doReverse = false) const; - - Matrix localTransportJacobianAlt(const TrajectoryStateOnSurface& start, - const std::pair& propresult, - bool doReverse = false) const; - - Matrix curv2localTransportJacobian(const FreeTrajectoryState& start, - const std::pair& propresult, - bool doReverse = false) const; - - Matrix curv2curvTransportJacobian(const FreeTrajectoryState& start, - const std::pair& propresult, - bool doReverse = false) const; - - Matrix curvtransportJacobian(const GlobalTrajectoryParameters& globalSource, - const GlobalTrajectoryParameters& globalDest, - const double& s, - const GlobalVector& bfield) const; - - Matrix bfieldJacobian(const GlobalTrajectoryParameters& globalSource, - const GlobalTrajectoryParameters& globalDest, - const double& s, - const GlobalVector& bfield) const; - - Matrix curv2localJacobianAlt(const TrajectoryStateOnSurface &state) const; - - Matrix curv2localJacobianAlteloss(const TrajectoryStateOnSurface &state, double dEdx, double mass) const; - + + GloballyPositioned surfaceToDouble(const Surface &surface, const Basic3DVector &gz) const; + + void applyAlignment(GloballyPositioned &surface, const DetId &detid) const; + + Matrix globalToLocal(const Matrix &state, const GloballyPositioned &surface) const; + + Matrix localToGlobal(const Matrix &localstate, const GloballyPositioned &surface) const; + Matrix curv2localJacobianAltelossD(const Matrix &state, const MagneticField *field, const GloballyPositioned &surface, double dEdx, double mass, double dBz = 0.) const; - Matrix curv2cartJacobianAlt(const FreeTrajectoryState &state) const; + Matrix curv2localhybridJacobianAltelossD(const Matrix &state, const MagneticField *field, const GloballyPositioned &surface, double dEdx, double mass, double dBz = 0.) const; + + Matrix curv2localJacobianAlignmentD(const Matrix &state, const MagneticField *field, const GloballyPositioned &surface, double dEdx, double mass, double dBz = 0.) const; + Matrix curv2cartJacobianAltD(const Matrix &state) const; - - Matrix hybrid2curvJacobian(const FreeTrajectoryState &state) const; Matrix hybrid2curvJacobianD(const Matrix &state, const MagneticField *field, double dBz = 0.) const; - - Matrix hybrid2localTransportJacobian(const FreeTrajectoryState& start, - const std::pair& propresult) const; - - Matrix hybrid2curvTransportJacobian(const FreeTrajectoryState& start, - const std::pair& propresult) const; - - Matrix hybrid2curvTransportJacobian(const GlobalTrajectoryParameters& globalSource, - const GlobalTrajectoryParameters& globalDest, - const double& s, - const GlobalVector& bfield) const; - - Matrix hybrid2curvTransportJacobianVar(const GlobalTrajectoryParameters& globalSource, - const GlobalTrajectoryParameters& globalDest, - const double& s, - const GlobalVector& bfield) const; - - AlgebraicVector5 localMSConvolution(const TrajectoryStateOnSurface& tsos, const MaterialEffectsUpdator& updator) const; - - Matrix materialEffectsJacobian(const TrajectoryStateOnSurface& tsos, const MaterialEffectsUpdator& updator); - - Matrix materialEffectsJacobianVar(const TrajectoryStateOnSurface& tsos, const MaterialEffectsUpdator& updator); - - std::array, 5> processNoiseJacobians(const TrajectoryStateOnSurface& tsos, const MaterialEffectsUpdator& updator) const; - - Matrix localPositionConvolution(const TrajectoryStateOnSurface& tsos, const Matrix &curvcov) const; - - Matrix localPositionConvolutionD(const Matrix& state, const Matrix &curvcov, const GloballyPositioned &surface) const; - template - AlgebraicVector5 lupdate(const TrajectoryStateOnSurface& tsos, const TrackingRecHit& aRecHit); + Matrix pca2cart(const Matrix &statepca, const reco::BeamSpot &bs) const; + + Matrix cart2pca(const Matrix &state, const reco::BeamSpot &bs) const; - AlgebraicVector5 update(const TrajectoryStateOnSurface& tsos, const TrackingRecHit& aRecHit); + Matrix pca2curvJacobianD(const Matrix &state, const MagneticField *field, const reco::BeamSpot &bs, double dBz = 0.) const; + + Matrix pca2cartJacobianD(const Matrix &state, const reco::BeamSpot &bs) const; + + std::array, 2> twoTrackPca2cart(const Matrix &statepca) const; + + Matrix twoTrackCart2pca(const Matrix &state0, const Matrix &state1) const; + + Matrix twoTrackPca2curvJacobianD(const Matrix &state0, const Matrix &state1, const MagneticField *field, double dBz0 = 0., double dBz1 = 0.) const; + + +// Matrix twoTrackCart2pcaJacobianD(const Matrix &state0, const Matrix &state 1, const MagneticField *field, const reco::BeamSpot &bs, double dBz0 = 0., double dBz1 = 0.); + + Matrix localPositionConvolutionD(const Matrix& state, const Matrix &curvcov, const GloballyPositioned &surface) const; template void init_twice_active_var(T &ad, const unsigned int d_num, const unsigned int idx) const; @@ -267,36 +218,26 @@ class ResidualGlobalCorrectionMakerBase : public edm::stream::EDProducer<> template void init_twice_active_null(T &ad, const unsigned int d_num) const; -// Matrix vertexToCurvilinearJacobian(const FreeTrajectoryState &state) const; - Matrix cartesianToCartesianJacobian(const FreeTrajectoryState &state) const; - - Matrix massJacobianAlt(const FreeTrajectoryState &state0, const FreeTrajectoryState &state1, double dmass) const; Matrix massJacobianAltD(const Matrix &state0, const Matrix &state1, double dmass) const; - Matrix massJacobianInvSq(const FreeTrajectoryState &state0, const FreeTrajectoryState &state1, double dmass) const; - - Matrix mrJacobian(const FreeTrajectoryState &state0, const FreeTrajectoryState &state1, double dmass) const; - - Matrix elossAdHocJacobian(const FreeTrajectoryState &state, double mass) const; - Matrix elossAdHocJacobianD(const Matrix &state, double mass) const; - + Matrix massHessianAltD(const Matrix &state0, const Matrix &state1, double dmass) const; -// Point3DBase toGlobal(const Surface &surface, const Point3DBase &lp) const; -// -// Vector3DBase toGlobal(const Surface &surface, const Vector3DBase &lv) const; -// -// Point3DBase toLocal(const Surface &surface, const Point3DBase &gp) const; -// -// Vector3DBase toLocal(const Surface &surface, const Vector3DBase &gv) const; + Matrix massinvsqJacobianAltD(const Matrix &state0, const Matrix &state1, double dmass) const; + Matrix massinvsqHessianAltD(const Matrix &state0, const Matrix &state1, double dmass) const; + // ----------member data --------------------------- edm::EDGetTokenT> inputTraj_; // edm::EDGetTokenT> GenParticlesToken_; edm::EDGetTokenT> GenParticlesToken_; + edm::EDGetTokenT genXyz0Token_; edm::EDGetTokenT genEventInfoToken_; edm::EDGetTokenT> genParticlesBarcodeToken_; // edm::EDGetTokenT inputTrack_; + + edm::EDGetTokenT> pileupSummaryToken_; + edm::EDGetTokenT inputTrack_; edm::EDGetTokenT inputTrackOrig_; edm::EDGetTokenT > inputIndices_; @@ -312,7 +253,15 @@ class ResidualGlobalCorrectionMakerBase : public edm::stream::EDProducer<> edm::EDGetTokenT>> inputMuonAssoc_; bool doMuonAssoc_; - std::string corFile_; + edm::EDGetTokenT inputTriggerResults_; + bool doTrigger_; + edm::ParameterSetID triggerNamesId_; + std::vector triggers_; + std::vector triggerIdxs_; + std::vector triggerDecisions_; + + + std::vector corFiles_; // SiStripClusterInfo siStripClusterInfo_; @@ -396,6 +345,8 @@ class ResidualGlobalCorrectionMakerBase : public edm::stream::EDProducer<> std::vector clusterSizeX; std::vector clusterSizeY; std::vector clusterCharge; + + std::vector stripsToEdge; std::vector clusterChargeBin; std::vector clusterOnEdge; @@ -406,6 +357,20 @@ class ResidualGlobalCorrectionMakerBase : public edm::stream::EDProducer<> std::vector dxreccluster; std::vector dyreccluster; + std::vector simlocalqop; + std::vector simlocaldxdz; + std::vector simlocaldydz; + std::vector simlocalx; + std::vector simlocaly; + + std::vector simlocalqopprop; + std::vector simlocaldxdzprop; + std::vector simlocaldydzprop; + std::vector simlocalxprop; + std::vector simlocalyprop; + + std::vector landauDelta; + std::vector landauW; std::vector localqop; std::vector localdxdz; @@ -413,13 +378,40 @@ class ResidualGlobalCorrectionMakerBase : public edm::stream::EDProducer<> std::vector localx; std::vector localy; + std::vector localqoperr; + std::vector localdxdzerr; + std::vector localdydzerr; + std::vector localxerr; + std::vector localyerr; + + std::vector hitlocalx; + std::vector hitlocaly; + + std::vector localqop_iter; + std::vector localdxdz_iter; + std::vector localdydz_iter; + std::vector localx_iter; + std::vector localy_iter; + + std::vector dxrecgen_iter; + std::vector dyrecgen_iter; + + std::vector localqoperralt; + + std::vector localphi; + std::vector hitphi; + std::map, unsigned int> detidparms; std::vector> detidparmsrev; std::map> detidlayermap; std::map> surfacemap_; std::map> surfacemapD_; - std::vector corparms_; + std::map> surfacemapIdealD_; + std::map> rgluemap_; + + std::vector> corparmsIncremental_; + std::vector corparms_; unsigned int run; unsigned int lumi; @@ -430,20 +422,27 @@ class ResidualGlobalCorrectionMakerBase : public edm::stream::EDProducer<> std::unordered_map, double> hessaggsparse; bool fitFromGenParms_; + bool fitFromSimParms_; bool fillTrackTree_; bool fillGrads_; + bool fillJac_; bool fillRunTree_; + bool alignGlued_ = true; bool debugprintout_; bool doGen_; bool doSim_; bool doMuons_; + bool requireGen_; bool bsConstraint_; bool applyHitQuality_; + bool doRes_ = false; + bool useIdealGeometry_ = false; + float dxpxb1; float dypxb1; @@ -481,6 +480,7 @@ class ResidualGlobalCorrectionMakerBase : public edm::stream::EDProducer<> std::vector deigy; float edmval; + float edmvalref; float deltachisqval; unsigned int niter; @@ -489,53 +489,21 @@ class ResidualGlobalCorrectionMakerBase : public edm::stream::EDProducer<> float genweight; + int Pileup_nPU = 0; + float Pileup_nTrueInt = 0.; + + float genl3d = -99.; + + std::vector gradchisqv; + TH2D *hetaphi = nullptr; + + std::string outprefix; // bool filledRunTree_; }; -template -AlgebraicVector5 ResidualGlobalCorrectionMakerBase::lupdate(const TrajectoryStateOnSurface& tsos, const TrackingRecHit& aRecHit) { - typedef typename AlgebraicROOTObject::Matrix MatD5; - typedef typename AlgebraicROOTObject<5, D>::Matrix Mat5D; - typedef typename AlgebraicROOTObject::SymMatrix SMatDD; - typedef typename AlgebraicROOTObject::Vector VecD; - using ROOT::Math::SMatrixNoInit; - - auto&& x = tsos.localParameters().vector(); - auto&& C = tsos.localError().matrix(); - - // projection matrix (assume element of "H" to be just 0 or 1) - ProjectMatrix pf; - - // Measurement matrix - VecD r, rMeas; - SMatDD V(SMatrixNoInit{}), VMeas(SMatrixNoInit{}); - - KfComponentsHolder holder; - holder.template setup(&r, &V, &pf, &rMeas, &VMeas, x, C); - aRecHit.getKfComponents(holder); - - r -= rMeas; - - // and covariance matrix of residuals - SMatDD R = V + VMeas; - bool ok = invertPosDefMatrix(R); - if (!ok) { - return AlgebraicVector5(); - } - - // Compute Kalman gain matrix - AlgebraicMatrix55 M = AlgebraicMatrixID(); - Mat5D K = C * pf.project(R); - pf.projectAndSubtractFrom(M, K); - - // Compute local filtered state vector - AlgebraicVector5 fsvdiff = K * r; - return fsvdiff; -} - template void ResidualGlobalCorrectionMakerBase::init_twice_active_var(T &ad, const unsigned int d_num, const unsigned int idx) const { // initialize derivative direction in value field of outer active variable diff --git a/Analysis/HitAnalyzer/plugins/ResidualGlobalCorrectionMakerG4e.cc b/Analysis/HitAnalyzer/plugins/ResidualGlobalCorrectionMakerG4e.cc index e44c9bfe1c1cd..cf4ab37ef721e 100644 --- a/Analysis/HitAnalyzer/plugins/ResidualGlobalCorrectionMakerG4e.cc +++ b/Analysis/HitAnalyzer/plugins/ResidualGlobalCorrectionMakerG4e.cc @@ -1,5 +1,4 @@ #include "ResidualGlobalCorrectionMakerBase.h" -#include "MagneticFieldOffset.h" #include "DataFormats/MuonReco/interface/Muon.h" #include "TrackPropagation/Geant4e/interface/Geant4ePropagator.h" @@ -11,6 +10,12 @@ #include +#include "TRandom.h" + +#include "Geometry/CommonTopologies/interface/TrapezoidalStripTopology.h" + + + class ResidualGlobalCorrectionMakerG4e : public ResidualGlobalCorrectionMakerBase { public: @@ -26,6 +31,8 @@ class ResidualGlobalCorrectionMakerG4e : public ResidualGlobalCorrectionMakerBas edm::EDGetTokenT> inputAssoc_; + bool trackHighPurity = false; + float muonPt; bool muonLoose; bool muonMedium; @@ -52,6 +59,8 @@ class ResidualGlobalCorrectionMakerG4e : public ResidualGlobalCorrectionMakerBas float outEtaStart; float outPhiStart; + float trackQopErr = 0.; + edm::EDPutTokenT> outputCorPt_; edm::EDPutTokenT> outputCorEta_; edm::EDPutTokenT> outputCorPhi_; @@ -96,12 +105,15 @@ void ResidualGlobalCorrectionMakerG4e::beginStream(edm::StreamID streamid) tree->Branch("trackEta", &trackEta, basketSize); tree->Branch("trackPhi", &trackPhi, basketSize); tree->Branch("trackCharge", &trackCharge, basketSize); + tree->Branch("trackQopErr", &trackQopErr); //workaround for older ROOT version inability to store std::array automatically // tree->Branch("trackOrigParms", trackOrigParms.data(), "trackOrigParms[5]/F", basketSize); // tree->Branch("trackOrigCov", trackOrigCov.data(), "trackOrigCov[25]/F", basketSize); tree->Branch("trackParms", trackParms.data(), "trackParms[5]/F", basketSize); tree->Branch("trackCov", trackCov.data(), "trackCov[25]/F", basketSize); + tree->Branch("trackHighPurity", &trackHighPurity); + tree->Branch("refParms_iter0", refParms_iter0.data(), "refParms_iter0[5]/F", basketSize); tree->Branch("refCov_iter0", refCov_iter0.data(), "refCov_iter0[25]/F", basketSize); // tree->Branch("refParms_iter2", refParms_iter2.data(), "refParms_iter2[5]/F", basketSize); @@ -125,12 +137,14 @@ void ResidualGlobalCorrectionMakerG4e::beginStream(edm::StreamID streamid) tree->Branch("nHits", &nHits, basketSize); tree->Branch("nValidHits", &nValidHits, basketSize); tree->Branch("nValidPixelHits", &nValidPixelHits, basketSize); - tree->Branch("nJacRef", &nJacRef, basketSize); tree->Branch("nValidHitsFinal", &nValidHitsFinal); tree->Branch("nValidPixelHitsFinal", &nValidPixelHitsFinal); - - tree->Branch("jacrefv",jacrefv.data(),"jacrefv[nJacRef]/F", basketSize); + + if (fillJac_) { + tree->Branch("nJacRef", &nJacRef, basketSize); + tree->Branch("jacrefv",jacrefv.data(),"jacrefv[nJacRef]/F", basketSize); + } tree->Branch("dEpred", &dEpred); tree->Branch("dE", &dE); @@ -161,26 +175,6 @@ void ResidualGlobalCorrectionMakerG4e::beginStream(edm::StreamID streamid) tree->Branch("trackExtraAssoc", &trackExtraAssoc); if (fitFromGenParms_) { - - // tree->Branch("dxpxb1", &dxpxb1); - // tree->Branch("dypxb1", &dypxb1); - // - // tree->Branch("dxttec9rphi", &dxttec9rphi); - // tree->Branch("dxttec9stereo", &dxttec9stereo); - // - // tree->Branch("dxttec4rphi", &dxttec4rphi); - // tree->Branch("dxttec4stereo", &dxttec4stereo); - // - // tree->Branch("dxttec4rphisimgen", &dxttec4rphisimgen); - // tree->Branch("dyttec4rphisimgen", &dyttec4rphisimgen); - // tree->Branch("dxttec4rphirecsim", &dxttec4rphirecsim); - // - // tree->Branch("dxttec9rphisimgen", &dxttec9rphisimgen); - // tree->Branch("dyttec9rphisimgen", &dyttec9rphisimgen); - // - // tree->Branch("simlocalxref", &simlocalxref); - // tree->Branch("simlocalyref", &simlocalyref); - tree->Branch("hitidxv", &hitidxv); tree->Branch("dxrecgen", &dxrecgen); tree->Branch("dyrecgen", &dyrecgen); @@ -204,9 +198,26 @@ void ResidualGlobalCorrectionMakerG4e::beginStream(edm::StreamID streamid) tree->Branch("clusterProbXY", &clusterProbXY); tree->Branch("clusterSN", &clusterSN); + + tree->Branch("stripsToEdge", &stripsToEdge); tree->Branch("dxreccluster", &dxreccluster); tree->Branch("dyreccluster", &dyreccluster); + + tree->Branch("simlocalqop", &simlocalqop); + tree->Branch("simlocaldxdz", &simlocaldxdz); + tree->Branch("simlocaldydz", &simlocaldydz); + tree->Branch("simlocalx", &simlocalx); + tree->Branch("simlocaly", &simlocaly); + + tree->Branch("simlocalqopprop", &simlocalqopprop); + tree->Branch("simlocaldxdzprop", &simlocaldxdzprop); + tree->Branch("simlocaldydzprop", &simlocaldydzprop); + tree->Branch("simlocalxprop", &simlocalxprop); + tree->Branch("simlocalyprop", &simlocalyprop); + + tree->Branch("landauDelta", &landauDelta); + tree->Branch("landauW", &landauW); tree->Branch("localqop", &localqop); tree->Branch("localdxdz", &localdxdz); @@ -214,6 +225,30 @@ void ResidualGlobalCorrectionMakerG4e::beginStream(edm::StreamID streamid) tree->Branch("localx", &localx); tree->Branch("localy", &localy); + tree->Branch("localqoperr", &localqoperr); + tree->Branch("localdxdzerr", &localdxdzerr); + tree->Branch("localdydzerr", &localdydzerr); + tree->Branch("localxerr", &localxerr); + tree->Branch("localyerr", &localyerr); + + tree->Branch("hitlocalx", &hitlocalx); + tree->Branch("hitlocaly", &hitlocaly); + + tree->Branch("localqop_iter", &localqop_iter); + tree->Branch("localdxdz_iter", &localdxdz_iter); + tree->Branch("localdydz_iter", &localdydz_iter); + tree->Branch("localx_iter", &localx_iter); + tree->Branch("localy_iter", &localy_iter); + + tree->Branch("dxrecgen_iter", &dxrecgen_iter); + tree->Branch("dyrecgen_iter", &dyrecgen_iter); + + tree->Branch("localqoperralt", &localqoperralt); + + tree->Branch("localphi", &localphi); + tree->Branch("hitphi", &hitphi); + + tree->Branch("simtestz", &simtestz); tree->Branch("simtestvz", &simtestvz); tree->Branch("simtestrho", &simtestrho); @@ -244,114 +279,49 @@ void ResidualGlobalCorrectionMakerG4e::produce(edm::Event &iEvent, const edm::Ev { const bool dogen = fitFromGenParms_; + const bool dolocalupdate = fitFromSimParms_; + // const bool dolocalupdate = true; - const bool dolocalupdate = true; - + const bool dores = doRes_; using namespace edm; Handle trackOrigH; iEvent.getByToken(inputTrackOrig_, trackOrigH); - - -// bool foundmodule = false; -// for (const reco::Track &track : *trackOrigH) { -// if (track.innerDetId() == 302055944) { -// foundmodule = true; -// break; -// } -// // for (auto it = track.recHitsBegin(); it != track.recHitsEnd(); ++it) { -// // if ((*it)->geographicalId().rawId() == 302055944) { -// // foundmodule = true; -// // break; -// // } -// // } -// // if (foundmodule) { -// // break; -// // } -// } -// if (!foundmodule) { -// // printf("not found, returning\n"); -// return; -// } - - - - // loop over gen particles edm::ESHandle globalGeometry; iSetup.get().get(globalGeometry); - -// edm::ESHandle globalGeometry; -// iSetup.get().get("idealForDigi", globalGeometry); - + edm::ESHandle trackerTopology; iSetup.get().get(trackerTopology); -// ESHandle magfield; -// iSetup.get().get(magfield); -// auto field = magfield.product(); - edm::ESHandle ttrh; iSetup.get().get("WithAngleAndTemplate",ttrh); ESHandle thePropagator; -// iSetup.get().get("RungeKuttaTrackerPropagator", thePropagator); -// iSetup.get().get("PropagatorWithMaterial", thePropagator); -// iSetup.get().get("PropagatorWithMaterialParabolicMf", thePropagator); iSetup.get().get("Geant4ePropagator", thePropagator); const Geant4ePropagator *g4prop = dynamic_cast(thePropagator.product()); const MagneticField* field = thePropagator->magneticField(); -// edm::ESHandle fit; -// iSetup.get().get("G4eFitterSmoother", fit); -// const KFTrajectoryFitter *kffit = dynamic_cast(fit.product()); -// const Propagator *thePropagator = kffit->propagator(); - - -// ESHandle theAnalyticPropagator; -// iSetup.get().get("PropagatorWithMaterial", theAnalyticPropagator); - - ESHandle fieldh; - iSetup.get().get("", fieldh); - std::unique_ptr fieldOffset = std::make_unique(&(*fieldh)); - - std::unique_ptr fPropagator = std::make_unique(alongMomentum, 0.105, fieldOffset.get(), 1.6, true, -1., true); - -// const MagneticField* field = fPropagator->magneticField(); - constexpr double mmu = 0.1056583745; - -// Handle trackH; -// Handle trackH; -// iEvent.getByToken(inputTrack_, trackH); - - - -// Handle > indicesH; -// iEvent.getByToken(inputIndices_, indicesH); - -// Handle > trajH; -// iEvent.getByToken(inputTraj_, trajH); - - - Handle bsH; iEvent.getByToken(inputBs_, bsH); -// Handle> genPartCollection; Handle> genPartCollection; + Handle genXyz0; Handle genEventInfo; Handle> genPartBarcodes; + Handle> pileupSummary; if (doGen_) { iEvent.getByToken(GenParticlesToken_, genPartCollection); + iEvent.getByToken(genXyz0Token_, genXyz0); iEvent.getByToken(genEventInfoToken_, genEventInfo); + iEvent.getByToken(pileupSummaryToken_, pileupSummary); } -// Handle> tecSimHits; std::vector>> simHits(inputSimHits_.size()); edm::Handle> simTracks; if (doSim_) { @@ -362,7 +332,6 @@ void ResidualGlobalCorrectionMakerG4e::produce(edm::Event &iEvent, const edm::Ev iEvent.getByToken(inputSimTracks_, simTracks); } -// Handle muons; Handle > muons; if (doMuons_) { iEvent.getByToken(inputMuons_, muons); @@ -372,431 +341,9 @@ void ResidualGlobalCorrectionMakerG4e::produce(edm::Event &iEvent, const edm::Ev if (doMuonAssoc_) { iEvent.getByToken(inputMuonAssoc_, muonAssoc); } - -// Handle> assoc; -// if (doMuons_) { -// iEvent.getByToken(inputAssoc_, assoc); -// } - -// if (doSim_) { -// iEvent.getByToken(inputSimHits_, tecSimHits); -// } - -// const float mass = 0.105; -// const float maxDPhi = 1.6; -// PropagatorWithMaterial rPropagator(oppositeToMomentum, mass, field, maxDPhi, true, -1., false); -// PropagatorWithMaterial fPropagator(alongMomentum, mass, field, maxDPhi, true, -1., false); -// std::unique_ptr fPropagator(static_cast(thePropagator->clone())); -// fPropagator->setPropagationDirection(alongMomentum); -// -// std::unique_ptr fAnalyticPropagator(static_cast(theAnalyticPropagator->clone())); -// fAnalyticPropagator->setPropagationDirection(alongMomentum); - - KFUpdator updator; TkClonerImpl const& cloner = static_cast(ttrh.product())->cloner(); - -// siStripClusterInfo_.initEvent(iSetup); - -// edm::ESHandle globalPositionRcd; -// iSetup.get().get(globalPositionRcd); -// -// printf("globalPositionRcd translation = %e, %e, %e\n", globalPositionRcd->m_align.front().translation().x(), -// globalPositionRcd->m_align.front().translation().y(), -// globalPositionRcd->m_align.front().translation().z()); -// std::cout << "globalPositionRcd rotation" << globalPositionRcd->m_align.front().rotation() << std::endl; - - // set up cylindrical surface for beam pipe -// const double ABe = 9.0121831; -// const double ZBe = 4.; -// const double K = 0.307075*1e-3; -// const double dr = 0.08; -// // const double xibeampipe = 0.5*K*dr*ZBe/ABe; -// const double xibeampipe = 0.*0.5*K*dr*ZBe/ABe; - - - - -// auto beampipe = Cylinder::build(Surface::PositionType(0.,0.,0.), Surface::RotationType(), 2.94); -// beampipe->setMediumProperties(MediumProperties(0., xibeampipe)); - -// std::cout << "xi beampipe: " << xibeampipe << std::endl; - -// const GeomDet *testdet = nullptr; -// //debugging -// for (const GeomDet* det : globalGeometry->detUnits()) { -// if (!det) { -// continue; -// } -// -// if (det->subDetector() == GeomDetEnumerators::TEC) { -// const DetId& detid = det->geographicalId(); -// // TECDetId detid(det->geographicalId()); -// // layer = -1 * (detid.side() == 1) * detid.wheel() + (detid.side() == 2) * detid.wheel(); -// unsigned int side = trackerTopology->tecSide(detid); -// unsigned int wheel = trackerTopology->tecWheel(detid); -// int layer = -1 * (side == 1) * wheel + (side == 2) * wheel; -// bool stereo = trackerTopology->isStereo(det->geographicalId()); -// -// if (layer == -9) { -// testdet = det; -// break; -// -// } -// } -// -// -// -// } -// -// if (testdet) { -// const GlobalPoint center = testdet->surface().toGlobal(LocalPoint(1.,0.)); -// -// const GlobalVector centerv(center.x(), center.y(), center.z()); -// const GlobalVector dir = centerv/centerv.mag(); -// const double sintheta = dir.perp(); -// const GlobalVector mom = (100000./sintheta)*dir; -// const GlobalPoint pos(0.,0.,0.); -// -// FreeTrajectoryState ftsplus(pos, mom, 1., field); -// FreeTrajectoryState ftsminus(pos, mom, -1., field); -// -// const TrajectoryStateOnSurface tsosplus = fPropagator->propagate(ftsplus, testdet->surface()); -// const TrajectoryStateOnSurface tsosminus = fPropagator->propagate(ftsminus, testdet->surface()); -// -// std::cout << "global target" << std::endl; -// std::cout << center << std::endl; -// -// std::cout << "momentum" << std::endl; -// std::cout << mom << std::endl; -// -// std::cout << "tsosplus local:" << std::endl; -// std::cout << tsosplus.localPosition() << std::endl; -// std::cout << "tsosminus local:" << std::endl; -// std::cout << tsosminus.localPosition() << std::endl; -// -// std::cout << "delta local" << std::endl; -// std::cout << tsosplus.localPosition() - tsosminus.localPosition() << std::endl; -// -// } - - - simtestz = -99.; - simtestvz = -99.; - simtestrho = -99.; - simtestzlocalref = -99.; - simtestdx = -99.; - simtestdxrec = -99.; - simtestdy = -99.; - simtestdyrec = -99.; - simtestdxprop = -99.; - simtestdyprop = -99.; - simtestdetid = 0; - - if (false) { - - //sim hit debugging - const reco::Candidate* genmuon = nullptr; - for (const reco::Candidate& genPart : *genPartCollection) { - if (genPart.status()==1 && std::abs(genPart.pdgId()) == 13) { - genmuon = &genPart; - break; - } - } - - if (genmuon) { - genPt = genmuon->pt(); - genCharge = genmuon->charge(); - genEta = genmuon->eta(); - genPhi = genmuon->phi(); - - simtestvz = genmuon->vertex().z(); - - auto const& refpoint = genmuon->vertex(); - auto const& trackmom = genmuon->momentum(); - const GlobalPoint refpos(refpoint.x(), refpoint.y(), refpoint.z()); - const GlobalVector refmom(trackmom.x(), trackmom.y(), trackmom.z()); - // const GlobalTrajectoryParameters refglobal(refpos, refmom, genmuon->charge(), field); - - // std::cout << "gen ref state" << std::endl; - // std::cout << refpos << std::endl; - // std::cout << refmom << std::endl; - // std::cout << genpart->charge() << std::endl; - - //zero uncertainty on generated parameters - // AlgebraicSymMatrix55 nullerr; - // const CurvilinearTrajectoryError referr(nullerr); - - const FreeTrajectoryState fts = FreeTrajectoryState(refpos, refmom, genmuon->charge(), field); - - std::cout << "gen muon charge: " << genmuon->charge() << std::endl; - - TrajectoryStateOnSurface tsos; - - std::vector simhitsflat; - for (auto const& simhith : simHits) { - for (const PSimHit& simHit : *simhith) { - simhitsflat.push_back(&simHit); - } - } - - auto simhitcompare = [&](const PSimHit *hit0, const PSimHit *hit1) { - return globalGeometry->idToDet(hit0->detUnitId())->surface().toGlobal(hit0->localPosition()).mag() < globalGeometry->idToDet(hit1->detUnitId())->surface().toGlobal(hit1->localPosition()).mag(); - }; - - std::sort(simhitsflat.begin(), simhitsflat.end(), simhitcompare); - - unsigned int ihit = 0; - for (auto const& simHitp : simhitsflat) { - auto const &simHit = *simHitp; - if (std::abs(simHit.particleType()) != 13) { - continue; - } - -// if (std::abs(simHit.localPosition().z()) > 1e-9) { -// continue; -// } - -// for (auto const& simhith : simHits) { -// for (const PSimHit& simHit : *simhith) { - - const GeomDet *detectorG = globalGeometry->idToDet(simHit.detUnitId()); - - bool isbarrel = detectorG->subDetector() == GeomDetEnumerators::PixelBarrel || detectorG->subDetector() == GeomDetEnumerators::TIB || detectorG->subDetector() == GeomDetEnumerators::TOB; - - float absz = std::abs(detectorG->surface().toGlobal(LocalVector(0.,0.,1.)).z()); - - bool idealdisk = absz == 1.; - bool idealbarrel = absz<1e-9; - - // idealdisk = false; - // idealbarrel = false; - - bool isstereo = trackerTopology->isStereo(simHit.detUnitId()); - - std::cout << "isbarrel: " << isbarrel << " idealbarrel: " << idealbarrel << " idealdisk: " << idealdisk << "stereo: " << isstereo << " globalpos: " << detectorG->surface().position() << std::endl; - - LocalPoint proplocal(0.,0.,0.); - -// auto const propresult = fPropagator->geometricalPropagator().propagateWithPath(fts, detectorG->surface()); -// if (propresult.first.isValid()) { -// proplocal = propresult.first.localPosition(); -// } - - if (!tsos.isValid()) { - tsos = fPropagator->geometricalPropagator().propagate(fts, detectorG->surface()); - } - else { - tsos = fPropagator->geometricalPropagator().propagate(tsos, detectorG->surface()); - } - - if (tsos.isValid()) { - proplocal = tsos.localPosition(); - } - - - - - // Vector3d refprop; - - // LocalTrajectoryParameters - Point3DBase reflocal(0, 0., 0.); - - simtestz = detectorG->surface().position().z(); - simtestrho = detectorG->surface().position().perp(); - - - GlobalPoint refglobal; - - auto const simhitglobal = detectorG->surface().toGlobal(Point3DBase(simHit.localPosition().x(), - simHit.localPosition().y(), - simHit.localPosition().z())); - - const Vector3d Msim(simhitglobal.x(), simhitglobal.y(), simhitglobal.z()); - - auto const propglobal = detectorG->surface().toGlobal(Point3DBase(proplocal.x(), - proplocal.y(), - proplocal.z())); - - - const Vector3d Mprop(propglobal.x(), propglobal.y(), propglobal.z()); - - Vector3d M(genmuon->vertex().x(), - genmuon->vertex().y(), - genmuon->vertex().z()); - - Vector3d P(genmuon->momentum().x(), - genmuon->momentum().y(), - genmuon->momentum().z()); - - - - - // if (true) { - for (unsigned int iref=0; iref<1; ++iref) { - const double zs = detectorG->surface().position().z(); - - const Vector3d T0 = P.normalized(); - - // const Vector3d T0 = P.normalized(); - - const Vector3d H(0.,0.,1.); - - const double rho = fts.transverseCurvature(); - - double s; - - if (idealdisk) { - s = (zs - M[2])/T0[2]; - } - else if (idealbarrel) { - HelixBarrelPlaneCrossingByCircle crossing(GlobalPoint(M[0],M[1],M[2]), GlobalVector(P[0],P[1],P[2]), rho); - s = crossing.pathLength(detectorG->surface()).second; - } - else { - HelixArbitraryPlaneCrossing crossing(Basic3DVector(M[0],M[1],M[2]), Basic3DVector(P[0],P[1],P[2]), rho); - s = crossing.pathLength(detectorG->surface()).second; - // s = propresult.second; - } - - const Vector3d HcrossT = H.cross(T0); - const double alpha = HcrossT.norm(); - const Vector3d N0 = HcrossT.normalized(); - - const double gamma = T0[2]; - const double q = genmuon->charge(); - const double Q = -3.8*2.99792458e-3*q/P.norm(); - const double theta = Q*s; - - const Vector3d dM = gamma*(theta-std::sin(theta))/Q*H + std::sin(theta)/Q*T0 + alpha*(1.-std::cos(theta))/Q*N0; - M = M + dM; - const Vector3d dT = gamma*(1.-std::cos(theta))*H + std::cos(theta)*T0 + alpha*std::sin(theta)*N0; - const Vector3d T = T0 + dT; - const double pmag = P.norm(); - P = pmag*T; - - refglobal = GlobalPoint(M[0], M[1], M[2]); - reflocal = detectorG->surface().toLocal(Point3DBase(M[0], M[1], M[2])); - simtestzlocalref = reflocal.z(); - - const Vector3d xhat = Vector3d(0.,0.,1.).cross(M).normalized(); - -// const LocalVector localxhat = LocalVector(1.,0.,0.); -// const GlobalVector globalxhat = detectorG->surface().toGlobal(localxhat); -// const Vector3d xhat(globalxhat.x(), globalxhat.y(), globalxhat.z()); - - const double dx = xhat.dot(Msim-M); - const double dxrec = xhat.dot(Mprop - Msim); - - const Vector3d yhat = Vector3d(0.,0.,1.).cross(xhat).normalized(); - - const double dy = yhat.dot(Msim-M); - const double dyrec = yhat.dot(Mprop - Msim); - - simtestdx = dx; -// simtestdxrec = dxrec; - simtestdxrec = proplocal.x() - simHit.localPosition().x(); - simtestdxprop = xhat.dot(Mprop-M); - - simtestdy = dy; -// simtestdyrec = dyrec; - simtestdyrec = proplocal.y() - simHit.localPosition().y(); - simtestdyprop = yhat.dot(Mprop-M); - - simtestdetid = detectorG->geographicalId().rawId(); - - if (idealdisk) { - break; - } - - // refprop = M; - - // const Vector3d Mprop(updtsosnomat.globalPosition().x(), - // updtsosnomat.globalPosition().y(), - // updtsosnomat.globalPosition().z()); - } - - tree->Fill(); - // else { - // const TrajectoryStateOnSurface propresult = fAnalyticPropagator->geometricalPropagator().propagate(fts, detectorG->surface()); - // if (propresult.isValid()) { - // reflocal = propresult.localPosition(); - // // refprop << propresult.globalPosition().x(), propresult.globalPosition().y(), propresult.globalPosition().z(); - // } - // // else { - // // refprop << 0., 0., 0.; - // // } - // } - - - // const LocalPoint reflocal = detectorG->surface().toLocal(GlobalPoint(refprop[0], refprop[1], refprop[2])); - - - - const LocalPoint simlocal = simHit.localPosition(); - -// std::cout << "isbarrel: " << isbarrel << " idealbarrel: " << idealbarrel << " idealdisk: " << idealdisk << "stereo: " << isstereo << " globalpos: " << detectorG->surface().position() << std::endl; - std::cout << "detid: " << simHit.detUnitId() << std::endl; - std::cout << "local z to global: " << detectorG->surface().toGlobal(LocalVector(0.,0.,1.)) << std::endl; - std::cout << "ref : " << reflocal << std::endl; - std::cout << "proplocal: " << proplocal << std::endl; - std::cout << "simlocal: " << simlocal << std::endl; - std::cout << "sim entry point: " << simHit.entryPoint() << std::endl; - std::cout << "sim exit point: " << simHit.exitPoint() << std::endl; - std::cout << "refglobal: " << refglobal << std::endl; - std::cout << "propglobal: " << propglobal << std::endl; - std::cout << "simglobal: " << simhitglobal << std::endl; - std::cout << "sim-ref : " << simlocal - reflocal << std::endl; - std::cout << "sim-ref (global): " << simhitglobal - refglobal << std::endl; - std::cout << "prop-ref: " << proplocal - reflocal << std::endl; - std::cout << "sim-prop: " << simlocal - proplocal << std::endl; - std::cout << "sim-prop (global): " << simhitglobal - propglobal << std::endl; - - - std::cout << "simtestdx: " << simtestdx << std::endl; - std::cout << "simtestdy: " << simtestdy << std::endl; - - std::cout << "simtestdxrec: " << simtestdxrec << std::endl; - std::cout << "simtestdyrec: " << simtestdyrec << std::endl; - - std::cout << "simtestdxprop: " << simtestdxprop << std::endl; - std::cout << "simtestdyprop: " << simtestdyprop << std::endl; - -// assert(std::abs(simtestdy)<0.1e-4); - -// assert(simHit.entryPoint().z()*simHit.exitPoint().z() < 0.); - -// assert(std::abs(simlocal.z())<1e-4); - -// assert(std::abs(simtestdxrec) < 40e-4); -// assert(simtestdxprop > -950e-4); - - ++ihit; - - // if (simHit.detUnitId() == preciseHit->geographicalId()) { - // dxsimgen.push_back(simHit.localPosition().x() - updtsos.localPosition().x()); - // dysimgen.push_back(simHit.localPosition().y() - updtsos.localPosition().y()); - // - // dxrecsim.push_back(preciseHit->localPosition().x() - simHit.localPosition().x()); - // dyrecsim.push_back(-99.); - // - // simvalid = true; - // break; - // } -// } - } - - - } - return; - - } - -// TkClonerImpl hitCloner; -// TKCloner const* cloner = static_cast(builder)->cloner() -// TrajectoryStateCombiner combiner; - run = iEvent.run(); lumi = iEvent.luminosityBlock(); event = iEvent.id().event(); @@ -804,9 +351,10 @@ void ResidualGlobalCorrectionMakerG4e::produce(edm::Event &iEvent, const edm::Ev genweight = 1.; if (doGen_) { genweight = genEventInfo->weight(); - } - + Pileup_nPU = pileupSummary->front().getPU_NumInteractions(); + Pileup_nTrueInt = pileupSummary->front().getTrueNumInteractions(); + } std::vector corPtV; std::vector corEtaV; @@ -838,41 +386,29 @@ void ResidualGlobalCorrectionMakerG4e::produce(edm::Event &iEvent, const edm::Ev const edm::Ref> muonref = doMuonAssoc_ ? (*muonAssoc)[trackref] : edm::Ref>(); -// const Trajectory& traj = (*trajH)[itraj]; + const bool iscosmic = track.algo() == reco::TrackBase::ctf || track.algo() == reco::TrackBase::cosmics; -// const edm::Ref > trajref(trajH, j); -// const reco::Track& track = *(*trackH)[trajref]; -// const reco::Track& track = (*trackH)[itraj]; -// const reco::Track& trackOrig = (*trackOrigH)[(*indicesH)[j]]; - -// std::cout << "j " << j << " (*indicesH)[j] " << (*indicesH)[j] < 5.0) { -// continue; -// } trackPt = track.pt(); trackEta = track.eta(); trackPhi = track.phi(); trackCharge = track.charge(); trackPtErr = track.ptError(); - - + trackQopErr = track.qoverpError(); + +// if (abs(trackEta) > 0.1) { +// continue; +// } + + trackHighPurity = track.quality(reco::TrackBase::highPurity); normalizedChi2 = track.normalizedChi2(); -// std::cout << "track pt: " << trackPt << " track eta: " << trackEta << " track phi: " << trackPhi << " trackCharge: " << trackCharge << " qop: " << track.parameters()[0] << std::endl; - auto const& tkparms = track.parameters(); auto const& tkcov = track.covariance(); trackParms.fill(0.); @@ -893,19 +429,12 @@ void ResidualGlobalCorrectionMakerG4e::produce(edm::Event &iEvent, const edm::Ev genY = -99.; genZ = -99.; genParms.fill(0.); + genl3d = -99.; int genBarcode = -99; if (doGen_) { -// bool isjpsi = false; -// for (std::vector::const_iterator g = genPartCollection->begin(); g != genPartCollection->end(); ++g) -// { -// if (std::abs(g->pdgId()) == 443) { -// isjpsi = true; -// break; -// } -// } float drmin = 0.1; @@ -918,11 +447,7 @@ void ResidualGlobalCorrectionMakerG4e::produce(edm::Event &iEvent, const edm::Ev continue; } -// if (isjpsi && g->charge() != track.charge()) { -// continue; -// } - - float dR = deltaR(g->phi(), trackPhi, g->eta(), trackEta); + float dR = deltaR(*g, track); if (dR < drmin) { @@ -942,7 +467,9 @@ void ResidualGlobalCorrectionMakerG4e::produce(edm::Event &iEvent, const edm::Ev genX = g->vertex().x(); genY = g->vertex().y(); genZ = g->vertex().z(); - + + genl3d = std::sqrt((g->vertex() - *genXyz0).mag2()); + auto const& vtx = g->vertex(); auto const& myBeamSpot = bsH->position(vtx.z()); @@ -966,6 +493,10 @@ void ResidualGlobalCorrectionMakerG4e::produce(edm::Event &iEvent, const edm::Ev // std::cout << "genPt = " << genPt << " genEta = " << genEta << " genPhi = " << genPhi << " genCharge = " << genCharge << " genX = " << genX << " genY = " << genY << " genZ = " << genZ << std::endl; + if (requireGen_ && genpart == nullptr) { + continue; + } + int simtrackid = -99; if (genpart != nullptr && doSim_) { for (auto const& simTrack : *simTracks) { @@ -1001,28 +532,7 @@ void ResidualGlobalCorrectionMakerG4e::produce(edm::Event &iEvent, const edm::Ev matchedmuon = &muon; } } - - -// if (muon.innerTrack() == trackref) { -// muonPt = muon.pt(); -// muonLoose = muon.passed(reco::Muon::CutBasedIdLoose); -// muonMedium = muon.passed(reco::Muon::CutBasedIdMedium); -// muonTight = muon.passed(reco::Muon::CutBasedIdTight); -// muonIsPF = muon.isPFMuon(); -// muonIsTracker = muon.isTrackerMuon(); -// muonIsGlobal = muon.isGlobalMuon(); -// muonIsStandalone = muon.isStandAloneMuon(); -// if (muon.muonBestTrack() == trackref) { -// muonInnerTrackBest = true; -// } -// } } -// if (assoc->contains(track.extra().id())) { -// const reco::TrackExtraRef &trackextraref = (*assoc)[track.extra()]; -// if (trackextraref.isNonnull()) { -// trackExtraAssoc = true; -// } -// } } if (matchedmuon != nullptr) { @@ -1037,23 +547,9 @@ void ResidualGlobalCorrectionMakerG4e::produce(edm::Event &iEvent, const edm::Ev muonInnerTrackBest = matchedmuon->muonBestTrackType() == reco::Muon::InnerTrack; } - - -// PropagationDirection rpropdir = traj.direction(); -// PropagationDirection fpropdir = rpropdir == alongMomentum ? oppositeToMomentum : alongMomentum; - - //TODO properly handle the outside-in case (maybe ok now) -// assert(track.seedDirection() == alongMomentum); - //prepare hits TransientTrackingRecHit::RecHitContainer hits; hits.reserve(track.recHitsSize()); -// hits.reserve(track.recHitsSize()+1); -// hits.push_back(RecHitPointer(InvalidTrackingRecHit()); - -// printf("building hits\n"); - - std::set> hitlayers; // std::cout << "track: algo = " << track.algo() << " originalAlgo = " << track.originalAlgo() << std::endl; // @@ -1084,25 +580,34 @@ void ResidualGlobalCorrectionMakerG4e::produce(edm::Event &iEvent, const edm::Ev // split matched invalid hits if (detglued != nullptr && !(*it)->isValid()) { - bool order = detglued->stereoDet()->surface().position().mag() > detglued->monoDet()->surface().position().mag(); -// if (track.seedDirection() == oppositeToMomentum) { -// order = !order; -// } +// bool order = detglued->stereoDet()->surface().position().mag() > detglued->monoDet()->surface().position().mag(); + + const auto stereopos = detglued->stereoDet()->surface().position(); + const auto monopos = detglued->monoDet()->surface().position(); + + const Eigen::Vector3d stereoposv(stereopos.x(), stereopos.y(), stereopos.z()); + const Eigen::Vector3d monoposv(monopos.x(), monopos.y(), monopos.z()); + const Eigen::Vector3d trackmomv(track.momentum().x(), track.momentum().y(), track.momentum().z()); + + bool order = (stereoposv - monoposv).dot(trackmomv) > 0.; + + const GeomDetUnit* detinner = order ? detglued->monoDet() : detglued->stereoDet(); const GeomDetUnit* detouter = order ? detglued->stereoDet() : detglued->monoDet(); + +// std::cout << "splitting matched hit, inner = " << detinner->geographicalId().rawId() <<" outer = " << detouter->geographicalId().rawId() << std::endl; + if (hits.size() > 0) { +// std::cout << "previous = " << hits.back()->geographicalId().rawId() << std::endl; + const bool duplicate = detinner->geographicalId() == hits.back()->geographicalId() || detouter->geographicalId() == hits.back()->geographicalId(); + + if (duplicate) { + std::cout << "WARNING: Duplicate hits from glued module splitting!\n"; + } + } hits.push_back(TrackingRecHit::RecHitPointer(new InvalidTrackingRecHit(*detinner, (*it)->type()))); hits.push_back(TrackingRecHit::RecHitPointer(new InvalidTrackingRecHit(*detouter, (*it)->type()))); -// auto const& layerinner = detidlayermap.at(detinner->geographicalId()); -// auto const& layerouter = detidlayermap.at(detouter->geographicalId()); - -// if (!hitlayers.count(layerinner)) -// hits.push_back(TrackingRecHit::RecHitPointer(new InvalidTrackingRecHit(*detinner, (*it)->type()))); -// hitlayers.insert(layerinner); -// if (!hitlayers.count(layerouter)) -// hits.push_back(TrackingRecHit::RecHitPointer(new InvalidTrackingRecHit(*detouter, (*it)->type()))); -// hitlayers.insert(layerouter); } else { // apply hit quality criteria @@ -1119,47 +624,23 @@ void ResidualGlobalCorrectionMakerG4e::produce(edm::Event &iEvent, const edm::Ev assert(pixhit != nullptr); hitquality = !pixhit->isOnEdge() && cluster.sizeX() > 1; -// hitquality = !pixhit->isOnEdge() && cluster.sizeX() > 1 && cluster.sizeY() > 1; - - -// hitquality = !pixhit->isOnEdge(); -// hitquality = cluster.sizeX() > 1; - -// hitquality = pixhit->hasFilledProb() && pixhit->clusterProbability(0) > 0.000125 && pixhit->qBin()>0 && pixhit->qBin()<4; - -// if (pixhit->hasFilledProb() && pixhit->clusterProbability(0) > 0.000125 && pixhit->qBin()>0 && pixhit->qBin()<4) { -// if (pixhit->hasFilledProb() && pixhit->clusterProbability(0) > 0.000125 && pixhit->qBin()>0 && pixhit->qBin()<3) { -// hitquality = true; -// } +// hitquality = false; } else { assert(tkhit->cluster_strip().isNonnull()); const SiStripCluster& cluster = *tkhit->cluster_strip(); const StripTopology* striptopology = dynamic_cast(&(detectorG->topology())); assert(striptopology); - + const uint16_t firstStrip = cluster.firstStrip(); const uint16_t lastStrip = cluster.firstStrip() + cluster.amplitudes().size() - 1; const bool isOnEdge = firstStrip == 0 || lastStrip == (striptopology->nstrips() - 1); + + const bool isstereo = trackerTopology->isStereo((*it)->geographicalId()); -// if (isOnEdge) { -// std::cout << "strip hit isOnEdge" << std::endl; -// } - -// hitquality = !isOnEdge; hitquality = true; - -// const bool isstereo = trackerTopology->isStereo(detectorG->geographicalId()); -// hitquality = !isstereo; - - - - -// SiStripClusterInfo clusterInfo = SiStripClusterInfo(cluster, iSetup, (*it)->geographicalId().rawId()); -// if (clusterInfo.signalOverNoise() > 12.) { -// hitquality = true; -// } -// hitquality = true; +// hitquality = false; + // hitquality = !isstereo; } } @@ -1167,71 +648,15 @@ void ResidualGlobalCorrectionMakerG4e::produce(edm::Event &iEvent, const edm::Ev hitquality = true; } -// std::cout << "detid: " << (*it)->geographicalId().rawId() << std::endl; - -// bool foundsim = false; -// for (auto const& simhith : simHits) { -// for (const PSimHit& simHit : *simhith) { -// if (std::abs(simHit.particleType()) == 13 && simHit.detUnitId() == (*it)->geographicalId()) { -// foundsim = true; -// break; -// } -// } -// if (foundsim) { -// break; -// } -// } -// -// if (!foundsim) { -// hitquality = false; -// } - -// for (auto const& simhith : simHits) { -// for (const PSimHit& simHit : *simhith) { -// if (simHit.detUnitId() == (*it)->geographicalId()) { -// // std::cout << "particle type: " << simHit.particleType() << std::endl; -// // std::cout << "track id: " << simHit.trackId() << std::endl; -// // std::cout << "entry point: " << simHit.entryPoint() << std::endl; -// // std::cout << "exit point: " << simHit.exitPoint() << std::endl; -// // std::cout << "local position: " << simHit.localPosition() << std::endl; -// if (std::abs(simHit.particleType()) == 13 && std::abs(simHit.localPosition().z()) > 1e-4) { -// std::cout << "anomalous simhit!" << std::endl; -// hitquality = false; -// } -// } -// } -// } - - if (hitquality) { hits.push_back((*it)->cloneForFit(*detectorG)); } else { hits.push_back(TrackingRecHit::RecHitPointer(new InvalidTrackingRecHit(*detectorG, TrackingRecHit::inactive))); } - - -// auto const &layer = detidlayermap.at((*it)->geographicalId()); -// -// if (!hitlayers.count(layer)) { -// if (hitquality) { -// hits.push_back((*it)->cloneForFit(*detectorG)); -// } -// else { -// hits.push_back(TrackingRecHit::RecHitPointer(new InvalidTrackingRecHit(*detectorG, TrackingRecHit::inactive))); -// } -// } -// hitlayers.insert(layer); } } -// if (track.seedDirection() == oppositeToMomentum) { -// std::reverse(hits.begin(), hits.end()); -// } - -// printf("done building hits\n"); - -// const unsigned int nhits = track.recHitsSize(); const unsigned int nhits = hits.size(); if (nhits == 0) { @@ -1239,34 +664,28 @@ void ResidualGlobalCorrectionMakerG4e::produce(edm::Event &iEvent, const edm::Ev } nHits = nhits; -// unsigned int npixhits = 0; unsigned int nvalid = 0; unsigned int nvalidpixel = 0; unsigned int nvalidalign2d = 0; -// std::set> hitlayers; // count valid hits since this is needed to size the arrays for (auto const& hit : hits) { -// auto const &layers = detidlayermap.at(hit->geographicalId()); -// if (hitlayers.count(layers)) { -// std::cout << "WARNING: multiple hits on the same layer!!!" << std::endl; -// std::cout << layers[0] << " " << layers[1] << " " << layers[2] << std::endl; -// } -// hitlayers.insert(layers); assert(hit->dimension()<=2); if (hit->isValid()) { nvalid += 1; -// const uint32_t gluedid = trackerTopology->glued(hit->geographicalId()); -// const bool isglued = gluedid != 0; -// const DetId parmdetid = isglued ? DetId(gluedid) : hit->geographicalId(); -// const bool align2d = detidparms.count(std::make_pair(1, parmdetid)); -// const bool align2d = detidparms.count(std::make_pair(2, hit->geographicalId())); + const uint32_t gluedid = trackerTopology->glued(hit->geographicalId()); + const bool isglued = gluedid != 0; + const DetId parmdetid = isglued ? DetId(gluedid) : hit->geographicalId(); + + const DetId aligndetid = alignGlued_ ? parmdetid : hit->geographicalId(); + - const bool align2d = detidparms.count(std::make_pair(1, hit->geographicalId())); +// const bool align2d = detidparms.count(std::make_pair(1, hit->geographicalId())); + const bool align2d = detidparms.count(std::make_pair(1, aligndetid)); // if (align2d) { nvalidalign2d += 1; @@ -1276,21 +695,6 @@ void ResidualGlobalCorrectionMakerG4e::produce(edm::Event &iEvent, const edm::Ev } } } - -// //count valid hits since this is needed to size the arrays -// auto const& hitsbegin = track.recHitsBegin(); -// for (unsigned int ihit = 0; ihit < track.recHitsSize(); ++ihit) { -// auto const& hit = *(hitsbegin + ihit); -// if (hit->isValid() && hit->dimension()<=2) { -// nvalid += 1; -// -// const GeomDet *detectorG = globalGeometry->idToDet(hit->geographicalId()); -// if (hit->dimension()==2 && GeomDetEnumerators::isTrackerPixel(detectorG->subDetector())) { -// // if (hit->dimension()==2) { -// nvalidpixel += 1; -// } -// } -// } if (nvalid == 0) { continue; @@ -1302,118 +706,53 @@ void ResidualGlobalCorrectionMakerG4e::produce(edm::Event &iEvent, const edm::Ev nValidHitsFinal = 0; nValidPixelHitsFinal = 0; -// const unsigned int nstriphits = nhits-npixhits; -// const unsigned int nparsAlignment = nstriphits + 2*npixhits; -// const unsigned int nvalidstrip = nvalid - nvalidpixel; -// const unsigned int nparsAlignment = nvalidstrip + 2*nvalidpixel; - const unsigned int nparsAlignment = 2*nvalid + nvalidalign2d; -// const unsigned int nparsAlignment = 6*nvalid; + const unsigned int nparsAlignment = 5*nvalid + nvalidalign2d; const unsigned int nparsBfield = nhits; const unsigned int nparsEloss = nhits; -// const unsigned int nparsEloss = nhits + 1; - const unsigned int npars = nparsAlignment + nparsBfield + nparsEloss; +// const unsigned int nparsRes = nhits + nvalid + nvalidpixel; + const unsigned int nparsRes = dores ? 2*nhits + nvalid + nvalidpixel : 0; + const unsigned int npars = nparsAlignment + nparsBfield + nparsEloss + nparsRes; const unsigned int nstateparms = 5*(nhits+1); -// const unsigned int nstateparms = 3*(nhits+1) - 1; -// const unsigned int nstateparms = 3*nhits - 1; const unsigned int nparmsfull = nstateparms + npars; + const int nbscons = bsConstraint_ ? 3 : 0; + const int ncons = 5*nhits + nvalid + nvalidpixel + nbscons; - const unsigned int nstateparmspost = 5*(nhits+1); - -// std::cout << "nhits " << nhits << std::endl; -// std::cout << "nstateparms " << nstateparms << std::endl; -// std::cout << "nparmsfull " << nparmsfull << std::endl; -// std::cout << "nparmsfull " << nparmsfull << std::endl; -// std::cout << "nparmsfull " << nparmsfull << std::endl; -// std::cout << "nparmsfull " << nparmsfull << std::endl; -// std::cout << "nparmsfull " << nparmsfull << std::endl; - -// const unsigned int npropparms = 5*(nhits-1); -// const unsigned int nhitparms = 2*nhits; -// const unsigned int nmomparms = 3*(nhits-1); -// const unsigned int nposparms = 2*(nhits-1); -// constexpr unsigned int nrefparms = 5; - - - - //active double for autodiff gradients -// using Adouble = AutoDiffScalar; -// using AVectorXd = Matrix; -// //double double for autodiff hessians -// using AAdouble = AutoDiffScalar; - - - -// using AAXd = AANT; -// using AAdouble = AAXd; -// -// using AA2d = AANT; -// using AA3d = AANT; -// using AA4d = AANT; -// using AA12d = AANT; -// -// using ScalarConst = AANT; - -// using AConstd = AutoDiffScalar; -// using AConstd = AutoDiffScalar>; - - -// using VectorXAd = Matrix; -// using MatrixXAd = Matrix; - - //two position parameters and and one alignment parameter - using StripHitScalar = AANT;; + using VectorXb = Matrix; + VectorXb freestatemask = VectorXb::Ones(nstateparms); - using StripHit1DJacobian = Matrix; - - using StripHitVector = Matrix; - using StripHit2DCovariance = Matrix; - using StripHit2DJacobian = Matrix; - - - - //two hit dimensions and two alignment parameters - using PixelHit2DScalar = AANT; - using PixelHit2DVector = Matrix; - using PixelHit2DCovariance = Matrix; - using PixelHit2DJacobian = Matrix; - - - //2x5 state parameters, one bfield parameter, and one material parameter -// using MSScalar = AANT;; -// using MSScalar = AANT; -// using MSVector = Matrix; -// using MSProjection = Matrix; -// using MSJacobian = Matrix; -// using MSCovariance = Matrix; + if (dogen) { + freestatemask.head<5>() = Matrix::Zero(); + } + if (fitFromSimParms_) { + freestatemask = VectorXb::Zero(nstateparms); + } - using BSScalar = AANT; + // if (dogen) { + // for (unsigned int istate = 0; istate < nstateparms; ++istate) { + // if (istate % 5 == 0) { + // freestatemask[istate] = false; + // } + // } + // } -// using HitProjection = Matrix; -// using HitCovariance = Matrix; -// using HitVector = Matrix; + std::vector freestateidxs; + freestateidxs.reserve(nstateparms); -// evector Vinv(nhits, HitCovarianceMatrix::Zero()); -// evector Hh(nhits, HitProjection::Zero()); -// evector dy0(nhits, HitVector::Zero()); -// evector dx(nhits, StateVector::Zero()); -// //initialize backpropagation indexing -// for (unsigned int i=0; i dhessv; - + if (bsConstraint_) { + ndof += 2; + } MatrixXd validdxeigjac; VectorXd validdxeig; @@ -1421,106 +760,50 @@ void ResidualGlobalCorrectionMakerG4e::produce(edm::Event &iEvent, const edm::Ev Matrix rxfull(nvalid, 2); Matrix ryfull(nvalid, 2); -// VectorXd gradfull = chisq.value().derivatives(); -// MatrixXd hessfull = MatrixXd::Zero(nparmsfull, nparmsfull); -// for (unsigned int i=0; iSetBranchAddress("globalidxv", globalidxv.data()); -// } - -// TrajectoryStateOnSurface currtsos; - - - VectorXd dxfull; MatrixXd dxdparms; VectorXd grad; MatrixXd hess; - LDLT Cinvd; -// ColPivHouseholderQR Cinvd; +// LDLT Cinvd; MatrixXd covfull = MatrixXd::Zero(nstateparms, nstateparms); + VectorXd rfull; + MatrixXd Ffull; + MatrixXd Jfull; + MatrixXd Vinvfull; + VectorXd dxfree; + + MatrixXd Vinvfullalt; + + SparseMatrix Fsparse; + SparseMatrix Vinvsparse; + SparseMatrix VinvF; + + SimplicialLDLT> Cinvd; + + std::vector> dVs; + dVs.reserve(nhits + nvalid); + + std::vector residxs; + residxs.reserve(nhits + nvalid); + if (dogen && genpart==nullptr) { + std::cout << "no gen part, skipping track\n"; continue; } -// if (dogen && genpart->eta()>-2.3) { -// continue; -// } -// if (genpart==nullptr) { -// continue; -// } -// if (genpart->pt()>10.) { -// continue; -// } -// if (genpart->pt()<100.) { -// continue; -// } -// if (genpart->eta()>-2.3) { -// continue; -// } - if (debugprintout_) { std::cout << "initial reference point parameters:" << std::endl; std::cout << track.parameters() << std::endl; } -// //prepare hits -// TransientTrackingRecHit::RecHitContainer hits; -// hits.reserve(track.recHitsSize()); -// for (auto it = track.recHitsBegin(); it != track.recHitsEnd(); ++it) { -// const GeomDet *detectorG = globalGeometry->idToDet((*it)->geographicalId()); -// hits.push_back((*it)->cloneForFit(*detectorG)); -// } - -// // fix mixed up clusters? -// for (unsigned int ihit=0; ihit<(hits.size()-1); ++ihit) { -// TrackerSingleRecHit* hit = const_cast(dynamic_cast(hits[ihit].get())); -// TrackerSingleRecHit* nexthit = const_cast(dynamic_cast(hits[ihit+1].get())); -// -// // const TrackingRecHitSingle* nexthit = hits[ihit+1]; -// -// if (!hit || !nexthit) { -// continue; -// } -// -// const DetId partnerid = trackerTopology->partnerDetId(hit->geographicalId()); -// // -// if (partnerid == nexthit->geographicalId()) { -// // std::cout << "swapping clusters" << std::endl; -// const OmniClusterRef::ClusterStripRef cluster = hit->cluster_strip(); -// const OmniClusterRef::ClusterStripRef nextcluster = nexthit->cluster_strip(); -// -// hit->setClusterStripRef(nextcluster); -// nexthit->setClusterStripRef(cluster); -// } -// -// } - -// if (genpart==nullptr) { -// continue; -// } -// -// if (genpart->eta()<-2.4 || genpart->eta()>-2.3) { -// continue; -// } - - -// FreeTrajectoryState refFts; Matrix refFts; if (dogen) { -// if (true) { - if (genpart==nullptr) { continue; } @@ -1537,24 +820,6 @@ void ResidualGlobalCorrectionMakerG4e::produce(edm::Event &iEvent, const edm::Ev refFts[5] = trackmom.z(); refFts[6] = genpart->charge(); - - -// const GlobalPoint refpos(refpoint.x(), refpoint.y(), refpoint.z()); -// const GlobalVector refmom(trackmom.x(), trackmom.y(), trackmom.z()); -// const GlobalTrajectoryParameters refglobal(refpos, refmom, genpart->charge(), field); - -// std::cout << "gen ref state" << std::endl; -// std::cout << refpos << std::endl; -// std::cout << refmom << std::endl; -// std::cout << genpart->charge() << std::endl; - - //zero uncertainty on generated parameters -// AlgebraicSymMatrix55 nullerr; -// const CurvilinearTrajectoryError referr(nullerr); -// const CurvilinearTrajectoryError referr; - -// refFts = FreeTrajectoryState(refpos, refmom, genpart->charge(), field); -// refFts = FreeTrajectoryState(refglobal, referr); } else { //init from track state @@ -1571,76 +836,46 @@ void ResidualGlobalCorrectionMakerG4e::produce(edm::Event &iEvent, const edm::Ev refFts[6] = track.charge(); -// const GlobalPoint refpos(refpoint.x(), refpoint.y(), refpoint.z()); -// const GlobalVector refmom(trackmom.x(), trackmom.y(), trackmom.z()); -// const GlobalTrajectoryParameters refglobal(refpos, refmom, track.charge(), field); -// const CurvilinearTrajectoryError referr(track.covariance()); -// const CurvilinearTrajectoryError referr; - - //null uncertainty (tracking process noise sum only) -// AlgebraicSymMatrix55 nullerr; -// const CurvilinearTrajectoryError referr(nullerr); - -// refFts = FreeTrajectoryState(refpos, refmom, track.charge(), field); -// refFts = FreeTrajectoryState(refglobal, referr); + // special case for cosmics, start from "inner" state with straight line extrapolation back 1cm to preserve the propagation logic + // (the reference point is instead the PCA to the beamline and is therefore in the middle of the trajectory and not compatible + // with the fitter/propagation logic + // TODO change this so that cosmics directly use a local parameterization on the first hit? + if (iscosmic) { + auto const& startpoint = track.extra()->innerPosition(); + auto const& startmom = track.extra()->innerMomentum(); + + const Eigen::Vector3d startpointv(startpoint.x(), startpoint.y(), startpoint.z()); + const Eigen::Vector3d startmomv(startmom.x(), startmom.y(), startmom.z()); + + refFts.head<3>() = startpointv - 1.0*startmomv.normalized(); + refFts.segment<3>(3) = startmomv; + + } + } + + if (dopca) { + // enforce that reference state is really a consistent PCA to the beamline (by adjusting the reference position if needed) + const Matrix statepca = cart2pca(refFts, *bsH); + refFts = pca2cart(statepca, *bsH); } -// std::vector> layerStates; -// std::vector layerStates; std::vector> layerStates; -// std::vector> layerStatesStart; - layerStates.reserve(nhits); -// layerStatesStart.reserve(nhits); bool valid = true; - -// //do propagation and prepare states -// auto propresult = fPropagator->propagateWithPath(refFts, *hits.front()->surface()); -// if (!propresult.first.isValid()) { -// std::cout << "Abort: Propagation from reference point failed" << std::endl; -// continue; -// } -// layerStates.push_back(propresult); -// -// for (auto const& hit : hits) { -// propresult = fPropagator->propagateWithPath(layerStates.back().first, *hit->surface()); -// if (!propresult.first.isValid()) { -// std::cout << "Abort: Propagation failed" << std::endl; -// valid = false; -// break; -// } -// layerStates.push_back(propresult); -// } -// -// if (!valid) { -// continue; -// } - - -// const bool islikelihood = true; const bool islikelihood = false; - //inflate errors -// refFts.rescaleError(100.); - + + std::vector localxsmearedsim(hits.size(), 0.); -// unsigned int ntotalhitdim = 0; -// unsigned int alignmentidx = 0; -// unsigned int bfieldidx = 0; -// unsigned int elossidx = 0; double chisqvalold = std::numeric_limits::max(); - bool anomDebug = false; - -// constexpr unsigned int niters = 1; -// constexpr unsigned int niters = 3; -// constexpr unsigned int niters = 5; - constexpr unsigned int niters = 10; -// constexpr unsigned int niters = 20; + const bool anomDebug = false; + + const unsigned int niters = (dogen && !dolocalupdate) || (dogen && fitFromSimParms_) ? 1 : 10; for (unsigned int iiter=0; iiter 2; -// const bool islikelihood = false; - -// const bool islikelihood = iiter > 0; -// const bool islikelihood = true; - - gradfull = VectorXd::Zero(nparmsfull); - hessfull = MatrixXd::Zero(nparmsfull, nparmsfull); - - if (islikelihood) { - dhessv.assign(nhits, MatrixXd::Zero(nstateparms, nstateparms)); - } + rfull = VectorXd::Zero(ncons); + Ffull = MatrixXd::Zero(ncons, nstateparms); + Jfull = MatrixXd::Zero(ncons, npars); + Vinvfull = MatrixXd::Zero(ncons, ncons); + Vinvfullalt = MatrixXd::Zero(ncons, ncons); + dVs.clear(); + residxs.clear(); validdxeigjac = MatrixXd::Zero(2*nvalid, nstateparms); -// evector, 11> > dhessv; -// std::vector> dhessv; -// if (islikelihood) { -// dhessv.resize(nhits); -// } double chisq0val = 0.; @@ -1791,170 +1082,62 @@ void ResidualGlobalCorrectionMakerG4e::produce(edm::Event &iEvent, const edm::Ev simlocalxref = -99.; simlocalyref = -99.; - - -// const uint32_t gluedid0 = trackerTopology->glued(hits[0]->det()->geographicalId()); -// const bool isglued0 = gluedid0 != 0; -// const DetId parmdetid0 = isglued0 ? DetId(gluedid0) : hits[0]->geographicalId(); -// const unsigned int bfieldidx = detidparms.at(std::make_pair(6, parmdetid0)); -// fieldOffset->setOffset(corparms_[bfieldidx]); - -// dhessv.reserve(nhits-1); - - unsigned int parmidx = 0; - unsigned int alignmentparmidx = 0; unsigned int ivalidhit = 0; + + unsigned int icons = 0; + unsigned int iparm = 0; if (iiter > 0) { //update current state from reference point state (errors not needed beyond first iteration) -// JacobianCurvilinearToCartesian curv2cart(refFts.parameters()); -// const AlgebraicMatrix65& jac = curv2cart.jacobian(); - const Matrix jac = curv2cartJacobianAltD(refFts); -// const AlgebraicVector6 glob = refFts.parameters().vector(); auto const& dxlocal = dxfull.head<5>(); -// const Matrix globupd = Map>(glob.Array()) + Map>(jac.Array())*dxlocal; -// const Matrix globupd = Map>(glob.Array()) + jac*dxlocal; - const Matrix globupd = refFts.head<6>() + jac*dxlocal; - - - const double qbp = refFts[6]/refFts.segment<3>(3).norm(); - const double lam = std::atan(refFts[5]/std::sqrt(refFts[3]*refFts[3] + refFts[4]*refFts[4])); - const double phi = std::atan2(refFts[4], refFts[3]); - - -// const Vector6d dglob = Map>(jac.Array())*dxlocal; -// -// std::cout << "iiter = " << iiter << std::endl; -// std::cout << "dxlocal " << dxlocal << std::endl; -// std::cout << "glob " << glob << std::endl; -// std::cout << "dglob " << dglob << std::endl; - -// const GlobalPoint pos(globupd[0], globupd[1], globupd[2]); -// const GlobalVector mom(globupd[3], globupd[4], globupd[5]); -// const double charge = std::copysign(1., refFts.charge()/refFts.momentum().mag() + dxlocal[0]); + -// const CurvilinearTrajectoryParameters curv(refFts.position(), refFts.momentum(), refFts.charge()); - - const double qbpupd = qbp + dxfull(0); - const double lamupd = lam + dxfull(1); - const double phiupd = phi + dxfull(2); - - const double charge = std::copysign(1., qbpupd); - const double pupd = std::abs(1./qbpupd); - - const double pxupd = pupd*std::cos(lamupd)*std::cos(phiupd); - const double pyupd = pupd*std::cos(lamupd)*std::sin(phiupd); - const double pzupd = pupd*std::sin(lamupd); - - refFts.head<3>() = globupd.head<3>(); - refFts[3] = pxupd; - refFts[4] = pyupd; - refFts[5] = pzupd; - refFts[6] = charge; - -// const GlobalVector mom(pxupd, pyupd, pzupd); - -// const GlobalTrajectoryParameters refglobal(pos, mom, charge, field); -// const CurvilinearTrajectoryError referr; - -// std::cout << "before update: reffts:" << std::endl; -// std::cout << refFts.parameters().vector() << std::endl; -// std::cout << "charge " << refFts.charge() << std::endl; -// refFts = FreeTrajectoryState(pos, mom, charge, field); -// refFts = FreeTrajectoryState(refglobal, referr); -// std::cout << "after update: reffts:" << std::endl; -// std::cout << refFts.parameters().vector() << std::endl; -// std::cout << "charge " << refFts.charge() << std::endl; -// currentFts = refFts; - } - -// Matrix5d Hlm = Matrix5d::Identity(); -// currentFts = refFts; -// TrajectoryStateOnSurface currentTsos; + if (dopca) { + // position transformation from track-beamline pca to cartesian is non-linear so do it exactly + const Matrix statepca = cart2pca(refFts, *bsH); + const Matrix statepcaupd = statepca + dxlocal; -// ; - -// std::cout << "reffts p = " << refFts.momentum().mag() << " pt = " << refFts.momentum().perp() << " eta = " << refFts.momentum().eta() << std::endl; - - const ROOT::Math::PxPyPzMVector momtmp(refFts[3], refFts[4], refFts[5], mmu); + refFts = pca2cart(statepcaupd, *bsH); - - if (std::abs(momtmp.eta()) > 4.0) { - std::cout << "WARNING: Invalid reference state!!!" << std::endl; - valid = false; - break; + } + else { + // position transformation from curvilinear to cartesian is linear so just use the jacobian + + const Matrix jac = curv2cartJacobianAltD(refFts); + const Matrix globupd = refFts.head<6>() + jac*dxlocal; + + const double qbp = refFts[6]/refFts.segment<3>(3).norm(); + const double lam = std::atan(refFts[5]/std::sqrt(refFts[3]*refFts[3] + refFts[4]*refFts[4])); + const double phi = std::atan2(refFts[4], refFts[3]); + + const double qbpupd = qbp + dxlocal(0); + const double lamupd = lam + dxlocal(1); + const double phiupd = phi + dxlocal(2); + + const double charge = std::copysign(1., qbpupd); + const double pupd = std::abs(1./qbpupd); + + const double pxupd = pupd*std::cos(lamupd)*std::cos(phiupd); + const double pyupd = pupd*std::cos(lamupd)*std::sin(phiupd); + const double pzupd = pupd*std::sin(lamupd); + + refFts.head<3>() = globupd.head<3>(); + refFts[3] = pxupd; + refFts[4] = pyupd; + refFts[5] = pzupd; + refFts[6] = charge; + } } -// const Matrix Felossadhoc = elossAdHocJacobianD(refFts, mmu); -// const unsigned int etaphiidx = hetaphi->FindFixBin(momtmp.eta(), momtmp.phi()); - - -// std::cout << "beamline p = " << refFts.momentum().mag() << std::endl; - -// auto const &surface0 = *hits[0]->surface(); -// const Plane &surface0 = *hits[0]->surface(); -// auto const &surface0 = *surfacemap_.at(hits[0]->geographicalId()); -// printf("detid = %u, parmdetid = %u, old = %p, new = %p\n", hits[0]->geographicalId().rawId(), parmdetid0.rawId(), &surface0orig, &surface0); -// std::cout << "old xi = " << surface0orig.mediumProperties().xi() << " new xi = " << surface0.mediumProperties().xi() << " dxi = " << surface0.mediumProperties().xi() - surface0orig.mediumProperties().xi() << std::endl; -// auto propresultref = thePropagator->propagateWithPath(refFts, surface0); -// auto const propresultref = g4prop->propagateGenericWithJacobian(refFts, surface0); -// auto const propresultref = g4prop->propagateGenericWithJacobianAlt(refFts, surface0); -// auto propresult = fPropagator->geometricalPropagator().propagateWithPath(refFts, *hits[0]->surface()); -// auto propresult = fPropagator->geometricalPropagator().propagateWithPath(refFts, *beampipe); -// if (!std::get<0>(propresultref).isValid()) { -// std::cout << "Abort: Propagation of reference state Failed!" << std::endl; -// valid = false; -// break; -// } - -// auto propresultreforig = g4prop->propagateGenericWithJacobian(refFts, surface0); -// // -// std::cout << "jacref" << std::endl; -// std::cout << std::get<1>(propresultref) << std::endl; -// std::cout << "jacorig" << std::endl; -// std::cout << std::get<1>(propresultreforig) << std::endl; -// -// std::cout << "errref" << std::endl; -// std::cout << std::get<0>(propresultref).localError().matrix() << std::endl; -// std::cout << "errorig" << std::endl; -// std::cout << std::get<0>(propresultreforig).localError().matrix() << std::endl; - -// assert(std::get<0>(propresultref).globalMomentum().mag() <= refFts.momentum().mag()); - -// TrajectoryStateOnSurface updtsos = std::get<0>(propresultref); - -// Matrix FdFm = Map>(std::get<1>(propresultref).Array()); - -// Matrix dQ = Map>(std::get<2>(propresultref).Array()); - -// double dEdxlast = std::get<3>(propresultref); - + Matrix ref2curvjac = dopca ? pca2curvJacobianD(refFts, field, *bsH) : Matrix::Identity(); + Matrix updtsos = refFts; - -// double dEdxout = 0.; - - -// double dqop = propresult.first.signedInverseMomentum() - refFts.signedInverseMomentum(); - -// std::cout << "position on beampipe " << propresult.first.globalParameters().position() << std::endl; - -// const Matrix FdFp = curv2curvTransportJacobian(refFts, propresult, false); -// Matrix FdFm = curv2curvTransportJacobian(refFts, propresult, false); -// Matrix FdFm = curv2localTransportJacobian(refFts, propresultref, false); -// -// const Matrix Fcurv = curv2curvTransportJacobian(refFts, propresultref, false); -// -// auto const propresultjac = g4prop->propagateGenericWithJacobian(refFts, surface0); -// // -// std::cout << "Fcurv" << std::endl; -// std::cout << Fcurv << std::endl; -// // -// std::cout << "Fcurv g4e" << std::endl; -// std::cout << propresultjac.second << std::endl; - + + Matrix propfromtsos = refFts; + Matrix Qtot = Matrix::Zero(); - + float e = genpart == nullptr ? -99. : std::sqrt(genpart->momentum().mag2() + mmu*mmu); float epred = std::sqrt(refFts.segment<3>(3).squaredNorm() + mmu*mmu); @@ -1962,20 +1145,17 @@ void ResidualGlobalCorrectionMakerG4e::produce(edm::Event &iEvent, const edm::Ev if (bsConstraint_) { // apply beamspot constraint // TODO add residual corrections for beamspot parameters? + // TODO make this a 2D constraint? - constexpr unsigned int nlocalstate = 2; + //TODO convert this to Ffull/Vinvfull parameterization + constexpr unsigned int nlocalcons = 3; + constexpr unsigned int nlocalstate = 5; constexpr unsigned int nlocal = nlocalstate; - constexpr unsigned int localstateidx = 0; + constexpr unsigned int fullstateidx = 0; - constexpr unsigned int fullstateidx = 3; - - using BSScalar = AANT; - -// JacobianCurvilinearToCartesian curv2cart(refFts.parameters()); -// const AlgebraicMatrix65& jac = curv2cart.jacobian(); - const Matrix jac = curv2cartJacobianAltD(refFts); + const Matrix jac = dopca ? pca2cartJacobianD(refFts, *bsH) : curv2cartJacobianAltD(refFts); const double sigb1 = bsH->BeamWidthX(); const double sigb2 = bsH->BeamWidthY(); @@ -1990,6 +1170,8 @@ void ResidualGlobalCorrectionMakerG4e::produce(edm::Event &iEvent, const edm::Ev // covariance matrix of luminous region in global coordinates // taken from https://github.com/cms-sw/cmssw/blob/abc1f17b230effd629c9565fb5d95e527abcb294/RecoVertex/BeamSpotProducer/src/FcnBeamSpotFitPV.cc#L63-L90 + // TODO check consistency of this parameterization + // FIXME xy correlation is not stored and assumed to be zero const double corrb12 = 0.; @@ -1997,7 +1179,7 @@ void ResidualGlobalCorrectionMakerG4e::produce(edm::Event &iEvent, const edm::Ev const double varb2 = sigb2*sigb2; const double varb3 = sigb3*sigb3; - Matrix covBS = Matrix::Zero(); + Matrix covBS = Matrix::Zero(); // parametrisation: rotation (dx/dz, dy/dz); covxy covBS(0,0) = varb1; covBS(1,0) = covBS(0,1) = corrb12*sigb1*sigb2; @@ -2005,63 +1187,72 @@ void ResidualGlobalCorrectionMakerG4e::produce(edm::Event &iEvent, const edm::Ev covBS(2,0) = covBS(0,2) = dxdz*(varb3-varb1)-dydz*covBS(1,0); covBS(2,1) = covBS(1,2) = dydz*(varb3-varb2)-dxdz*covBS(1,0); covBS(2,2) = varb3; - -// std::cout << "covBS:" << std::endl; -// std::cout << covBS << std::endl; - - Matrix du = Matrix::Zero(); - for (unsigned int j=0; j dbs0; - dbs0[0] = BSScalar(refFts[0] - x0); - dbs0[1] = BSScalar(refFts[1] - y0); - dbs0[2] = BSScalar(refFts[2] - z0); - -// std::cout << "dposition / d(qop, lambda, phi) (should be 0?):" << std::endl; -// std::cout << Map>(jac.Array()).topLeftCorner<3,3>() << std::endl; - -// const Matrix jacpos = Map>(jac.Array()).topRightCorner<3,2>().cast(); - const Matrix jacpos = jac.topRightCorner<3,2>().cast(); - const Matrix covBSinv = covBS.inverse().cast(); - const Matrix dbs = dbs0 + jacpos*du; - const BSScalar chisq = dbs.transpose()*covBSinv*dbs; - - chisq0val += chisq.value().value(); + Matrix dbs0; + dbs0[0] = refFts[0] - x0; + dbs0[1] = refFts[1] - y0; + dbs0[2] = refFts[2] - z0; - auto const& gradlocal = chisq.value().derivatives(); - //fill local hessian - Matrix hesslocal; - for (unsigned int j=0; j Fbs = jac.topRows<3>(); + const Matrix covBSinv = covBS.inverse(); - //fill global gradient - gradfull.segment(fullstateidx) += gradlocal.head(); - - //fill global hessian (upper triangular blocks only) - hessfull.block(fullstateidx, fullstateidx) += hesslocal.topLeftCorner(); + rfull.segment(icons) = dbs0; + Ffull.block(icons, fullstateidx) = Fbs; + Vinvfull.block(icons, icons) = covBSinv; + Vinvfullalt.block(icons, icons) = covBSinv; + icons += nlocalcons; } for (unsigned int ihit = 0; ihit < hits.size(); ++ihit) { -// std::cout << "ihit " << ihit << std::endl; +// std::cout << "iiter = " << iiter << " ihit " << ihit << std::endl; + auto const& hit = hits[ihit]; - const uint32_t gluedid = trackerTopology->glued(hit->det()->geographicalId()); + // auto const &surface = surfacemapD_.at(hit->geographicalId()); + GloballyPositioned surface = surfacemapD_.at(hit->geographicalId()); + + // check propagation direction to choose correct bfield and material parameters + // when propagating inside-out the parameters correspond to the target module + // when propagating outside-in (e.g. for cosmics) they correspond to the source module + // For the first hit always use the target module + // (only relevant for cosmics) + + bool sourceParms = false; + if (iscosmic && ihit > 0) { + auto const &lzhat = surface.rotation().z(); + auto const &pos = surface.position(); + + const double zdotpos = lzhat.x()*pos.x() + lzhat.y()*pos.y() + lzhat.z()*pos.z(); + + const Point3DBase globalpos(updtsos[0], updtsos[1], updtsos[2]); + const Point3DBase localpos = surface.toLocal(globalpos); + + sourceParms = zdotpos*localpos.z() > 0.; + } + +// std::cout << "ihit = " << ihit << " sourceParms = " << sourceParms << std::endl; + + auto const& prophit = sourceParms ? hits[ihit - 1] : hit; + const uint32_t gluedidprop = trackerTopology->glued(prophit->geographicalId()); + const bool isgluedprop = gluedidprop != 0; + const DetId propdetid = isgluedprop ? DetId(gluedidprop) : prophit->geographicalId(); + + const uint32_t gluedid = trackerTopology->glued(hit->geographicalId()); const bool isglued = gluedid != 0; const DetId parmdetid = isglued ? DetId(gluedid) : hit->geographicalId(); - const GeomDet* parmDet = isglued ? globalGeometry->idToDet(parmdetid) : hit->det(); - const double xifraction = isglued ? hit->det()->surface().mediumProperties().xi()/parmDet->surface().mediumProperties().xi() : 1.; + const DetId aligndetid = alignGlued_ ? parmdetid : hit->geographicalId(); - const unsigned int bfieldglobalidx = detidparms.at(std::make_pair(6, parmdetid)); - const unsigned int elossglobalidx = detidparms.at(std::make_pair(7, parmdetid)); + const unsigned int bfieldglobalidx = detidparms.at(std::make_pair(6, propdetid)); + const unsigned int elossglobalidx = detidparms.at(std::make_pair(7, propdetid)); + const unsigned int msglobalidx = dores ? detidparms.at(std::make_pair(10, propdetid)) : 0; + const unsigned int ioniglobalidx = dores ? detidparms.at(std::make_pair(11, propdetid)) : 0; const double dbetaval = corparms_[bfieldglobalidx]; const double dxival = corparms_[elossglobalidx]; + const double dmsval = dores ? corparms_[msglobalidx] : dxival; + const double dionival = dores ? corparms_[ioniglobalidx] : dxival; const PSimHit *simhit = nullptr; @@ -2078,148 +1269,86 @@ void ResidualGlobalCorrectionMakerG4e::produce(edm::Event &iEvent, const edm::Ev } } } - - -// const DetId partnerid = isglued ? trackerTopology->partnerDetId(hit->det()->geographicalId()) : DetId(); -// -// const bool isfront = ihit != (hits.size() - 1) && isglued && hits[ihit+1]->det()->geographicalId() == partnerid; -// const bool isback = ihit !=0 && isglued && hits[ihit-1]->det()->geographicalId() == partnerid; -// -// const double xifraction = isfront ? 0. : 1.; - - - const ROOT::Math::PxPyPzMVector momtmp(updtsos[3], updtsos[4], updtsos[5], mmu); - - if (std::abs(momtmp.eta()) > 4.0) { - std::cout << "WARNING: Invalid state!!!" << std::endl; + // const bool simhitdebug = true; + const bool simhitdebug = false; + + if (simhitdebug && !simhit) { + std::cout << "WARNING: no simhit for debugging scenario, abort!\n"; valid = false; break; } + + if ((simhitdebug || fitFromSimParms_) && simhit) { + //move surface to plane of sim entry point for consistency in debugging vs sim state + + const double lz = simhit->entryPoint().z(); + const Vector3DBase dlocal(0., 0., lz); + const Vector3DBase dglobal = surface.toGlobal(dlocal); + surface.move(dglobal); + } + +// std::cout << "iiter = " << iiter << " ihit = " << ihit << " updtsos:\n" << updtsos << std::endl; + // auto const &propresult = g4prop->propagateGenericWithJacobianAltD(updtsos, surface, dbetaval, dxival, dradval); + // auto const &propresult = g4prop->propagateGenericWithJacobianAltD(propfromtsos, surface, dbetaval, dxival, dradval); - - -// auto const &surfaceip1 = *hits[ihit+1]->surface(); -// auto const &surface = *hit->surface(); -// const Plane &surface = *hit->surface(); -// const Surface &surface = *hit->surface(); - const GloballyPositioned &surface = surfacemapD_.at(hit->geographicalId()); -// auto const &surface = *surfacemap_.at(hit->geographicalId()); -// auto propresult = thePropagator->propagateWithPath(updtsos, surface); -// auto propresult = g4prop->propagateGenericWithJacobian(*updtsos.freeState(), surface); - -// std::cout << "propagation for iiter = " << iiter << " ihit = " << ihit << std::endl; - auto propresult = g4prop->propagateGenericWithJacobianAltD(updtsos, surface, dbetaval, dxival); + auto const &propresult = simhitdebug ? g4prop->propagateGenericWithJacobianAltD(propfromtsos, surface, dbetaval, dxival, dmsval, dionival) : g4prop->propagateGenericWithJacobianAltD(updtsos, surface, dbetaval, dxival, dmsval, dionival); + + + if (simhitdebug && simhit) { + // reset propagation start point to sim hit for debugging purposes + auto const &surfaceideal = surfacemapIdealD_.at(hit->geographicalId()); + const Point3DBase pos = surfaceideal.toGlobal(simhit->entryPoint()); + const Vector3DBase mom = surfaceideal.toGlobal(simhit->momentumAtEntry()); + + propfromtsos[0] = pos.x(); + propfromtsos[1] = pos.y(); + propfromtsos[2] = pos.z(); + propfromtsos[3] = mom.x(); + propfromtsos[4] = mom.y(); + propfromtsos[5] = mom.z(); + propfromtsos[6] = genpart->charge(); + } -// propresult = fPropagator->geometricalPropagator().propagateWithPath(updtsos, *hits[ihit+1]->surface()); if (!std::get<0>(propresult)) { std::cout << "Abort: Propagation Failed!" << std::endl; valid = false; break; } -// auto propresultorig = g4prop->propagateGenericWithJacobian(*updtsos.freeState(), surface); -// // -// std::cout << "jac" << std::endl; -// std::cout << std::get<1>(propresult) << std::endl; -// std::cout << "jacorig" << std::endl; -// std::cout << std::get<1>(propresultorig) << std::endl; -// -// std::cout << "err" << std::endl; -// std::cout << std::get<0>(propresult).localError().matrix() << std::endl; -// std::cout << "errorig" << std::endl; -// std::cout << std::get<0>(propresultorig).localError().matrix() << std::endl; - -// std::cout << "pPre = " << updtsos.globalMomentum().mag() << " pPost = " << std::get<0>(propresult).globalMomentum().mag() << std::endl; - -// assert(std::get<0>(propresult).globalMomentum().mag() <= updtsos.globalMomentum().mag()); - -// const Matrix FdFm = Map>(std::get<1>(propresult).Array()); -// FdFm = localTransportJacobian(updtsos, propresult, false); - -// const Matrix dQ = Map>(std::get<2>(propresult).Array()); -// const Matrix dQ = Matrix::Zero(); - updtsos = std::get<1>(propresult); const Matrix Qcurv = std::get<2>(propresult); - const Matrix FdFm = std::get<3>(propresult); + const Matrix FdFmcurv = std::get<3>(propresult); const double dEdxlast = std::get<4>(propresult); -// const Matrix dQcurv = std::get<5>(propresult); + const Matrix dQMScurv = std::get<5>(propresult); + const Matrix dQIcurv = std::get<6>(propresult); +// const Matrix dQcurv = Qcurv; + const double deltaTotal = std::get<7>(propresult); + const double wTotal = std::get<8>(propresult); + + Matrix FdFm = FdFmcurv; + if (ihit == 0) { + // extra jacobian from reference state to curvilinear potentially needed + FdFm.leftCols<5>() = FdFmcurv.leftCols<5>()*ref2curvjac; + } -// if (ihit == (hits.size() - 1)) { -// dEdxout = dEdxlast; -// } - Qtot = (FdFm.leftCols<5>()*Qtot*FdFm.leftCols<5>().transpose()).eval(); - Qtot += Qcurv; - -// const GlobalPoint postmp(updtsos[0], updtsos[1], updtsos[2]); -// const GlobalVector bvtmp = field->inTesla(postmp); -// -// std::cout << "postmp" << std::endl; -// std::cout << postmp << std::endl; -// std::cout << "bvtmp" << std::endl; -// std::cout << bvtmp << std::endl; - -// if (trackEta > 2.2) { -// std::cout << "nominal bfield" << std::endl; -// std::cout << updtsos.magneticField()->inTesla(updtsos.globalPosition()) << std::endl; -// -// const double dx = 10e-4; -// -// GlobalPoint altpos(updtsos.globalPosition().x(), updtsos.globalPosition().y(), updtsos.globalPosition().z() + dx); -// -// const GlobalVector bgrad = (updtsos.magneticField()->inTesla(altpos) - updtsos.magneticField()->inTesla(updtsos.globalPosition()))/dx; -// -// std::cout << "bgrad" << std::endl; -// std::cout << bgrad << std::endl; -// } - - -// std::cout << "updtsos localposition = " << updtsos.localPosition() << std::endl; - - + // propfromtsos = updtsos; +// //zero energy loss contribution +// FdFm.rightCols<1>() *= 0.; - -// std::cout << "FdFm" << std::endl; -// std::cout << FdFm << std::endl; - -// std::cout << "ihit = " << ihit << " p = " << updtsos.globalMomentum().mag() << std::endl; - - + Qtot = (FdFmcurv.leftCols<5>()*Qtot*FdFmcurv.leftCols<5>().transpose()).eval(); + Qtot += Qcurv; + + if (simhitdebug) { + Qtot = Qcurv; + } - - // curvilinear to local jacobian -// JacobianCurvilinearToLocal curv2localm(updtsos.surface(), updtsos.localParameters(), *updtsos.magneticField()); -// const AlgebraicMatrix55& curv2localjacm = curv2localm.jacobian(); -// const Matrix Hm = Map>(curv2localjacm.Array()); -// const Matrix Hm = curv2localJacobianAlt(updtsos); const Matrix Hm = curv2localJacobianAltelossD(updtsos, field, surface, dEdxlast, mmu, dbetaval); - // compute convolution correction in local coordinates (BEFORE material effects are applied) -// const Matrix dxlocalconv = localPositionConvolution(updtsos); - - //get the process noise matrix -// AlgebraicMatrix55 const Qmat = updtsos.localError().matrix(); -// const Map>Q(Qmat.Array()); - const Matrix Q = Hm*Qcurv*Hm.transpose(); - -// const Matrix dQ = Hm*dQcurv*Hm.transpose(); - -// const Map>Qorig(Qmat.Array()); -// Matrix Q = Qorig; -// Q(0,0) = 100.*dqop*dqop; - -// std::cout<< "Q" << std::endl; -// std::cout<< "ihit = " << ihit << " Q" << std::endl; -// std::cout<< Q << std::endl; -// -// std::cout<< "dQ" << std::endl; -// std::cout<< dQ << std::endl; - const float enext = simhit == nullptr ? -99. : std::sqrt(std::pow(simhit->pabs(), 2) + mmu*mmu) - 0.5*simhit->energyLoss(); const float eprednext = std::sqrt(updtsos.segment<3>(3).squaredNorm() + mmu*mmu); @@ -2230,38 +1359,65 @@ void ResidualGlobalCorrectionMakerG4e::produce(edm::Event &iEvent, const edm::Ev e = enext; epred = eprednext; - const float sigmadEval = std::pow(updtsos.segment<3>(3).norm(), 3)/e*std::sqrt(Q(0, 0)); + const float sigmadEval = std::pow(updtsos.segment<3>(3).norm(), 3)/e*std::sqrt(Qcurv(0, 0)); - Matrix localparmsprop; - const Point3DBase posprop(updtsos[0], updtsos[1], updtsos[2]); - const Vector3DBase momprop(updtsos[3], updtsos[4], updtsos[5]); - - const Point3DBase localpos = surface.toLocal(posprop); - const Vector3DBase localmom = surface.toLocal(momprop); + const Matrix localparmsprop = globalToLocal(updtsos, surface); + + Matrix localparms = localparmsprop; -// const Point3DBase localpos = toLocal(surface, posprop); -// const Vector3DBase localmom = toLocal(surface, momprop); + // update state from previous iteration + //momentum kink residual + Matrix dx0 = Matrix::Zero(); - localparmsprop[0] = updtsos[6]/updtsos.segment<3>(3).norm(); - localparmsprop[1] = localmom.x()/localmom.z(); - localparmsprop[2] = localmom.y()/localmom.z(); - localparmsprop[3] = localpos.x(); - localparmsprop[4] = localpos.y(); + if (iiter == 0 && fitFromSimParms_) { + if (simhit == nullptr) { + std::cout << "ABORT: Propagating from sim parameters, but sim hit is not matched!\n"; + valid = false; + break; + } + + auto const &surfaceideal = surfacemapIdealD_.at(hit->geographicalId()); + + + // alternate version with propagation from entry state + + const Point3DBase simlocalpos = simhit->entryPoint(); + const Vector3DBase simlocalmom = simhit->momentumAtEntry(); + + const Point3DBase simglobalpos = surfaceideal.toGlobal(simlocalpos); + const Vector3DBase simglobalmom = surfaceideal.toGlobal(simlocalmom); + + updtsos[0] = simglobalpos.x(); + updtsos[1] = simglobalpos.y(); + updtsos[2] = simglobalpos.z(); + updtsos[3] = simglobalmom.x(); + updtsos[4] = simglobalmom.y(); + updtsos[5] = simglobalmom.z(); + updtsos[6] = genpart->charge(); + + if (false) { + auto const &propresultsim = g4prop->propagateGenericWithJacobianAltD(updtsos, surface, dbetaval, dxival, dmsval, dionival); + + if (!std::get<0>(propresultsim)) { + std::cout << "Abort: Sim state Propagation Failed!" << std::endl; + valid = false; + break; + } + + updtsos = std::get<1>(propresultsim); + } + + localparms = globalToLocal(updtsos, surface); + + dx0 = (localparms - localparmsprop).head<5>(); + + } - Matrix localparms = localparmsprop; - // update state from previous iteration - //momentum kink residual -// AlgebraicVector5 idx0(0., 0., 0., 0., 0.); - Matrix idx0 = Matrix::Zero(); if (dolocalupdate) { if (iiter==0) { - // if (true) { layerStates.push_back(updtsos); - // layerStatesStart.push_back(updtsos); - - } else { //current state from previous state on this layer @@ -2271,754 +1427,338 @@ void ResidualGlobalCorrectionMakerG4e::produce(edm::Event &iEvent, const edm::Ev const Matrix Hold = curv2localJacobianAltelossD(oldtsos, field, surface, dEdxlast, mmu, dbetaval); const Matrix dxlocal = Hold*dxfull.segment<5>(5*(ihit+1)); - const Point3DBase pos(oldtsos[0], oldtsos[1], oldtsos[2]); - const Point3DBase localpos = surface.toLocal(pos); - - const Point3DBase localposupd(localpos.x() + dxlocal[3], localpos.y() + dxlocal[4], localpos.z()); - const Point3DBase posupd = surface.toGlobal(localposupd); - - - const Vector3DBase mom(oldtsos[3], oldtsos[4], oldtsos[5]); - const Vector3DBase localmom = surface.toLocal(mom); - - const double dxdz = localmom.x()/localmom.z(); - const double dydz = localmom.y()/localmom.z(); + localparms = globalToLocal(oldtsos, surface); + localparms.head<5>() += dxlocal; + oldtsos = localToGlobal(localparms, surface); - const double dxdzupd = dxdz + dxlocal[1]; - const double dydzupd = dydz + dxlocal[2]; + updtsos = oldtsos; - const double qop = oldtsos[6]/oldtsos.segment<3>(3).norm(); - const double qopupd = qop + dxlocal[0]; + dx0 = (localparms - localparmsprop).head<5>(); + } + } + + Matrix localparmsalignprop = localparms; + Matrix updtsosalign = updtsos; + + if (fitFromSimParms_) { + // re-propagate to unmodified surface - const double pupd = std::abs(1./qopupd); - const double charge = std::copysign(1., qopupd); + + auto const &surfaceideal = surfacemapIdealD_.at(hit->geographicalId()); - const double signpz = std::copysign(1., localmom.z()); - const double localmomfact = signpz/std::sqrt(1. + dxdzupd*dxdzupd + dydzupd*dydzupd); - const Vector3DBase localmomupd(pupd*dxdzupd*localmomfact, pupd*dydzupd*localmomfact, pupd*localmomfact); - const Vector3DBase momupd = surface.toGlobal(localmomupd); - oldtsos[0] = posupd.x(); - oldtsos[1] = posupd.y(); - oldtsos[2] = posupd.z(); - oldtsos[3] = momupd.x(); - oldtsos[4] = momupd.y(); - oldtsos[5] = momupd.z(); - oldtsos[6] = charge; + // alternate version with propagation from entry state - updtsos = oldtsos; + const Point3DBase simlocalpos = simhit->entryPoint(); + const Vector3DBase simlocalmom = simhit->momentumAtEntry(); - localparms[0] = qopupd; - localparms[1] = dxdzupd; - localparms[2] = dydzupd; - localparms[3] = localposupd.x(); - localparms[4] = localposupd.y(); + const Point3DBase simglobalpos = surfaceideal.toGlobal(simlocalpos); + const Vector3DBase simglobalmom = surfaceideal.toGlobal(simlocalmom); - idx0 = localparms - localparmsprop; + Matrix simtsos; + simtsos[0] = simglobalpos.x(); + simtsos[1] = simglobalpos.y(); + simtsos[2] = simglobalpos.z(); + simtsos[3] = simglobalmom.x(); + simtsos[4] = simglobalmom.y(); + simtsos[5] = simglobalmom.z(); + simtsos[6] = genpart->charge(); + + auto const &surfacealign = surfacemapD_.at(hit->geographicalId()); + auto const &propresultsalign = g4prop->propagateGenericWithJacobianAltD(simtsos, surfacealign, dbetaval, dxival, dmsval, dionival); + if (!std::get<0>(propresultsalign)) { + std::cout << "WARNING propagation for alignment failed!\n"; + valid = false; + break; } + + updtsosalign = std::get<1>(propresultsalign); + + localparmsalignprop = globalToLocal(updtsosalign, surfacealign); } + + //TODO optimize this without ternary functions -// if (false) { -// //current state from previous state on this layer -// //save current parameters -// //debug version -// -// std::cout << "update ihit = " << ihit << std::endl; -// -// Matrix& oldtsos = layerStates[ihit]; -// -// // std::cout << "proptsos" << std::endl; -// // std::cout << updtsos.transpose() << std::endl; -// // std::cout << "oldtsos" << std::endl; -// // std::cout << oldtsos.transpose() << std::endl; -// -// // JacobianCurvilinearToLocal curv2localold(oldtsos.surface(), oldtsos.localParameters(), *oldtsos.magneticField()); -// // const AlgebraicMatrix55& curv2localjacold = curv2localold.jacobian(); -// // const Matrix Hold = Map>(curv2localjacold.Array()); -// // const Matrix Hold = curv2localJacobianAlt(oldtsos); -// const Matrix Hold = curv2localJacobianAltelossD(oldtsos, field, surface, dEdxlast, mmu); -// // const Matrix jacold = curv2cartJacobianAltD(oldtsos); -// -// -// -// // auto const& dxlocal = Hold*dxfull.segment<5>(5*(ihit+1)); -// -// const Matrix dxlocal = Hold*dxfull.segment<5>(5*(ihit+1)); -// -// // const Matrix dxcart = jacold*dxfull.segment<5>(5*(ihit+1)); -// -// -// -// std::cout << "dxfull.segment<5>(5*(ihit+1))" << std::endl; -// std::cout << dxfull.segment<5>(5*(ihit+1)).transpose() << std::endl; -// std::cout << "dxlocal" << std::endl; -// std::cout << dxlocal << std::endl; -// // std::cout << "dxcart" < deltaposlocal(dxlocal[3], dxlocal[4], 0.); -// const Vector3DBase deltaposglobal = toGlobal(surface, deltaposlocal); -// -// std::cout << "deltaposlocal" << std::endl; -// std::cout << deltaposlocal << std::endl; -// std::cout << "deltaposglobal" << std::endl; -// std::cout << deltaposglobal << std::endl; -// -// std::cout << "deltaposlocal.mag()" << std::endl; -// std::cout << deltaposlocal.mag() << std::endl; -// std::cout << "deltaposglobal.mag()" << std::endl; -// std::cout << deltaposglobal.mag() << std::endl; -// -// // Vector3DBase localx(1., 0., 0.); -// // Vector3DBase localy(0., 1., 0.); -// -// const Point3DBase pos(oldtsos[0], oldtsos[1], oldtsos[2]); -// // const Point3DBase localpos = surface.toLocal(pos); -// const Point3DBase localpos = toLocal(surface, pos); -// -// const Point3DBase localposupd(localpos.x() + dxlocal[3], localpos.y() + dxlocal[4], localpos.z()); -// // const Point3DBase posupd = surface.toGlobal(localposupd); -// const Point3DBase posupd = toGlobal(surface, localposupd); -// const Point3DBase postest = toGlobal(surface, localpos); -// -// -// const Vector3DBase mom(oldtsos[3], oldtsos[4], oldtsos[5]); -// // const Vector3DBase localmom = surface.toLocal(mom); -// const Vector3DBase localmom = toLocal(surface, mom); -// -// -// std::cout << "localposupd - localpos" << std::endl; -// std::cout << localposupd - localpos << std::endl; -// std::cout << "posupd - pos" << std::endl; -// std::cout << posupd - pos << std::endl; -// std::cout << "postest - pos" << std::endl; -// std::cout << postest - pos << std::endl; -// -// Matrix rot; -// rot << surface.rotation().xx(), surface.rotation().xy(), surface.rotation().xz(), -// surface.rotation().yx(), surface.rotation().yy(), surface.rotation().yz(), -// surface.rotation().zx(), surface.rotation().zy(), surface.rotation().zz(); -// -// std::cout << "rotpre" << std::endl; -// std::cout << rot << std::endl; -// -// for (unsigned int i = 0; i < 3; ++i) { -// for (unsigned int j = 0; j < i; ++j) { -// rot.row(i) = (rot.row(i) - (rot.row(i)*rot.row(j).transpose())[0]/rot.row(j).squaredNorm()*rot.row(j)).eval(); -// } -// } -// for (unsigned int i = 0; i < 3; ++i) { -// rot.row(i) = (rot.row(i)/rot.row(i).norm()).eval(); -// } -// -// -// const Matrix Itest = rot*rot.transpose(); -// -// std::cout << "rot" << std::endl; -// std::cout << rot << std::endl; -// std::cout << "Itest" << std::endl; -// std::cout << Itest << std::endl; -// -// const Matrix deltposlocaleig(dxlocal[3], dxlocal[4], 0.); -// const Matrix deltposglobaleig = rot.transpose()*deltposlocaleig; -// -// std::cout << "deltposlocaleig" << std::endl; -// std::cout << deltposlocaleig.transpose() << std::endl; -// std::cout << "deltposglobaleig" << std::endl; -// std::cout << deltposglobaleig.transpose() << std::endl; -// -// std::cout << "deltposlocaleig.norm()" << std::endl; -// std::cout << deltposlocaleig.norm() << std::endl; -// std::cout << "deltposglobaleig.norm()" << std::endl; -// std::cout << deltposglobaleig.norm() << std::endl; -// -// const Matrix sposeig(surface.position().x(), surface.position().y(), surface.position().z()); -// -// const Matrix poseig(oldtsos[0], oldtsos[1], oldtsos[2]); -// const Matrix localposeig = rot*(poseig - sposeig); -// const Matrix postesteig = rot.transpose()*localposeig + sposeig; -// -// std::cout << "postesteig - poseig" << std::endl; -// std::cout << (postesteig - poseig).transpose() << std::endl; -// -// -// -// -// const double dxdz = localmom.x()/localmom.z(); -// const double dydz = localmom.y()/localmom.z(); -// -// -// -// const double dxdzupd = dxdz + dxlocal[1]; -// const double dydzupd = dydz + dxlocal[2]; -// -// const double qop = oldtsos[6]/oldtsos.segment<3>(3).norm(); -// const double qopupd = qop + dxlocal[0]; -// -// const double pupd = std::abs(1./qopupd); -// const double charge = std::copysign(1., qopupd); -// -// const double signpz = std::copysign(1., localmom.z()); -// const double localmomfact = signpz/std::sqrt(1. + dxdzupd*dxdzupd + dydzupd*dydzupd); -// const Vector3DBase localmomupd(pupd*dxdzupd*localmomfact, pupd*dydzupd*localmomfact, pupd*localmomfact); -// // const Vector3DBase momupd = surface.toGlobal(localmomupd); -// const Vector3DBase momupd = toGlobal(surface, localmomupd); -// -// const Matrix oldtsosorig = oldtsos; -// -// oldtsos[0] = posupd.x(); -// oldtsos[1] = posupd.y(); -// oldtsos[2] = posupd.z(); -// oldtsos[3] = momupd.x(); -// oldtsos[4] = momupd.y(); -// oldtsos[5] = momupd.z(); -// oldtsos[6] = charge; -// -// updtsos = oldtsos; -// -// std::cout << "oldtsos:" << std::endl; -// std::cout << oldtsosorig.transpose() << std::endl; -// std::cout << "updtsos:" << std::endl; -// std::cout << updtsos.transpose() << std::endl; -// std::cout << "diff:" << std::endl; -// std::cout << (updtsos - oldtsosorig).transpose() << std::endl; -// -// // std::cout << "updtsos" << std::endl; -// // std::cout << updtsos.transpose() << std::endl; -// // -// // std::cout << "updtsos p = " << updtsos.segment<3>(3).norm() << " target p = " << pupd << std::endl; -// -// localparms[0] = qopupd; -// localparms[1] = dxdzupd; -// localparms[2] = dydzupd; -// localparms[3] = localposupd.x(); -// localparms[4] = localposupd.y(); -// -// idx0 = localparms - localparmsprop; -// -// -// } + Matrix Hp = Hm; - //apply measurement update if applicable -// std::cout << "constructing preciseHit" << std::endl; -// const Matrix starttsos = layerStatesStart[ihit]; -// const GlobalPoint pos(starttsos[0], starttsos[1], starttsos[2]); -// const GlobalVector mom(starttsos[3], starttsos[4], starttsos[5]); -// const GlobalTrajectoryParameters glob(pos, mom, starttsos[6], field); - const GlobalPoint pos(updtsos[0], updtsos[1], updtsos[2]); - const GlobalVector mom(updtsos[3], updtsos[4], updtsos[5]); - const GlobalTrajectoryParameters glob(pos, mom, updtsos[6], field); -// const TrajectoryStateOnSurface tsostmp(glob, surface); - const TrajectoryStateOnSurface tsostmp(glob, *hit->surface()); - - auto const& preciseHit = hit->isValid() ? cloner.makeShared(hit, tsostmp) : hit; - if (hit->isValid() && !preciseHit->isValid()) { - std::cout << "Abort: Failed updating hit" << std::endl; - valid = false; - break; + if (dolocalupdate) { + Hp = curv2localJacobianAltelossD(updtsos, field, surface, dEdxlast, mmu, dbetaval); + } + + Matrix Q = Qcurv; + if (dolocalupdate) { + Q = Hm*Qcurv*Hm.transpose(); } -// const uint32_t gluedid = trackerTopology->glued(preciseHit->det()->geographicalId()); -// const bool isglued = gluedid != 0; -// const DetId parmdetid = isglued ? DetId(gluedid) : preciseHit->geographicalId(); -// const bool align2d = detidparms.count(std::make_pair(1, parmdetid)); -// const GeomDet* parmDet = isglued ? globalGeometry->idToDet(parmdetid) : preciseHit->det(); - - const bool align2d = detidparms.count(std::make_pair(1, preciseHit->geographicalId())); - - // curvilinear to local jacobian -// JacobianCurvilinearToLocal curv2localp(updtsos.surface(), updtsos.localParameters(), *updtsos.magneticField()); -// const AlgebraicMatrix55& curv2localjacp = curv2localp.jacobian(); -// const Matrix Hp = Map>(curv2localjacp.Array()); -// const Matrix Hp = curv2localJacobianAlt(updtsos); - const Matrix Hp = curv2localJacobianAltelossD(updtsos, field, surface, dEdxlast, mmu, dbetaval); + const Matrix Qinv = Q.inverse(); -// const Matrix curvcov = covfull.block<5, 5>(5*(ihit+1), 5*(ihit+1)); -// - const Matrix localconv = localPositionConvolutionD(updtsos, Qtot, surface); -// -// if (abs(localconv[0]) > 5e-3 || abs(localconv[1]) > 5e-3) { -// std::cout << "localconv" << std::endl; -// std::cout << localconv << std::endl; -// } + Matrix dQMS = dQMScurv; + Matrix dQI = dQIcurv; + if (dolocalupdate) { + dQMS = Hm*dQMScurv*Hm.transpose(); + dQI = Hm*dQIcurv*Hm.transpose(); + } + { + constexpr unsigned int nlocalcons = 5; + constexpr unsigned int nlocalstateparms = 5; + constexpr unsigned int nlocalbfield = 1; + constexpr unsigned int nlocaleloss = 1; + constexpr unsigned int nlocalparms = nlocalbfield + nlocaleloss; - //FIXME take care of this elsewhere for the moment -// const bool genconstraint = dogen && ihit==0; -// const bool genconstraint = false; - - if (true) { + const unsigned int fullstateidx = 5*ihit; + const unsigned int fullparmidx = iparm; - //momentum kink residual -// const Vector5d dx0 = Map(idx0.Array()); - const Vector5d dx0 = idx0; + rfull.segment(icons) = dx0; - -// const uint32_t gluedidip1 = trackerTopology->glued(hits[ihit + 1]->det()->geographicalId()); -// const bool isgluedip1 = gluedidip1 != 0; -// const DetId parmdetidip1 = isgluedip1 ? DetId(gluedidip1) : hits[ihit + 1]->geographicalId(); -// const unsigned int bfieldidx = detidparms.at(std::make_pair(6, parmdetidip1)); -// fieldOffset->setOffset(corparms_[bfieldidx]); + if (dolocalupdate) { + Ffull.block(icons, fullstateidx) = -Hm*FdFm.leftCols(); + Ffull.block(icons, fullstateidx + nlocalstateparms) = Hp; + Jfull.block(icons, fullparmidx) = -Hm*FdFm.rightCols(); + } + else { + Ffull.block(icons, fullstateidx) = -FdFm.leftCols(); + Ffull.block(icons, fullstateidx + nlocalstateparms) = Matrix::Identity(); + Jfull.block(icons, fullparmidx) = -FdFm.rightCols(); + } -// if (ihit==0) { -// FreeTrajectoryState tmpfts(updtsospos, updtsos.globalParameters().momentum(), updtsos.charge(), field); -// propresult = fPropagator->geometricalPropagator().propagateWithPath(tmpfts, *hits[ihit+1]->surface()); -// } -// else { -// propresult = fPropagator->geometricalPropagator().propagateWithPath(updtsos, *hits[ihit+1]->surface()); -// } + Vinvfull.block(icons, icons) = Qinv; +// Vinvfullalt.block(icons, icons) = Qinv; + Vinvfullalt.block(icons, icons) = (Q - 0.1*dQMS).inverse(); + if (dores) { + std::vector> coeffs; + for (unsigned int irow = 0; irow < nlocalcons; ++irow) { + for (unsigned int icol = 0; icol < nlocalcons; ++icol) { + coeffs.emplace_back(icons + irow, icons + icol, dQMS(irow, icol)); + } + } + SparseMatrix &dV = dVs.emplace_back(ncons, ncons); + dV.setFromTriplets(coeffs.begin(), coeffs.end()); + // MS resolution parameter is decoupled from energy loss + residxs.push_back(iparm + 2); + } + + if (dores) { + std::vector> coeffs; + for (unsigned int irow = 0; irow < nlocalcons; ++irow) { + for (unsigned int icol = 0; icol < nlocalcons; ++icol) { + coeffs.emplace_back(icons + irow, icons + icol, dQI(irow, icol)); + } + } + SparseMatrix &dV = dVs.emplace_back(ncons, ncons); + dV.setFromTriplets(coeffs.begin(), coeffs.end()); + // energy loss resolution parameter is decoupled from the energy loss mean value + residxs.push_back(iparm + 3); + } -// // auto const &surfaceip1 = *hits[ihit+1]->surface(); -// auto const &surfaceip1 = *surfacemap_.at(hits[ihit+1]->geographicalId()); -// propresult = thePropagator->propagateWithPath(updtsos, surfaceip1); -// // propresult = fPropagator->geometricalPropagator().propagateWithPath(updtsos, *hits[ihit+1]->surface()); -// if (!propresult.first.isValid()) { -// std::cout << "Abort: Propagation Failed!" << std::endl; -// valid = false; -// break; -// } - -// const double dqopnext = propresult.first.signedInverseMomentum() - updtsos.signedInverseMomentum(); - if (true) { + icons += nlocalcons; - constexpr unsigned int nlocalstate = 10; - constexpr unsigned int nlocalbfield = 1; - constexpr unsigned int nlocaleloss = 1; - constexpr unsigned int nlocalparms = nlocalbfield + nlocaleloss; - - constexpr unsigned int nlocal = nlocalstate + nlocalbfield + nlocaleloss; - - using MSScalar = AANT; - - constexpr unsigned int localstateidx = 0; + globalidxv[iparm] = bfieldglobalidx; + iparm++; - constexpr unsigned int localparmidx = localstateidx + nlocalstate; - - const unsigned int fullstateidx = 5*ihit; - const unsigned int fullparmidx = nstateparms + parmidx; - - - Matrix Fstate = FdFm.leftCols<5>().cast(); - Matrix Fb = FdFm.col(5).cast(); - Matrix Fxi = FdFm.col(6).cast(); - - Matrix Hmstate = Hm.cast(); - Matrix Hpstate = Hp.cast(); - - Matrix Qinv = Q.inverse().cast(); - - // initialize active scalars for state parameters - Matrix dum = Matrix::Zero(); - //suppress gradients of reference point parameters when fitting with gen constraint - for (unsigned int j=0; j du = Matrix::Zero(); - for (unsigned int j=0; j dprop = dx0.cast() + Hpstate*du - Hmstate*Fstate*dum - Hmstate*Fb*dbeta - Hmstate*Fxi*dxi; -// const Matrix dprop = dx0.cast() + du - Fstate*dum - Fb*dbeta; - - -// const MSScalar chisq = dprop.transpose()*Qinv*dprop; - - MSScalar chisq; - - if (!islikelihood) { - chisq = dprop.transpose()*Qinv*dprop; - } - else { -// const Matrix dQdqop = dQ.cast(); -// const Matrix dQdxi = dQ.cast(); -// const Matrix Qinvfull = (1. - dxi)*Qinv; -// const Matrix Qinvfull = Qinv/(1. + dxi); - const Matrix Qinvfull = Qinv*(1. - dxi); -// const Matrix Qinvfull = Qinv - dxi*Qinv*dQdxi*Qinv; - chisq = dprop.transpose()*Qinvfull*dprop; -// chisq = dprop.transpose()*Qinvfull*dprop + 5.*internal::log(1. + dxi); - - const MSScalar dchisq = -dprop.transpose()*Qinv*dprop; -// const MSScalar dchisq = -dprop.transpose()*Qinv*dQdxi*Qinv*dprop; - - //TODO should this be 11x11 instead? - //TODO check additional factor of 2 - -// std::cout << "extra xi grad = " << dchisq.value().value() << std::endl; - - - Matrix dhesslocal; - for (unsigned int j=0; jisValid()) { + + //apply measurement update if applicable + LocalTrajectoryParameters locparm(localparmsalignprop[0], + localparmsalignprop[1], + localparmsalignprop[2], + localparmsalignprop[3], + localparmsalignprop[4], + localparmsalignprop[5]); + const TrajectoryStateOnSurface tsostmp(locparm, *hit->surface(), field); + + auto const& preciseHit = cloner.makeShared(hit, tsostmp); + if (!preciseHit->isValid()) { + std::cout << "Abort: Failed updating hit" << std::endl; + valid = false; + break; + } -// const unsigned int fullxiidx = fullparmidx + 1; - - dhessv[ihit].block(fullstateidx, fullstateidx) += dhesslocal.topLeftCorner(); - -// for (unsigned int i=1; i(fullstateidx) += gradlocal.head(); - gradfull.segment(fullparmidx) += gradlocal.segment(localparmidx); - - //fill global hessian (upper triangular blocks only) - hessfull.block(fullstateidx, fullstateidx) += hesslocal.topLeftCorner(); - hessfull.block(fullstateidx, fullparmidx) += hesslocal.topRightCorner(); - hessfull.block(fullparmidx, fullparmidx) += hesslocal.bottomRightCorner(); - + const bool align2d = detidparms.count(std::make_pair(1, aligndetid)); - } - - const unsigned int bfieldglobalidx = detidparms.at(std::make_pair(6, parmdetid)); - globalidxv[parmidx] = bfieldglobalidx; - parmidx++; - - const unsigned int elossglobalidx = detidparms.at(std::make_pair(7, parmdetid)); - globalidxv[parmidx] = elossglobalidx; - parmidx++; - - //backwards propagation jacobian (local to local) to be used at the next layer -// FdFm = curv2curvTransportJacobian(*updtsos.freeState(), propresult, false); -// FdFm = localTransportJacobian(updtsos, propresult, false); -// dqop = dqopnext; - - } + const Matrix &Rglued = rgluemap_.at(preciseHit->geographicalId()); + const GloballyPositioned &surfaceglued = surfacemapD_.at(parmdetid); - if (preciseHit->isValid()) { + const Matrix curvcov = covfull.block<5, 5>(5*(ihit+1), 5*(ihit+1)); - auto fillAlignGrads = [&](auto Nalign) { - constexpr unsigned int nlocalstate = 2; - constexpr unsigned int localstateidx = 0; - constexpr unsigned int localalignmentidx = nlocalstate; - constexpr unsigned int localparmidx = localalignmentidx; + const Matrix localconv = localPositionConvolutionD(updtsos, curvcov, surface); - // abusing implicit template argument to pass - // a template value via std::integral_constant - constexpr unsigned int nlocalalignment = Nalign(); - constexpr unsigned int nlocalparms = nlocalalignment; - constexpr unsigned int nlocal = nlocalstate + nlocalparms; + { + constexpr unsigned int nlocalstate = 2; - using AlignScalar = AANT; + const unsigned int nlocalalignment = align2d ? 6 : 5; + const unsigned int nlocalparms = nlocalalignment; const unsigned int fullstateidx = 5*(ihit+1) + 3; - const unsigned int fullparmidx = nstateparms + nparsBfield + nparsEloss + alignmentparmidx; + const unsigned int fullparmidx = iparm; const bool ispixel = GeomDetEnumerators::isTrackerPixel(preciseHit->det()->subDetector()); - - //TODO add hit validation stuff - //TODO add simhit stuff - -// const PSimHit *matchedsim = nullptr; -// for (auto const& simhith : simHits) { -// for (const PSimHit& simHit : *simhith) { -// if (std::abs(simHit.particleType()) == 13 && simHit.detUnitId() == preciseHit->geographicalId()) { -// matchedsim = &simHit; -// break; -// } -// } -// if (matchedsim) { -// break; -// } -// } - -// const bool hit1d = preciseHit->dimension() == 1 || (detidlayermap.at(preciseHit->geographicalId())[0] == 0 && detidlayermap.at(preciseHit->geographicalId())[1] == 1); -// const bool hit1d = preciseHit->dimension() == 1 || ispixel; const bool hit1d = preciseHit->dimension() == 1; - - Matrix Hu = Hp.bottomRightCorner<2,2>().cast(); + const Matrix Hu = Hp.bottomRightCorner<2,2>(); - Matrix dy0; - Matrix Vinv; + Matrix dy0; + Matrix iV; // rotation from module to strip coordinates -// Matrix R; Matrix2d R; -// if (preciseHit->dimension() == 1) { + + const double lxcor = localparmsalignprop[3]; + const double lycor = localparmsalignprop[4]; + + + const Topology &topology = preciseHit->det()->topology(); + + // undo deformation correction + const LocalPoint lpnull(0., 0.); + const MeasurementPoint mpnull = topology.measurementPosition(lpnull); + const Topology::LocalTrackPred pred(tsostmp.localParameters().vector()); + + auto const defcorr = topology.localPosition(mpnull, pred) - topology.localPosition(mpnull); + + const double hitx = preciseHit->localPosition().x() - defcorr.x(); + const double hity = preciseHit->localPosition().y() - defcorr.y(); + + double dxrecsimval = -99.; + double dyrecsimval = -99.; + + if (simhit != nullptr) { + dxrecsimval = hitx - simhit->localPosition().x(); + dyrecsimval = hity - simhit->localPosition().y(); + } + + double lyoffset = 0.; + double hitphival = -99.; + double localphival = -99.; + + if (false) { + std::cout << "ihit = " << ihit << " orig x y = " << preciseHit->localPosition().x() << " " << preciseHit->localPosition().y() << " defcor x y = " << defcorr.x() << " " << defcorr.y() << " hit x y = " << hitx << " " << hity << std::endl; + } + if (hit1d) { -// std::cout << "1d hit" << std::endl; -// assert(!align2d); -// dy0[0] = AlignScalar(matchedsim->localPosition().x() - updtsos.localPosition().x()); - dy0[0] = AlignScalar(preciseHit->localPosition().x() - localparms[3]); - dy0[1] = AlignScalar(0.); - -// bool simvalid = false; -// for (auto const& simhith : simHits) { -// for (const PSimHit& simHit : *simhith) { -// if (simHit.detUnitId() == preciseHit->geographicalId()) { -// -// dy0[0] = AlignScalar(simHit.localPosition().x() - updtsos.localPosition().x()); -// -// simvalid = true; -// break; -// } -// } -// if (simvalid) { -// break; -// } -// } + const ProxyStripTopology *proxytopology = dynamic_cast(&(preciseHit->det()->topology())); + + dy0[0] = hitx - lxcor; + dy0[1] = hity - lycor; + + const double striplength = proxytopology->stripLength(); + const double yerr2 = striplength*striplength/12.; - Vinv = Matrix::Zero(); - Vinv(0,0) = 1./preciseHit->localPositionError().xx(); + iV = Matrix::Zero(); + iV(0, 0) = preciseHit->localPositionError().xx(); + //iV(1, 1) = yerr2; -// R = Matrix::Identity(); R = Matrix2d::Identity(); + + +// std::cout << "1d hit, original x = " << preciseHit->localPosition().x() << " y = " << preciseHit->localPosition().y() << " corrected x = " << hitx << " y = " << hity << std::endl; } else { // 2d hit // assert(align2d); - Matrix2d iV; - iV << preciseHit->localPositionError().xx(), preciseHit->localPositionError().xy(), - preciseHit->localPositionError().xy(), preciseHit->localPositionError().yy(); - if (ispixel) { -// if (true) { -// std::cout << "2d pixel hit" << std::endl; - //take 2d hit as-is for pixels -// dy0[0] = AlignScalar(matchedsim->localPosition().x() - updtsos.localPosition().x()); -// dy0[1] = AlignScalar(matchedsim->localPosition().y() - updtsos.localPosition().y()); - dy0[0] = AlignScalar(preciseHit->localPosition().x() - localparms[3]); - dy0[1] = AlignScalar(preciseHit->localPosition().y() - localparms[4]); - Vinv = iV.inverse().cast(); - //FIXME various temporary hacks; - -// dy0[1] = AlignScalar(0.); -// Vinv = Matrix::Zero(); -// Vinv(0,0) = 1./preciseHit->localPositionError().xx(); - -// if (GeomDetEnumerators::isEndcap(preciseHit->det()->subDetector())) { -// if (GeomDetEnumerators::isBarrel(preciseHit->det()->subDetector())) { -// PXBDetId detidtest(preciseHit->det()->geographicalId()); -// int layertest = detidtest.layer(); -// -// if (layertest > 1) { -// Vinv = Matrix::Zero(); -// } -// -// // Vinv = Matrix::Zero(); -// // dy0[0] = AlignScalar(0.); -// // dy0[1] = AlignScalar(0.); -// } - -// bool simvalid = false; -// for (auto const& simhith : simHits) { -// for (const PSimHit& simHit : *simhith) { -// if (simHit.detUnitId() == preciseHit->geographicalId()) { -// -// if (GeomDetEnumerators::isBarrel(preciseHit->det()->subDetector())) { -// dy0[0] = AlignScalar(simHit.localPosition().x() - updtsos.localPosition().x()); -// dy0[1] = AlignScalar(simHit.localPosition().y() - updtsos.localPosition().y()); -// } -// -// // dy0[1] = AlignScalar(0.); -// -// simvalid = true; -// break; -// } -// } -// if (simvalid) { -// break; -// } -// } - - -// R = Matrix::Identity(); + if (ispixel) { + + dy0[0] = hitx - lxcor; + dy0[1] = hity - lycor; + + iV << preciseHit->localPositionError().xx(), preciseHit->localPositionError().xy(), + preciseHit->localPositionError().xy(), preciseHit->localPositionError().yy(); + R = Matrix2d::Identity(); } else { -// std::cout << "2d strip hit" << std::endl; - if (std::abs(trackEta) < 0.9) { - std::cout << "2d strip hit in the barrel, trackEta = " << trackEta << std::endl; - } - - // diagonalize and take only smallest eigenvalue for 2d hits in strip wedge modules, - // since the constraint parallel to the strip is spurious - SelfAdjointEigenSolver eigensolver(iV); - -// std::cout << "iV" << std::endl; -// std::cout << iV << std::endl; -// std::cout << "2d strip hit eigenvalues:" << std::endl; -// std::cout << eigensolver.eigenvalues() << std::endl; - -// const Matrix2d& v = eigensolver.eigenvectors(); - R = eigensolver.eigenvectors().transpose(); - if (R(0,0) < 0.) { - R.row(0) *= -1.; - } - if (R(1,1) <0.) { - R.row(1) *= -1.; + // transform to polar coordinates to end the madness + //TODO handle the module deformations consistently here (currently equivalent to dropping/undoing deformation correction) + +// std::cout << "wedge\n" << std::endl; + + const ProxyStripTopology *proxytopology = dynamic_cast(&(preciseHit->det()->topology())); + + const TkRadialStripTopology *radialtopology = dynamic_cast(&proxytopology->specificTopology()); + + const double rdir = radialtopology->yAxisOrientation(); + const double radius = radialtopology->originToIntersection(); + + const double phihit = rdir*std::atan2(hitx, rdir*hity + radius); + const double rhohit = std::sqrt(hitx*hitx + std::pow(rdir*hity + radius, 2)); + + // invert original calculation of covariance matrix to extract variance on polar angle + const double detHeight = radialtopology->detHeight(); + const double radsigma = detHeight*detHeight/12.; + + const double t1 = std::tan(phihit); + const double t2 = t1*t1; + + const double tt = preciseHit->localPositionError().xx() - t2*radsigma; + + const double phierr2 = tt / std::pow(radialtopology->centreToIntersection(), 2); + + const double striplength = detHeight * std::sqrt(1. + std::pow( hitx/(rdir*hity + radius), 2) ); + + const double rhoerr2 = striplength*striplength/12.; + + +// std::cout << "rhohit = " << rhohit << " rhobar = " << rhobar << " rhoerr2lin = " << rhoerr2lin << " rhoerr2 = " << rhoerr2 << std::endl; + + // TODO apply (inverse) corrections for module deformations here? (take into account for jacobian?) + const double phistate = rdir*std::atan2(lxcor, rdir*lycor + radius); + const double rhostate = std::sqrt(lxcor*lxcor + std::pow(rdir*lycor + radius, 2)); + + if (simhit != nullptr) { + const double lxsim = simhit->localPosition().x(); + const double lysim = simhit->localPosition().y(); + + const double phisim = rdir*std::atan2(lxsim, rdir*lysim + radius); + const double rhosim = std::sqrt(lxsim*lxsim + std::pow(rdir*lysim + radius, 2)); + + dxrecsimval = phihit - phisim; + dyrecsimval = rhohit - rhosim; } - -// std::cout << "R:" << std::endl; -// std::cout << R << std::endl; - - Matrix dy0local; -// dy0local[0] = matchedsim->localPosition().x() - updtsos.localPosition().x(); -// dy0local[1] = matchedsim->localPosition().y() - updtsos.localPosition().y(); - dy0local[0] = preciseHit->localPosition().x() - localparms[3]; - dy0local[1] = preciseHit->localPosition().y() - localparms[4]; - -// std::cout << "preciseHit->localPosition()" << std::endl; -// std::cout << preciseHit->localPosition() << std::endl; -// std::cout << "updtsos.localPosition()" << std::endl; -// std::cout << updtsos.localPosition() << std::endl; -// std::cout << "dy0local" << std::endl; -// std::cout << dy0local << std::endl; - -// bool simvalid = false; -// for (auto const& simhith : simHits) { -// for (const PSimHit& simHit : *simhith) { -// if (simHit.detUnitId() == preciseHit->geographicalId()) { -// -// dy0local[0] = simHit.localPosition().x() - updtsos.localPosition().x(); -// dy0local[1] = simHit.localPosition().y() - updtsos.localPosition().y(); -// -// simvalid = true; -// break; -// } -// } -// if (simvalid) { -// break; -// } -// } - - const Matrix dy0eig = R*dy0local; - -// std::cout << "dy0eig" << std::endl; -// std::cout << dy0eig << std::endl; - - //TODO deal properly with rotations (rotate back to module local coords?) - dy0[0] = AlignScalar(dy0eig[0]); - dy0[1] = AlignScalar(0.); - - Vinv = Matrix::Zero(); - Vinv(0,0) = AlignScalar(1./eigensolver.eigenvalues()[0]); - -// Matrix dy0eig = R*dy0local; -// dy0eig[1] = 0.; -// -// Matrix2d diag = Matrix2d::Zero(); -// diag(0, 0) = 1./eigensolver.eigenvalues()[0]; -// // diag(1, 1) = 1./eigensolver.eigenvalues()[1]; -// -// dy0 = (R.transpose()*dy0eig).cast(); -// Vinv = (R.transpose()*diag*R).cast(); -// -// R = Matrix2d::Identity(); - - -// dy0[0] = AlignScalar(dy0eig[0]); -// dy0[1] = AlignScalar(dy0eig[1]); -// -// Vinv = Matrix::Zero(); -// Vinv(0,0) = AlignScalar(1./eigensolver.eigenvalues()[0]); -// Vinv(1,1) = AlignScalar(1./eigensolver.eigenvalues()[1]); - - -// R = v.transpose().cast(); - + + iV = Matrix::Zero(); + iV(0, 0) = phierr2; + //iV(1, 1) = rhoerr2; + + // jacobian from localx-localy to localphi-localrho + R = Matrix2d::Zero(); + + const double yp = rdir*lycor + radius; + const double invden = 1./(lxcor*lxcor + yp*yp); + + // dphi / dx + R(0, 0) = rdir*yp*invden; + // dphi / dy + R(0, 1) = -lxcor*invden; + // drho / dx + R(1, 0) = lxcor/rhostate; + // drho / dy + R(1, 1) = rdir*(rdir*lycor + radius)/rhostate; + + + dy0[0] = phihit - phistate; + dy0[1] = rhohit - rhostate; + +// std::cout << "wedge hit, original x = " << preciseHit->localPosition().x() << " y = " << preciseHit->localPosition().y() << " corrected x = " << hitx << " y = " << hity << std::endl; + } } @@ -3027,154 +1767,173 @@ void ResidualGlobalCorrectionMakerG4e::produce(edm::Event &iEvent, const edm::Ev validdxeigjac.block<2,2>(2*ivalidhit, 3*(ihit+1)) = R*Hp.bottomRightCorner<2,2>(); - const Matrix Ralign = R.cast(); - - Matrix dx = Matrix::Zero(); - for (unsigned int j=0; j Aval = Matrix::Zero(); + +// const Matrix &localparmsalign = alignGlued_ ? globalToLocal(updtsos, surfaceglued) : localparms; - Matrix dalpha = Matrix::Zero(); - // order in which to use parameters, especially relevant in case nlocalalignment < 6 - constexpr std::array alphaidxs = {{5, 0, 1, 2, 3, 4}}; - for (unsigned int idim=0; idim localparmsalign = localparms; + Matrix localparmsalign = localparmsalignprop; + if (alignGlued_) { + localparmsalign = globalToLocal(updtsosalign, surfaceglued); } - - // alignment jacobian - Matrix A = Matrix::Zero(); - const double localqopval = localparms[0]; - const double localdxdzval = localparms[1]; - const double localdydzval = localparms[2]; - const double localxval = localparms[3]; - const double localyval = localparms[4]; + const double localqopval = localparmsalign[0]; + const double localdxdzval = localparmsalign[1]; + const double localdydzval = localparmsalign[2]; + const double localxval = localparmsalign[3]; + const double localyval = localparmsalign[4]; + //standard case + // dx/dx - A(0,0) = AlignScalar(1.); + Aval(0,0) = -1.; // dy/dy - A(1,1) = AlignScalar(1.); + Aval(1,1) = -1.; // dx/dz - A(0,2) = localdxdzval; + Aval(0,2) = localdxdzval; // dy/dz - A(1,2) = localdydzval; + Aval(1,2) = localdydzval; // dx/dtheta_x - A(0,3) = -localyval*localdxdzval; + Aval(0,3) = localyval*localdxdzval; // dy/dtheta_x - A(1,3) = -localyval*localdydzval; + Aval(1,3) = localyval*localdydzval; // dx/dtheta_y - A(0,4) = -localxval*localdxdzval; + Aval(0,4) = -localxval*localdxdzval; // dy/dtheta_y - A(1,4) = -localxval*localdydzval; + Aval(1,4) = -localxval*localdydzval; // dx/dtheta_z - A(0,5) = -localyval; + Aval(0,5) = localyval; // dy/dtheta_z - A(1,5) = localxval; - - -// std::cout << "strip local z shift gradient: " << (Ralign*A.col(2))[0].value().value() << std::endl; + Aval(1,5) = -localxval; + +// const Matrix &A = alignGlued_ ? Rglued*Aval : Aval; - // rotation from alignment basis to module local coordinates -// Matrix A; -// if (isglued) { -// const GlobalVector modx = preciseHit->det()->surface().toGlobal(LocalVector(1.,0.,0.)); -// const GlobalVector mody = preciseHit->det()->surface().toGlobal(LocalVector(0.,1.,0.)); -// -// const GlobalVector gluedx = parmDet->surface().toGlobal(LocalVector(1.,0.,0.)); -// const GlobalVector gluedy = parmDet->surface().toGlobal(LocalVector(0.,1.,0.)); -// -// A(0,0) = AlignScalar(modx.dot(gluedx)); -// A(0,1) = AlignScalar(modx.dot(gluedy)); -// A(1,0) = AlignScalar(mody.dot(gluedx)); -// A(1,1) = AlignScalar(mody.dot(gluedy)); -// } -// else { -// A = Matrix::Identity(); -// } -// -// Matrix dh = dy0 - R*Hu*dx - R*A*dalpha; + Matrix A = R*Aval; + if (alignGlued_) { + A = R*Rglued*Aval; + } - - double thetaincidence = std::asin(1./std::sqrt(std::pow(localdxdzval,2) + std::pow(localdydzval,2) + 1.)); + const unsigned int xresglobalidx = dores ? detidparms.at(std::make_pair(8, hit->geographicalId())) : 0; + const unsigned int yresglobalidx = dores && ispixel ? detidparms.at(std::make_pair(9, hit->geographicalId())) : 0; + + // apply correction to hit covariance from previous iterations if applicable + const double xxresfact = dores ? std::exp(corparms_[xresglobalidx]) : 1.; + const double yyresfact = dores && ispixel ? std::exp(corparms_[yresglobalidx]) : 1.; + const double xyresfact = std::sqrt(xxresfact*yyresfact); -// bool morehitquality = applyHitQuality_ ? thetaincidence > 0.25 : true; - bool morehitquality = true; +// std::cout << "xxresfact = " << xxresfact << " yyresfact = " << yyresfact << " xyresfact = " << xyresfact << std::endl; + + iV(0, 0) *= xxresfact; + iV(0, 1) *= xyresfact; + iV(1, 0) *= xyresfact; + iV(1, 1) *= yyresfact; - if (morehitquality) { - nValidHitsFinal++; + // local x/phi resolution variation + if (dores) { + // scale diagonal element by exp(parm), preserve correlations + const double dVxx = iV(0, 0); + + std::vector> coeffs; + coeffs.reserve(3); + coeffs.emplace_back(icons, icons, dVxx); + + // cross terms only relevant/exist for pixel hits if (ispixel) { - nValidPixelHitsFinal++; + const double dVxy = 0.5*iV(0, 1); + coeffs.emplace_back(icons, icons + 1, dVxy); + coeffs.emplace_back(icons + 1, icons, dVxy); } + + SparseMatrix &dV = dVs.emplace_back(ncons, ncons); + dV.setFromTriplets(coeffs.begin(), coeffs.end()); + residxs.push_back(iparm + nlocalalignment); } - else { - Vinv = Matrix::Zero(); + + // local y resolution variation + if (dores && ispixel) { + // scale diagonal element by exp(parm), preserve correlations + const double dVxy = 0.5*iV(0, 1); + const double dVyy = iV(1, 1); + + std::vector> coeffs; + coeffs.reserve(3); + coeffs.emplace_back(icons, icons + 1, dVxy); + coeffs.emplace_back(icons + 1, icons, dVxy); + coeffs.emplace_back(icons + 1, icons + 1, dVyy); + + SparseMatrix &dV = dVs.emplace_back(ncons, ncons); + dV.setFromTriplets(coeffs.begin(), coeffs.end()); + residxs.push_back(iparm + nlocalalignment + 1); } - Matrix dh = dy0 - Ralign*Hu*dx - Ralign*A*dalpha; -// Matrix dh = dy0 - Ralign*dx - Ralign*A*dalpha; - AlignScalar chisq = dh.transpose()*Vinv*dh; - - - chisq0val += chisq.value().value(); - -// std::cout << "ihit = " << ihit << " meas chisq = " << chisq.value().value() << std::endl; + constexpr std::array alphaidxs = {{0, 2, 3, 4, 5, 1}}; - auto const& gradlocal = chisq.value().derivatives(); - //fill local hessian - Matrix hesslocal; - for (unsigned int j=0; j gradloctest0; -// Matrix gradloctest1; -// Matrix gradloctest2; +// const double scalecov = hit1d ? 1.2 : 1.0; + const double scalecov = ispixel ? 0.8 : 1.2; -// std::cout << "nlocalalignment: " << nlocalalignment << " nlocal: " << nlocal << std::endl; -// std::cout << "gradlocal type: " << typeid(gradlocal).name() << std::endl; -// std::cout << "gradloctest0 type: " << typeid(gradloctest0).name() << std::endl; -// std::cout << "gradloctest1 type: " << typeid(gradloctest1).name() << std::endl; -// std::cout << "gradloctest2 type: " << typeid(gradloctest2).name() << std::endl; -// -// std::cout << "nhits: " << nhits << " nvalid: " << nvalid << " nvalidalign2d: " << nvalidalign2d << " ihit: " << ihit << std::endl; -// std::cout << "gradfull.size(): " << gradfull.size() << " nlocalstate: " << nlocalstate << " fullstateidx: " << fullstateidx << " nlocalparms: " << nlocalparms << " fullparmidx: " << fullparmidx << std::endl; - - // FIXME the templated block functions don't work here for some reason - //fill global gradient - gradfull.segment(fullstateidx) += gradlocal.head(nlocalstate); - gradfull.segment(fullparmidx) += gradlocal.segment(localparmidx, nlocalparms); - - //fill global hessian (upper triangular blocks only) - hessfull.block(fullstateidx, fullstateidx) += hesslocal.topLeftCorner(nlocalstate,nlocalstate); - hessfull.block(fullstateidx, fullparmidx) += hesslocal.topRightCorner(nlocalstate, nlocalparms); - hessfull.block(fullparmidx, fullparmidx) += hesslocal.bottomRightCorner(nlocalparms, nlocalparms); + if (ispixel) { + constexpr unsigned int nlocalcons = 2; + + rfull.segment(icons) = dy0; + Ffull.block(icons, fullstateidx) = -R*Hu; + + for (unsigned int ialign = 0; ialign < nlocalalignment; ++ialign) { + Jfull.block(icons, fullparmidx + ialign) = -A.col(alphaidxs[ialign]); + } + + Vinvfull.block(icons, icons) = iV.inverse(); + Vinvfullalt.block(icons, icons) = (scalecov*iV).inverse(); + + icons += nlocalcons; + } + else { + constexpr unsigned int nlocalcons = 1; + + rfull(icons) = dy0(0); + Ffull.block<1, nlocalstate>(icons, fullstateidx) = (-R*Hu).row(0); + + for (unsigned int ialign = 0; ialign < nlocalalignment; ++ialign) { + Jfull(icons, fullparmidx + ialign) = -A(0, alphaidxs[ialign]); + } + + Vinvfull(icons, icons) = 1./iV(0, 0); + Vinvfullalt(icons, icons) = 1./(scalecov*iV)(0, 0); + + icons += nlocalcons; + } for (unsigned int idim=0; idimgeographicalId())); - globalidxv[nparsBfield + nparsEloss + alignmentparmidx] = xglobalidx; - alignmentparmidx++; + const unsigned int xglobalidx = detidparms.at(std::make_pair(alphaidxs[idim], aligndetid)); + globalidxv[iparm] = xglobalidx; + iparm++; if (alphaidxs[idim]==0) { hitidxv.push_back(xglobalidx); } } + if (dores) { + globalidxv[iparm] = xresglobalidx; + ++iparm; + } + + if (dores && ispixel) { + globalidxv[iparm] = yresglobalidx; + ++iparm; + } + + localqop_iter[ivalidhit] = localqopval; + localdxdz_iter[ivalidhit] = localdxdzval; + localdydz_iter[ivalidhit] = localdydzval; + localx_iter[ivalidhit] = localxval; + localy_iter[ivalidhit] = localyval; + + dxrecgen_iter[ivalidhit] = dy0[0]; + dyrecgen_iter[ivalidhit] = dy0[1]; + + const double localqopvar = covfull(5*(ihit+1), 5*(ihit+1)); + localqoperralt[ivalidhit] = std::sqrt(localqopvar); + if (iiter == 0) { // fill hit validation information @@ -3183,23 +1942,48 @@ void ResidualGlobalCorrectionMakerG4e::produce(edm::Event &iEvent, const edm::Ev // const Vector2d dyrecgeneig = R*dyrecgenlocal; // dxrecgen.push_back(dyrecgeneig[0]); // dyrecgen.push_back(dyrecgeneig[1]); - dxrecgen.push_back(dy0[0].value().value()); - dyrecgen.push_back(dy0[1].value().value()); + dxrecgen.push_back(dy0[0]); + dyrecgen.push_back(dy0[1]); - dxerr.push_back(1./std::sqrt(Vinv(0,0).value().value())); - dyerr.push_back(1./std::sqrt(Vinv(1,1).value().value())); - - localqop.push_back(localqopval); - localdxdz.push_back(localdxdzval); - localdydz.push_back(localdydzval); - localx.push_back(localxval); - localy.push_back(localyval); + dxerr.push_back(std::sqrt(iV(0,0))); + dyerr.push_back(std::sqrt(iV(1,1))); + + const Matrix localstatedebug = globalToLocal(updtsos, surface); + + localqop.push_back(localstatedebug[0]); + localdxdz.push_back(localstatedebug[1]); + localdydz.push_back(localstatedebug[2]); + localx.push_back(localstatedebug[3]); + localy.push_back(localstatedebug[4]); + +// localqop.push_back(localqopval); +// localdxdz.push_back(localdxdzval); +// localdydz.push_back(localdydzval); +// localx.push_back(localxval); +// localy.push_back(localyval); + + const Matrix Qtotlocal = Hp*Qtot*Hp.transpose(); + + localqoperr.push_back(std::sqrt(Qtotlocal(0, 0))); + localdxdzerr.push_back(std::sqrt(Qtotlocal(1, 1))); + localdydzerr.push_back(std::sqrt(Qtotlocal(2, 2))); + localxerr.push_back(std::sqrt(Qtotlocal(3, 3))); + localyerr.push_back(std::sqrt(Qtotlocal(4, 4))); + + hitlocalx.push_back(hitx); + hitlocaly.push_back(hity); dEpred.push_back(dEpredval); sigmadE.push_back(sigmadEval); Epred.push_back(epred); E.push_back(e); + hitphi.push_back(hitphival); + localphi.push_back(localphival); + + landauDelta.push_back(deltaTotal); + landauW.push_back(wTotal); + const TrackerSingleRecHit* tkhit = dynamic_cast(preciseHit.get()); assert(tkhit != nullptr); @@ -3222,8 +2006,10 @@ void ResidualGlobalCorrectionMakerG4e::produce(edm::Event &iEvent, const edm::Ev } clusterSN.push_back(-99.); + stripsToEdge.push_back(-99); } else { + const StripTopology* striptopology = dynamic_cast(&(tkhit->det()->topology())); const SiStripCluster& cluster = *tkhit->cluster_strip(); // siStripClusterInfo_.setCluster(cluster, preciseHit->geographicalId().rawId()); SiStripClusterInfo clusterInfo = SiStripClusterInfo(cluster, iSetup, preciseHit->geographicalId().rawId()); @@ -3235,6 +2021,11 @@ void ResidualGlobalCorrectionMakerG4e::produce(edm::Event &iEvent, const edm::Ev clusterOnEdge.push_back(-99); clusterProbXY.push_back(-99.); clusterSN.push_back(clusterInfo.signalOverNoise()); + + + const uint16_t firstStrip = cluster.firstStrip(); + const uint16_t lastStrip = cluster.firstStrip() + cluster.amplitudes().size() - 1; + stripsToEdge.push_back(std::min(firstStrip, striptopology->nstrips() - 1 - lastStrip)); } // if (ispixel) { @@ -3250,6 +2041,8 @@ void ResidualGlobalCorrectionMakerG4e::produce(edm::Event &iEvent, const edm::Ev if (doSim_) { if (simhit != nullptr) { +// std::cout << "ihit = " << ihit << " eloss = " << simhit->energyLoss() << std::endl; + Vector2d dy0simgenlocal; dy0simgenlocal << simhit->localPosition().x() - localxval, simhit->localPosition().y() - localyval; @@ -3269,15 +2062,100 @@ void ResidualGlobalCorrectionMakerG4e::produce(edm::Event &iEvent, const edm::Ev dxsimgenlocal.push_back(dy0simgenlocal[0]); dysimgenlocal.push_back(dy0simgenlocal[1]); - Vector2d dyrecsimlocal; - dyrecsimlocal << preciseHit->localPosition().x() - simhit->localPosition().x(), - preciseHit->localPosition().y() - simhit->localPosition().y(); - const Vector2d dyrecsimeig = R*dyrecsimlocal; - dxrecsim.push_back(dyrecsimeig[0]); - dyrecsim.push_back(dyrecsimeig[1]); + // Vector2d dyrecsimlocal; + // dyrecsimlocal << preciseHit->localPosition().x() - simhit->localPosition().x(), + // preciseHit->localPosition().y() - simhit->localPosition().y(); + // const Vector2d dyrecsimeig = R*dyrecsimlocal; + // dxrecsim.push_back(dyrecsimeig[0]); + // dyrecsim.push_back(dyrecsimeig[1]); + + dxrecsim.push_back(dxrecsimval); + dyrecsim.push_back(dyrecsimval); + + dE.push_back(dEval); + auto const momsim = simhit->momentumAtEntry(); + + const double eentry = std::sqrt(std::pow(simhit->pabs(), 2) + mmu*mmu); + const double emid = eentry - 0.5*simhit->energyLoss(); + const double simqopval = genpart->charge()/std::sqrt(emid*emid - mmu*mmu); +// std::cout << "eloss = " << simhit->energyLoss() << std::endl; + + // "hybrid state" trying to adjust for entry point -> midpoint + // simlocalqop.push_back(simqopval); + // simlocaldxdz.push_back(momsim.x()/momsim.z()); + // simlocaldydz.push_back(momsim.y()/momsim.z()); + // simlocalx.push_back(simhit->localPosition().x()); + // simlocaly.push_back(simhit->localPosition().y()); + + // just fill entry state for debugging + simlocalqop.push_back(genpart->charge()/simhit->pabs()); + simlocaldxdz.push_back(momsim.x()/momsim.z()); + simlocaldydz.push_back(momsim.y()/momsim.z()); + simlocalx.push_back(simhit->entryPoint().x()); + simlocaly.push_back(simhit->entryPoint().y()); + + if (false) { + + const Point3DBase simlocalpos = simhit->entryPoint(); + const Vector3DBase simlocalmom = simhit->momentumAtEntry(); + + // std::cout << "simlocalpos" << simlocalpos << std::endl; + + const Point3DBase simglobalpos = surface.toGlobal(simlocalpos); + const Vector3DBase simglobalmom = surface.toGlobal(simlocalmom); + + Matrix simtsos; + simtsos[0] = simglobalpos.x(); + simtsos[1] = simglobalpos.y(); + simtsos[2] = simglobalpos.z(); + simtsos[3] = simglobalmom.x(); + simtsos[4] = simglobalmom.y(); + simtsos[5] = simglobalmom.z(); + simtsos[6] = genpart->charge(); + + auto propresultsim = g4prop->propagateGenericWithJacobianAltD(simtsos, surface, dbetaval, dxival); + + if (std::get<0>(propresultsim)) { + simtsos = std::get<1>(propresultsim); + + const Point3DBase simglobalposprop(simtsos[0], simtsos[1], simtsos[2]); + const Vector3DBase simglobalmomprop(simtsos[3], simtsos[4], simtsos[5]); + + const Point3DBase simlocalposprop = surface.toLocal(simglobalposprop); + const Vector3DBase simlocalmomprop = surface.toLocal(simglobalmomprop); + + Matrix simlocalparms; + simlocalparms[0] = simtsos[6]/simtsos.segment<3>(3).norm(); + simlocalparms[1] = simlocalmomprop.x()/simlocalmomprop.z(); + simlocalparms[2] = simlocalmomprop.y()/simlocalmomprop.z(); + simlocalparms[3] = simlocalposprop.x(); + simlocalparms[4] = simlocalposprop.y(); + + simlocalqopprop.push_back(simlocalparms[0]); + simlocaldxdzprop.push_back(simlocalparms[1]); + simlocaldydzprop.push_back(simlocalparms[2]); + simlocalxprop.push_back(simlocalparms[3]); + simlocalyprop.push_back(simlocalparms[4]); + } + else { + simlocalqopprop.push_back(-99.); + simlocaldxdzprop.push_back(-99.); + simlocaldydzprop.push_back(-99.); + simlocalxprop.push_back(-99.); + simlocalyprop.push_back(-99.); + } + } + else { + simlocalqopprop.push_back(-99.); + simlocaldxdzprop.push_back(-99.); + simlocaldydzprop.push_back(-99.); + simlocalxprop.push_back(-99.); + simlocalyprop.push_back(-99.); + } + } else { dxsimgen.push_back(-99.); @@ -3289,177 +2167,156 @@ void ResidualGlobalCorrectionMakerG4e::produce(edm::Event &iEvent, const edm::Ev dxrecsim.push_back(-99.); dyrecsim.push_back(-99.); dE.push_back(-99.); + + simlocalqop.push_back(-99.); + simlocaldxdz.push_back(-99.); + simlocaldydz.push_back(-99.); + simlocalx.push_back(-99.); + simlocaly.push_back(-99.); + + simlocalqopprop.push_back(-99.); + simlocaldxdzprop.push_back(-99.); + simlocaldydzprop.push_back(-99.); + simlocalxprop.push_back(-99.); + simlocalyprop.push_back(-99.); } } } }; - - if (align2d) { - fillAlignGrads(std::integral_constant()); - } - else { - fillAlignGrads(std::integral_constant()); - } -// fillAlignGrads(std::integral_constant()); ivalidhit++; } -// std::cout << "hit " << ihit << " isvalid " << preciseHit->isValid() << std::endl; -// std::cout << "global position: " << updtsos.globalParameters().position() << std::endl; - //hit information - //FIXME consolidate this special cases into templated function(s) -// if (preciseHit->isValid()) { } if (!valid) { break; } - - assert(parmidx == (nparsBfield + nparsEloss)); - assert(alignmentparmidx == nparsAlignment); - - //fake constraint on reference point parameters - if (dogen) { -// if (false) { - for (unsigned int i=0; i<5; ++i) { - gradfull[i] = 0.; - hessfull.row(i) *= 0.; - hessfull.col(i) *= 0.; - hessfull(i,i) = 1e6; - } - } - -// std::cout << "dchisqdx" << std::endl; -// std::cout << dchisqdx << std::endl; -// std::cout << "d2chisqdx2 diagonal" << std::endl; -// std::cout << d2chisqdx2.diagonal() << std::endl; -// std::cout << "d2chisqdx2" << std::endl; -// std::cout << d2chisqdx2 << std::endl; -// -// auto const& eigenvalues = d2chisqdx2.eigenvalues(); -// std::cout << "iiter = " << iiter << std::endl; -// std::cout << "d2chisqdx2 eigenvalues" << std::endl; -// std::cout << eigenvalues << std::endl; -// std::cout << "condition = " << eigenvalues[nstateparms-1]/eigenvalues[0] << std::endl; - -// auto const& Cinvd = d2chisqdx2.ldlt(); - - - //now do the expensive calculations and fill outputs - auto const& dchisqdx = gradfull.head(nstateparms); - auto const& dchisqdparms = gradfull.tail(npars); - - auto const& d2chisqdx2 = hessfull.topLeftCorner(nstateparms, nstateparms); - auto const& d2chisqdxdparms = hessfull.topRightCorner(nstateparms, npars); - auto const& d2chisqdparms2 = hessfull.bottomRightCorner(npars, npars); + assert(iparm == npars); - Cinvd.compute(d2chisqdx2); +// Fsparse = Ffull.rightCols(nstatefree).sparseView(); + Fsparse = Ffull(Eigen::all, freestateidxs).sparseView(); + Vinvsparse = Vinvfull.sparseView(); + VinvF = Vinvsparse*Fsparse; - - dxfull = -Cinvd.solve(dchisqdx); - - -// const Matrix deltachisq = dchisqdx.transpose()*dxfull + 0.5*dxfull.transpose()*d2chisqdx2*dxfull; - - const Matrix deltachisq = 0.5*dchisqdx.transpose()*dxfull; - -// if (islikelihood) { -// dxfull *= 0.5; -// } - -// const Matrix deltachisqalt = dchisqdx.transpose()*dxfull + 0.5*dxfull.transpose()*d2chisqdx2*dxfull; -// // -// const VectorXd deltachisqv = 0.5*dchisqdx.array()*dxfull.array(); -// // const VectorXd deltachisqvabs = deltachisqv.array().abs(); -// // // const unsigned int maxidx = deltachisqvabs.maxCoeff(); -// // -// unsigned int maxidx = 0; -// double maxval = 0.; -// for (unsigned int i=0; i < deltachisqv.size(); ++i) { -// const double absval = std::abs(deltachisqv[i]); -// if (absval > maxval) { -// maxval = absval; -// maxidx = i; -// } -// } -// // -// std::cout << "deltachisq = " << deltachisq[0] << " deltachisqalt = " << deltachisqalt << " size = " << deltachisqv.size() << " maxidx = " << maxidx << " maxval = " << maxval << std::endl; -// // -// // -// std::cout << "i dchisqdx dxfull deltachisqv" << std::endl; -// for (unsigned int i=0; i < deltachisqv.size(); ++i) { -// std::cout << i << " " << dchisqdx[i] << " " << dxfull[i] << " " << deltachisqv[i] << std::endl; -// } -// std::cout << "deltachisqv" << std::endl; -// std::cout << deltachisqv << std::endl; - - - - chisqval = chisq0val + deltachisq[0]; - - deltachisqval = chisq0val + deltachisq[0] - chisqvalold; - - chisqvalold = chisq0val + deltachisq[0]; + Cinvd.compute(Fsparse.transpose()*VinvF); + + + // randomize residuals given covariance matrix + if (false) { + const SparseMatrix Vinvsparsealt = Vinvfullalt.sparseView(); + const SparseMatrix VinvFalt = Vinvsparsealt*Fsparse; + + const SimplicialLDLT> Cinvdalt(Fsparse.transpose()*VinvFalt); - ndof = 5*nhits + nvalid + nvalidalign2d - nstateparms; - - if (bsConstraint_) { - ndof += 2; - } - - covfull = 2.*Cinvd.solve(MatrixXd::Identity(nstateparms,nstateparms)); - -// std::cout << "iiter = " << iiter << ", deltachisq = " << deltachisq[0] << std::endl; + const SparseMatrix FtVinvalt = VinvFalt.transpose(); + const SparseMatrix Rsparsealt = Vinvsparsealt - VinvFalt*Cinvdalt.solve(FtVinvalt); + const MatrixXd Ralt = Rsparsealt; + + const Eigen::SelfAdjointEigenSolver Reig(Ralt); + + rfull = VectorXd::Zero(ncons); + for (int ieig = ncons - ndof; ieig < ncons; ++ieig) { + const double rnd = gRandom->Gaus(); + rfull += (rnd/std::sqrt(Reig.eigenvalues()(ieig)))*Reig.eigenvectors().col(ieig); + } + + } -// const Vector5d dxRef = dx.head<5>(); -// // const Vector5d dxRef = -Cinvd.solve(dchisqdx).head<5>(); -// const Matrix5d Cinner = Cinvd.solve(MatrixXd::Identity(nstateparms,nstateparms)).topLeftCorner<5,5>(); +// SimplicialLLT> lltsparse; +// lltsparse.compute(Fsparse.transpose()*VinvF); +// +// const SparseMatrix lfact = lltsparse.matrixL(); +// std::cout << "lfact size = " << lfact.rows()*lfact.cols() << " nonzeros = " << lfact.nonZeros() << std::endl; -// dxdparms = -Cinvd.solve(d2chisqdxdparms).transpose(); +// const SparseMatrix testsparse = Fsparse.transpose()*VinvF; +// const MatrixXd testdense = testsparse; +// SimplicialLDLT> ldltsparse; +// LDLT ldltdense(testdense.rows()); +// +// constexpr unsigned int ntest = 10000; +// +// double sparsetime; +// double densetime; // -// grad = dchisqdparms + dxdparms*dchisqdx; -// hess = d2chisqdparms2 + 2.*dxdparms*d2chisqdxdparms + dxdparms*d2chisqdx2*dxdparms.transpose(); +// { +// auto start = std::chrono::high_resolution_clock::now(); +// for (unsigned int itest = 0; itest < ntest; ++itest) { +// ldltsparse.compute(testsparse); +// } +// auto stop = std::chrono::high_resolution_clock::now(); +// +// auto duration = stop - start; +// sparsetime = duration.count(); +// std::cout << "sparse decomp: " << sparsetime << std::endl; +// } +// +// { +// auto start = std::chrono::high_resolution_clock::now(); +// for (unsigned int itest = 0; itest < ntest; ++itest) { +// ldltdense.compute(testdense); +// } +// auto stop = std::chrono::high_resolution_clock::now(); +// +// auto duration = stop - start; +// densetime = duration.count(); +// std::cout << "dense decomp: " << densetime << std::endl; +// } // -// std::cout << "dxfull" << std::endl; -// std::cout << dxfull << std::endl; -// std::cout << "errsq" << std::endl; -// std::cout << Cinvd.solve(MatrixXd::Identity(nstateparms,nstateparms)).diagonal() << std::endl; +// const double timeratio = sparsetime/densetime; +// std::cout << "size = " << testdense.rows() << " fillfactor = " << double(testsparse.nonZeros())/double(testsparse.rows()*testsparse.cols()) << " sparse/dense = " << timeratio << std::endl; + + dxfree = -Cinvd.solve(VinvF.transpose()*rfull); + + const double deltachisq = rfull.transpose()*VinvF*dxfree; + edmval = -deltachisq; - const Vector5d dxRef = dxfull.head<5>(); -// const Matrix5d Cinner = 2.*(Cinvd.solve(MatrixXd::Identity(nstateparms,nstateparms))).topLeftCorner<5,5>(); - const Matrix5d Cinner = covfull.topLeftCorner<5,5>(); + dxfull = VectorXd::Zero(nstateparms); + dxfull(freestateidxs) = dxfree; -// const Matrix5d Cinner = Cinvd.solve(MatrixXd::Identity(nstateparms,nstateparms)).topLeftCorner<5,5>(); + const Vector5d dxref = dxfull.head<5>(); - if (debugprintout_) { - std::cout<< "dxRef" << std::endl; - std::cout<< dxRef << std::endl; +// std::cout << "iiter = " << iiter << std::endl; +// std::cout << "dxfree.head<5>():\n" << dxfree.head<5>() << std::endl; +// std::cout << "dxfull.head<5>():\n" << dxfull.head<5>() << std::endl; +// std::cout << "dxref:\n" < Cinner = covfull.topLeftCorner<5,5>(); + + if (dogen) { + edmvalref = 0.; + } + else { + const Matrix hessref = Cinner.inverse(); + const double deltachisqref = -0.5*dxref.transpose()*hessref*dxref; + edmvalref = -deltachisqref; } + //fill output with corrected state and covariance at reference point refParms.fill(0.); refCov.fill(0.); -// const AlgebraicVector5& refVec = track.parameters(); -// CurvilinearTrajectoryParameters curvparms(refFts.position(), refFts.momentum(), refFts.charge()); -// const AlgebraicVector5& refVec = curvparms.vector(); -// Map(refParms.data()) = (Map(refVec.Array()) + dxRef).cast(); const double qbp = refFts[6]/refFts.segment<3>(3).norm(); const double lam = std::atan(refFts[5]/std::sqrt(refFts[3]*refFts[3] + refFts[4]*refFts[4])); const double phi = std::atan2(refFts[4], refFts[3]); - const double qbpupd = qbp + dxRef[0]; - const double lamupd = lam + dxRef[1]; - const double phiupd = phi + dxRef[2]; + const double qbpupd = qbp + dxref[0]; + const double lamupd = lam + dxref[1]; + const double phiupd = phi + dxref[2]; refParms[0] = qbpupd; refParms[1] = lamupd; @@ -3477,208 +2334,40 @@ void ResidualGlobalCorrectionMakerG4e::produce(edm::Event &iEvent, const edm::Ev refCov_iter0 = refCov; } -// // fill values for valuemap -// if (muonref.isNonnull()) { -// const double ptupd = std::cos(lamupd)/std::abs(qbpupd); -// const double chargeupd = std::copysign(1., qbpupd); -// -// const double thetaupd = M_PI_2 - lamupd; -// const double etaupd = -std::log(std::tan(0.5*thetaupd)); -// -// corPtV[muonref.index()] = ptupd; -// corEtaV[muonref.index()] = etaupd; -// corPhiV[muonref.index()] = phiupd; -// corChargeV[muonref.index()] = chargeupd; -// } - - - - - //current state from previous state on this layer - //save current parameters -// TrajectoryStateOnSurface& oldtsosout = layerStates.back(); -// -// if (iiter == 0) { -// outPtStart = oldtsosout.globalMomentum().perp(); -// outEtaStart = oldtsosout.globalMomentum().eta(); -// outPhiStart = oldtsosout.globalMomentum().phi(); -// } -// -// // JacobianCurvilinearToLocal curv2localold(oldtsos.surface(), oldtsos.localParameters(), *oldtsos.magneticField()); -// // const AlgebraicMatrix55& curv2localjacold = curv2localold.jacobian(); -// // const Matrix Hold = Map>(curv2localjacold.Array()); -// // const Matrix Hold = curv2localJacobianAlt(oldtsos); -// const Matrix Hold = curv2localJacobianAlteloss(oldtsosout, dEdxlast, mmu); -// -// const AlgebraicVector5 local = oldtsosout.localParameters().vector(); -// // auto const& dxlocal = dxfull.segment<5>(5*(ihit+1)); -// auto const& dxlocal = Hold*dxfull.tail<5>(); -// const Matrix localupd = Map>(local.Array()) + dxlocal; -// AlgebraicVector5 localvecupd(localupd[0],localupd[1],localupd[2],localupd[3],localupd[4]); -// -// -// const LocalTrajectoryParameters localparms(localvecupd, oldtsosout.localParameters().pzSign()); -// -// const TrajectoryStateOnSurface updtsosout(localparms, oldtsosout.surface(), field, oldtsosout.surfaceSide()); -// -// outPt = updtsosout.globalMomentum().perp(); -// outEta = updtsosout.globalMomentum().eta(); -// outPhi = updtsosout.globalMomentum().phi(); niter = iiter + 1; - edmval = -deltachisq[0]; - const double threshparam = dolocalupdate ? edmval : std::fabs(deltachisqval); - -// if (iiter>0 && threshparam > 1e4) { -// anomDebug = true; -// } - - if (std::isnan(edmval) || std::isinf(edmval) || std::abs(lamupd) > M_PI_2 || (iiter>0 && threshparam > 1e5) || (iiter>1 && threshparam > 1e4) ) { + if (std::isnan(edmval) || std::isinf(edmval)) { std::cout << "WARNING: invalid parameter update!!!" << " edmval = " << edmval << " lamupd = " << lamupd << " deltachisqval = " << deltachisqval << std::endl; valid = false; break; } - - -// const double nll0val = 0.5*chisq0val - 0.5*logdetH; -// const double nllval = nll0val + deltachisq[0]; - - const double ptupd = (1./std::abs(qbpupd))*std::cos(lamupd); - -// std::cout << "iiter = " << iiter << " edmval = " << edmval << " chisqval = " << chisqval << " chisq0val = " << chisq0val << " qbpupd = " << qbpupd << " lamupd = " << lamupd << " phiupd = " << phiupd << " ptupd = " << ptupd << std::endl; - - -// std::cout << "iiter = " << iiter << " edmval = " << edmval << " chisqval = " << chisqval << " chisq0val = " << chisq0val << " logdetH = " << logdetH << " nll0val = " << nll0val << " nllval = " << nllval << std::endl; - - -// std::cout << "iiter = " << iiter << " edmval = " << edmval << " chisqval = " << chisqval << std::endl; - -// std::cout << "iiter = " << iiter << " edmval = " << edmval << " deltachisqval = " << deltachisqval << " chisqval = " << chisqval << std::endl; +// std::cout << "iiter = " << iiter << " edmval = " << edmval << " edmvalref " << edmvalref << " deltachisqval = " << deltachisqval << " chisqval = " << chisqval << std::endl; if (anomDebug) { std::cout << "anomDebug: iiter = " << iiter << " edmval = " << edmval << " deltachisqval = " << deltachisqval << " chisqval = " << chisqval << std::endl; } -// std::cout <<"dxRef" << std::endl; -// std::cout << dxRef << std::endl; -// std::cout <<"dxOut" << std::endl; -// std::cout << dxfull.tail<5>() << std::endl; - -// std::cout << "dxfull" << std::endl; -// std::cout << dxfull << std::endl; - -// if (iiter > 1 && std::abs(deltachisq[0])<1e-3) { -// break; -// } - -// if (std::abs(deltachisqval)<1e-2) { -// break; -// } - if (dolocalupdate && edmval < 1e-5) { + if (iiter > 0 && dolocalupdate && edmval < 1e-5) { break; } - else if (!dolocalupdate && std::fabs(deltachisqval)<1e-5) { + else if (iiter > 0 && !dolocalupdate && edmvalref < 1e-5) { break; } -// else if (iiter==2) { -// refParms_iter2 = refParms; -// refCov_iter2 = refCov; -// } - -// std::cout << "refParms" << std::endl; -// std::cout << Map(refParms.data()) << std::endl; - -// // gradv.clear(); -// jacrefv.clear(); -// -// // gradv.resize(npars,0.); -// jacrefv.resize(5*npars, 0.); -// -// nJacRef = 5*npars; -// // tree->SetBranchAddress("gradv", gradv.data()); -// tree->SetBranchAddress("jacrefv", jacrefv.data()); -// -// //eigen representation of the underlying vector storage -// // Map gradout(gradv.data(), npars); -// Map > jacrefout(jacrefv.data(), 5, npars); -// -// jacrefout = dxdparms.leftCols<5>().transpose().cast(); } if (!valid) { + // chisqval = -99.; + // tree->Fill(); continue; } - - if (!nValidHitsFinal) { - continue; - } - -// if (islikelihood) { - if (false) { - for (unsigned int ihit = 0; ihit < nhits; ++ihit) { - - if (dogen) { - // if (false) { - for (unsigned int i=0; i<5; ++i) { - dhessv[ihit].row(i) *= 0.; - dhessv[ihit].col(i) *= 0.; - } - } - - dhessv[ihit] = (Cinvd.solve(dhessv[ihit])).eval(); -// const double sumfull = dhessv[ihit].array().abs().sum(); -// const double sumblock = dhessv[ihit].block<10, 10>(5*ihit, 5*ihit).array().abs().sum(); -// const double diffsum = sumfull - sumblock; -// std::cout << "ihit = " << ihit << " diffsum:" << diffsum << std::endl; -// std::cout << "ihit = " << ihit << " dhessv:" << std::endl; -// std::cout << dhessv[ihit] << std::endl; -// Matrix testmat = Matrix::Zero(nstateparms, nstateparms); -// for (unsigned int j = 0; j < nstateparms; ++j) { -// for (unsigned int k = 0; k < nstateparms; ++k) { -// if (std::abs(dhessv[ihit](j,k)) > 0.) { -// testmat(j,k) = 1; -// } -// } -// } -// std::cout << "ihit = " << ihit << " testmat:" << std::endl; -// std::cout << testmat << std::endl; - } - - for (unsigned int ihit = 0; ihit < nhits; ++ihit) { - const double gradval = -dhessv[ihit].trace(); - const unsigned int ifull = nstateparms + 2*ihit + 1; - gradfull[ifull] += gradval; - std::cout << "ihit = " << ihit << " gradval = " << gradval << std::endl; - for (unsigned int jhit = ihit; jhit < nhits; ++jhit) { - const unsigned int jfull = nstateparms + 2*jhit + 1; - const double hessval = dhessv[ihit].transpose().cwiseProduct(dhessv[jhit]).sum(); - std::cout << "ihit = " << ihit << " jhit = " << jhit << " hessval = " << hessval << std::endl; - - hessfull(ifull, jfull) += hessval; - if (ihit != jhit) { - hessfull(jfull, ifull) += hessval; - } - } - } - - } - - - auto const& dchisqdx = gradfull.head(nstateparms); - auto const& dchisqdparms = gradfull.tail(npars); - - auto const& d2chisqdx2 = hessfull.topLeftCorner(nstateparms, nstateparms); - auto const& d2chisqdxdparms = hessfull.topRightCorner(nstateparms, npars); - auto const& d2chisqdparms2 = hessfull.bottomRightCorner(npars, npars); - std::unordered_map idxmap; globalidxvfinal.clear(); @@ -3694,85 +2383,390 @@ void ResidualGlobalCorrectionMakerG4e::produce(edm::Event &iEvent, const edm::Ev const unsigned int nparsfinal = globalidxvfinal.size(); - VectorXd dchisqdparmsfinal = VectorXd::Zero(nparsfinal); - MatrixXd d2chisqdxdparmsfinal = MatrixXd::Zero(nstateparms, nparsfinal); - MatrixXd d2chisqdparms2final = MatrixXd::Zero(nparsfinal, nparsfinal); + MatrixXd Jfinal = MatrixXd::Zero(ncons, nparsfinal); for (unsigned int i = 0; i < npars; ++i) { const unsigned int iidx = idxmap.at(globalidxv[i]); - dchisqdparmsfinal[iidx] += dchisqdparms[i]; - d2chisqdxdparmsfinal.col(iidx) += d2chisqdxdparms.col(i); - for (unsigned int j = 0; j < npars; ++j) { - const unsigned int jidx = idxmap.at(globalidxv[j]); - d2chisqdparms2final(iidx, jidx) += d2chisqdparms2(i, j); - } + Jfinal.col(iidx) += Jfull.col(i); + } + + const SparseMatrix Jsparse = Jfinal.sparseView(); + + std::vector residxsfinal; + residxsfinal.reserve(residxs.size()); + for (auto idx : residxs) { + const unsigned int iidx = idxmap.at(globalidxv[idx]); + residxsfinal.push_back(iidx); } - dxdparms = -Cinvd.solve(d2chisqdxdparmsfinal).transpose(); +// dxdparms = -Cinvd.solve(d2chisqdxdparmsfinal).transpose(); // grad = dchisqdparmsfinal + dxdparms*dchisqdx; - grad = dchisqdparmsfinal + d2chisqdxdparmsfinal.transpose()*dxfull; - hess = d2chisqdparms2final + dxdparms*d2chisqdxdparmsfinal; +// grad = dchisqdparmsfinal + d2chisqdxdparmsfinal.transpose()*dxfull; +// hess = d2chisqdparms2final + dxdparms*d2chisqdxdparmsfinal; -// dxdparms = -Cinvd.solve(d2chisqdxdparms).transpose(); + nParms = nparsfinal; + + //TODO avoid explicitly storing the transpose? + const SparseMatrix FtVinv = VinvF.transpose(); +// const MatrixXd R = Vinvsparse - VinvF*Cinvd.solve(FtVinv); + const SparseMatrix Rsparse = Vinvsparse - VinvF*Cinvd.solve(FtVinv); + const MatrixXd R = Rsparse; -// if (islikelihood) { +// constexpr unsigned int ntest = 1000; // -// for (unsigned int ihit = 0; ihit < nhits; ++ihit) { -// const unsigned int iparmfull = 2*ihit + 1; -// dxdparms.row(iparmfull) += -(dhessv[ihit]*dxfull).transpose(); -// } +// auto time0 = std::chrono::high_resolution_clock::now(); +// for (unsigned int itest = 0; itest < ntest; ++itest) { +// const SparseMatrix R1 = Vinvsparse - VinvF*Cinvd.solve(FtVinv); +// const MatrixXd R2 = R1; +// } // +// auto time1 = std::chrono::high_resolution_clock::now(); +// for (unsigned int itest = 0; itest < ntest; ++itest) { +// const MatrixXd R1 = Vinvsparse - VinvF*Cinvd.solve(FtVinv); +// const SparseMatrix R2 = R1.sparseView(); // } +// +// auto time2 = std::chrono::high_resolution_clock::now(); +// +// +// const double d0 = (time1-time0).count(); +// const double d1 = (time2-time1).count(); +// // const double d2 = (time3-time2).count(); +// // const double d3 = (time4-time3).count(); +// // +// std::cout << "d0 = " << d0 << " d1 = " << d1 << std::endl; -// if (debugprintout_) { -// std::cout << "dxrefdparms" << std::endl; -// std::cout << dxdparms.leftCols<5>() << std::endl; -// } -// grad = dchisqdparms + dxdparms*dchisqdx; - //TODO check the simplification -// hess = d2chisqdparms2 + 2.*dxdparms*d2chisqdxdparms + dxdparms*d2chisqdx2*dxdparms.transpose(); -// hess = d2chisqdparms2 + dxdparms*d2chisqdxdparms; +// std::vector>> triplets; +// // SparseMatrix> testsparse(100, 100); +// SparseMatrix> testsparse(100, 100); +// triplets.emplace_back(0, 0, Matrix::Zero()); +// +// testsparse.setFromTriplets(triplets.begin(), triplets.end()); +// testsparse.insert(0, 0) = Matrix::Identity(); -// std::cout << "grad:" << std::endl; -// std::cout << grad.transpose() << std::endl; -// std::cout << "hess diagonal:" << std::endl; -// std::cout << hess.diagonal().transpose() << std::endl; +// std::cout << "R total size = " << R.rows()*R.cols() << " nonzeros = " << R.nonZeros() << std::endl; -// const Vector5d dxRef = dxfull.head<5>(); -// const Matrix5d Cinner = Cinvd.solve(MatrixXd::Identity(nstateparms,nstateparms)).topLeftCorner<5,5>(); +// std::cout << "VinvF total size = " << VinvF.rows()*VinvF.cols() << " nonzeros = " << VinvF.nonZeros() << std::endl; -// std::cout << "dchisqdparms.head<6>()" << std::endl; -// std::cout << dchisqdparms.head<6>() << std::endl; -// -// std::cout << "grad.head<6>()" << std::endl; -// std::cout << grad.head<6>() << std::endl; + + //TODO make R dense (and find the best solution for sparse = sparse*dense case) + + +// chisqval = rfull.transpose()*R*rfull; // -// std::cout << "d2chisqdparms2.topLeftCorner<6, 6>():" << std::endl; -// std::cout << d2chisqdparms2.topLeftCorner<6, 6>() << std::endl; -// std::cout << "hess.topLeftCorner<6, 6>():" << std::endl; -// std::cout << hess.topLeftCorner<6, 6>() << std::endl; +// grad = 2.*Jsparse.transpose()*R*rfull; +// hess = 2.*Jsparse.transpose()*R*Jsparse; + + //TODO check this against explicit version + const VectorXd Rr = Vinvsparse*(rfull + Fsparse*dxfree); + +// const VectorXd Rrtest = R*rfull; +// const double Rrdiff = (Rr-Rrtest).array().square().sum(); +// std::cout << "Rrdiff = " << Rrdiff << std::endl; + +// constexpr unsigned int ntest = 1000; // -// std::cout << "dchisqdparms.segment<6>(nparsBfield+nparsEloss)" << std::endl; -// std::cout << dchisqdparms.segment<6>(nparsBfield+nparsEloss) << std::endl; +// auto time0 = std::chrono::high_resolution_clock::now(); +// for (unsigned int itest = 0; itest < ntest; ++itest) { +// const VectorXd Rrtest = Vinvsparse*(rfull + Fsparse*dxfull); +// } // -// std::cout << "grad.segment<6>(nparsBfield+nparsEloss)" << std::endl; -// std::cout << grad.segment<6>(nparsBfield+nparsEloss) << std::endl; +// auto time1 = std::chrono::high_resolution_clock::now(); +// for (unsigned int itest = 0; itest < ntest; ++itest) { +// // VectorXd Rrtest = Vinvsparse*rfull; +// // Rrtest += Vinvsparse*Fsparse*dxfull; +// const VectorXd Rrtest = R*rfull; +// } // -// std::cout << "d2chisqdparms2.block<6, 6>(nparsBfield+nparsEloss, nparsBfield+nparsEloss):" << std::endl; -// std::cout << d2chisqdparms2.block<6, 6>(nparsBfield+nparsEloss, nparsBfield+nparsEloss) << std::endl; -// std::cout << "hess.block<6, 6>(nparsBfield+nparsEloss, nparsBfield+nparsEloss):" << std::endl; -// std::cout << hess.block<6, 6>(nparsBfield+nparsEloss, nparsBfield+nparsEloss) << std::endl; +// auto time2 = std::chrono::high_resolution_clock::now(); +// // -// std::cout << "d2chisqdparms2.bottomRightCorner<6, 6>():" << std::endl; -// std::cout << d2chisqdparms2.bottomRightCorner<6, 6>() << std::endl; -// std::cout << "hess.bottomRightCorner<6, 6>():" << std::endl; -// std::cout << hess.bottomRightCorner<6, 6>() << std::endl; +// const double d0 = (time1-time0).count(); +// const double d1 = (time2-time1).count(); +// +// std::cout << "d0 = " << d0 << " d1 = " << d1 << std::endl; - nParms = nparsfinal; + + chisqval = rfull.transpose()*Rr; + + grad = 2.*Jsparse.transpose()*Rr; + hess = 2.*Jsparse.transpose()*R*Jsparse; + + + dxdparms = MatrixXd::Zero(nparsfinal, nstateparms); + + dxdparms(Eigen::all, freestateidxs) = -Cinvd.solve(VinvF.transpose()*Jsparse).transpose(); + + + //additional contributions from resolution variations + + std::vector> dVRs; + dVRs.reserve(dVs.size()); + + MatrixXd dVRr = MatrixXd::Zero(ncons, nparsfinal); + + VectorXd gradll = VectorXd::Zero(nparsfinal); + + for (unsigned int ires = 0; ires < dVs.size(); ++ires) { + auto const &dVi = dVs[ires]; + const unsigned int residxi = residxsfinal[ires]; + + SparseMatrix &dViR = dVRs.emplace_back(); + dViR = dVi*Rsparse; + +// const MatrixXd Rdense = R; + +// const SparseMatrix dViRtest = dVi*(Rdense.sparseView().transpose().transpose()); +// const SparseMatrix dViRtest = Rdense.sparseView().transpose(); + +// +// MatrixXd tmpdense = MatrixXd::Zero(ncons, ncons); +// +// constexpr unsigned int ntest = 1000; +// +// auto time0 = std::chrono::high_resolution_clock::now(); +// for (unsigned int itest = 0; itest < ntest; ++itest) { +// const SparseMatrix dViRtest = (dVi*Rdense).sparseView(); +// } +// auto time1 = std::chrono::high_resolution_clock::now(); +// +// for (unsigned int itest = 0; itest < ntest; ++itest) { +// const SparseMatrix Rsparse = Rdense.sparseView(); +// const SparseMatrix dViRtest = dVi*Rsparse; +// } +// +// auto time2 = std::chrono::high_resolution_clock::now(); +// +// for (unsigned int itest = 0; itest < ntest; ++itest) { +// const SparseMatrix dViRtest = dVi*R; +// } +// +// auto time3 = std::chrono::high_resolution_clock::now(); +// +// +// for (unsigned int itest = 0; itest < ntest; ++itest) { +// tmpdense = dVi*Rdense; +// // const SparseMatrix dViRtest = tmpdense.sparseView(); +// } +// +// auto time4 = std::chrono::high_resolution_clock::now(); +// +// const double d0 = (time1-time0).count(); +// const double d1 = (time2-time1).count(); +// const double d2 = (time3-time2).count(); +// const double d3 = (time4-time3).count(); +// +// std::cout << "d0 = " << d0 << " d1 = " << d1 << " d2 = " << d2 << " d3 = " << d3 << std::endl; + +// const double testfail = (dVi*MatrixXd::Identity(ncons, ncons)).sparseView(); + +// const int testcount = (dVi*MatrixXd(R)).nonZeros(); +// const Matrix testdense = Matrix::Identity(); +// const int testcount2 = testdense.nonZeros(); +// +// std::cout << "testcount2 = " << testcount2 << std::endl; +// +// std::cout << "dViR total size = " << dViR.rows()*dViR.cols() << " testcount = " << testcount << std::endl; + + +// const MatrixXd ident = MatrixXd::Identity(ncons, ncons); + +// SparseMatrix testsparse = SparseMatrix(dVi*ident); + +// std::cout << "dViR total size = " << dViR.rows()*dViR.cols() << " nonzeros = " << dViR.nonZeros() << std::endl; + +// dVRr.col(residxi) += dViR*rfull; + dVRr.col(residxi) += dVi*Rr; + + //TODO optimize traces for sparse matrices + +// const double gradres = MatrixXd(dViR).trace(); + + //trace is not implemented directly for sparse matrices so use sum of diagonals explicitly + const double gradres = dViR.diagonal().sum(); + +// const double gradres = -rfull.transpose()*R*dViR*rfull + MatrixXd(dViR).trace(); + +// grad(residxi) += gradres; + gradll(residxi) += gradres; + +// VectorXd hesscross = -2.*Jsparse.transpose()*R*dViR*rfull; + // adjust diagonal element since it will be added twice +// hesscross(residxi) *= 0.5; + +// hess.col(residxi) += hesscross; +// hess.row(residxi) += hesscross.transpose(); + +// if (!dogen) { +// dxdparms.row(residxi) += (M*dViR*rfull).transpose(); +// } + + const SparseMatrix dViRT = dViR.transpose(); + + for (unsigned int jres = 0; jres <= ires; ++jres) { + auto const &dVjR = dVRs[jres]; + const unsigned int residxj = residxsfinal[jres]; + +// const SparseMatrix dViRdVjR = dViR*dVjR; + +// const double hessres = -MatrixXd(dViR*dVjR).trace(); +// const double hessres = 2.*rfull.transpose()*R*dViRdVjR*rfull - MatrixXd(dViRdVjR).trace(); + + // below is equivalent to -(dViR*dVjR).trace() +// const double hessres = -dViRT.cwiseProduct(dVjR).sum(); + const double hessres = dViRT.cwiseProduct(dVjR).sum(); +// const double hessresalt = -MatrixXd(dViR*dVjR).trace(); + +// std::cout << "ires = " << ires << " jres = " << jres << " hessres = " << hessres << " hessresalt = " << hessresalt << std::endl; + + hess(residxi, residxj) += hessres; + if (ires != jres) { + hess(residxj, residxi) += hessres; + } + + } + + + + + +// // const SparseMatrix dViR = (dVi*R).sparseView(); +// const SparseMatrix dViR = dVi*R; +// // const MatrixXd dViR = dVi*R; +// +// //TODO optimized trace for sparse matrices +// //TODO optimize use of sparse vs dense matrices (and avoid redundant conversions) +// +// grad(residxi) += -rfull.transpose()*R*dViR*rfull + MatrixXd(dViR).trace(); +// +// VectorXd hesscross = -2.*Jsparse.transpose()*R*dViR*rfull; +// +// // adjust diagonal element since it will be added twice +// hesscross(residxi) *= 0.5; +// +// hess.col(residxi) += hesscross; +// hess.row(residxi) += hesscross.transpose(); +// +// if (!dogen) { +// dxdparms.row(residxi) += (M*dViR*rfull).transpose(); +// } +// +// for (unsigned int jres = 0; jres < dVs.size(); ++jres) { +// auto const &dVj = dVs[jres]; +// const unsigned int residxj = residxsfinal[jres]; +// +// // const SparseMatrix dViRdVjR = (dViR*dVj*R).sparseView(); +// const SparseMatrix dViRdVjR = dViR*dVj*R; +// // const MatrixXd dViRdVjR = dViR*dVj*R; +// +// const double hessres = 2.*rfull.transpose()*R*dViRdVjR*rfull - MatrixXd(dViRdVjR).trace(); +// +// hess(residxi, residxj) += hessres; +// if (residxi != residxj) { +// hess(residxj, residxi) += hessres; +// } +// } + + } + + const SparseMatrix dVRrsparse = dVRr.sparseView(); + +// std::cout << "dVRrsparse size = " << dVRrsparse.rows()*dVRrsparse.cols() << " nonzeros = " << dVRrsparse.nonZeros() << std::endl; + +// grad += -(rfull.transpose()*R*dVRrsparse).transpose(); +// grad += -(Rr.transpose()*dVRrsparse).transpose(); + grad += -dVRrsparse.transpose()*Rr; + + if (false) { + hess += 2.*dVRrsparse.transpose()*R*dVRrsparse; + + const SparseMatrix hesscross = -2.*Jsparse.transpose()*Rsparse*dVRrsparse; + hess += hesscross; + hess += hesscross.transpose(); + } + +// hess += hesscross + hesscross.transpose(); + //TODO deduplicate with above + dxdparms(Eigen::all, freestateidxs) += Cinvd.solve(FtVinv*dVRrsparse).transpose(); + +// if (!dogen) { +// //TODO deduplicate with above +// // const MatrixXd M = Cinvd.solve(FtVinv); +// // dxdparms += (M*dVRrsparse).transpose(); +// dxdparms += Cinvd.solve(FtVinv*dVRrsparse).transpose(); +// } +// + + //TODO check this against element-wise version +// grad += -(rfull.transpose()*R*dVRrsparse).transpose(); +// hess += 2.*dVRrsparse.transpose()*R*dVRrsparse; +// +// MatrixXd hesstest = 2.*dVRrsparse.transpose()*R*dVRrsparse; +// MatrixXd hesstest2 = MatrixXd::Zero(nparsfinal, nparsfinal); +// for (unsigned int ires = 0; ires < dVs.size(); ++ires) { +// auto const &dVi = dVs[ires]; +// const unsigned int residxi = residxsfinal[ires]; +// +// const SparseMatrix &dViR = dVRs[ires]; +// +// for (unsigned int jres = 0; jres <= ires; ++jres) { +// auto const &dVjR = dVRs[jres]; +// const unsigned int residxj = residxsfinal[jres]; +// +// const double hesstestval = 2.*rfull.transpose()*R*dViR*dVjR*rfull; +// // const double hesstestval = 2.*rfull.transpose()*dViR*dVjR*rfull; +// +// // const double hesstestval2 = 2.*rfull.transpose()*dViR.transpose()*R*dVjR*rfull; +// +// hesstest2(residxi, residxj) += hesstestval; +// // if (residxi != residxj) { +// if (ires != jres) { +// hesstest2(residxj, residxi) += hesstestval; +// } +// +// // std::cout << "ires = " << ires << " jres = " << jres << " residxi = " << residxi << " residxj = " << residxj << " hesstestval = " << hesstestval << " hesstest(residxi, residxj) = " << hesstest(residxi, residxj) << std::endl; +// +// } +// } +// +// for (unsigned int ipar = 0; ipar < nparsfinal; ++ipar) { +// for (unsigned int jpar = 0; jpar < nparsfinal; ++jpar) { +// const double diff = hesstest2(ipar, jpar) - hesstest(ipar, jpar); +// +// if (std::fabs(diff) > 1e-16 && std::fabs(diff)/hesstest(ipar, jpar) > 1e-3) { +// std::cout << "ipar = " << ipar << " jpar = " << jpar << " hesstest(ipar, jpar) = " << hesstest(ipar, jpar) << " hessest2(ipar, jpar) = " << hesstest2(ipar, jpar) << std::endl; +// } +// +// } +// } +// +// const double diffsq = (hesstest2-hesstest).array().square().sum(); +// std::cout << "diffsq = " << diffsq << std::endl; + + + +// MatrixXd hesscross = -2.*Jsparse.transpose()*R*dVRr; +// //correct diagonal since it gets added twice +// hesscross.diagonal() *= 0.5; + +// hess += hesscross + hesscross.transpose(); + +// std::cout << "hesscross.bottomRightCorner<10,10>():\n" << hesscross.bottomRightCorner<10,10>() << std::endl; + +// if (!dogen) { +// dxdparms += (M*dVRrsparse).transpose(); +// } + + gradchisqv.clear(); + gradchisqv.resize(nparsfinal, 0.); + + Map(gradchisqv.data(), nparsfinal) = grad.cast(); + + grad += gradll; gradv.clear(); jacrefv.clear(); @@ -3784,7 +2778,7 @@ void ResidualGlobalCorrectionMakerG4e::produce(edm::Event &iEvent, const edm::Ev if (fillTrackTree_ && fillGrads_) { tree->SetBranchAddress("gradv", gradv.data()); } - if (fillTrackTree_) { + if (fillTrackTree_ && fillJac_) { tree->SetBranchAddress("jacrefv", jacrefv.data()); } @@ -3794,6 +2788,19 @@ void ResidualGlobalCorrectionMakerG4e::produce(edm::Event &iEvent, const edm::Ev // jacrefout = dxdparms.leftCols<5>().transpose().cast(); jacrefout = ( (dxdparms).leftCols<5>().transpose() ).cast(); + + + if (false) { + const double refpt = std::fabs(1./refParms[0])*std::sin(M_PI_2 - refParms[1]); + const double refphi = refParms[2]; + const double reftheta = M_PI_2 - refParms[1]; + const double refeta = -std::log(std::tan(0.5*reftheta)); + + std::cout << "ref pt eta phi: " << refpt << " " << refeta << " " << refphi << std::endl; + std::cout << "jacref qop lam phi:\n" << jacrefout.topRows<3>() << std::endl; + + } + gradout = grad.cast(); @@ -3813,35 +2820,16 @@ void ResidualGlobalCorrectionMakerG4e::produce(edm::Event &iEvent, const edm::Ev Map > ryout(ry.data(), nvalid, 2); ryout = ryfull; - deigx.resize(nvalid); - deigy.resize(nvalid); - - validdxeig = validdxeigjac*dxfull; - - for (unsigned int ivalid = 0; ivalid < nvalid; ++ivalid) { - deigx[ivalid] = validdxeig[2*ivalid]; - deigy[ivalid] = validdxeig[2*ivalid + 1]; - } - -// unsigned int ivalid = 0; -// for (unsigned int ihit = 0; ihit < nhits; ++ihit) { -// auto const& hit = hits[ihit]; -// if (hit->isValid()) { -// // if (ihit < (nhits-1)) { -// // std::cout << "ihit = " << ihit << ", ivalid = " << ivalid << std::endl; -// // std::cout << "dxfull.segment<3>(3*(ihit+1)):" << std::endl; -// // std::cout << dxfull.segment<3>(3*(ihit+1)) << std::endl; -// // std::cout << "dxstate.segment<5>(5*(ihit+1))" << std::endl; -// // std::cout << dxstate.segment<5>(5*(ihit+1)) << std::endl; -// // } -// const unsigned int dxidx = 3*(ihit + 1); -// dlocalx[ivalid] = dxfull[dxidx]; -// dlocaly[ivalid] = dxfull[dxidx + 1]; -// ++ivalid; -// } +// deigx.resize(nvalid); +// deigy.resize(nvalid); +// +// validdxeig = validdxeigjac*dxfull; +// +// for (unsigned int ivalid = 0; ivalid < nvalid; ++ivalid) { +// deigx[ivalid] = validdxeig[2*ivalid]; +// deigy[ivalid] = validdxeig[2*ivalid + 1]; // } - float refPt = dogen ? genpart->pt() : std::abs(1./refParms[0])*std::sin(M_PI_2 - refParms[1]); gradmax = 0.; @@ -3867,38 +2855,7 @@ void ResidualGlobalCorrectionMakerG4e::produce(edm::Event &iEvent, const edm::Ev } } - -// if (gradmax < 1e5 && refPt > 5.5) { -// //fill aggregrate gradient and hessian -// for (unsigned int i=0; ihessmax) { -// hessmax = absval; -// } -// -// const std::pair key = std::make_pair(std::min(iidx,jidx), std::max(iidx,jidx)); -// -// auto it = hessaggsparse.find(key); -// if (it==hessaggsparse.end()) { -// hessaggsparse[key] = hess(i,j); -// } -// else { -// it->second += hess(i,j); -// } -// } -// } -// } - - + if (debugprintout_) { const Matrix5d Cinner = (Cinvd.solve(MatrixXd::Identity(nstateparms,nstateparms))).topLeftCorner<5,5>(); std::cout << "hess debug" << std::endl; @@ -3945,6 +2902,8 @@ void ResidualGlobalCorrectionMakerG4e::produce(edm::Event &iEvent, const edm::Ev packedidx += segmentsize; } +// std::cout << "refParms[0]: " << refParms[0] << std::endl; + if (fillTrackTree_) { tree->Fill(); } diff --git a/Analysis/HitAnalyzer/plugins/ResidualGlobalCorrectionMakerSim.cc b/Analysis/HitAnalyzer/plugins/ResidualGlobalCorrectionMakerSim.cc deleted file mode 100644 index 344160481c990..0000000000000 --- a/Analysis/HitAnalyzer/plugins/ResidualGlobalCorrectionMakerSim.cc +++ /dev/null @@ -1,2430 +0,0 @@ -#include "ResidualGlobalCorrectionMakerBase.h" - -class ResidualGlobalCorrectionMakerSim : public ResidualGlobalCorrectionMakerBase -{ -public: - explicit ResidualGlobalCorrectionMakerSim(const edm::ParameterSet &); - ~ResidualGlobalCorrectionMakerSim() {} - -// static void fillDescriptions(edm::ConfigurationDescriptions &descriptions); - -private: - - virtual void beginStream(edm::StreamID) override; - virtual void produce(edm::Event &, const edm::EventSetup &) override; - - -}; - - -ResidualGlobalCorrectionMakerSim::ResidualGlobalCorrectionMakerSim(const edm::ParameterSet &iConfig) : ResidualGlobalCorrectionMakerBase(iConfig) -{ - -} - -void ResidualGlobalCorrectionMakerSim::beginStream(edm::StreamID streamid) -{ - ResidualGlobalCorrectionMakerBase::beginStream(streamid); - - if (fillTrackTree_) { - const int basketSize = 4*1024*1024; - - tree->Branch("trackPt", &trackPt, basketSize); - tree->Branch("trackPtErr", &trackPtErr, basketSize); - tree->Branch("trackEta", &trackEta, basketSize); - tree->Branch("trackPhi", &trackPhi, basketSize); - tree->Branch("trackCharge", &trackCharge, basketSize); - //workaround for older ROOT version inability to store std::array automatically - // tree->Branch("trackOrigParms", trackOrigParms.data(), "trackOrigParms[5]/F", basketSize); - // tree->Branch("trackOrigCov", trackOrigCov.data(), "trackOrigCov[25]/F", basketSize); - tree->Branch("trackParms", trackParms.data(), "trackParms[5]/F", basketSize); - tree->Branch("trackCov", trackCov.data(), "trackCov[25]/F", basketSize); - - tree->Branch("refParms_iter0", refParms_iter0.data(), "refParms_iter0[5]/F", basketSize); - tree->Branch("refCov_iter0", refCov_iter0.data(), "refCov_iter0[25]/F", basketSize); - // tree->Branch("refParms_iter2", refParms_iter2.data(), "refParms_iter2[5]/F", basketSize); - // tree->Branch("refCov_iter2", refCov_iter2.data(), "refCov_iter2[25]/F", basketSize); - - tree->Branch("refParms", refParms.data(), "refParms[5]/F", basketSize); - tree->Branch("refCov", refCov.data(), "refCov[25]/F", basketSize); - tree->Branch("genParms", genParms.data(), "genParms[5]/F", basketSize); - - tree->Branch("genPt", &genPt, basketSize); - tree->Branch("genEta", &genEta, basketSize); - tree->Branch("genPhi", &genPhi, basketSize); - tree->Branch("genCharge", &genCharge, basketSize); - - tree->Branch("genX", &genX, basketSize); - tree->Branch("genY", &genY, basketSize); - tree->Branch("genZ", &genZ, basketSize); - - tree->Branch("normalizedChi2", &normalizedChi2, basketSize); - - tree->Branch("nHits", &nHits, basketSize); - tree->Branch("nValidHits", &nValidHits, basketSize); - tree->Branch("nValidPixelHits", &nValidPixelHits, basketSize); - tree->Branch("nJacRef", &nJacRef, basketSize); - - tree->Branch("nValidHitsFinal", &nValidHitsFinal); - tree->Branch("nValidPixelHitsFinal", &nValidPixelHitsFinal); - - tree->Branch("jacrefv",jacrefv.data(),"jacrefv[nJacRef]/F", basketSize); - - - -// tree->Branch("dxpxb1", &dxpxb1); -// tree->Branch("dypxb1", &dypxb1); -// -// tree->Branch("dxttec9rphi", &dxttec9rphi); -// tree->Branch("dxttec9stereo", &dxttec9stereo); -// -// tree->Branch("dxttec4rphi", &dxttec4rphi); -// tree->Branch("dxttec4stereo", &dxttec4stereo); -// -// tree->Branch("dxttec4rphisimgen", &dxttec4rphisimgen); -// tree->Branch("dyttec4rphisimgen", &dyttec4rphisimgen); -// tree->Branch("dxttec4rphirecsim", &dxttec4rphirecsim); -// -// tree->Branch("dxttec9rphisimgen", &dxttec9rphisimgen); -// tree->Branch("dyttec9rphisimgen", &dyttec9rphisimgen); -// -// tree->Branch("simlocalxref", &simlocalxref); -// tree->Branch("simlocalyref", &simlocalyref); - - tree->Branch("hitidxv", &hitidxv); - tree->Branch("dxrecgen", &dxrecgen); - tree->Branch("dyrecgen", &dyrecgen); - tree->Branch("dxsimgen", &dxsimgen); - tree->Branch("dysimgen", &dysimgen); - tree->Branch("dxrecsim", &dxrecsim); - tree->Branch("dyrecsim", &dyrecsim); - tree->Branch("dxerr", &dxerr); - tree->Branch("dyerr", &dyerr); - - tree->Branch("clusterSize", &clusterSize); - tree->Branch("clusterSizeX", &clusterSizeX); - tree->Branch("clusterSizeY", &clusterSizeY); - tree->Branch("clusterCharge", &clusterCharge); - tree->Branch("clusterChargeBin", &clusterChargeBin); - tree->Branch("clusterOnEdge", &clusterOnEdge); - - tree->Branch("clusterProbXY", &clusterProbXY); - tree->Branch("clusterSN", &clusterSN); - - tree->Branch("dxreccluster", &dxreccluster); - tree->Branch("dyreccluster", &dyreccluster); - - tree->Branch("localqop", &localqop); - tree->Branch("localdxdz", &localdxdz); - tree->Branch("localdydz", &localdydz); - tree->Branch("localx", &localx); - tree->Branch("localy", &localy); - - tree->Branch("simtestz", &simtestz); - tree->Branch("simtestvz", &simtestvz); - tree->Branch("simtestrho", &simtestrho); - tree->Branch("simtestzlocalref", &simtestzlocalref); - tree->Branch("simtestdx", &simtestdx); - tree->Branch("simtestdxrec", &simtestdxrec); - tree->Branch("simtestdy", &simtestdy); - tree->Branch("simtestdyrec", &simtestdyrec); - tree->Branch("simtestdxprop", &simtestdxprop); - tree->Branch("simtestdyprop", &simtestdyprop); - tree->Branch("simtestdetid", &simtestdetid); - - tree->Branch("rx", &rx); - tree->Branch("ry", &ry); - - tree->Branch("deigx", &deigx); - tree->Branch("deigy", &deigy); - - nJacRef = 0.; - } -} - - -// ------------ method called for each event ------------ -void ResidualGlobalCorrectionMakerSim::produce(edm::Event &iEvent, const edm::EventSetup &iSetup) -{ - - const bool dogen = fitFromGenParms_; - - using namespace edm; - - Handle trackOrigH; - iEvent.getByToken(inputTrackOrig_, trackOrigH); - - -// bool foundmodule = false; -// for (const reco::Track &track : *trackOrigH) { -// if (track.innerDetId() == 302055944) { -// foundmodule = true; -// break; -// } -// // for (auto it = track.recHitsBegin(); it != track.recHitsEnd(); ++it) { -// // if ((*it)->geographicalId().rawId() == 302055944) { -// // foundmodule = true; -// // break; -// // } -// // } -// // if (foundmodule) { -// // break; -// // } -// } -// if (!foundmodule) { -// // printf("not found, returning\n"); -// return; -// } - - - - // loop over gen particles - - edm::ESHandle globalGeometry; - iSetup.get().get(globalGeometry); - -// edm::ESHandle globalGeometry; -// iSetup.get().get("idealForDigi", globalGeometry); - - edm::ESHandle trackerTopology; - iSetup.get().get(trackerTopology); - -// ESHandle magfield; -// iSetup.get().get(magfield); -// auto field = magfield.product(); - - edm::ESHandle ttrh; - iSetup.get().get("WithAngleAndTemplate",ttrh); - - ESHandle thePropagator; - iSetup.get().get("RungeKuttaTrackerPropagator", thePropagator); -// iSetup.get().get("PropagatorWithMaterial", thePropagator); -// iSetup.get().get("PropagatorWithMaterialParabolicMf", thePropagator); -// iSetup.get().get("Geant4ePropagator", thePropagator); - const MagneticField* field = thePropagator->magneticField(); - - ESHandle theAnalyticPropagator; - iSetup.get().get("PropagatorWithMaterial", theAnalyticPropagator); - -// Handle trackH; -// Handle trackH; -// iEvent.getByToken(inputTrack_, trackH); - - - -// Handle > indicesH; -// iEvent.getByToken(inputIndices_, indicesH); - -// Handle > trajH; -// iEvent.getByToken(inputTraj_, trajH); - - - - - Handle bsH; - iEvent.getByToken(inputBs_, bsH); - - - Handle> genPartCollection; - if (doGen_) { - iEvent.getByToken(GenParticlesToken_, genPartCollection); - } - -// Handle> tecSimHits; - std::vector>> simHits(inputSimHits_.size()); - if (doSim_) { - for (unsigned int isimhit = 0; isimhit fPropagator(static_cast(thePropagator->clone())); - fPropagator->setPropagationDirection(alongMomentum); - - std::unique_ptr fAnalyticPropagator(static_cast(theAnalyticPropagator->clone())); - fAnalyticPropagator->setPropagationDirection(alongMomentum); - - KFUpdator updator; - TkClonerImpl const& cloner = static_cast(ttrh.product())->cloner(); - - -// siStripClusterInfo_.initEvent(iSetup); - -// edm::ESHandle globalPositionRcd; -// iSetup.get().get(globalPositionRcd); -// -// printf("globalPositionRcd translation = %e, %e, %e\n", globalPositionRcd->m_align.front().translation().x(), -// globalPositionRcd->m_align.front().translation().y(), -// globalPositionRcd->m_align.front().translation().z()); -// std::cout << "globalPositionRcd rotation" << globalPositionRcd->m_align.front().rotation() << std::endl; - - // set up cylindrical surface for beam pipe -// const double ABe = 9.0121831; -// const double ZBe = 4.; -// const double K = 0.307075*1e-3; -// const double dr = 0.08; -// // const double xibeampipe = 0.5*K*dr*ZBe/ABe; -// const double xibeampipe = 0.*0.5*K*dr*ZBe/ABe; - - - - -// auto beampipe = Cylinder::build(Surface::PositionType(0.,0.,0.), Surface::RotationType(), 2.94); -// beampipe->setMediumProperties(MediumProperties(0., xibeampipe)); - -// std::cout << "xi beampipe: " << xibeampipe << std::endl; - -// const GeomDet *testdet = nullptr; -// //debugging -// for (const GeomDet* det : globalGeometry->detUnits()) { -// if (!det) { -// continue; -// } -// -// if (det->subDetector() == GeomDetEnumerators::TEC) { -// const DetId& detid = det->geographicalId(); -// // TECDetId detid(det->geographicalId()); -// // layer = -1 * (detid.side() == 1) * detid.wheel() + (detid.side() == 2) * detid.wheel(); -// unsigned int side = trackerTopology->tecSide(detid); -// unsigned int wheel = trackerTopology->tecWheel(detid); -// int layer = -1 * (side == 1) * wheel + (side == 2) * wheel; -// bool stereo = trackerTopology->isStereo(det->geographicalId()); -// -// if (layer == -9) { -// testdet = det; -// break; -// -// } -// } -// -// -// -// } -// -// if (testdet) { -// const GlobalPoint center = testdet->surface().toGlobal(LocalPoint(1.,0.)); -// -// const GlobalVector centerv(center.x(), center.y(), center.z()); -// const GlobalVector dir = centerv/centerv.mag(); -// const double sintheta = dir.perp(); -// const GlobalVector mom = (100000./sintheta)*dir; -// const GlobalPoint pos(0.,0.,0.); -// -// FreeTrajectoryState ftsplus(pos, mom, 1., field); -// FreeTrajectoryState ftsminus(pos, mom, -1., field); -// -// const TrajectoryStateOnSurface tsosplus = fPropagator->propagate(ftsplus, testdet->surface()); -// const TrajectoryStateOnSurface tsosminus = fPropagator->propagate(ftsminus, testdet->surface()); -// -// std::cout << "global target" << std::endl; -// std::cout << center << std::endl; -// -// std::cout << "momentum" << std::endl; -// std::cout << mom << std::endl; -// -// std::cout << "tsosplus local:" << std::endl; -// std::cout << tsosplus.localPosition() << std::endl; -// std::cout << "tsosminus local:" << std::endl; -// std::cout << tsosminus.localPosition() << std::endl; -// -// std::cout << "delta local" << std::endl; -// std::cout << tsosplus.localPosition() - tsosminus.localPosition() << std::endl; -// -// } - - - simtestz = -99.; - simtestvz = -99.; - simtestrho = -99.; - simtestzlocalref = -99.; - simtestdx = -99.; - simtestdxrec = -99.; - simtestdy = -99.; - simtestdyrec = -99.; - simtestdxprop = -99.; - simtestdyprop = -99.; - simtestdetid = 0; - - std::vector hits; - for (auto const& simhith : simHits) { - for (const PSimHit& simHit : *simhith) { - if (std::abs(simHit.particleType()) == 13) { -// if (std::abs(simHit.localPosition().z()) > 1e-9) { -// continue; -// } - hits.push_back(&simHit); - } - } - } - - auto simhitcompare = [&](const PSimHit *hit0, const PSimHit *hit1) { - return globalGeometry->idToDet(hit0->detUnitId())->surface().toGlobal(hit0->localPosition()).mag() < globalGeometry->idToDet(hit1->detUnitId())->surface().toGlobal(hit1->localPosition()).mag(); - }; - - std::sort(hits.begin(), hits.end(), simhitcompare); - - - -// TkClonerImpl hitCloner; -// TKCloner const* cloner = static_cast(builder)->cloner() -// TrajectoryStateCombiner combiner; - - run = iEvent.run(); - lumi = iEvent.luminosityBlock(); - event = iEvent.id().event(); - -// for (const reco::Track &track : *trackOrigH) { - if (true) { -// const Trajectory& traj = (*trajH)[itraj]; - -// const edm::Ref > trajref(trajH, j); -// const reco::Track& track = *(*trackH)[trajref]; -// const reco::Track& track = (*trackH)[itraj]; -// const reco::Track& trackOrig = (*trackOrigH)[(*indicesH)[j]]; - -// std::cout << "j " << j << " (*indicesH)[j] " << (*indicesH)[j] <(trackParms.data()) = Map(tkparms.Array()).cast(); -// Map >(trackCov.data()).triangularView() = Map >(tkcov.Array()).cast().triangularView(); -// -// // std::cout << "track charge: " << track.charge() << " trackorig charge " << trackOrig.charge() << "inner state charge " << tms.back().updatedState().charge() << std::endl; - - const reco::GenParticle* genpart = nullptr; - - genPt = -99.; - genEta = -99.; - genPhi = -99.; - genCharge = -99; - genX = -99.; - genY = -99.; - genZ = -99.; - genParms.fill(0.); - - if (doGen_) { - for (std::vector::const_iterator g = genPartCollection->begin(); g != genPartCollection->end(); ++g) - { - if (g->status() != 1) { - continue; - } - if (std::abs(g->pdgId()) != 13) { - continue; - } - -// float dR = deltaR(g->phi(), trackPhi, g->eta(), trackEta); - - if (true) - { - genpart = &(*g); - - genPt = g->pt(); - genEta = g->eta(); - genPhi = g->phi(); - genCharge = g->charge(); - - genX = g->vertex().x(); - genY = g->vertex().y(); - genZ = g->vertex().z(); - - auto const& vtx = g->vertex(); - auto const& myBeamSpot = bsH->position(vtx.z()); - - //q/|p| - genParms[0] = g->charge()/g->p(); - //lambda - genParms[1] = M_PI_2 - g->momentum().theta(); - //phi - genParms[2] = g->phi(); - //dxy - genParms[3] = (-(vtx.x() - myBeamSpot.x()) * g->py() + (vtx.y() - myBeamSpot.y()) * g->px()) / g->pt(); - //dsz - genParms[4] = (vtx.z() - myBeamSpot.z()) * g->pt() / g->p() - - ((vtx.x() - myBeamSpot.x()) * g->px() + (vtx.y() - myBeamSpot.y()) * g->py()) / g->pt() * g->pz() / g->p(); - } - else { - continue; - } - } - } - - -// printf("done building hits\n"); - -// const unsigned int nhits = track.recHitsSize(); - const unsigned int nhits = hits.size(); - nHits = nhits; -// unsigned int npixhits = 0; - - unsigned int nvalid = 0; - unsigned int nvalidpixel = 0; - unsigned int nvalidalign2d = 0; - - // count valid hits since this is needed to size the arrays - for (auto const& hit : hits) { -// assert(hit->dimension()<=2); - if (true) { - nvalid += 1; - -// const uint32_t gluedid = trackerTopology->glued(hit->geographicalId()); -// const bool isglued = gluedid != 0; -// const DetId parmdetid = isglued ? DetId(gluedid) : hit->geographicalId(); -// const bool align2d = detidparms.count(std::make_pair(1, parmdetid)); -// const bool align2d = detidparms.count(std::make_pair(2, hit->geographicalId())); - - const bool align2d = detidparms.count(std::make_pair(1, hit->detUnitId())); -// - if (align2d) { - nvalidalign2d += 1; - } - const GeomDet* det = globalGeometry->idToDet(hit->detUnitId()); - if (GeomDetEnumerators::isTrackerPixel(det->subDetector())) { - nvalidpixel += 1; - } - } - } - -// //count valid hits since this is needed to size the arrays -// auto const& hitsbegin = track.recHitsBegin(); -// for (unsigned int ihit = 0; ihit < track.recHitsSize(); ++ihit) { -// auto const& hit = *(hitsbegin + ihit); -// if (hit->isValid() && hit->dimension()<=2) { -// nvalid += 1; -// -// const GeomDet *detectorG = globalGeometry->idToDet(hit->geographicalId()); -// if (hit->dimension()==2 && GeomDetEnumerators::isTrackerPixel(detectorG->subDetector())) { -// // if (hit->dimension()==2) { -// nvalidpixel += 1; -// } -// } -// } - - nValidHits = nvalid; - nValidPixelHits = nvalidpixel; - - nValidHitsFinal = 0; - nValidPixelHitsFinal = 0; - -// const unsigned int nstriphits = nhits-npixhits; -// const unsigned int nparsAlignment = nstriphits + 2*npixhits; -// const unsigned int nvalidstrip = nvalid - nvalidpixel; -// const unsigned int nparsAlignment = nvalidstrip + 2*nvalidpixel; - const unsigned int nparsAlignment = 2*nvalid + nvalidalign2d; -// const unsigned int nparsAlignment = 6*nvalid; - const unsigned int nparsBfield = nhits; - const unsigned int nparsEloss = nhits - 1; - const unsigned int npars = nparsAlignment + nparsBfield + nparsEloss; - -// const unsigned int nstateparms = 5*(nhits+1); - const unsigned int nstateparms = 3*(nhits+1) - 1; -// const unsigned int nstateparms = 3*nhits - 1; - const unsigned int nparmsfull = nstateparms + npars; - - - const unsigned int nstateparmspost = 5*nhits; - -// std::cout << "nhits " << nhits << std::endl; -// std::cout << "nstateparms " << nstateparms << std::endl; -// std::cout << "nparmsfull " << nparmsfull << std::endl; -// std::cout << "nparmsfull " << nparmsfull << std::endl; -// std::cout << "nparmsfull " << nparmsfull << std::endl; -// std::cout << "nparmsfull " << nparmsfull << std::endl; -// std::cout << "nparmsfull " << nparmsfull << std::endl; - -// const unsigned int npropparms = 5*(nhits-1); -// const unsigned int nhitparms = 2*nhits; -// const unsigned int nmomparms = 3*(nhits-1); -// const unsigned int nposparms = 2*(nhits-1); -// constexpr unsigned int nrefparms = 5; - - - - //active double for autodiff gradients -// using Adouble = AutoDiffScalar; -// using AVectorXd = Matrix; -// //double double for autodiff hessians -// using AAdouble = AutoDiffScalar; - - - -// using AAXd = AANT; -// using AAdouble = AAXd; -// -// using AA2d = AANT; -// using AA3d = AANT; -// using AA4d = AANT; -// using AA12d = AANT; -// -// using ScalarConst = AANT; - -// using AConstd = AutoDiffScalar; -// using AConstd = AutoDiffScalar>; - - -// using VectorXAd = Matrix; -// using MatrixXAd = Matrix; - - //two position parameters and and one alignment parameter - using StripHitScalar = AANT;; - - using StripHit1DJacobian = Matrix; - - using StripHitVector = Matrix; - using StripHit2DCovariance = Matrix; - using StripHit2DJacobian = Matrix; - - - - //two hit dimensions and two alignment parameters - using PixelHit2DScalar = AANT; - using PixelHit2DVector = Matrix; - using PixelHit2DCovariance = Matrix; - using PixelHit2DJacobian = Matrix; - - - //2x5 state parameters, one bfield parameter, and one material parameter -// using MSScalar = AANT;; - using MSScalar = AANT; - using MSVector = Matrix; - using MSProjection = Matrix; - using MSJacobian = Matrix; - using MSCovariance = Matrix; - - using BSScalar = AANT; - -// using HitProjection = Matrix; -// using HitCovariance = Matrix; -// using HitVector = Matrix; - -// evector Vinv(nhits, HitCovarianceMatrix::Zero()); -// evector Hh(nhits, HitProjection::Zero()); -// evector dy0(nhits, HitVector::Zero()); -// evector dx(nhits, StateVector::Zero()); -// //initialize backpropagation indexing -// for (unsigned int i=0; i rxfull(nvalid, 2); - Matrix ryfull(nvalid, 2); - -// VectorXd gradfull = chisq.value().derivatives(); -// MatrixXd hessfull = MatrixXd::Zero(nparmsfull, nparmsfull); -// for (unsigned int i=0; iSetBranchAddress("globalidxv", globalidxv.data()); -// } - -// TrajectoryStateOnSurface currtsos; - - - - VectorXd dxfull; - MatrixXd dxdparms; - VectorXd grad; - MatrixXd hess; - LDLT Cinvd; - -// if (dogen && genpart==nullptr) { -// continue; -// } -// -// if (dogen && genpart->eta()>-2.3) { -// continue; -// } - -// if (genpart==nullptr) { -// continue; -// } -// if (genpart->pt()>10.) { -// continue; -// } -// if (genpart->pt()<100.) { -// continue; -// } -// if (genpart->eta()>-2.3) { -// continue; -// } - -// if (debugprintout_) { -// std::cout << "initial reference point parameters:" << std::endl; -// std::cout << track.parameters() << std::endl; -// } - -// //prepare hits -// TransientTrackingRecHit::RecHitContainer hits; -// hits.reserve(track.recHitsSize()); -// for (auto it = track.recHitsBegin(); it != track.recHitsEnd(); ++it) { -// const GeomDet *detectorG = globalGeometry->idToDet((*it)->geographicalId()); -// hits.push_back((*it)->cloneForFit(*detectorG)); -// } - -// // fix mixed up clusters? -// for (unsigned int ihit=0; ihit<(hits.size()-1); ++ihit) { -// TrackerSingleRecHit* hit = const_cast(dynamic_cast(hits[ihit].get())); -// TrackerSingleRecHit* nexthit = const_cast(dynamic_cast(hits[ihit+1].get())); -// -// // const TrackingRecHitSingle* nexthit = hits[ihit+1]; -// -// if (!hit || !nexthit) { -// continue; -// } -// -// const DetId partnerid = trackerTopology->partnerDetId(hit->geographicalId()); -// // -// if (partnerid == nexthit->geographicalId()) { -// // std::cout << "swapping clusters" << std::endl; -// const OmniClusterRef::ClusterStripRef cluster = hit->cluster_strip(); -// const OmniClusterRef::ClusterStripRef nextcluster = nexthit->cluster_strip(); -// -// hit->setClusterStripRef(nextcluster); -// nexthit->setClusterStripRef(cluster); -// } -// -// } - -// if (genpart==nullptr) { -// continue; -// } -// -// if (genpart->eta()<-2.4 || genpart->eta()>-2.3) { -// continue; -// } - - - FreeTrajectoryState refFts; - -// if (dogen) { - if (true) { - -// if (genpart==nullptr) { -// continue; -// } - //init from gen state - auto const& refpoint = genpart->vertex(); - auto const& trackmom = genpart->momentum(); - const GlobalPoint refpos(refpoint.x(), refpoint.y(), refpoint.z()); -// const GlobalPoint refpos(refpoint.x()+50e-4, refpoint.y()+50e-4, refpoint.z()+200e-4); - const GlobalVector refmom(trackmom.x(), trackmom.y(), trackmom.z()); -// const GlobalVector refmom(1.02*trackmom.x(), 1.01*trackmom.y(), 1.01*trackmom.z()); - const GlobalTrajectoryParameters refglobal(refpos, refmom, genpart->charge(), field); - -// std::cout << "gen ref state" << std::endl; -// std::cout << refpos << std::endl; -// std::cout << refmom << std::endl; -// std::cout << genpart->charge() << std::endl; - - //zero uncertainty on generated parameters -// AlgebraicSymMatrix55 nullerr; -// const CurvilinearTrajectoryError referr(nullerr); - - refFts = FreeTrajectoryState(refpos, refmom, genpart->charge(), field); -// refFts = FreeTrajectoryState(refpos, 0.98*refmom, genpart->charge(), field); -// refFts = FreeTrajectoryState(refglobal, referr); - } - else { - } - -// std::vector> layerStates; - std::vector layerStates; - layerStates.reserve(nhits); - - bool valid = true; - - -// //do propagation and prepare states -// auto propresult = fPropagator->propagateWithPath(refFts, *hits.front()->surface()); -// if (!propresult.first.isValid()) { -// std::cout << "Abort: Propagation from reference point failed" << std::endl; -// continue; -// } -// layerStates.push_back(propresult); -// -// for (auto const& hit : hits) { -// propresult = fPropagator->propagateWithPath(layerStates.back().first, *hit->surface()); -// if (!propresult.first.isValid()) { -// std::cout << "Abort: Propagation failed" << std::endl; -// valid = false; -// break; -// } -// layerStates.push_back(propresult); -// } -// -// if (!valid) { -// continue; -// } - - - - //inflate errors -// refFts.rescaleError(100.); - - -// unsigned int ntotalhitdim = 0; -// unsigned int alignmentidx = 0; -// unsigned int bfieldidx = 0; -// unsigned int elossidx = 0; - - constexpr unsigned int niters = 1; -// constexpr unsigned int niters = 3; - - for (unsigned int iiter=0; iiter 0; -// const bool islikelihood = true; - - gradfull = VectorXd::Zero(nparmsfull); - hessfull = MatrixXd::Zero(nparmsfull, nparmsfull); - statejac = MatrixXd::Zero(nstateparmspost, nparmsfull); - - validdxeigjac = MatrixXd::Zero(2*nvalid, nstateparms); - - evector, 11> > dhessv; - if (islikelihood) { - dhessv.resize(nhits-1); - } - - dxpxb1 = -99.; - dypxb1 = -99.; - dxttec9rphi = -99.; - dxttec9stereo = -99.; - dxttec4rphi = -99.; - dxttec4stereo = -99.; - - dxttec4rphisimgen = -99.; - dyttec4rphisimgen = -99.; - dxttec4rphirecsim = -99.; - - dxttec9rphisimgen = -99.; - dyttec9rphisimgen = -99.; - - simlocalxref = -99.; - simlocalyref = -99.; - - -// dhessv.reserve(nhits-1); - - unsigned int parmidx = 0; - unsigned int alignmentparmidx = 0; - unsigned int ivalidhit = 0; - - if (iiter > 0) { - //update current state from reference point state (errors not needed beyond first iteration) - JacobianCurvilinearToCartesian curv2cart(refFts.parameters()); - const AlgebraicMatrix65& jac = curv2cart.jacobian(); - const AlgebraicVector6 glob = refFts.parameters().vector(); - - auto const& dxlocal = dxstate.head<5>(); - const Matrix globupd = Map>(glob.Array()) + Map>(jac.Array())*dxlocal; - - const GlobalPoint pos(globupd[0], globupd[1], globupd[2]); - const GlobalVector mom(globupd[3], globupd[4], globupd[5]); - double charge = std::copysign(1., refFts.charge()/refFts.momentum().mag() + dxlocal[0]); -// std::cout << "before update: reffts:" << std::endl; -// std::cout << refFts.parameters().vector() << std::endl; -// std::cout << "charge " << refFts.charge() << std::endl; - refFts = FreeTrajectoryState(pos, mom, charge, field); -// std::cout << "after update: reffts:" << std::endl; -// std::cout << refFts.parameters().vector() << std::endl; -// std::cout << "charge " << refFts.charge() << std::endl; -// currentFts = refFts; - } - -// Matrix5d Hlm = Matrix5d::Identity(); -// currentFts = refFts; -// TrajectoryStateOnSurface currentTsos; - -// ; - - const GeomDet *det0 = globalGeometry->idToDet(hits[0]->detUnitId()); - auto propresult = fPropagator->geometricalPropagator().propagateWithPath(refFts, det0->surface()); -// auto propresult = fPropagator->geometricalPropagator().propagateWithPath(refFts, *beampipe); - if (!propresult.first.isValid()) { - std::cout << "Abort: Propagation of reference state Failed!" << std::endl; - valid = false; - break; - } - -// std::cout << "position on beampipe " << propresult.first.globalParameters().position() << std::endl; - - const Matrix FdFp = curv2curvTransportJacobian(refFts, propresult, false); - - Matrix J = FdFp.block<2, 2>(3, 3); - // (du/dalphap)^-1 - Matrix Sinv = FdFp.block<2, 2>(3, 1).inverse(); - // du/dqopp - Matrix D = FdFp.block<2, 1>(3, 0); - // du/dBp - Matrix Bpref = FdFp.block<2, 1>(3, 5); - - constexpr unsigned int jacstateidxout = 0; - constexpr unsigned int jacstateidxin = 0; - - // qop_i - statejac(jacstateidxout, jacstateidxin + 2) = 1.; - // d(lambda, phi)_i/dqop_i - statejac.block<2, 1>(jacstateidxout + 1, jacstateidxin + 2) = -Sinv*D; - // d(lambda, phi)_i/(dxy, dsz) - statejac.block<2, 2>(jacstateidxout + 1, jacstateidxin) = -Sinv*J; - // d(lambda, phi)_i/du_(i+1) - statejac.block<2, 2>(jacstateidxout + 1, jacstateidxin + 3) = Sinv; - // d(lambda, phi) / dbeta - statejac.block<2, 1>(jacstateidxout + 1, nstateparms + parmidx) = -Sinv*Bpref; - // dxy - statejac(jacstateidxout + 3, jacstateidxin) = 1.; - // dsz - statejac(jacstateidxout + 4, jacstateidxin + 1) = 1.; - - if (bsConstraint_) { - // apply beamspot constraint - // TODO add residual corrections for beamspot parameters? - - constexpr unsigned int nlocalstate = 2; - constexpr unsigned int nlocalbs = 0; - constexpr unsigned int nlocalparms = nlocalbs; - - constexpr unsigned int nlocal = nlocalstate + nlocalparms; - - constexpr unsigned int localstateidx = 0; - constexpr unsigned int localparmidx = localstateidx + nlocalstate; - - constexpr unsigned int fullstateidx = 0; - const unsigned int fullparmidx = nstateparms + parmidx; - - JacobianCurvilinearToCartesian curv2cart(refFts.parameters()); - const AlgebraicMatrix65& jac = curv2cart.jacobian(); - - const double sigb1 = bsH->BeamWidthX(); - const double sigb2 = bsH->BeamWidthY(); - const double sigb3 = bsH->sigmaZ(); - const double dxdz = bsH->dxdz(); - const double dydz = bsH->dydz(); - const double x0 = bsH->x0(); - const double y0 = bsH->y0(); - const double z0 = bsH->z0(); - - - // covariance matrix of luminous region in global coordinates - // taken from https://github.com/cms-sw/cmssw/blob/abc1f17b230effd629c9565fb5d95e527abcb294/RecoVertex/BeamSpotProducer/src/FcnBeamSpotFitPV.cc#L63-L90 - - // FIXME xy correlation is not stored and assumed to be zero - const double corrb12 = 0.; - - const double varb1 = sigb1*sigb1; - const double varb2 = sigb2*sigb2; - const double varb3 = sigb3*sigb3; - - Matrix covBS = Matrix::Zero(); - // parametrisation: rotation (dx/dz, dy/dz); covxy - covBS(0,0) = varb1; - covBS(1,0) = covBS(0,1) = corrb12*sigb1*sigb2; - covBS(1,1) = varb2; - covBS(2,0) = covBS(0,2) = dxdz*(varb3-varb1)-dydz*covBS(1,0); - covBS(2,1) = covBS(1,2) = dydz*(varb3-varb2)-dxdz*covBS(1,0); - covBS(2,2) = varb3; - -// std::cout << "covBS:" << std::endl; -// std::cout << covBS << std::endl; - - Matrix du = Matrix::Zero(); - for (unsigned int j=0; j dbs0; - dbs0[0] = BSScalar(refFts.position().x() - x0); - dbs0[1] = BSScalar(refFts.position().y() - y0); - dbs0[2] = BSScalar(refFts.position().z() - z0); - -// std::cout << "dposition / d(qop, lambda, phi) (should be 0?):" << std::endl; -// std::cout << Map>(jac.Array()).topLeftCorner<3,3>() << std::endl; - - const Matrix jacpos = Map>(jac.Array()).topRightCorner<3,2>().cast(); - const Matrix covBSinv = covBS.inverse().cast(); - - const Matrix dbs = dbs0 + jacpos*du; - const BSScalar chisq = dbs.transpose()*covBSinv*dbs; - - auto const& gradlocal = chisq.value().derivatives(); - //fill local hessian - Matrix hesslocal; - for (unsigned int j=0; j(fullstateidx) += gradlocal.head(); - gradfull.segment(fullparmidx) += gradlocal.segment(localparmidx); - - //fill global hessian (upper triangular blocks only) - hessfull.block(fullstateidx, fullstateidx) += hesslocal.topLeftCorner(); - hessfull.block(fullstateidx, fullparmidx) += hesslocal.topRightCorner(); - hessfull.block(fullparmidx, fullparmidx) += hesslocal.bottomRightCorner(); - - - } - - - Matrix FdFm = curv2curvTransportJacobian(refFts, propresult, true); - - for (unsigned int ihit = 0; ihit < hits.size(); ++ihit) { - std::cout << "ihit " << ihit << std::endl; - auto const& hit = hits[ihit]; - - TrajectoryStateOnSurface updtsos = propresult.first; - - //apply measurement update if applicable -// std::cout << "constructing preciseHit" << std::endl; - auto const& preciseHit = hit; -// if (hit->isValid() && !preciseHit->isValid()) { -// std::cout << "Abort: Failed updating hit" << std::endl; -// valid = false; -// break; -// } - const GeomDet *det = globalGeometry->idToDet(preciseHit->detUnitId()); - -// const uint32_t gluedid = trackerTopology->glued(preciseHit->det()->geographicalId()); -// const bool isglued = gluedid != 0; -// const DetId parmdetid = isglued ? DetId(gluedid) : preciseHit->geographicalId(); -// const bool align2d = detidparms.count(std::make_pair(1, parmdetid)); -// const GeomDet* parmDet = isglued ? globalGeometry->idToDet(parmdetid) : preciseHit->det(); - - const bool align2d = detidparms.count(std::make_pair(1, preciseHit->detUnitId())); - - - // compute convolution correction in local coordinates (BEFORE material effects are applied) -// const Matrix dxlocalconv = localPositionConvolution(updtsos); - - // curvilinear to local jacobian - JacobianCurvilinearToLocal curv2localm(updtsos.surface(), updtsos.localParameters(), *updtsos.magneticField()); - const AlgebraicMatrix55& curv2localjacm = curv2localm.jacobian(); - const Matrix Hm = Map>(curv2localjacm.Array()); - - //energy loss jacobian - const Matrix EdE = materialEffectsJacobian(updtsos, fPropagator->materialEffectsUpdator()); - - //process noise jacobians - const std::array, 5> dQs = processNoiseJacobians(updtsos, fPropagator->materialEffectsUpdator()); - - //TODO update code to allow doing this in one step with nominal update - //temporary tsos to extract process noise without loss of precision - TrajectoryStateOnSurface tmptsos(updtsos); - tmptsos.update(tmptsos.localParameters(), - LocalTrajectoryError(0.,0.,0.,0.,0.), - tmptsos.surface(), - tmptsos.magneticField(), - tmptsos.surfaceSide()); - - //apply the state update from the material effects - bool ok = fPropagator->materialEffectsUpdator().updateStateInPlace(tmptsos, alongMomentum); - if (!ok) { - std::cout << "Abort: material update failed" << std::endl; - valid = false; - break; - } - - const AlgebraicVector5 dxeloss = tmptsos.localParameters().vector() - updtsos.localParameters().vector(); - - // compute convolution effects -// const AlgebraicVector5 dlocalconv = localMSConvolution(updtsos, fPropagator->materialEffectsUpdator()); - -// const GlobalPoint updtsospos = updtsos.globalParameters().position(); -// std::cout << "before material update: " << updtsos.globalParameters().position() << " " << updtsos.globalParameters().momentum() << std::endl; -// ok = fPropagator->materialEffectsUpdator().updateStateInPlace(updtsos, alongMomentum); - ok = true; - if (!ok) { - std::cout << "Abort: material update failed" << std::endl; - valid = false; - break; - } -// std::cout << "after material update: " << updtsos.globalParameters().position() << " " << updtsos.globalParameters().momentum() << std::endl; - - -// std::cout << "local parameters" << std::endl; -// std::cout << updtsos.localParameters().vector() << std::endl; -// std::cout << "dlocalconv" << std::endl; -// std::cout << dlocalconv << std::endl; -// -// // apply convolution effects -// const LocalTrajectoryParameters localupd(updtsos.localParameters().vector() + dlocalconv, -// updtsos.localParameters().pzSign()); -// updtsos.update(localupd, -// // updtsos.localError(), -// updtsos.surface(), -// updtsos.magneticField(), -// updtsos.surfaceSide()); - - //get the process noise matrix - AlgebraicMatrix55 const Qmat = tmptsos.localError().matrix(); - const Map>Q(Qmat.Array()); -// std::cout<< "Q" << std::endl; -// std::cout<< Q << std::endl; - - - // FIXME this is not valid for multiple iterations - // curvilinear to local jacobian - JacobianCurvilinearToLocal curv2localp(updtsos.surface(), updtsos.localParameters(), *updtsos.magneticField()); - const AlgebraicMatrix55& curv2localjacp = curv2localp.jacobian(); - const Matrix Hp = Map>(curv2localjacp.Array()); - - - //FIXME take care of this elsewhere for the moment - const bool genconstraint = dogen && ihit==0; -// const bool genconstraint = false; - - if (ihit < (nhits-1)) { - - //momentum kink residual - AlgebraicVector5 idx0(0., 0., 0., 0., 0.); -// if (iiter==0) { -// layerStates.push_back(updtsos); -// } -// else { -// //FIXME this is not valid for the updated parameterization -// -// //current state from previous state on this layer -// //save current parameters -// TrajectoryStateOnSurface& oldtsos = layerStates[ihit]; -// -// const AlgebraicVector5 local = oldtsos.localParameters().vector(); -// auto const& dxlocal = dxstate.segment<5>(5*(ihit+1)); -// const Matrix localupd = Map>(local.Array()) + dxlocal; -// AlgebraicVector5 localvecupd(localupd[0],localupd[1],localupd[2],localupd[3],localupd[4]); -// -// idx0 = localvecupd - updtsos.localParameters().vector(); -// -// const LocalTrajectoryParameters localparms(localvecupd, oldtsos.localParameters().pzSign()); -// -// // std::cout << "before update: oldtsos:" << std::endl; -// // std::cout << oldtsos.localParameters().vector() << std::endl; -// oldtsos.update(localparms, oldtsos.surface(), field, oldtsos.surfaceSide()); -// // std::cout << "after update: oldtsos:" << std::endl; -// // std::cout << oldtsos.localParameters().vector() << std::endl; -// updtsos = oldtsos; -// -// } - - const Vector5d dx0 = Map(idx0.Array()); - - -// if (ihit==0) { -// FreeTrajectoryState tmpfts(updtsospos, updtsos.globalParameters().momentum(), updtsos.charge(), field); -// propresult = fPropagator->geometricalPropagator().propagateWithPath(tmpfts, *hits[ihit+1]->surface()); -// } -// else { -// propresult = fPropagator->geometricalPropagator().propagateWithPath(updtsos, *hits[ihit+1]->surface()); -// } - const GeomDet *detplus1 = globalGeometry->idToDet(hits[ihit+1]->detUnitId()); - propresult = fPropagator->geometricalPropagator().propagateWithPath(updtsos, detplus1->surface()); - if (!propresult.first.isValid()) { - std::cout << "Abort: Propagation Failed!" << std::endl; - valid = false; - break; - } - - if (true) { -// if (false) { - //forward propagation jacobian (local to local) - const Matrix FdFp = curv2curvTransportJacobian(*updtsos.freeState(), propresult, false); - - Matrix J = FdFp.block<2, 2>(3, 3); - // (du/dalphap)^-1 - Matrix Sinv = FdFp.block<2, 2>(3, 1).inverse(); - // du/dqopp - Matrix D = FdFp.block<2, 1>(3, 0); - // du/dBp - Matrix Bstate = FdFp.block<2, 1>(3, 5); - - const unsigned int jacstateidxout = 5*(ihit+1); - const unsigned int jacstateidxin = 3*(ihit+1); - - // qop_i - statejac(jacstateidxout, jacstateidxin + 2) = 1.; - // dalpha_i/dqop_i - statejac.block<2, 1>(jacstateidxout + 1, jacstateidxin + 2) = -Sinv*D; - // dalpha_i/du_i - statejac.block<2, 2>(jacstateidxout + 1, jacstateidxin) = -Sinv*J; - // dalpha_i/du_(i+1) - statejac.block<2, 2>(jacstateidxout + 1, jacstateidxin + 3) = Sinv; - // d(lambda, phi) / dbeta - statejac.block<2, 1>(jacstateidxout + 1, nstateparms + parmidx) = -Sinv*Bstate; - // xlocal_i - statejac(jacstateidxout + 3, jacstateidxin) = 1.; - // ylocal_i - statejac(jacstateidxout + 4, jacstateidxin + 1) = 1.; - - - -// std::cout << "FdFm" << std::endl; -// std::cout << FdFm << std::endl; -// std::cout << "FdFp" << std::endl; -// std::cout << FdFp << std::endl; - - constexpr unsigned int nlocalstate = 8; - constexpr unsigned int nlocalbfield = 3; - constexpr unsigned int nlocaleloss = 2; - constexpr unsigned int nlocalparms = nlocalbfield + nlocaleloss; - - constexpr unsigned int nlocal = nlocalstate + nlocalbfield + nlocaleloss; - - constexpr unsigned int localstateidx = 0; - // constexpr unsigned int localbfieldidx = localstateidx + nlocalstate; - // constexpr unsigned int localelossidx = localbfieldidx + nlocalbfield; - constexpr unsigned int localparmidx = localstateidx + nlocalstate; - - const unsigned int fullstateidx = 3*ihit; -// const unsigned int fullstateidx = 3*(ihit-1); - const unsigned int fullparmidx = (nstateparms + parmidx) - 2; - -// std::cout << "ihit = " << ihit << " nstateparms = " << nstateparms << " parmidx = " << parmidx << " fullparmidx = " << fullparmidx << std::endl; - - // individual pieces, now starting to cast to active scalars for autograd, - // as in eq (3) of https://doi.org/10.1016/j.cpc.2011.03.017 - // du/dum - Matrix Jm = FdFm.block<2, 2>(3, 3).cast(); - // (du/dalpham)^-1 - Matrix Sinvm = FdFm.block<2, 2>(3, 1).inverse().cast(); - // du/dqopm - Matrix Dm = FdFm.block<2, 1>(3, 0).cast(); - // du/dBm - Matrix Bm = FdFm.block<2, 1>(3, 5).cast(); - - // du/dup - Matrix Jp = FdFp.block<2, 2>(3, 3).cast(); - // (du/dalphap)^-1 - Matrix Sinvp = FdFp.block<2, 2>(3, 1).inverse().cast(); - // du/dqopp - Matrix Dp = FdFp.block<2, 1>(3, 0).cast(); - // du/dBp - Matrix Bp = FdFp.block<2, 1>(3, 5).cast(); - -// std::cout << "Jm" << std::endl; -// std::cout << Jm << std::endl; -// std::cout << "Sinvm" << std::endl; -// std::cout << Sinvm << std::endl; -// std::cout << "Dm" << std::endl; -// std::cout << Dm << std::endl; -// std::cout << "Bm" << std::endl; -// std::cout << Bm << std::endl; -// -// std::cout << "Jp" << std::endl; -// std::cout << Jp << std::endl; -// std::cout << "Sinvp" << std::endl; -// std::cout << Sinvp << std::endl; -// std::cout << "Dp" << std::endl; -// std::cout << Dp << std::endl; -// std::cout << "Bp" << std::endl; -// std::cout << Bp << std::endl; - - // energy loss jacobians - // const MSJacobian E = EdE.leftCols<5>().cast(); - // const MSVector dE = EdE.rightCols<1>().cast(); - - // fraction of material on this layer compared to glued layer if relevant -// double xifraction = isglued ? preciseHit->det()->surface().mediumProperties().xi()/parmDet->surface().mediumProperties().xi() : 1.; - -// std::cout << "xifraction: " << xifraction << std::endl; - - const MSScalar Eqop(EdE(0,0)); - const Matrix Ealpha = EdE.block<1, 2>(0, 1).cast(); - const MSScalar dE(EdE(0,5)); -// const MSScalar dE(xifraction*EdE(0,5)); -// (void)EdE; - - const MSScalar muE(dxeloss[0]); - -// std::cout<<"EdE" << std::endl; -// std::cout << EdE << std::endl; - - //energy loss inverse variance - MSScalar invSigmaE(1./Q(0,0)); - - // multiple scattering inverse covariance - Matrix Qinvms = Q.block<2,2>(1,1).inverse().cast(); - - // initialize active scalars for state parameters - Matrix dum = Matrix::Zero(); - //suppress gradients of reference point parameters when fitting with gen constraint - for (unsigned int j=0; j du = Matrix::Zero(); - for (unsigned int j=0; j dup = Matrix::Zero(); - for (unsigned int j=0; j()*dbeta; - } - else if (dogen && ihit==1) { - init_twice_active_var(dbetam, nlocal, localparmidx); - dum = Bpref.cast()*dbetam; - } - - //multiple scattering kink term - - Matrix Halphalamphim = Hm.block<2,2>(1, 1).cast(); - Matrix Halphaum = Hm.block<2,2>(1, 3).cast(); - - Matrix Halphalamphip = Hp.block<2,2>(1, 1).cast(); - Matrix Halphaup = Hp.block<2,2>(1, 3).cast(); - - const Matrix dalpha0 = dx0.segment<2>(1).cast(); - - const Matrix dlamphim = Sinvm*(dum - Jm*du - Dm*dqopm - Bm*dbeta); - const Matrix dlamphip = Sinvp*(dup - Jp*du - Dp*dqop - Bp*dbetap); - - const Matrix dalpham = Halphalamphim*dlamphim + Halphaum*du; - const Matrix dalphap = Halphalamphip*dlamphip + Halphaup*du; - - -// const Matrix dalpham = Sinvm*(dum - Jm*du - Dm*dqopm - Bm*dbeta); -// const Matrix dalphap = Sinvp*(dup - Jp*du - Dp*dqop - Bp*dbetap); -// const Matrix dalpham = Sinvm*(dum - Jm*du - Dm*dqopm); -// const Matrix dalphap = Sinvp*(dup - Jp*du - Dp*dqop); - - - const MSScalar deloss0(dx0[0]); - - - - -// const Matrix dms = dalpha0 + dalphap - dalpham; -// const MSScalar chisqms = dms.transpose()*Qinvms*dms; -// //energy loss term -// -// -// const MSScalar deloss = deloss0 + dqop - Eqop*dqopm - (Ealpha*dalpham)[0] - dE*dxi; -// const MSScalar chisqeloss = deloss*deloss*invSigmaE; -// -// const MSScalar chisq = chisqms + chisqeloss; - - - - -// const bool dolikelihood = false; -// - MSScalar chisq; - - if (!islikelihood) { - //standard chisquared contribution - - const Matrix dms = dalpha0 + dalphap - dalpham; - const MSScalar chisqms = dms.transpose()*Qinvms*dms; - //energy loss term - - - const MSScalar deloss = deloss0 + dqop - Eqop*dqopm - (Ealpha*dalpham)[0] - dE*dxi; - const MSScalar chisqeloss = deloss*invSigmaE*deloss; - - chisq = chisqms + chisqeloss; - } - else { -// islikelihood = true; - //maximum likelihood contribution - const MSCovariance dQdqop = dQs[0].cast(); -// const MSCovariance dQddxdz = dQs[1].cast(); -// const MSCovariance dQddydz = dQs[2].cast(); -// const MSCovariance dQdxi = dQs[3].cast(); - -// const MSCovariance dQ = dqopm*dQdqop + dalpham[0]*dQddxdz + dalpham[1]*dQddydz + dxi*dQdxi; -// const MSCovariance dQ = 0.5*(dqop+dqopm)*dQdqop; - const MSCovariance dQ = dqopm*dQdqop; -// const MSCovariance dQ = 0.5*(dqop+dqopm)*dQdqop + 0.5*(dalpham[0] + dalphap[0])*dQddxdz + 0.5*(dalpham[1]+dalphap[1])*dQddydz + dxi*dQdxi; - - const Matrix Qmsnom = Q.block<2,2>(1,1).cast(); - const Matrix Qmsnominv = Qmsnom.inverse(); - const Matrix Qmsinv = Qmsnominv - Qmsnominv*dQ.block<2,2>(1,1)*Qmsnominv; - - -// const Matrix Qms = Q.block<2,2>(1,1).cast() + dQ.block<2,2>(1,1); -// const Matrix Qmsinv = Qms.inverse(); -// const MSScalar logdetQms = Eigen::log(Qms.determinant()); - - const Matrix dms = dalpha0 + dalphap - dalpham; - MSScalar chisqms = dms.transpose()*Qmsinv*dms; -// chisqms = chisqms + logdetQms; - - //energy loss term -// const MSScalar sigmaE = MSScalar(Q(0,0)) + dQ(0,0); -// const MSScalar sigmaEinv = 1./sigmaE; - - const MSScalar sigmaEnom = MSScalar(Q(0,0)); - const MSScalar sigmaEnominv = 1./sigmaEnom; - - const MSScalar sigmaEinv = sigmaEnominv - sigmaEnominv*dQ(0,0)*sigmaEnominv; - -// const MSScalar logsigmaE = Eigen::log(sigmaE); - - const MSScalar deloss = deloss0 + dqop - Eqop*dqopm - (Ealpha*dalpham)[0] - dE*dxi; - MSScalar chisqeloss = deloss*sigmaEinv*deloss; -// chisqeloss = chisqeloss + logsigmaE; - - chisq = chisqms + chisqeloss; - - //compute contributions to hessian matrix-vector derivative - for (unsigned int i=0; i dQmsinv; - for (unsigned int j=0; j<2; ++j) { - for (unsigned int k=0; k<2; ++k) { - dQmsinv(j,k) = MSScalar(Qmsinv(j,k).value().derivatives()[i]); - } - } - const MSScalar dSigmaEinv(sigmaEinv.value().derivatives()[i]); - - MSScalar dchisqms = dms.transpose()*dQmsinv*x*dms; -// dchisqms = 3.*dchisqms; - MSScalar dchisqeloss = deloss*deloss*dSigmaEinv*x; -// dchisqeloss = 3.*dchisqeloss; - const MSScalar dchisq = dchisqms + dchisqeloss; - - //TODO should this be 11x11 instead? - //TODO check additional factor of 2 - for (unsigned int j=0; j<8; ++j) { - for (unsigned int k=0; k<8; ++k) { - dhessv[ihit][i](j,k) = dchisq.derivatives()[j].derivatives()[k]; - } - } - - } - - } - - - -// const MSScalar chisq = chisqms; - - // std::cout << "chisq.value()" << std::endl; - // std::cout << chisq.value() << std::endl; - // std::cout << "chisq.value().derivatives()" << std::endl; - // std::cout << chisq.value().derivatives() << std::endl; - // std::cout << "chisq.derivatives()[0].derivatives()" << std::endl; - // std::cout << chisq.derivatives()[0].derivatives() << std::endl; - - - // const MSVector dms = dx0 + H*dx - E*Hprop*F*dxprev - E*Hprop*dF*dbeta - dE*dxi; - - - - - // MSScalar chisq; - // - // if (ihit==0 || ihit == (nhits-1)) { - // //standard fit - // const MSVector dms = dx0 + H*dx - E*Hprop*F*dxprev - E*Hprop*dF*dbeta - dE*dxi; - // chisq = dms.transpose()*Qinv*dms; - // } - // else { - // //maximum likelihood fit - // const MSVector dxprop = Hprop*F*dxprev; - // const MSCovariance dQdxprop0 = dQs[0].cast(); - // const MSCovariance dQdxprop1 = dQs[1].cast(); - // const MSCovariance dQdxprop2 = dQs[2].cast(); - // const MSCovariance dQdxi = dQs[3].cast(); - // - // const MSCovariance dQ = dxprop[0]*dQdxprop0 + dxprop[1]*dQdxprop1 + dxprop[2]*dQdxprop2 + dxi*dQdxi; - // - // // const MSCovariance dQdxprop0 = dQs[0].cast(); - // // const MSCovariance d2Qdxprop02 = dQs[1].cast(); - // // // - // // const MSCovariance dQ = dxprop[0]*dQdxprop0 + 0.5*dxprop[0]*dxprop[0]*d2Qdxprop02; - // - // // const Matrix Qms = iQ.topLeftCorner<3,3>().cast() + dQ.topLeftCorner<3,3>(); - // // Qinv.topLeftCorner<3,3>() = Qms.inverse(); - // const Matrix Qms = iQ.block<2,2>(1,1).cast() + dQ.block<2,2>(1,1); - // Qinv.block<2,2>(1,1) = Qms.inverse(); - // - // const MSScalar logdetQ = Eigen::log(Qms.determinant()); - // - // const MSVector dms = dx0 + H*dx - E*dxprop - E*Hprop*dF*dbeta - dE*dxi; - // chisq = dms.transpose()*Qinv*dms; - // chisq = chisq + logdetQ; - // - // } - - // MSCovariance Q = iQ.cast(); - - // const MSVector dxprop = Hprop*F*dxprev; - // const MSCovariance dQdxprop0 = dQs[0].cast(); - // const MSCovariance dQdxprop1 = dQs[1].cast(); - // const MSCovariance dQdxprop2 = dQs[2].cast(); - // const MSCovariance dQdxi = dQs[3].cast(); - // // - // const MSCovariance dQ = dxprop[0]*dQdxprop0 + dxprop[1]*dQdxprop1 + dxprop[2]*dQdxprop2 + dxi*dQdxi; - - // const MSVector dxprop = Hprop*F*dxprev; - // const MSCovariance dQdxprop0 = dQs[0].cast(); - // const MSCovariance d2Qdxprop02 = dQs[1].cast(); - // - // const MSCovariance dQ = dxprop[0]*dQdxprop0 + 0.5*dxprop[0]*dxprop[0]*d2Qdxprop02; - - // MSCovariance Qinv = MSCovariance::Zero(); - // Qinv(3,3) = MSScalar(1./epsxy/epsxy); - // Qinv(4,4) = MSScalar(1./epsxy/epsxy); - // Qinv.block<2,2>(1,1) = iQ.block<2,2>(1,1).inverse().cast(); - // const MSScalar Qelos = MSScalar(iQ(0,0)) + dQ(0,0); - // Qinv(0,0) = 1./Qelos; - // // const Matrix Qms = iQ.topLeftCorner<3,3>().cast() + dQ.topLeftCorner<3,3>(); - // // Qinv.topLeftCorner<3,3>() = Qms.inverse(); - // // const MSScalar logdetQ = Eigen::log(Qms.determinant()); - // const MSScalar logdetQ = Eigen::log(Qelos); - // // - // const MSVector dms = dx0 + H*dx - E*dxprop - E*Hprop*dF*dbeta - dE*dxi; - // MSScalar chisq = dms.transpose()*Qinv*dms; - // chisq = chisq + logdetQ; - - // const MSCovariance Qinvmod = Qinv - Qinv*dQ*Qinv; - // const MSScalar dlogdetQ = Eigen::log(1. + (Qinv*dQ).trace()); - // - // const MSVector dms = dx0 + H*dx - E*dxprop - E*Hprop*dF*dbeta - dE*dxi; - // MSScalar chisq = dms.transpose()*Qinvmod*dms; - // chisq = chisq + dlogdetQ; - - - auto const& gradlocal = chisq.value().derivatives(); - //fill local hessian - Matrix hesslocal; - for (unsigned int j=0; j(fullstateidx) += gradlocal.head(); - gradfull.segment(fullparmidx) += gradlocal.segment(localparmidx); - - //fill global hessian (upper triangular blocks only) - hessfull.block(fullstateidx, fullstateidx) += hesslocal.topLeftCorner(); - hessfull.block(fullstateidx, fullparmidx) += hesslocal.topRightCorner(); - hessfull.block(fullparmidx, fullparmidx) += hesslocal.bottomRightCorner(); - -// const unsigned int bfieldglobalidx = detidparms.at(std::make_pair(6,preciseHit->detUnitId())); - const unsigned int bfieldglobalidx = 0; - globalidxv[parmidx] = bfieldglobalidx; - parmidx++; - -// const unsigned int elossglobalidx = detidparms.at(std::make_pair(7,preciseHit->detUnitId())); - const unsigned int elossglobalidx = 0; - globalidxv[parmidx] = elossglobalidx; - parmidx++; - } - - //backwards propagation jacobian (local to local) to be used at the next layer - FdFm = curv2curvTransportJacobian(*updtsos.freeState(), propresult, true); - - } - else { -// const unsigned int bfieldglobalidx = detidparms.at(std::make_pair(6,preciseHit->detUnitId())); - const unsigned int bfieldglobalidx = 0; - globalidxv[parmidx] = bfieldglobalidx; - parmidx++; - } - - if (true) { - - auto fillAlignGrads = [&](auto Nalign) { - constexpr unsigned int nlocalstate = 2; - constexpr unsigned int localstateidx = 0; - constexpr unsigned int localalignmentidx = nlocalstate; - constexpr unsigned int localparmidx = localalignmentidx; - - // abusing implicit template argument to pass - // a template value via std::integral_constant - constexpr unsigned int nlocalalignment = Nalign(); - constexpr unsigned int nlocalparms = nlocalalignment; - constexpr unsigned int nlocal = nlocalstate + nlocalparms; - - using AlignScalar = AANT; - - //FIXME dirty hack to abuse state idx for reference point magnetic field - const unsigned int fullstateidx = genconstraint ? nstateparms : 3*(ihit+1); - // const unsigned int fullstateidx = 3*ihit; - const unsigned int fullparmidx = nstateparms + nparsBfield + nparsEloss + alignmentparmidx; - - const bool ispixel = GeomDetEnumerators::isTrackerPixel(det->subDetector()); - - //TODO add hit validation stuff - //TODO add simhit stuff - -// const PSimHit *matchedsim = nullptr; -// for (auto const& simhith : simHits) { -// for (const PSimHit& simHit : *simhith) { -// if (std::abs(simHit.particleType()) == 13 && simHit.detUnitId() == preciseHit->geographicalId()) { -// matchedsim = &simHit; -// break; -// } -// } -// if (matchedsim) { -// break; -// } -// } - - - Matrix Hu = Hp.bottomRightCorner<2,2>().cast(); - - Matrix dy0; - Matrix Vinv; - // rotation from module to strip coordinates -// Matrix R; - Matrix2d R; - if (!ispixel) { -// dy0[0] = AlignScalar(matchedsim->localPosition().x() - updtsos.localPosition().x()); - dy0[0] = AlignScalar(preciseHit->localPosition().x() - updtsos.localPosition().x()); - dy0[1] = AlignScalar(0.); - -// bool simvalid = false; -// for (auto const& simhith : simHits) { -// for (const PSimHit& simHit : *simhith) { -// if (simHit.detUnitId() == preciseHit->geographicalId()) { -// -// dy0[0] = AlignScalar(simHit.localPosition().x() - updtsos.localPosition().x()); -// -// simvalid = true; -// break; -// } -// } -// if (simvalid) { -// break; -// } -// } - - Vinv = Matrix::Zero(); - Vinv(0,0) = 1./std::pow(30e-4, 2); - -// R = Matrix::Identity(); - R = Matrix2d::Identity(); - } - else { - // 2d hit - Matrix2d iV; - iV << std::pow(30e-4, 2), 0., - 0., std::pow(30e-4, 2); - if (true) { -// if (ispixel) { - //take 2d hit as-is for pixels -// dy0[0] = AlignScalar(matchedsim->localPosition().x() - updtsos.localPosition().x()); -// dy0[1] = AlignScalar(matchedsim->localPosition().y() - updtsos.localPosition().y()); - dy0[0] = AlignScalar(preciseHit->localPosition().x() - updtsos.localPosition().x()); - dy0[1] = AlignScalar(preciseHit->localPosition().y() - updtsos.localPosition().y()); - - Vinv = iV.inverse().cast(); - //FIXME various temporary hacks; - -// dy0[1] = AlignScalar(0.); -// Vinv = Matrix::Zero(); -// Vinv(0,0) = 1./preciseHit->localPositionError().xx(); - -// if (GeomDetEnumerators::isEndcap(preciseHit->det()->subDetector())) { -// if (GeomDetEnumerators::isBarrel(preciseHit->det()->subDetector())) { -// PXBDetId detidtest(preciseHit->det()->geographicalId()); -// int layertest = detidtest.layer(); -// -// if (layertest > 1) { -// Vinv = Matrix::Zero(); -// } -// -// // Vinv = Matrix::Zero(); -// // dy0[0] = AlignScalar(0.); -// // dy0[1] = AlignScalar(0.); -// } - -// bool simvalid = false; -// for (auto const& simhith : simHits) { -// for (const PSimHit& simHit : *simhith) { -// if (simHit.detUnitId() == preciseHit->geographicalId()) { -// -// if (GeomDetEnumerators::isBarrel(preciseHit->det()->subDetector())) { -// dy0[0] = AlignScalar(simHit.localPosition().x() - updtsos.localPosition().x()); -// dy0[1] = AlignScalar(simHit.localPosition().y() - updtsos.localPosition().y()); -// } -// -// // dy0[1] = AlignScalar(0.); -// -// simvalid = true; -// break; -// } -// } -// if (simvalid) { -// break; -// } -// } - - -// R = Matrix::Identity(); - R = Matrix2d::Identity(); - } - else { - // diagonalize and take only smallest eigenvalue for 2d hits in strip wedge modules, - // since the constraint parallel to the strip is spurious - SelfAdjointEigenSolver eigensolver(iV); -// const Matrix2d& v = eigensolver.eigenvectors(); - R = eigensolver.eigenvectors().transpose(); - if (R(0,0) < 0.) { - R.row(0) *= -1.; - } - if (R(1,1) <0.) { - R.row(1) *= -1.; - } - - Matrix dy0local; -// dy0local[0] = matchedsim->localPosition().x() - updtsos.localPosition().x(); -// dy0local[1] = matchedsim->localPosition().y() - updtsos.localPosition().y(); - dy0local[0] = preciseHit->localPosition().x() - updtsos.localPosition().x(); - dy0local[1] = preciseHit->localPosition().y() - updtsos.localPosition().y(); - -// bool simvalid = false; -// for (auto const& simhith : simHits) { -// for (const PSimHit& simHit : *simhith) { -// if (simHit.detUnitId() == preciseHit->geographicalId()) { -// -// dy0local[0] = simHit.localPosition().x() - updtsos.localPosition().x(); -// dy0local[1] = simHit.localPosition().y() - updtsos.localPosition().y(); -// -// simvalid = true; -// break; -// } -// } -// if (simvalid) { -// break; -// } -// } - - const Matrix dy0eig = R*dy0local; - - //TODO deal properly with rotations (rotate back to module local coords?) - dy0[0] = AlignScalar(dy0eig[0]); - dy0[1] = AlignScalar(0.); - - Vinv = Matrix::Zero(); - Vinv(0,0) = AlignScalar(1./eigensolver.eigenvalues()[0]); - -// R = v.transpose().cast(); - - } - } - - rxfull.row(ivalidhit) = R.row(0).cast(); - ryfull.row(ivalidhit) = R.row(1).cast(); - - validdxeigjac.block<2,2>(2*ivalidhit, 3*(ihit+1)) = R*Hp.bottomRightCorner<2,2>(); - - const Matrix Ralign = R.cast(); - - Matrix dx = Matrix::Zero(); - AlignScalar dbeta(0.); - if (!genconstraint) { - for (unsigned int j=0; j()*dbeta; - } - - Matrix dalpha = Matrix::Zero(); - // order in which to use parameters, especially relevant in case nlocalalignment < 6 - constexpr std::array alphaidxs = {{5, 0, 1, 2, 3, 4}}; - for (unsigned int idim=0; idim A = Matrix::Zero(); - - - // dx/dx - A(0,0) = AlignScalar(1.); - // dy/dy - A(1,1) = AlignScalar(1.); - // dx/dz - A(0,2) = updtsos.localParameters().dxdz(); - // dy/dz - A(1,2) = updtsos.localParameters().dydz(); - // dx/dtheta_x - A(0,3) = -updtsos.localPosition().y()*updtsos.localParameters().dxdz(); - // dy/dtheta_x - A(1,3) = -updtsos.localPosition().y()*updtsos.localParameters().dydz(); - // dx/dtheta_y - A(0,4) = -updtsos.localPosition().x()*updtsos.localParameters().dxdz(); - // dy/dtheta_y - A(1,4) = -updtsos.localPosition().x()*updtsos.localParameters().dydz(); - // dx/dtheta_z - A(0,5) = -updtsos.localPosition().y(); - // dy/dtheta_z - A(1,5) = updtsos.localPosition().x(); - - -// std::cout << "strip local z shift gradient: " << (Ralign*A.col(2))[0].value().value() << std::endl; - - // rotation from alignment basis to module local coordinates -// Matrix A; -// if (isglued) { -// const GlobalVector modx = preciseHit->det()->surface().toGlobal(LocalVector(1.,0.,0.)); -// const GlobalVector mody = preciseHit->det()->surface().toGlobal(LocalVector(0.,1.,0.)); -// -// const GlobalVector gluedx = parmDet->surface().toGlobal(LocalVector(1.,0.,0.)); -// const GlobalVector gluedy = parmDet->surface().toGlobal(LocalVector(0.,1.,0.)); -// -// A(0,0) = AlignScalar(modx.dot(gluedx)); -// A(0,1) = AlignScalar(modx.dot(gluedy)); -// A(1,0) = AlignScalar(mody.dot(gluedx)); -// A(1,1) = AlignScalar(mody.dot(gluedy)); -// } -// else { -// A = Matrix::Identity(); -// } -// -// Matrix dh = dy0 - R*Hu*dx - R*A*dalpha; - - - double thetaincidence = std::asin(1./std::sqrt(std::pow(updtsos.localParameters().dxdz(),2) + std::pow(updtsos.localParameters().dydz(),2) + 1.)); - -// bool morehitquality = applyHitQuality_ ? thetaincidence > 0.25 : true; - bool morehitquality = true; - - if (morehitquality) { - nValidHitsFinal++; - if (ispixel) { - nValidPixelHitsFinal++; - } - } - else { - Vinv = Matrix::Zero(); - } - - Matrix dh = dy0 - Ralign*Hu*dx - Ralign*A*dalpha; - AlignScalar chisq = dh.transpose()*Vinv*dh; - - auto const& gradlocal = chisq.value().derivatives(); - //fill local hessian - Matrix hesslocal; - for (unsigned int j=0; j gradloctest0; -// Matrix gradloctest1; -// Matrix gradloctest2; - -// std::cout << "nlocalalignment: " << nlocalalignment << " nlocal: " << nlocal << std::endl; -// std::cout << "gradlocal type: " << typeid(gradlocal).name() << std::endl; -// std::cout << "gradloctest0 type: " << typeid(gradloctest0).name() << std::endl; -// std::cout << "gradloctest1 type: " << typeid(gradloctest1).name() << std::endl; -// std::cout << "gradloctest2 type: " << typeid(gradloctest2).name() << std::endl; -// -// std::cout << "nhits: " << nhits << " nvalid: " << nvalid << " nvalidalign2d: " << nvalidalign2d << " ihit: " << ihit << std::endl; -// std::cout << "gradfull.size(): " << gradfull.size() << " nlocalstate: " << nlocalstate << " fullstateidx: " << fullstateidx << " nlocalparms: " << nlocalparms << " fullparmidx: " << fullparmidx << std::endl; - - // FIXME the templated block functions don't work here for some reason - //fill global gradient - gradfull.segment(fullstateidx) += gradlocal.head(nlocalstate); - gradfull.segment(fullparmidx) += gradlocal.segment(localparmidx, nlocalparms); - - //fill global hessian (upper triangular blocks only) - hessfull.block(fullstateidx, fullstateidx) += hesslocal.topLeftCorner(nlocalstate,nlocalstate); - hessfull.block(fullstateidx, fullparmidx) += hesslocal.topRightCorner(nlocalstate, nlocalparms); - hessfull.block(fullparmidx, fullparmidx) += hesslocal.bottomRightCorner(nlocalparms, nlocalparms); - - for (unsigned int idim=0; idimdetUnitId())); - globalidxv[nparsBfield + nparsEloss + alignmentparmidx] = xglobalidx; - alignmentparmidx++; - if (alphaidxs[idim]==0) { - hitidxv.push_back(xglobalidx); - } - } - - // fill hit validation information - Vector2d dyrecgenlocal; - dyrecgenlocal << dy0[0].value().value(), dy0[1].value().value(); - const Vector2d dyrecgeneig = R*dyrecgenlocal; - dxrecgen.push_back(dyrecgeneig[0]); - dyrecgen.push_back(dyrecgeneig[1]); - - dxerr.push_back(1./std::sqrt(Vinv(0,0).value().value())); - dyerr.push_back(1./std::sqrt(Vinv(1,1).value().value())); - - localqop.push_back(updtsos.localParameters().qbp()); - localdxdz.push_back(updtsos.localParameters().dxdz()); - localdydz.push_back(updtsos.localParameters().dydz()); - localx.push_back(updtsos.localPosition().x()); - localy.push_back(updtsos.localPosition().y()); - - }; - - if (align2d) { - fillAlignGrads(std::integral_constant()); - } - else { - fillAlignGrads(std::integral_constant()); - } -// fillAlignGrads(std::integral_constant()); - - ivalidhit++; - - } - -// std::cout << "hit " << ihit << " isvalid " << preciseHit->isValid() << std::endl; -// std::cout << "global position: " << updtsos.globalParameters().position() << std::endl; - //hit information - //FIXME consolidate this special cases into templated function(s) -// if (preciseHit->isValid()) { - } - - if (!valid) { - break; - } - - assert(parmidx == (nparsBfield + nparsEloss)); - assert(alignmentparmidx == nparsAlignment); - - //fake constraint on reference point parameters - if (dogen) { -// if (false) { - for (unsigned int i=0; i<5; ++i) { - gradfull[i] = 0.; - hessfull.row(i) *= 0.; - hessfull.col(i) *= 0.; - hessfull(i,i) = 1e6; - } -// //b field from reference point not consistently used in this case -// gradfull[nstateparms] = 0.; -// hessfull.row(nstateparms) *= 0.; -// hessfull.col(nstateparms) *= 0.; - } - - std::cout << "now do the expensive calculations and fill outputs" << std::endl; - - //now do the expensive calculations and fill outputs - auto const& dchisqdx = gradfull.head(nstateparms); - auto const& dchisqdparms = gradfull.tail(npars); - - auto const& d2chisqdx2 = hessfull.topLeftCorner(nstateparms, nstateparms); - auto const& d2chisqdxdparms = hessfull.topRightCorner(nstateparms, npars); - auto const& d2chisqdparms2 = hessfull.bottomRightCorner(npars, npars); - -// std::cout << "dchisqdx" << std::endl; -// std::cout << dchisqdx << std::endl; -// std::cout << "d2chisqdx2 diagonal" << std::endl; -// std::cout << d2chisqdx2.diagonal() << std::endl; -// std::cout << "d2chisqdx2" << std::endl; -// std::cout << d2chisqdx2 << std::endl; -// -// auto const& eigenvalues = d2chisqdx2.eigenvalues(); -// std::cout << "d2chisqdx2 eigenvalues" << std::endl; -// std::cout << eigenvalues << std::endl; - -// auto const& Cinvd = d2chisqdx2.ldlt(); - Cinvd.compute(d2chisqdx2); - - - if (islikelihood) { - const MatrixXd Cfull = Cinvd.solve(MatrixXd::Identity(nstateparms,nstateparms)); - - // add ln det terms to gradient and hessian - // MatrixXd dhessfulli; - // MatrixXd dhessfullj; - // VectorXd dgradfull; - // TODO should this cover correction parameter part of the matrix as well? - for (unsigned int ihit=0; ihit<(nhits-1); ++ihit) { - constexpr unsigned int localstateidx = 0; - const unsigned int fullstateidx = 3*ihit; - - auto const& Cblock = Cfull.block<8,8>(fullstateidx, fullstateidx); - - // dhessfulli = MatrixXd::Zero(nstateparms, nstateparms); - // dhessfullj = MatrixXd::Zero(nstateparms, nstateparms); - - //TODO fill correction parameter block as well - for (unsigned int i=0; i<8; ++i) { - gradfull[fullstateidx + i] += (Cblock*dhessv[ihit][i]).trace(); - for (unsigned int j=0; j<8; ++j) { - hessfull(fullstateidx + i, fullstateidx + j) += (-Cblock*dhessv[ihit][j]*Cblock*dhessv[ihit][i]).trace(); - } - } - - } - - Cinvd.compute(d2chisqdx2); - - } - - dxfull = -Cinvd.solve(dchisqdx); - - dxstate = statejac.leftCols(nstateparms)*dxfull; - -// const Vector5d dxRef = dx.head<5>(); -// // const Vector5d dxRef = -Cinvd.solve(dchisqdx).head<5>(); -// const Matrix5d Cinner = Cinvd.solve(MatrixXd::Identity(nstateparms,nstateparms)).topLeftCorner<5,5>(); - -// dxdparms = -Cinvd.solve(d2chisqdxdparms).transpose(); -// -// grad = dchisqdparms + dxdparms*dchisqdx; -// hess = d2chisqdparms2 + 2.*dxdparms*d2chisqdxdparms + dxdparms*d2chisqdx2*dxdparms.transpose(); -// -// std::cout << "dxfull" << std::endl; -// std::cout << dxfull << std::endl; -// std::cout << "errsq" << std::endl; -// std::cout << Cinvd.solve(MatrixXd::Identity(nstateparms,nstateparms)).diagonal() << std::endl; - - const Vector5d dxRef = dxstate.head<5>(); - const Matrix5d Cinner = (statejac.leftCols(nstateparms)*Cinvd.solve(MatrixXd::Identity(nstateparms,nstateparms))*statejac.leftCols(nstateparms).transpose()).topLeftCorner<5,5>(); - -// const Matrix5d Cinner = Cinvd.solve(MatrixXd::Identity(nstateparms,nstateparms)).topLeftCorner<5,5>(); - - if (debugprintout_) { - std::cout<< "dxRef" << std::endl; - std::cout<< dxRef << std::endl; - } - - //fill output with corrected state and covariance at reference point - refParms.fill(0.); - refCov.fill(0.); -// const AlgebraicVector5& refVec = track.parameters(); - CurvilinearTrajectoryParameters curvparms(refFts.position(), refFts.momentum(), refFts.charge()); - const AlgebraicVector5& refVec = curvparms.vector(); - Map(refParms.data()) = (Map(refVec.Array()) + dxRef).cast(); - Map >(refCov.data()).triangularView() = (2.*Cinner).cast().triangularView(); - - if (iiter==0) { - refParms_iter0 = refParms; - refCov_iter0 = refCov; - } -// else if (iiter==2) { -// refParms_iter2 = refParms; -// refCov_iter2 = refCov; -// } - -// std::cout << "refParms" << std::endl; -// std::cout << Map(refParms.data()) << std::endl; - -// // gradv.clear(); -// jacrefv.clear(); -// -// // gradv.resize(npars,0.); -// jacrefv.resize(5*npars, 0.); -// -// nJacRef = 5*npars; -// // tree->SetBranchAddress("gradv", gradv.data()); -// tree->SetBranchAddress("jacrefv", jacrefv.data()); -// -// //eigen representation of the underlying vector storage -// // Map gradout(gradv.data(), npars); -// Map > jacrefout(jacrefv.data(), 5, npars); -// -// jacrefout = dxdparms.leftCols<5>().transpose().cast(); - - } - - if (!valid) { - return; - } - -// if (!nValidPixelHitsFinal) { -// continue; -// } - - std::cout << "now do the expensive calculations and fill outputs (again)" << std::endl; - - auto const& dchisqdx = gradfull.head(nstateparms); - auto const& dchisqdparms = gradfull.tail(npars); - - auto const& d2chisqdx2 = hessfull.topLeftCorner(nstateparms, nstateparms); - auto const& d2chisqdxdparms = hessfull.topRightCorner(nstateparms, npars); - auto const& d2chisqdparms2 = hessfull.bottomRightCorner(npars, npars); - - dxdparms = -Cinvd.solve(d2chisqdxdparms).transpose(); - -// if (debugprintout_) { -// std::cout << "dxrefdparms" << std::endl; -// std::cout << dxdparms.leftCols<5>() << std::endl; -// } - - grad = dchisqdparms + dxdparms*dchisqdx; - //TODO check the simplification -// hess = d2chisqdparms2 + 2.*dxdparms*d2chisqdxdparms + dxdparms*d2chisqdx2*dxdparms.transpose(); - hess = d2chisqdparms2 + dxdparms*d2chisqdxdparms; - -// const Vector5d dxRef = dxfull.head<5>(); -// const Matrix5d Cinner = Cinvd.solve(MatrixXd::Identity(nstateparms,nstateparms)).topLeftCorner<5,5>(); - -// std::cout << "dchisqdparms.head<6>()" << std::endl; -// std::cout << dchisqdparms.head<6>() << std::endl; -// -// std::cout << "grad.head<6>()" << std::endl; -// std::cout << grad.head<6>() << std::endl; -// -// std::cout << "d2chisqdparms2.topLeftCorner<6, 6>():" << std::endl; -// std::cout << d2chisqdparms2.topLeftCorner<6, 6>() << std::endl; -// std::cout << "hess.topLeftCorner<6, 6>():" << std::endl; -// std::cout << hess.topLeftCorner<6, 6>() << std::endl; -// -// std::cout << "dchisqdparms.segment<6>(nparsBfield+nparsEloss)" << std::endl; -// std::cout << dchisqdparms.segment<6>(nparsBfield+nparsEloss) << std::endl; -// -// std::cout << "grad.segment<6>(nparsBfield+nparsEloss)" << std::endl; -// std::cout << grad.segment<6>(nparsBfield+nparsEloss) << std::endl; -// -// std::cout << "d2chisqdparms2.block<6, 6>(nparsBfield+nparsEloss, nparsBfield+nparsEloss):" << std::endl; -// std::cout << d2chisqdparms2.block<6, 6>(nparsBfield+nparsEloss, nparsBfield+nparsEloss) << std::endl; -// std::cout << "hess.block<6, 6>(nparsBfield+nparsEloss, nparsBfield+nparsEloss):" << std::endl; -// std::cout << hess.block<6, 6>(nparsBfield+nparsEloss, nparsBfield+nparsEloss) << std::endl; -// -// std::cout << "d2chisqdparms2.bottomRightCorner<6, 6>():" << std::endl; -// std::cout << d2chisqdparms2.bottomRightCorner<6, 6>() << std::endl; -// std::cout << "hess.bottomRightCorner<6, 6>():" << std::endl; -// std::cout << hess.bottomRightCorner<6, 6>() << std::endl; - - gradv.clear(); - jacrefv.clear(); - - gradv.resize(npars,0.); - jacrefv.resize(5*npars, 0.); - - nJacRef = 5*npars; - if (fillTrackTree_ && fillGrads_) { - tree->SetBranchAddress("gradv", gradv.data()); - } - if (fillTrackTree_) { - tree->SetBranchAddress("jacrefv", jacrefv.data()); - } - - //eigen representation of the underlying vector storage - Map gradout(gradv.data(), npars); - Map > jacrefout(jacrefv.data(), 5, npars); - -// jacrefout = dxdparms.leftCols<5>().transpose().cast(); - jacrefout = ( (dxdparms*statejac.leftCols(nstateparms).transpose()).leftCols<5>().transpose() + statejac.topRightCorner(5, npars) ).cast(); - - gradout = grad.cast(); - - - rx.resize(2*nvalid); - Map > rxout(rx.data(), nvalid, 2); - rxout = rxfull; -// std::cout << "rx:" << std::endl; -// for (auto elem : rx) { -// std::cout << elem << " "; -// } -// std::cout << std::endl; -// std::cout << rx << std::endl; - - ry.resize(2*nvalid); - Map > ryout(ry.data(), nvalid, 2); - ryout = ryfull; - - deigx.resize(nvalid); - deigy.resize(nvalid); - - validdxeig = validdxeigjac*dxfull; - - for (unsigned int ivalid = 0; ivalid < nvalid; ++ivalid) { - deigx[ivalid] = validdxeig[2*ivalid]; - deigy[ivalid] = validdxeig[2*ivalid + 1]; - } - -// unsigned int ivalid = 0; -// for (unsigned int ihit = 0; ihit < nhits; ++ihit) { -// auto const& hit = hits[ihit]; -// if (hit->isValid()) { -// // if (ihit < (nhits-1)) { -// // std::cout << "ihit = " << ihit << ", ivalid = " << ivalid << std::endl; -// // std::cout << "dxfull.segment<3>(3*(ihit+1)):" << std::endl; -// // std::cout << dxfull.segment<3>(3*(ihit+1)) << std::endl; -// // std::cout << "dxstate.segment<5>(5*(ihit+1))" << std::endl; -// // std::cout << dxstate.segment<5>(5*(ihit+1)) << std::endl; -// // } -// const unsigned int dxidx = 3*(ihit + 1); -// dlocalx[ivalid] = dxfull[dxidx]; -// dlocaly[ivalid] = dxfull[dxidx + 1]; -// ++ivalid; -// } -// } - - - float refPt = dogen ? genpart->pt() : std::abs(1./refParms[0])*std::sin(M_PI_2 - refParms[1]); - - gradmax = 0.; - for (unsigned int i=0; igradmax) { - gradmax = absval; - } - } - - - hessmax = 0.; - for (unsigned int i=0; ihessmax) { - hessmax = absval; - } - - } - - } - -// if (gradmax < 1e5 && refPt > 5.5) { -// //fill aggregrate gradient and hessian -// for (unsigned int i=0; ihessmax) { -// hessmax = absval; -// } -// -// const std::pair key = std::make_pair(std::min(iidx,jidx), std::max(iidx,jidx)); -// -// auto it = hessaggsparse.find(key); -// if (it==hessaggsparse.end()) { -// hessaggsparse[key] = hess(i,j); -// } -// else { -// it->second += hess(i,j); -// } -// } -// } -// } - - - if (debugprintout_) { - const Matrix5d Cinner = (statejac*Cinvd.solve(MatrixXd::Identity(nstateparms,nstateparms))*statejac.transpose()).topLeftCorner<5,5>(); - std::cout << "hess debug" << std::endl; - std::cout << "track parms" << std::endl; -// std::cout << tkparms << std::endl; - // std::cout << "dxRef" << std::endl; - // std::cout << dxRef << std::endl; -// std::cout << "original cov" << std::endl; -// std::cout << track.covariance() << std::endl; - std::cout << "recomputed cov" << std::endl; - std::cout << 2.*Cinner << std::endl; - } - -// std::cout << "dxinner/dparms" << std::endl; -// std::cout << dxdparms.bottomRows<5>() << std::endl; -// std::cout << "grad" << std::endl; -// std::cout << grad << std::endl; -// std::cout << "hess diagonal" << std::endl; -// std::cout << hess.diagonal() << std::endl; -// std::cout << "hess0 diagonal" << std::endl; -// std::cout << d2chisqdparms2.diagonal() << std::endl; -// std::cout << "hess1 diagonal" << std::endl; -// std::cout << 2.*(dxdparms.transpose()*d2chisqdxdparms).diagonal() << std::endl; -// std::cout << "hess2 diagonal" << std::endl; -// std::cout << (dxdparms.transpose()*d2chisqdx2*dxdparms).diagonal() << std::endl; - - //fill packed hessian and indices - const unsigned int nsym = npars*(1+npars)/2; - hesspackedv.clear(); - hesspackedv.resize(nsym, 0.); - - nSym = nsym; - if (fillTrackTree_ && fillGrads_) { - tree->SetBranchAddress("hesspackedv", hesspackedv.data()); - } - - Map hesspacked(hesspackedv.data(), nsym); - const Map globalidx(globalidxv.data(), npars); - - unsigned int packedidx = 0; - for (unsigned int ipar = 0; ipar < npars; ++ipar) { - const unsigned int segmentsize = npars - ipar; - hesspacked.segment(packedidx, segmentsize) = hess.block<1, Dynamic>(ipar, ipar, 1, segmentsize).cast(); - packedidx += segmentsize; - } - - if (fillTrackTree_) { - tree->Fill(); - } - } -} - -DEFINE_FWK_MODULE(ResidualGlobalCorrectionMakerSim); diff --git a/Analysis/HitAnalyzer/plugins/ResidualGlobalCorrectionMakerSimG4e.cc b/Analysis/HitAnalyzer/plugins/ResidualGlobalCorrectionMakerSimG4e.cc deleted file mode 100644 index f45b3cfbd3efa..0000000000000 --- a/Analysis/HitAnalyzer/plugins/ResidualGlobalCorrectionMakerSimG4e.cc +++ /dev/null @@ -1,2459 +0,0 @@ -#include "ResidualGlobalCorrectionMakerBase.h" -#include "TrackPropagation/Geant4e/interface/Geant4ePropagator.h" - -class ResidualGlobalCorrectionMakerSimG4e : public ResidualGlobalCorrectionMakerBase -{ -public: - explicit ResidualGlobalCorrectionMakerSimG4e(const edm::ParameterSet &); - ~ResidualGlobalCorrectionMakerSimG4e() {} - -// static void fillDescriptions(edm::ConfigurationDescriptions &descriptions); - -private: - - virtual void beginStream(edm::StreamID) override; - virtual void produce(edm::Event &, const edm::EventSetup &) override; - - -}; - - -ResidualGlobalCorrectionMakerSimG4e::ResidualGlobalCorrectionMakerSimG4e(const edm::ParameterSet &iConfig) : ResidualGlobalCorrectionMakerBase(iConfig) -{ - -} - -void ResidualGlobalCorrectionMakerSimG4e::beginStream(edm::StreamID streamid) -{ - ResidualGlobalCorrectionMakerBase::beginStream(streamid); - - if (fillTrackTree_) { - const int basketSize = 4*1024*1024; - - tree->Branch("trackPt", &trackPt, basketSize); - tree->Branch("trackPtErr", &trackPtErr, basketSize); - tree->Branch("trackEta", &trackEta, basketSize); - tree->Branch("trackPhi", &trackPhi, basketSize); - tree->Branch("trackCharge", &trackCharge, basketSize); - //workaround for older ROOT version inability to store std::array automatically - // tree->Branch("trackOrigParms", trackOrigParms.data(), "trackOrigParms[5]/F", basketSize); - // tree->Branch("trackOrigCov", trackOrigCov.data(), "trackOrigCov[25]/F", basketSize); - tree->Branch("trackParms", trackParms.data(), "trackParms[5]/F", basketSize); - tree->Branch("trackCov", trackCov.data(), "trackCov[25]/F", basketSize); - - tree->Branch("refParms_iter0", refParms_iter0.data(), "refParms_iter0[5]/F", basketSize); - tree->Branch("refCov_iter0", refCov_iter0.data(), "refCov_iter0[25]/F", basketSize); - // tree->Branch("refParms_iter2", refParms_iter2.data(), "refParms_iter2[5]/F", basketSize); - // tree->Branch("refCov_iter2", refCov_iter2.data(), "refCov_iter2[25]/F", basketSize); - - tree->Branch("refParms", refParms.data(), "refParms[5]/F", basketSize); - tree->Branch("refCov", refCov.data(), "refCov[25]/F", basketSize); - tree->Branch("genParms", genParms.data(), "genParms[5]/F", basketSize); - - tree->Branch("genPt", &genPt, basketSize); - tree->Branch("genEta", &genEta, basketSize); - tree->Branch("genPhi", &genPhi, basketSize); - tree->Branch("genCharge", &genCharge, basketSize); - - tree->Branch("genX", &genX, basketSize); - tree->Branch("genY", &genY, basketSize); - tree->Branch("genZ", &genZ, basketSize); - - tree->Branch("normalizedChi2", &normalizedChi2, basketSize); - - tree->Branch("nHits", &nHits, basketSize); - tree->Branch("nValidHits", &nValidHits, basketSize); - tree->Branch("nValidPixelHits", &nValidPixelHits, basketSize); - tree->Branch("nJacRef", &nJacRef, basketSize); - - tree->Branch("nValidHitsFinal", &nValidHitsFinal); - tree->Branch("nValidPixelHitsFinal", &nValidPixelHitsFinal); - - tree->Branch("jacrefv",jacrefv.data(),"jacrefv[nJacRef]/F", basketSize); - - - -// tree->Branch("dxpxb1", &dxpxb1); -// tree->Branch("dypxb1", &dypxb1); -// -// tree->Branch("dxttec9rphi", &dxttec9rphi); -// tree->Branch("dxttec9stereo", &dxttec9stereo); -// -// tree->Branch("dxttec4rphi", &dxttec4rphi); -// tree->Branch("dxttec4stereo", &dxttec4stereo); -// -// tree->Branch("dxttec4rphisimgen", &dxttec4rphisimgen); -// tree->Branch("dyttec4rphisimgen", &dyttec4rphisimgen); -// tree->Branch("dxttec4rphirecsim", &dxttec4rphirecsim); -// -// tree->Branch("dxttec9rphisimgen", &dxttec9rphisimgen); -// tree->Branch("dyttec9rphisimgen", &dyttec9rphisimgen); -// -// tree->Branch("simlocalxref", &simlocalxref); -// tree->Branch("simlocalyref", &simlocalyref); - - tree->Branch("hitidxv", &hitidxv); - tree->Branch("dxrecgen", &dxrecgen); - tree->Branch("dyrecgen", &dyrecgen); - tree->Branch("dxsimgen", &dxsimgen); - tree->Branch("dysimgen", &dysimgen); - tree->Branch("dxsimgenlocal", &dxsimgenlocal); - tree->Branch("dysimgenlocal", &dysimgenlocal); - tree->Branch("dxrecsim", &dxrecsim); - tree->Branch("dyrecsim", &dyrecsim); - tree->Branch("dxerr", &dxerr); - tree->Branch("dyerr", &dyerr); - - tree->Branch("clusterSize", &clusterSize); - tree->Branch("clusterSizeX", &clusterSizeX); - tree->Branch("clusterSizeY", &clusterSizeY); - tree->Branch("clusterCharge", &clusterCharge); - tree->Branch("clusterChargeBin", &clusterChargeBin); - tree->Branch("clusterOnEdge", &clusterOnEdge); - - tree->Branch("clusterProbXY", &clusterProbXY); - tree->Branch("clusterSN", &clusterSN); - - tree->Branch("dxreccluster", &dxreccluster); - tree->Branch("dyreccluster", &dyreccluster); - - tree->Branch("localqop", &localqop); - tree->Branch("localdxdz", &localdxdz); - tree->Branch("localdydz", &localdydz); - tree->Branch("localx", &localx); - tree->Branch("localy", &localy); - - tree->Branch("simtestz", &simtestz); - tree->Branch("simtestvz", &simtestvz); - tree->Branch("simtestrho", &simtestrho); - tree->Branch("simtestzlocalref", &simtestzlocalref); - tree->Branch("simtestdx", &simtestdx); - tree->Branch("simtestdxrec", &simtestdxrec); - tree->Branch("simtestdy", &simtestdy); - tree->Branch("simtestdyrec", &simtestdyrec); - tree->Branch("simtestdxprop", &simtestdxprop); - tree->Branch("simtestdyprop", &simtestdyprop); - tree->Branch("simtestdetid", &simtestdetid); - - tree->Branch("rx", &rx); - tree->Branch("ry", &ry); - - tree->Branch("deigx", &deigx); - tree->Branch("deigy", &deigy); - - nJacRef = 0.; - } -} - - -// ------------ method called for each event ------------ -void ResidualGlobalCorrectionMakerSimG4e::produce(edm::Event &iEvent, const edm::EventSetup &iSetup) -{ - - const bool dogen = fitFromGenParms_; - - using namespace edm; - - Handle trackOrigH; - iEvent.getByToken(inputTrackOrig_, trackOrigH); - - -// bool foundmodule = false; -// for (const reco::Track &track : *trackOrigH) { -// if (track.innerDetId() == 302055944) { -// foundmodule = true; -// break; -// } -// // for (auto it = track.recHitsBegin(); it != track.recHitsEnd(); ++it) { -// // if ((*it)->geographicalId().rawId() == 302055944) { -// // foundmodule = true; -// // break; -// // } -// // } -// // if (foundmodule) { -// // break; -// // } -// } -// if (!foundmodule) { -// // printf("not found, returning\n"); -// return; -// } - - - - // loop over gen particles - - edm::ESHandle globalGeometry; - iSetup.get().get(globalGeometry); - -// edm::ESHandle globalGeometry; -// iSetup.get().get("idealForDigi", globalGeometry); - - edm::ESHandle trackerTopology; - iSetup.get().get(trackerTopology); - -// ESHandle magfield; -// iSetup.get().get(magfield); -// auto field = magfield.product(); - - edm::ESHandle ttrh; - iSetup.get().get("WithAngleAndTemplate",ttrh); - -// ESHandle thePropagator; -// iSetup.get().get("RungeKuttaTrackerPropagator", thePropagator); -// iSetup.get().get("PropagatorWithMaterial", thePropagator); -// iSetup.get().get("PropagatorWithMaterialParabolicMf", thePropagator); -// iSetup.get().get("Geant4ePropagator", thePropagator); -// const MagneticField* field = thePropagator->magneticField(); - -// ESHandle theAnalyticPropagator; -// iSetup.get().get("PropagatorWithMaterial", theAnalyticPropagator); - -// Handle trackH; -// Handle trackH; -// iEvent.getByToken(inputTrack_, trackH); - - - -// Handle > indicesH; -// iEvent.getByToken(inputIndices_, indicesH); - -// Handle > trajH; -// iEvent.getByToken(inputTraj_, trajH); - - ESHandle thePropagator; -// iSetup.get().get("RungeKuttaTrackerPropagator", thePropagator); -// iSetup.get().get("PropagatorWithMaterial", thePropagator); -// iSetup.get().get("PropagatorWithMaterialParabolicMf", thePropagator); - iSetup.get().get("Geant4ePropagator", thePropagator); - - const MagneticField* field = thePropagator->magneticField(); - - - const Geant4ePropagator *g4prop = dynamic_cast(thePropagator.product()); - - - - - Handle bsH; - iEvent.getByToken(inputBs_, bsH); - - - Handle> genPartCollection; - if (doGen_) { - iEvent.getByToken(GenParticlesToken_, genPartCollection); - } - -// Handle> tecSimHits; - std::vector>> simHits(inputSimHits_.size()); - if (doSim_) { - for (unsigned int isimhit = 0; isimhit fPropagator(static_cast(thePropagator->clone())); -// fPropagator->setPropagationDirection(alongMomentum); -// -// std::unique_ptr fAnalyticPropagator(static_cast(theAnalyticPropagator->clone())); -// fAnalyticPropagator->setPropagationDirection(alongMomentum); - - KFUpdator updator; - TkClonerImpl const& cloner = static_cast(ttrh.product())->cloner(); - - -// siStripClusterInfo_.initEvent(iSetup); - -// edm::ESHandle globalPositionRcd; -// iSetup.get().get(globalPositionRcd); -// -// printf("globalPositionRcd translation = %e, %e, %e\n", globalPositionRcd->m_align.front().translation().x(), -// globalPositionRcd->m_align.front().translation().y(), -// globalPositionRcd->m_align.front().translation().z()); -// std::cout << "globalPositionRcd rotation" << globalPositionRcd->m_align.front().rotation() << std::endl; - - // set up cylindrical surface for beam pipe -// const double ABe = 9.0121831; -// const double ZBe = 4.; -// const double K = 0.307075*1e-3; -// const double dr = 0.08; -// // const double xibeampipe = 0.5*K*dr*ZBe/ABe; -// const double xibeampipe = 0.*0.5*K*dr*ZBe/ABe; - - - - -// auto beampipe = Cylinder::build(Surface::PositionType(0.,0.,0.), Surface::RotationType(), 2.94); -// beampipe->setMediumProperties(MediumProperties(0., xibeampipe)); - -// std::cout << "xi beampipe: " << xibeampipe << std::endl; - -// const GeomDet *testdet = nullptr; -// //debugging -// for (const GeomDet* det : globalGeometry->detUnits()) { -// if (!det) { -// continue; -// } -// -// if (det->subDetector() == GeomDetEnumerators::TEC) { -// const DetId& detid = det->geographicalId(); -// // TECDetId detid(det->geographicalId()); -// // layer = -1 * (detid.side() == 1) * detid.wheel() + (detid.side() == 2) * detid.wheel(); -// unsigned int side = trackerTopology->tecSide(detid); -// unsigned int wheel = trackerTopology->tecWheel(detid); -// int layer = -1 * (side == 1) * wheel + (side == 2) * wheel; -// bool stereo = trackerTopology->isStereo(det->geographicalId()); -// -// if (layer == -9) { -// testdet = det; -// break; -// -// } -// } -// -// -// -// } -// -// if (testdet) { -// const GlobalPoint center = testdet->surface().toGlobal(LocalPoint(1.,0.)); -// -// const GlobalVector centerv(center.x(), center.y(), center.z()); -// const GlobalVector dir = centerv/centerv.mag(); -// const double sintheta = dir.perp(); -// const GlobalVector mom = (100000./sintheta)*dir; -// const GlobalPoint pos(0.,0.,0.); -// -// FreeTrajectoryState ftsplus(pos, mom, 1., field); -// FreeTrajectoryState ftsminus(pos, mom, -1., field); -// -// const TrajectoryStateOnSurface tsosplus = fPropagator->propagate(ftsplus, testdet->surface()); -// const TrajectoryStateOnSurface tsosminus = fPropagator->propagate(ftsminus, testdet->surface()); -// -// std::cout << "global target" << std::endl; -// std::cout << center << std::endl; -// -// std::cout << "momentum" << std::endl; -// std::cout << mom << std::endl; -// -// std::cout << "tsosplus local:" << std::endl; -// std::cout << tsosplus.localPosition() << std::endl; -// std::cout << "tsosminus local:" << std::endl; -// std::cout << tsosminus.localPosition() << std::endl; -// -// std::cout << "delta local" << std::endl; -// std::cout << tsosplus.localPosition() - tsosminus.localPosition() << std::endl; -// -// } - - - simtestz = -99.; - simtestvz = -99.; - simtestrho = -99.; - simtestzlocalref = -99.; - simtestdx = -99.; - simtestdxrec = -99.; - simtestdy = -99.; - simtestdyrec = -99.; - simtestdxprop = -99.; - simtestdyprop = -99.; - simtestdetid = 0; - - std::vector hits; - for (auto const& simhith : simHits) { - for (const PSimHit& simHit : *simhith) { - if (std::abs(simHit.particleType()) == 13) { -// if (std::abs(simHit.localPosition().z()) > 1e-9) { -// continue; -// } - hits.push_back(&simHit); - } - } - } - - auto simhitcompare = [&](const PSimHit *hit0, const PSimHit *hit1) { - return globalGeometry->idToDet(hit0->detUnitId())->surface().toGlobal(hit0->localPosition()).mag() < globalGeometry->idToDet(hit1->detUnitId())->surface().toGlobal(hit1->localPosition()).mag(); - }; - - std::sort(hits.begin(), hits.end(), simhitcompare); - - - -// TkClonerImpl hitCloner; -// TKCloner const* cloner = static_cast(builder)->cloner() -// TrajectoryStateCombiner combiner; - - run = iEvent.run(); - lumi = iEvent.luminosityBlock(); - event = iEvent.id().event(); - -// for (const reco::Track &track : *trackOrigH) { - if (true) { -// const Trajectory& traj = (*trajH)[itraj]; - -// const edm::Ref > trajref(trajH, j); -// const reco::Track& track = *(*trackH)[trajref]; -// const reco::Track& track = (*trackH)[itraj]; -// const reco::Track& trackOrig = (*trackOrigH)[(*indicesH)[j]]; - -// std::cout << "j " << j << " (*indicesH)[j] " << (*indicesH)[j] <(trackParms.data()) = Map(tkparms.Array()).cast(); -// Map >(trackCov.data()).triangularView() = Map >(tkcov.Array()).cast().triangularView(); -// -// // std::cout << "track charge: " << track.charge() << " trackorig charge " << trackOrig.charge() << "inner state charge " << tms.back().updatedState().charge() << std::endl; - - const reco::GenParticle* genpart = nullptr; - - genPt = -99.; - genEta = -99.; - genPhi = -99.; - genCharge = -99; - genX = -99.; - genY = -99.; - genZ = -99.; - genParms.fill(0.); - - if (doGen_) { - for (std::vector::const_iterator g = genPartCollection->begin(); g != genPartCollection->end(); ++g) - { - if (g->status() != 1) { - continue; - } - if (std::abs(g->pdgId()) != 13) { - continue; - } - -// float dR = deltaR(g->phi(), trackPhi, g->eta(), trackEta); - - if (true) - { - genpart = &(*g); - - genPt = g->pt(); - genEta = g->eta(); - genPhi = g->phi(); - genCharge = g->charge(); - - genX = g->vertex().x(); - genY = g->vertex().y(); - genZ = g->vertex().z(); - - auto const& vtx = g->vertex(); - auto const& myBeamSpot = bsH->position(vtx.z()); - - //q/|p| - genParms[0] = g->charge()/g->p(); - //lambda - genParms[1] = M_PI_2 - g->momentum().theta(); - //phi - genParms[2] = g->phi(); - //dxy - genParms[3] = (-(vtx.x() - myBeamSpot.x()) * g->py() + (vtx.y() - myBeamSpot.y()) * g->px()) / g->pt(); - //dsz - genParms[4] = (vtx.z() - myBeamSpot.z()) * g->pt() / g->p() - - ((vtx.x() - myBeamSpot.x()) * g->px() + (vtx.y() - myBeamSpot.y()) * g->py()) / g->pt() * g->pz() / g->p(); - } - else { - continue; - } - } - } - - -// printf("done building hits\n"); - -// const unsigned int nhits = track.recHitsSize(); - const unsigned int nhits = hits.size(); - nHits = nhits; -// unsigned int npixhits = 0; - - unsigned int nvalid = 0; - unsigned int nvalidpixel = 0; - unsigned int nvalidalign2d = 0; - - // count valid hits since this is needed to size the arrays - for (auto const& hit : hits) { -// assert(hit->dimension()<=2); - if (true) { - nvalid += 1; - -// const uint32_t gluedid = trackerTopology->glued(hit->geographicalId()); -// const bool isglued = gluedid != 0; -// const DetId parmdetid = isglued ? DetId(gluedid) : hit->geographicalId(); -// const bool align2d = detidparms.count(std::make_pair(1, parmdetid)); -// const bool align2d = detidparms.count(std::make_pair(2, hit->geographicalId())); - - const bool align2d = detidparms.count(std::make_pair(1, hit->detUnitId())); -// - if (align2d) { - nvalidalign2d += 1; - } - const GeomDet* det = globalGeometry->idToDet(hit->detUnitId()); - if (GeomDetEnumerators::isTrackerPixel(det->subDetector())) { - nvalidpixel += 1; - } - } - } - -// //count valid hits since this is needed to size the arrays -// auto const& hitsbegin = track.recHitsBegin(); -// for (unsigned int ihit = 0; ihit < track.recHitsSize(); ++ihit) { -// auto const& hit = *(hitsbegin + ihit); -// if (hit->isValid() && hit->dimension()<=2) { -// nvalid += 1; -// -// const GeomDet *detectorG = globalGeometry->idToDet(hit->geographicalId()); -// if (hit->dimension()==2 && GeomDetEnumerators::isTrackerPixel(detectorG->subDetector())) { -// // if (hit->dimension()==2) { -// nvalidpixel += 1; -// } -// } -// } - - nValidHits = nvalid; - nValidPixelHits = nvalidpixel; - - nValidHitsFinal = 0; - nValidPixelHitsFinal = 0; - -// const unsigned int nstriphits = nhits-npixhits; -// const unsigned int nparsAlignment = nstriphits + 2*npixhits; -// const unsigned int nvalidstrip = nvalid - nvalidpixel; -// const unsigned int nparsAlignment = nvalidstrip + 2*nvalidpixel; - const unsigned int nparsAlignment = 2*nvalid + nvalidalign2d; -// const unsigned int nparsAlignment = 6*nvalid; - const unsigned int nparsBfield = nhits; - const unsigned int nparsEloss = nhits - 1; - const unsigned int npars = nparsAlignment + nparsBfield + nparsEloss; - -// const unsigned int nstateparms = 5*(nhits+1); - const unsigned int nstateparms = 3*(nhits+1) - 1; -// const unsigned int nstateparms = 3*nhits - 1; - const unsigned int nparmsfull = nstateparms + npars; - - - const unsigned int nstateparmspost = 5*nhits; - -// std::cout << "nhits " << nhits << std::endl; -// std::cout << "nstateparms " << nstateparms << std::endl; -// std::cout << "nparmsfull " << nparmsfull << std::endl; -// std::cout << "nparmsfull " << nparmsfull << std::endl; -// std::cout << "nparmsfull " << nparmsfull << std::endl; -// std::cout << "nparmsfull " << nparmsfull << std::endl; -// std::cout << "nparmsfull " << nparmsfull << std::endl; - -// const unsigned int npropparms = 5*(nhits-1); -// const unsigned int nhitparms = 2*nhits; -// const unsigned int nmomparms = 3*(nhits-1); -// const unsigned int nposparms = 2*(nhits-1); -// constexpr unsigned int nrefparms = 5; - - - - //active double for autodiff gradients -// using Adouble = AutoDiffScalar; -// using AVectorXd = Matrix; -// //double double for autodiff hessians -// using AAdouble = AutoDiffScalar; - - - -// using AAXd = AANT; -// using AAdouble = AAXd; -// -// using AA2d = AANT; -// using AA3d = AANT; -// using AA4d = AANT; -// using AA12d = AANT; -// -// using ScalarConst = AANT; - -// using AConstd = AutoDiffScalar; -// using AConstd = AutoDiffScalar>; - - -// using VectorXAd = Matrix; -// using MatrixXAd = Matrix; - - //two position parameters and and one alignment parameter - using StripHitScalar = AANT;; - - using StripHit1DJacobian = Matrix; - - using StripHitVector = Matrix; - using StripHit2DCovariance = Matrix; - using StripHit2DJacobian = Matrix; - - - - //two hit dimensions and two alignment parameters - using PixelHit2DScalar = AANT; - using PixelHit2DVector = Matrix; - using PixelHit2DCovariance = Matrix; - using PixelHit2DJacobian = Matrix; - - - //2x5 state parameters, one bfield parameter, and one material parameter -// using MSScalar = AANT;; - using MSScalar = AANT; - using MSVector = Matrix; - using MSProjection = Matrix; - using MSJacobian = Matrix; - using MSCovariance = Matrix; - - using BSScalar = AANT; - -// using HitProjection = Matrix; -// using HitCovariance = Matrix; -// using HitVector = Matrix; - -// evector Vinv(nhits, HitCovarianceMatrix::Zero()); -// evector Hh(nhits, HitProjection::Zero()); -// evector dy0(nhits, HitVector::Zero()); -// evector dx(nhits, StateVector::Zero()); -// //initialize backpropagation indexing -// for (unsigned int i=0; i rxfull(nvalid, 2); - Matrix ryfull(nvalid, 2); - -// VectorXd gradfull = chisq.value().derivatives(); -// MatrixXd hessfull = MatrixXd::Zero(nparmsfull, nparmsfull); -// for (unsigned int i=0; iSetBranchAddress("globalidxv", globalidxv.data()); -// } - -// TrajectoryStateOnSurface currtsos; - - - - VectorXd dxfull; - MatrixXd dxdparms; - VectorXd grad; - MatrixXd hess; - LDLT Cinvd; - -// if (dogen && genpart==nullptr) { -// continue; -// } -// -// if (dogen && genpart->eta()>-2.3) { -// continue; -// } - -// if (genpart==nullptr) { -// continue; -// } -// if (genpart->pt()>10.) { -// continue; -// } -// if (genpart->pt()<100.) { -// continue; -// } -// if (genpart->eta()>-2.3) { -// continue; -// } - -// if (debugprintout_) { -// std::cout << "initial reference point parameters:" << std::endl; -// std::cout << track.parameters() << std::endl; -// } - -// //prepare hits -// TransientTrackingRecHit::RecHitContainer hits; -// hits.reserve(track.recHitsSize()); -// for (auto it = track.recHitsBegin(); it != track.recHitsEnd(); ++it) { -// const GeomDet *detectorG = globalGeometry->idToDet((*it)->geographicalId()); -// hits.push_back((*it)->cloneForFit(*detectorG)); -// } - -// // fix mixed up clusters? -// for (unsigned int ihit=0; ihit<(hits.size()-1); ++ihit) { -// TrackerSingleRecHit* hit = const_cast(dynamic_cast(hits[ihit].get())); -// TrackerSingleRecHit* nexthit = const_cast(dynamic_cast(hits[ihit+1].get())); -// -// // const TrackingRecHitSingle* nexthit = hits[ihit+1]; -// -// if (!hit || !nexthit) { -// continue; -// } -// -// const DetId partnerid = trackerTopology->partnerDetId(hit->geographicalId()); -// // -// if (partnerid == nexthit->geographicalId()) { -// // std::cout << "swapping clusters" << std::endl; -// const OmniClusterRef::ClusterStripRef cluster = hit->cluster_strip(); -// const OmniClusterRef::ClusterStripRef nextcluster = nexthit->cluster_strip(); -// -// hit->setClusterStripRef(nextcluster); -// nexthit->setClusterStripRef(cluster); -// } -// -// } - -// if (genpart==nullptr) { -// continue; -// } -// -// if (genpart->eta()<-2.4 || genpart->eta()>-2.3) { -// continue; -// } - - - FreeTrajectoryState refFts; - -// if (dogen) { - if (true) { -// if (true) { - -// if (genpart==nullptr) { -// continue; -// } - //init from gen state - auto const& refpoint = genpart->vertex(); - auto const& trackmom = genpart->momentum(); - const GlobalPoint refpos(refpoint.x(), refpoint.y(), refpoint.z()); - const GlobalVector refmom(trackmom.x(), trackmom.y(), trackmom.z()); - const GlobalTrajectoryParameters refglobal(refpos, refmom, genpart->charge(), field); - -// std::cout << "gen ref state" << std::endl; -// std::cout << refpos << std::endl; -// std::cout << refmom << std::endl; -// std::cout << genpart->charge() << std::endl; - - //zero uncertainty on generated parameters - AlgebraicSymMatrix55 nullerr; - const CurvilinearTrajectoryError referr(nullerr); -// const CurvilinearTrajectoryError referr; - -// refFts = FreeTrajectoryState(refpos, refmom, genpart->charge(), field); - refFts = FreeTrajectoryState(refglobal, referr); - } - else { - } - -// std::vector> layerStates; - std::vector layerStates; - layerStates.reserve(nhits); - - bool valid = true; - - -// //do propagation and prepare states -// auto propresult = fPropagator->propagateWithPath(refFts, *hits.front()->surface()); -// if (!propresult.first.isValid()) { -// std::cout << "Abort: Propagation from reference point failed" << std::endl; -// continue; -// } -// layerStates.push_back(propresult); -// -// for (auto const& hit : hits) { -// propresult = fPropagator->propagateWithPath(layerStates.back().first, *hit->surface()); -// if (!propresult.first.isValid()) { -// std::cout << "Abort: Propagation failed" << std::endl; -// valid = false; -// break; -// } -// layerStates.push_back(propresult); -// } -// -// if (!valid) { -// continue; -// } - - - - //inflate errors -// refFts.rescaleError(100.); - - -// unsigned int ntotalhitdim = 0; -// unsigned int alignmentidx = 0; -// unsigned int bfieldidx = 0; -// unsigned int elossidx = 0; - - constexpr unsigned int niters = 1; -// constexpr unsigned int niters = 3; - - for (unsigned int iiter=0; iiter 0; -// const bool islikelihood = true; - - gradfull = VectorXd::Zero(nparmsfull); - hessfull = MatrixXd::Zero(nparmsfull, nparmsfull); - statejac = MatrixXd::Zero(nstateparmspost, nparmsfull); - - validdxeigjac = MatrixXd::Zero(2*nvalid, nstateparms); - - evector, 11> > dhessv; - if (islikelihood) { - dhessv.resize(nhits-1); - } - - dxpxb1 = -99.; - dypxb1 = -99.; - dxttec9rphi = -99.; - dxttec9stereo = -99.; - dxttec4rphi = -99.; - dxttec4stereo = -99.; - - dxttec4rphisimgen = -99.; - dyttec4rphisimgen = -99.; - dxttec4rphirecsim = -99.; - - dxttec9rphisimgen = -99.; - dyttec9rphisimgen = -99.; - - simlocalxref = -99.; - simlocalyref = -99.; - - -// dhessv.reserve(nhits-1); - - unsigned int parmidx = 0; - unsigned int alignmentparmidx = 0; - unsigned int ivalidhit = 0; - - if (iiter > 0) { - //update current state from reference point state (errors not needed beyond first iteration) - JacobianCurvilinearToCartesian curv2cart(refFts.parameters()); - const AlgebraicMatrix65& jac = curv2cart.jacobian(); - const AlgebraicVector6 glob = refFts.parameters().vector(); - - auto const& dxlocal = dxstate.head<5>(); - const Matrix globupd = Map>(glob.Array()) + Map>(jac.Array())*dxlocal; - - const GlobalPoint pos(globupd[0], globupd[1], globupd[2]); - const GlobalVector mom(globupd[3], globupd[4], globupd[5]); - double charge = std::copysign(1., refFts.charge()/refFts.momentum().mag() + dxlocal[0]); -// std::cout << "before update: reffts:" << std::endl; -// std::cout << refFts.parameters().vector() << std::endl; -// std::cout << "charge " << refFts.charge() << std::endl; - refFts = FreeTrajectoryState(pos, mom, charge, field); -// std::cout << "after update: reffts:" << std::endl; -// std::cout << refFts.parameters().vector() << std::endl; -// std::cout << "charge " << refFts.charge() << std::endl; -// currentFts = refFts; - } - -// Matrix5d Hlm = Matrix5d::Identity(); -// currentFts = refFts; -// TrajectoryStateOnSurface currentTsos; - -// ; - - const GeomDet *det0 = globalGeometry->idToDet(hits[0]->detUnitId()); - auto propresult = g4prop->propagateWithPath(refFts, det0->surface()); -// auto propresult = fPropagator->geometricalPropagator().propagateWithPath(refFts, *beampipe); - if (!propresult.first.isValid()) { - std::cout << "Abort: Propagation of reference state Failed!" << std::endl; - valid = false; - break; - } - -// std::cout << "position on beampipe " << propresult.first.globalParameters().position() << std::endl; - - const Matrix FdFp = curv2curvTransportJacobian(refFts, propresult, false); - - Matrix J = FdFp.block<2, 2>(3, 3); - // (du/dalphap)^-1 - Matrix Sinv = FdFp.block<2, 2>(3, 1).inverse(); - // du/dqopp - Matrix D = FdFp.block<2, 1>(3, 0); - // du/dBp - Matrix Bpref = FdFp.block<2, 1>(3, 5); - - constexpr unsigned int jacstateidxout = 0; - constexpr unsigned int jacstateidxin = 0; - - // qop_i - statejac(jacstateidxout, jacstateidxin + 2) = 1.; - // d(lambda, phi)_i/dqop_i - statejac.block<2, 1>(jacstateidxout + 1, jacstateidxin + 2) = -Sinv*D; - // d(lambda, phi)_i/(dxy, dsz) - statejac.block<2, 2>(jacstateidxout + 1, jacstateidxin) = -Sinv*J; - // d(lambda, phi)_i/du_(i+1) - statejac.block<2, 2>(jacstateidxout + 1, jacstateidxin + 3) = Sinv; - // d(lambda, phi) / dbeta - statejac.block<2, 1>(jacstateidxout + 1, nstateparms + parmidx) = -Sinv*Bpref; - // dxy - statejac(jacstateidxout + 3, jacstateidxin) = 1.; - // dsz - statejac(jacstateidxout + 4, jacstateidxin + 1) = 1.; - - if (bsConstraint_) { - // apply beamspot constraint - // TODO add residual corrections for beamspot parameters? - - constexpr unsigned int nlocalstate = 2; - constexpr unsigned int nlocalbs = 0; - constexpr unsigned int nlocalparms = nlocalbs; - - constexpr unsigned int nlocal = nlocalstate + nlocalparms; - - constexpr unsigned int localstateidx = 0; - constexpr unsigned int localparmidx = localstateidx + nlocalstate; - - constexpr unsigned int fullstateidx = 0; - const unsigned int fullparmidx = nstateparms + parmidx; - - JacobianCurvilinearToCartesian curv2cart(refFts.parameters()); - const AlgebraicMatrix65& jac = curv2cart.jacobian(); - - const double sigb1 = bsH->BeamWidthX(); - const double sigb2 = bsH->BeamWidthY(); - const double sigb3 = bsH->sigmaZ(); - const double dxdz = bsH->dxdz(); - const double dydz = bsH->dydz(); - const double x0 = bsH->x0(); - const double y0 = bsH->y0(); - const double z0 = bsH->z0(); - - - // covariance matrix of luminous region in global coordinates - // taken from https://github.com/cms-sw/cmssw/blob/abc1f17b230effd629c9565fb5d95e527abcb294/RecoVertex/BeamSpotProducer/src/FcnBeamSpotFitPV.cc#L63-L90 - - // FIXME xy correlation is not stored and assumed to be zero - const double corrb12 = 0.; - - const double varb1 = sigb1*sigb1; - const double varb2 = sigb2*sigb2; - const double varb3 = sigb3*sigb3; - - Matrix covBS = Matrix::Zero(); - // parametrisation: rotation (dx/dz, dy/dz); covxy - covBS(0,0) = varb1; - covBS(1,0) = covBS(0,1) = corrb12*sigb1*sigb2; - covBS(1,1) = varb2; - covBS(2,0) = covBS(0,2) = dxdz*(varb3-varb1)-dydz*covBS(1,0); - covBS(2,1) = covBS(1,2) = dydz*(varb3-varb2)-dxdz*covBS(1,0); - covBS(2,2) = varb3; - -// std::cout << "covBS:" << std::endl; -// std::cout << covBS << std::endl; - - Matrix du = Matrix::Zero(); - for (unsigned int j=0; j dbs0; - dbs0[0] = BSScalar(refFts.position().x() - x0); - dbs0[1] = BSScalar(refFts.position().y() - y0); - dbs0[2] = BSScalar(refFts.position().z() - z0); - -// std::cout << "dposition / d(qop, lambda, phi) (should be 0?):" << std::endl; -// std::cout << Map>(jac.Array()).topLeftCorner<3,3>() << std::endl; - - const Matrix jacpos = Map>(jac.Array()).topRightCorner<3,2>().cast(); - const Matrix covBSinv = covBS.inverse().cast(); - - const Matrix dbs = dbs0 + jacpos*du; - const BSScalar chisq = dbs.transpose()*covBSinv*dbs; - - auto const& gradlocal = chisq.value().derivatives(); - //fill local hessian - Matrix hesslocal; - for (unsigned int j=0; j(fullstateidx) += gradlocal.head(); - gradfull.segment(fullparmidx) += gradlocal.segment(localparmidx); - - //fill global hessian (upper triangular blocks only) - hessfull.block(fullstateidx, fullstateidx) += hesslocal.topLeftCorner(); - hessfull.block(fullstateidx, fullparmidx) += hesslocal.topRightCorner(); - hessfull.block(fullparmidx, fullparmidx) += hesslocal.bottomRightCorner(); - - - } - - - Matrix FdFm = curv2curvTransportJacobian(refFts, propresult, true); - - for (unsigned int ihit = 0; ihit < hits.size(); ++ihit) { - std::cout << "ihit " << ihit << std::endl; - auto const& hit = hits[ihit]; - - TrajectoryStateOnSurface updtsos = propresult.first; - - //apply measurement update if applicable -// std::cout << "constructing preciseHit" << std::endl; - auto const& preciseHit = hit; -// if (hit->isValid() && !preciseHit->isValid()) { -// std::cout << "Abort: Failed updating hit" << std::endl; -// valid = false; -// break; -// } - const GeomDet *det = globalGeometry->idToDet(preciseHit->detUnitId()); - -// const uint32_t gluedid = trackerTopology->glued(preciseHit->det()->geographicalId()); -// const bool isglued = gluedid != 0; -// const DetId parmdetid = isglued ? DetId(gluedid) : preciseHit->geographicalId(); -// const bool align2d = detidparms.count(std::make_pair(1, parmdetid)); -// const GeomDet* parmDet = isglued ? globalGeometry->idToDet(parmdetid) : preciseHit->det(); - - const bool align2d = detidparms.count(std::make_pair(1, preciseHit->detUnitId())); - - - // compute convolution correction in local coordinates (BEFORE material effects are applied) -// const Matrix dxlocalconv = localPositionConvolution(updtsos); - - // curvilinear to local jacobian - JacobianCurvilinearToLocal curv2localm(updtsos.surface(), updtsos.localParameters(), *updtsos.magneticField()); - const AlgebraicMatrix55& curv2localjacm = curv2localm.jacobian(); - const Matrix Hm = Map>(curv2localjacm.Array()); - - //energy loss jacobian -// const Matrix EdE = materialEffectsJacobian(updtsos, fPropagator->materialEffectsUpdator()); - const Matrix EdE = Matrix::Zero(); - - //process noise jacobians -// const std::array, 5> dQs = processNoiseJacobians(updtsos, fPropagator->materialEffectsUpdator()); - const std::array, 5> dQs; - - //TODO update code to allow doing this in one step with nominal update - //temporary tsos to extract process noise without loss of precision - TrajectoryStateOnSurface tmptsos(updtsos); - tmptsos.update(tmptsos.localParameters(), - LocalTrajectoryError(0.,0.,0.,0.,0.), - tmptsos.surface(), - tmptsos.magneticField(), - tmptsos.surfaceSide()); - - //apply the state update from the material effects -// bool ok = fPropagator->materialEffectsUpdator().updateStateInPlace(tmptsos, alongMomentum); - bool ok = true; - if (!ok) { - std::cout << "Abort: material update failed" << std::endl; - valid = false; - break; - } - - const AlgebraicVector5 dxeloss = tmptsos.localParameters().vector() - updtsos.localParameters().vector(); - - // compute convolution effects -// const AlgebraicVector5 dlocalconv = localMSConvolution(updtsos, fPropagator->materialEffectsUpdator()); - -// const GlobalPoint updtsospos = updtsos.globalParameters().position(); -// std::cout << "before material update: " << updtsos.globalParameters().position() << " " << updtsos.globalParameters().momentum() << std::endl; -// ok = fPropagator->materialEffectsUpdator().updateStateInPlace(updtsos, alongMomentum); - ok = true; - if (!ok) { - std::cout << "Abort: material update failed" << std::endl; - valid = false; - break; - } -// std::cout << "after material update: " << updtsos.globalParameters().position() << " " << updtsos.globalParameters().momentum() << std::endl; - - -// std::cout << "local parameters" << std::endl; -// std::cout << updtsos.localParameters().vector() << std::endl; -// std::cout << "dlocalconv" << std::endl; -// std::cout << dlocalconv << std::endl; -// -// // apply convolution effects -// const LocalTrajectoryParameters localupd(updtsos.localParameters().vector() + dlocalconv, -// updtsos.localParameters().pzSign()); -// updtsos.update(localupd, -// // updtsos.localError(), -// updtsos.surface(), -// updtsos.magneticField(), -// updtsos.surfaceSide()); - - //get the process noise matrix - AlgebraicMatrix55 const Qmat = tmptsos.localError().matrix(); - const Map>Q(Qmat.Array()); -// std::cout<< "Q" << std::endl; -// std::cout<< Q << std::endl; - - - // FIXME this is not valid for multiple iterations - // curvilinear to local jacobian - JacobianCurvilinearToLocal curv2localp(updtsos.surface(), updtsos.localParameters(), *updtsos.magneticField()); - const AlgebraicMatrix55& curv2localjacp = curv2localp.jacobian(); - const Matrix Hp = Map>(curv2localjacp.Array()); - - - //FIXME take care of this elsewhere for the moment - const bool genconstraint = dogen && ihit==0; -// const bool genconstraint = false; - - if (ihit < (nhits-1)) { - - //momentum kink residual - AlgebraicVector5 idx0(0., 0., 0., 0., 0.); -// if (iiter==0) { -// layerStates.push_back(updtsos); -// } -// else { -// //FIXME this is not valid for the updated parameterization -// -// //current state from previous state on this layer -// //save current parameters -// TrajectoryStateOnSurface& oldtsos = layerStates[ihit]; -// -// const AlgebraicVector5 local = oldtsos.localParameters().vector(); -// auto const& dxlocal = dxstate.segment<5>(5*(ihit+1)); -// const Matrix localupd = Map>(local.Array()) + dxlocal; -// AlgebraicVector5 localvecupd(localupd[0],localupd[1],localupd[2],localupd[3],localupd[4]); -// -// idx0 = localvecupd - updtsos.localParameters().vector(); -// -// const LocalTrajectoryParameters localparms(localvecupd, oldtsos.localParameters().pzSign()); -// -// // std::cout << "before update: oldtsos:" << std::endl; -// // std::cout << oldtsos.localParameters().vector() << std::endl; -// oldtsos.update(localparms, oldtsos.surface(), field, oldtsos.surfaceSide()); -// // std::cout << "after update: oldtsos:" << std::endl; -// // std::cout << oldtsos.localParameters().vector() << std::endl; -// updtsos = oldtsos; -// -// } - - const Vector5d dx0 = Map(idx0.Array()); - - -// if (ihit==0) { -// FreeTrajectoryState tmpfts(updtsospos, updtsos.globalParameters().momentum(), updtsos.charge(), field); -// propresult = fPropagator->geometricalPropagator().propagateWithPath(tmpfts, *hits[ihit+1]->surface()); -// } -// else { -// propresult = fPropagator->geometricalPropagator().propagateWithPath(updtsos, *hits[ihit+1]->surface()); -// } - const GeomDet *detplus1 = globalGeometry->idToDet(hits[ihit+1]->detUnitId()); - propresult = g4prop->propagateWithPath(updtsos, detplus1->surface()); - if (!propresult.first.isValid()) { - std::cout << "Abort: Propagation Failed!" << std::endl; - valid = false; - break; - } - - if (true) { -// if (false) { - //forward propagation jacobian (local to local) - const Matrix FdFp = curv2curvTransportJacobian(*updtsos.freeState(), propresult, false); - - Matrix J = FdFp.block<2, 2>(3, 3); - // (du/dalphap)^-1 - Matrix Sinv = FdFp.block<2, 2>(3, 1).inverse(); - // du/dqopp - Matrix D = FdFp.block<2, 1>(3, 0); - // du/dBp - Matrix Bstate = FdFp.block<2, 1>(3, 5); - - const unsigned int jacstateidxout = 5*(ihit+1); - const unsigned int jacstateidxin = 3*(ihit+1); - - // qop_i - statejac(jacstateidxout, jacstateidxin + 2) = 1.; - // dalpha_i/dqop_i - statejac.block<2, 1>(jacstateidxout + 1, jacstateidxin + 2) = -Sinv*D; - // dalpha_i/du_i - statejac.block<2, 2>(jacstateidxout + 1, jacstateidxin) = -Sinv*J; - // dalpha_i/du_(i+1) - statejac.block<2, 2>(jacstateidxout + 1, jacstateidxin + 3) = Sinv; - // d(lambda, phi) / dbeta - statejac.block<2, 1>(jacstateidxout + 1, nstateparms + parmidx) = -Sinv*Bstate; - // xlocal_i - statejac(jacstateidxout + 3, jacstateidxin) = 1.; - // ylocal_i - statejac(jacstateidxout + 4, jacstateidxin + 1) = 1.; - - - -// std::cout << "FdFm" << std::endl; -// std::cout << FdFm << std::endl; -// std::cout << "FdFp" << std::endl; -// std::cout << FdFp << std::endl; - - constexpr unsigned int nlocalstate = 8; - constexpr unsigned int nlocalbfield = 3; - constexpr unsigned int nlocaleloss = 2; - constexpr unsigned int nlocalparms = nlocalbfield + nlocaleloss; - - constexpr unsigned int nlocal = nlocalstate + nlocalbfield + nlocaleloss; - - constexpr unsigned int localstateidx = 0; - // constexpr unsigned int localbfieldidx = localstateidx + nlocalstate; - // constexpr unsigned int localelossidx = localbfieldidx + nlocalbfield; - constexpr unsigned int localparmidx = localstateidx + nlocalstate; - - const unsigned int fullstateidx = 3*ihit; -// const unsigned int fullstateidx = 3*(ihit-1); - const unsigned int fullparmidx = (nstateparms + parmidx) - 2; - -// std::cout << "ihit = " << ihit << " nstateparms = " << nstateparms << " parmidx = " << parmidx << " fullparmidx = " << fullparmidx << std::endl; - - // individual pieces, now starting to cast to active scalars for autograd, - // as in eq (3) of https://doi.org/10.1016/j.cpc.2011.03.017 - // du/dum - Matrix Jm = FdFm.block<2, 2>(3, 3).cast(); - // (du/dalpham)^-1 - Matrix Sinvm = FdFm.block<2, 2>(3, 1).inverse().cast(); - // du/dqopm - Matrix Dm = FdFm.block<2, 1>(3, 0).cast(); - // du/dBm - Matrix Bm = FdFm.block<2, 1>(3, 5).cast(); - - // du/dup - Matrix Jp = FdFp.block<2, 2>(3, 3).cast(); - // (du/dalphap)^-1 - Matrix Sinvp = FdFp.block<2, 2>(3, 1).inverse().cast(); - // du/dqopp - Matrix Dp = FdFp.block<2, 1>(3, 0).cast(); - // du/dBp - Matrix Bp = FdFp.block<2, 1>(3, 5).cast(); - -// std::cout << "Jm" << std::endl; -// std::cout << Jm << std::endl; -// std::cout << "Sinvm" << std::endl; -// std::cout << Sinvm << std::endl; -// std::cout << "Dm" << std::endl; -// std::cout << Dm << std::endl; -// std::cout << "Bm" << std::endl; -// std::cout << Bm << std::endl; -// -// std::cout << "Jp" << std::endl; -// std::cout << Jp << std::endl; -// std::cout << "Sinvp" << std::endl; -// std::cout << Sinvp << std::endl; -// std::cout << "Dp" << std::endl; -// std::cout << Dp << std::endl; -// std::cout << "Bp" << std::endl; -// std::cout << Bp << std::endl; - - // energy loss jacobians - // const MSJacobian E = EdE.leftCols<5>().cast(); - // const MSVector dE = EdE.rightCols<1>().cast(); - - // fraction of material on this layer compared to glued layer if relevant -// double xifraction = isglued ? preciseHit->det()->surface().mediumProperties().xi()/parmDet->surface().mediumProperties().xi() : 1.; - -// std::cout << "xifraction: " << xifraction << std::endl; - - const MSScalar Eqop(EdE(0,0)); - const Matrix Ealpha = EdE.block<1, 2>(0, 1).cast(); - const MSScalar dE(EdE(0,5)); -// const MSScalar dE(xifraction*EdE(0,5)); -// (void)EdE; - - const MSScalar muE(dxeloss[0]); - -// std::cout<<"EdE" << std::endl; -// std::cout << EdE << std::endl; - - //energy loss inverse variance - MSScalar invSigmaE(1./Q(0,0)); - - // multiple scattering inverse covariance - Matrix Qinvms = Q.block<2,2>(1,1).inverse().cast(); - - // initialize active scalars for state parameters - Matrix dum = Matrix::Zero(); - //suppress gradients of reference point parameters when fitting with gen constraint - for (unsigned int j=0; j du = Matrix::Zero(); - for (unsigned int j=0; j dup = Matrix::Zero(); - for (unsigned int j=0; j()*dbeta; - } - else if (dogen && ihit==1) { - init_twice_active_var(dbetam, nlocal, localparmidx); - dum = Bpref.cast()*dbetam; - } - - //multiple scattering kink term - - Matrix Halphalamphim = Hm.block<2,2>(1, 1).cast(); - Matrix Halphaum = Hm.block<2,2>(1, 3).cast(); - - Matrix Halphalamphip = Hp.block<2,2>(1, 1).cast(); - Matrix Halphaup = Hp.block<2,2>(1, 3).cast(); - - const Matrix dalpha0 = dx0.segment<2>(1).cast(); - - const Matrix dlamphim = Sinvm*(dum - Jm*du - Dm*dqopm - Bm*dbeta); - const Matrix dlamphip = Sinvp*(dup - Jp*du - Dp*dqop - Bp*dbetap); - - const Matrix dalpham = Halphalamphim*dlamphim + Halphaum*du; - const Matrix dalphap = Halphalamphip*dlamphip + Halphaup*du; - - -// const Matrix dalpham = Sinvm*(dum - Jm*du - Dm*dqopm - Bm*dbeta); -// const Matrix dalphap = Sinvp*(dup - Jp*du - Dp*dqop - Bp*dbetap); -// const Matrix dalpham = Sinvm*(dum - Jm*du - Dm*dqopm); -// const Matrix dalphap = Sinvp*(dup - Jp*du - Dp*dqop); - - - const MSScalar deloss0(dx0[0]); - - - - -// const Matrix dms = dalpha0 + dalphap - dalpham; -// const MSScalar chisqms = dms.transpose()*Qinvms*dms; -// //energy loss term -// -// -// const MSScalar deloss = deloss0 + dqop - Eqop*dqopm - (Ealpha*dalpham)[0] - dE*dxi; -// const MSScalar chisqeloss = deloss*deloss*invSigmaE; -// -// const MSScalar chisq = chisqms + chisqeloss; - - - - -// const bool dolikelihood = false; -// - MSScalar chisq; - - if (!islikelihood) { - //standard chisquared contribution - - const Matrix dms = dalpha0 + dalphap - dalpham; - const MSScalar chisqms = dms.transpose()*Qinvms*dms; - //energy loss term - - - const MSScalar deloss = deloss0 + dqop - Eqop*dqopm - (Ealpha*dalpham)[0] - dE*dxi; - const MSScalar chisqeloss = deloss*invSigmaE*deloss; - - chisq = chisqms + chisqeloss; - } - else { -// islikelihood = true; - //maximum likelihood contribution - const MSCovariance dQdqop = dQs[0].cast(); -// const MSCovariance dQddxdz = dQs[1].cast(); -// const MSCovariance dQddydz = dQs[2].cast(); -// const MSCovariance dQdxi = dQs[3].cast(); - -// const MSCovariance dQ = dqopm*dQdqop + dalpham[0]*dQddxdz + dalpham[1]*dQddydz + dxi*dQdxi; -// const MSCovariance dQ = 0.5*(dqop+dqopm)*dQdqop; - const MSCovariance dQ = dqopm*dQdqop; -// const MSCovariance dQ = 0.5*(dqop+dqopm)*dQdqop + 0.5*(dalpham[0] + dalphap[0])*dQddxdz + 0.5*(dalpham[1]+dalphap[1])*dQddydz + dxi*dQdxi; - - const Matrix Qmsnom = Q.block<2,2>(1,1).cast(); - const Matrix Qmsnominv = Qmsnom.inverse(); - const Matrix Qmsinv = Qmsnominv - Qmsnominv*dQ.block<2,2>(1,1)*Qmsnominv; - - -// const Matrix Qms = Q.block<2,2>(1,1).cast() + dQ.block<2,2>(1,1); -// const Matrix Qmsinv = Qms.inverse(); -// const MSScalar logdetQms = Eigen::log(Qms.determinant()); - - const Matrix dms = dalpha0 + dalphap - dalpham; - MSScalar chisqms = dms.transpose()*Qmsinv*dms; -// chisqms = chisqms + logdetQms; - - //energy loss term -// const MSScalar sigmaE = MSScalar(Q(0,0)) + dQ(0,0); -// const MSScalar sigmaEinv = 1./sigmaE; - - const MSScalar sigmaEnom = MSScalar(Q(0,0)); - const MSScalar sigmaEnominv = 1./sigmaEnom; - - const MSScalar sigmaEinv = sigmaEnominv - sigmaEnominv*dQ(0,0)*sigmaEnominv; - -// const MSScalar logsigmaE = Eigen::log(sigmaE); - - const MSScalar deloss = deloss0 + dqop - Eqop*dqopm - (Ealpha*dalpham)[0] - dE*dxi; - MSScalar chisqeloss = deloss*sigmaEinv*deloss; -// chisqeloss = chisqeloss + logsigmaE; - - chisq = chisqms + chisqeloss; - - //compute contributions to hessian matrix-vector derivative - for (unsigned int i=0; i dQmsinv; - for (unsigned int j=0; j<2; ++j) { - for (unsigned int k=0; k<2; ++k) { - dQmsinv(j,k) = MSScalar(Qmsinv(j,k).value().derivatives()[i]); - } - } - const MSScalar dSigmaEinv(sigmaEinv.value().derivatives()[i]); - - MSScalar dchisqms = dms.transpose()*dQmsinv*x*dms; -// dchisqms = 3.*dchisqms; - MSScalar dchisqeloss = deloss*deloss*dSigmaEinv*x; -// dchisqeloss = 3.*dchisqeloss; - const MSScalar dchisq = dchisqms + dchisqeloss; - - //TODO should this be 11x11 instead? - //TODO check additional factor of 2 - for (unsigned int j=0; j<8; ++j) { - for (unsigned int k=0; k<8; ++k) { - dhessv[ihit][i](j,k) = dchisq.derivatives()[j].derivatives()[k]; - } - } - - } - - } - - - -// const MSScalar chisq = chisqms; - - // std::cout << "chisq.value()" << std::endl; - // std::cout << chisq.value() << std::endl; - // std::cout << "chisq.value().derivatives()" << std::endl; - // std::cout << chisq.value().derivatives() << std::endl; - // std::cout << "chisq.derivatives()[0].derivatives()" << std::endl; - // std::cout << chisq.derivatives()[0].derivatives() << std::endl; - - - // const MSVector dms = dx0 + H*dx - E*Hprop*F*dxprev - E*Hprop*dF*dbeta - dE*dxi; - - - - - // MSScalar chisq; - // - // if (ihit==0 || ihit == (nhits-1)) { - // //standard fit - // const MSVector dms = dx0 + H*dx - E*Hprop*F*dxprev - E*Hprop*dF*dbeta - dE*dxi; - // chisq = dms.transpose()*Qinv*dms; - // } - // else { - // //maximum likelihood fit - // const MSVector dxprop = Hprop*F*dxprev; - // const MSCovariance dQdxprop0 = dQs[0].cast(); - // const MSCovariance dQdxprop1 = dQs[1].cast(); - // const MSCovariance dQdxprop2 = dQs[2].cast(); - // const MSCovariance dQdxi = dQs[3].cast(); - // - // const MSCovariance dQ = dxprop[0]*dQdxprop0 + dxprop[1]*dQdxprop1 + dxprop[2]*dQdxprop2 + dxi*dQdxi; - // - // // const MSCovariance dQdxprop0 = dQs[0].cast(); - // // const MSCovariance d2Qdxprop02 = dQs[1].cast(); - // // // - // // const MSCovariance dQ = dxprop[0]*dQdxprop0 + 0.5*dxprop[0]*dxprop[0]*d2Qdxprop02; - // - // // const Matrix Qms = iQ.topLeftCorner<3,3>().cast() + dQ.topLeftCorner<3,3>(); - // // Qinv.topLeftCorner<3,3>() = Qms.inverse(); - // const Matrix Qms = iQ.block<2,2>(1,1).cast() + dQ.block<2,2>(1,1); - // Qinv.block<2,2>(1,1) = Qms.inverse(); - // - // const MSScalar logdetQ = Eigen::log(Qms.determinant()); - // - // const MSVector dms = dx0 + H*dx - E*dxprop - E*Hprop*dF*dbeta - dE*dxi; - // chisq = dms.transpose()*Qinv*dms; - // chisq = chisq + logdetQ; - // - // } - - // MSCovariance Q = iQ.cast(); - - // const MSVector dxprop = Hprop*F*dxprev; - // const MSCovariance dQdxprop0 = dQs[0].cast(); - // const MSCovariance dQdxprop1 = dQs[1].cast(); - // const MSCovariance dQdxprop2 = dQs[2].cast(); - // const MSCovariance dQdxi = dQs[3].cast(); - // // - // const MSCovariance dQ = dxprop[0]*dQdxprop0 + dxprop[1]*dQdxprop1 + dxprop[2]*dQdxprop2 + dxi*dQdxi; - - // const MSVector dxprop = Hprop*F*dxprev; - // const MSCovariance dQdxprop0 = dQs[0].cast(); - // const MSCovariance d2Qdxprop02 = dQs[1].cast(); - // - // const MSCovariance dQ = dxprop[0]*dQdxprop0 + 0.5*dxprop[0]*dxprop[0]*d2Qdxprop02; - - // MSCovariance Qinv = MSCovariance::Zero(); - // Qinv(3,3) = MSScalar(1./epsxy/epsxy); - // Qinv(4,4) = MSScalar(1./epsxy/epsxy); - // Qinv.block<2,2>(1,1) = iQ.block<2,2>(1,1).inverse().cast(); - // const MSScalar Qelos = MSScalar(iQ(0,0)) + dQ(0,0); - // Qinv(0,0) = 1./Qelos; - // // const Matrix Qms = iQ.topLeftCorner<3,3>().cast() + dQ.topLeftCorner<3,3>(); - // // Qinv.topLeftCorner<3,3>() = Qms.inverse(); - // // const MSScalar logdetQ = Eigen::log(Qms.determinant()); - // const MSScalar logdetQ = Eigen::log(Qelos); - // // - // const MSVector dms = dx0 + H*dx - E*dxprop - E*Hprop*dF*dbeta - dE*dxi; - // MSScalar chisq = dms.transpose()*Qinv*dms; - // chisq = chisq + logdetQ; - - // const MSCovariance Qinvmod = Qinv - Qinv*dQ*Qinv; - // const MSScalar dlogdetQ = Eigen::log(1. + (Qinv*dQ).trace()); - // - // const MSVector dms = dx0 + H*dx - E*dxprop - E*Hprop*dF*dbeta - dE*dxi; - // MSScalar chisq = dms.transpose()*Qinvmod*dms; - // chisq = chisq + dlogdetQ; - - - auto const& gradlocal = chisq.value().derivatives(); - //fill local hessian - Matrix hesslocal; - for (unsigned int j=0; j(fullstateidx) += gradlocal.head(); - gradfull.segment(fullparmidx) += gradlocal.segment(localparmidx); - - //fill global hessian (upper triangular blocks only) - hessfull.block(fullstateidx, fullstateidx) += hesslocal.topLeftCorner(); - hessfull.block(fullstateidx, fullparmidx) += hesslocal.topRightCorner(); - hessfull.block(fullparmidx, fullparmidx) += hesslocal.bottomRightCorner(); - -// const unsigned int bfieldglobalidx = detidparms.at(std::make_pair(6,preciseHit->detUnitId())); - const unsigned int bfieldglobalidx = 0; - globalidxv[parmidx] = bfieldglobalidx; - parmidx++; - -// const unsigned int elossglobalidx = detidparms.at(std::make_pair(7,preciseHit->detUnitId())); - const unsigned int elossglobalidx = 0; - globalidxv[parmidx] = elossglobalidx; - parmidx++; - } - - //backwards propagation jacobian (local to local) to be used at the next layer - FdFm = curv2curvTransportJacobian(*updtsos.freeState(), propresult, true); - - } - else { -// const unsigned int bfieldglobalidx = detidparms.at(std::make_pair(6,preciseHit->detUnitId())); - const unsigned int bfieldglobalidx = 0; - globalidxv[parmidx] = bfieldglobalidx; - parmidx++; - } - - if (true) { - - auto fillAlignGrads = [&](auto Nalign) { - constexpr unsigned int nlocalstate = 2; - constexpr unsigned int localstateidx = 0; - constexpr unsigned int localalignmentidx = nlocalstate; - constexpr unsigned int localparmidx = localalignmentidx; - - // abusing implicit template argument to pass - // a template value via std::integral_constant - constexpr unsigned int nlocalalignment = Nalign(); - constexpr unsigned int nlocalparms = nlocalalignment; - constexpr unsigned int nlocal = nlocalstate + nlocalparms; - - using AlignScalar = AANT; - - //FIXME dirty hack to abuse state idx for reference point magnetic field - const unsigned int fullstateidx = genconstraint ? nstateparms : 3*(ihit+1); - // const unsigned int fullstateidx = 3*ihit; - const unsigned int fullparmidx = nstateparms + nparsBfield + nparsEloss + alignmentparmidx; - - const bool ispixel = GeomDetEnumerators::isTrackerPixel(det->subDetector()); - - //TODO add hit validation stuff - //TODO add simhit stuff - -// const PSimHit *matchedsim = nullptr; -// for (auto const& simhith : simHits) { -// for (const PSimHit& simHit : *simhith) { -// if (std::abs(simHit.particleType()) == 13 && simHit.detUnitId() == preciseHit->geographicalId()) { -// matchedsim = &simHit; -// break; -// } -// } -// if (matchedsim) { -// break; -// } -// } - - - Matrix Hu = Hp.bottomRightCorner<2,2>().cast(); - - Matrix dy0; - Matrix Vinv; - // rotation from module to strip coordinates -// Matrix R; - Matrix2d R; - if (!ispixel) { -// dy0[0] = AlignScalar(matchedsim->localPosition().x() - updtsos.localPosition().x()); - dy0[0] = AlignScalar(preciseHit->localPosition().x() - updtsos.localPosition().x()); -// dy0[1] = AlignScalar(0.); - dy0[1] = AlignScalar(preciseHit->localPosition().y() - updtsos.localPosition().y()); - -// bool simvalid = false; -// for (auto const& simhith : simHits) { -// for (const PSimHit& simHit : *simhith) { -// if (simHit.detUnitId() == preciseHit->geographicalId()) { -// -// dy0[0] = AlignScalar(simHit.localPosition().x() - updtsos.localPosition().x()); -// -// simvalid = true; -// break; -// } -// } -// if (simvalid) { -// break; -// } -// } - - Vinv = Matrix::Zero(); - Vinv(0,0) = 1./std::pow(30e-4, 2); - -// R = Matrix::Identity(); - R = Matrix2d::Identity(); - } - else { - // 2d hit - Matrix2d iV; - iV << std::pow(30e-4, 2), 0., - 0., std::pow(30e-4, 2); -// if (ispixel) { - if (true) { - //take 2d hit as-is for pixels -// dy0[0] = AlignScalar(matchedsim->localPosition().x() - updtsos.localPosition().x()); -// dy0[1] = AlignScalar(matchedsim->localPosition().y() - updtsos.localPosition().y()); - dy0[0] = AlignScalar(preciseHit->localPosition().x() - updtsos.localPosition().x()); - dy0[1] = AlignScalar(preciseHit->localPosition().y() - updtsos.localPosition().y()); - - Vinv = iV.inverse().cast(); - //FIXME various temporary hacks; - -// dy0[1] = AlignScalar(0.); -// Vinv = Matrix::Zero(); -// Vinv(0,0) = 1./preciseHit->localPositionError().xx(); - -// if (GeomDetEnumerators::isEndcap(preciseHit->det()->subDetector())) { -// if (GeomDetEnumerators::isBarrel(preciseHit->det()->subDetector())) { -// PXBDetId detidtest(preciseHit->det()->geographicalId()); -// int layertest = detidtest.layer(); -// -// if (layertest > 1) { -// Vinv = Matrix::Zero(); -// } -// -// // Vinv = Matrix::Zero(); -// // dy0[0] = AlignScalar(0.); -// // dy0[1] = AlignScalar(0.); -// } - -// bool simvalid = false; -// for (auto const& simhith : simHits) { -// for (const PSimHit& simHit : *simhith) { -// if (simHit.detUnitId() == preciseHit->geographicalId()) { -// -// if (GeomDetEnumerators::isBarrel(preciseHit->det()->subDetector())) { -// dy0[0] = AlignScalar(simHit.localPosition().x() - updtsos.localPosition().x()); -// dy0[1] = AlignScalar(simHit.localPosition().y() - updtsos.localPosition().y()); -// } -// -// // dy0[1] = AlignScalar(0.); -// -// simvalid = true; -// break; -// } -// } -// if (simvalid) { -// break; -// } -// } - - -// R = Matrix::Identity(); - R = Matrix2d::Identity(); - } - else { - // diagonalize and take only smallest eigenvalue for 2d hits in strip wedge modules, - // since the constraint parallel to the strip is spurious - SelfAdjointEigenSolver eigensolver(iV); -// const Matrix2d& v = eigensolver.eigenvectors(); - R = eigensolver.eigenvectors().transpose(); - if (R(0,0) < 0.) { - R.row(0) *= -1.; - } - if (R(1,1) <0.) { - R.row(1) *= -1.; - } - - Matrix dy0local; -// dy0local[0] = matchedsim->localPosition().x() - updtsos.localPosition().x(); -// dy0local[1] = matchedsim->localPosition().y() - updtsos.localPosition().y(); - dy0local[0] = preciseHit->localPosition().x() - updtsos.localPosition().x(); - dy0local[1] = preciseHit->localPosition().y() - updtsos.localPosition().y(); - -// bool simvalid = false; -// for (auto const& simhith : simHits) { -// for (const PSimHit& simHit : *simhith) { -// if (simHit.detUnitId() == preciseHit->geographicalId()) { -// -// dy0local[0] = simHit.localPosition().x() - updtsos.localPosition().x(); -// dy0local[1] = simHit.localPosition().y() - updtsos.localPosition().y(); -// -// simvalid = true; -// break; -// } -// } -// if (simvalid) { -// break; -// } -// } - - const Matrix dy0eig = R*dy0local; - - //TODO deal properly with rotations (rotate back to module local coords?) - dy0[0] = AlignScalar(dy0eig[0]); - dy0[1] = AlignScalar(0.); - - Vinv = Matrix::Zero(); - Vinv(0,0) = AlignScalar(1./eigensolver.eigenvalues()[0]); - -// R = v.transpose().cast(); - - } - } - - rxfull.row(ivalidhit) = R.row(0).cast(); - ryfull.row(ivalidhit) = R.row(1).cast(); - - validdxeigjac.block<2,2>(2*ivalidhit, 3*(ihit+1)) = R*Hp.bottomRightCorner<2,2>(); - - const Matrix Ralign = R.cast(); - - Matrix dx = Matrix::Zero(); - AlignScalar dbeta(0.); - if (!genconstraint) { - for (unsigned int j=0; j()*dbeta; - } - - Matrix dalpha = Matrix::Zero(); - // order in which to use parameters, especially relevant in case nlocalalignment < 6 - constexpr std::array alphaidxs = {{5, 0, 1, 2, 3, 4}}; - for (unsigned int idim=0; idim A = Matrix::Zero(); - - - // dx/dx - A(0,0) = AlignScalar(1.); - // dy/dy - A(1,1) = AlignScalar(1.); - // dx/dz - A(0,2) = updtsos.localParameters().dxdz(); - // dy/dz - A(1,2) = updtsos.localParameters().dydz(); - // dx/dtheta_x - A(0,3) = -updtsos.localPosition().y()*updtsos.localParameters().dxdz(); - // dy/dtheta_x - A(1,3) = -updtsos.localPosition().y()*updtsos.localParameters().dydz(); - // dx/dtheta_y - A(0,4) = -updtsos.localPosition().x()*updtsos.localParameters().dxdz(); - // dy/dtheta_y - A(1,4) = -updtsos.localPosition().x()*updtsos.localParameters().dydz(); - // dx/dtheta_z - A(0,5) = -updtsos.localPosition().y(); - // dy/dtheta_z - A(1,5) = updtsos.localPosition().x(); - - -// std::cout << "strip local z shift gradient: " << (Ralign*A.col(2))[0].value().value() << std::endl; - - // rotation from alignment basis to module local coordinates -// Matrix A; -// if (isglued) { -// const GlobalVector modx = preciseHit->det()->surface().toGlobal(LocalVector(1.,0.,0.)); -// const GlobalVector mody = preciseHit->det()->surface().toGlobal(LocalVector(0.,1.,0.)); -// -// const GlobalVector gluedx = parmDet->surface().toGlobal(LocalVector(1.,0.,0.)); -// const GlobalVector gluedy = parmDet->surface().toGlobal(LocalVector(0.,1.,0.)); -// -// A(0,0) = AlignScalar(modx.dot(gluedx)); -// A(0,1) = AlignScalar(modx.dot(gluedy)); -// A(1,0) = AlignScalar(mody.dot(gluedx)); -// A(1,1) = AlignScalar(mody.dot(gluedy)); -// } -// else { -// A = Matrix::Identity(); -// } -// -// Matrix dh = dy0 - R*Hu*dx - R*A*dalpha; - - - double thetaincidence = std::asin(1./std::sqrt(std::pow(updtsos.localParameters().dxdz(),2) + std::pow(updtsos.localParameters().dydz(),2) + 1.)); - -// bool morehitquality = applyHitQuality_ ? thetaincidence > 0.25 : true; - bool morehitquality = true; - - if (morehitquality) { - nValidHitsFinal++; - if (ispixel) { - nValidPixelHitsFinal++; - } - } - else { - Vinv = Matrix::Zero(); - } - - Matrix dh = dy0 - Ralign*Hu*dx - Ralign*A*dalpha; - AlignScalar chisq = dh.transpose()*Vinv*dh; - - auto const& gradlocal = chisq.value().derivatives(); - //fill local hessian - Matrix hesslocal; - for (unsigned int j=0; j gradloctest0; -// Matrix gradloctest1; -// Matrix gradloctest2; - -// std::cout << "nlocalalignment: " << nlocalalignment << " nlocal: " << nlocal << std::endl; -// std::cout << "gradlocal type: " << typeid(gradlocal).name() << std::endl; -// std::cout << "gradloctest0 type: " << typeid(gradloctest0).name() << std::endl; -// std::cout << "gradloctest1 type: " << typeid(gradloctest1).name() << std::endl; -// std::cout << "gradloctest2 type: " << typeid(gradloctest2).name() << std::endl; -// -// std::cout << "nhits: " << nhits << " nvalid: " << nvalid << " nvalidalign2d: " << nvalidalign2d << " ihit: " << ihit << std::endl; -// std::cout << "gradfull.size(): " << gradfull.size() << " nlocalstate: " << nlocalstate << " fullstateidx: " << fullstateidx << " nlocalparms: " << nlocalparms << " fullparmidx: " << fullparmidx << std::endl; - - // FIXME the templated block functions don't work here for some reason - //fill global gradient - gradfull.segment(fullstateidx) += gradlocal.head(nlocalstate); - gradfull.segment(fullparmidx) += gradlocal.segment(localparmidx, nlocalparms); - - //fill global hessian (upper triangular blocks only) - hessfull.block(fullstateidx, fullstateidx) += hesslocal.topLeftCorner(nlocalstate,nlocalstate); - hessfull.block(fullstateidx, fullparmidx) += hesslocal.topRightCorner(nlocalstate, nlocalparms); - hessfull.block(fullparmidx, fullparmidx) += hesslocal.bottomRightCorner(nlocalparms, nlocalparms); - - for (unsigned int idim=0; idimdetUnitId())); - globalidxv[nparsBfield + nparsEloss + alignmentparmidx] = xglobalidx; - alignmentparmidx++; - if (alphaidxs[idim]==0) { - hitidxv.push_back(xglobalidx); - } - } - - // fill hit validation information - Vector2d dyrecgenlocal; - dyrecgenlocal << dy0[0].value().value(), dy0[1].value().value(); - const Vector2d dyrecgeneig = R*dyrecgenlocal; - dxrecgen.push_back(dyrecgeneig[0]); - dyrecgen.push_back(dyrecgeneig[1]); - - dxerr.push_back(1./std::sqrt(Vinv(0,0).value().value())); - dyerr.push_back(1./std::sqrt(Vinv(1,1).value().value())); - - localqop.push_back(updtsos.localParameters().qbp()); - localdxdz.push_back(updtsos.localParameters().dxdz()); - localdydz.push_back(updtsos.localParameters().dydz()); - localx.push_back(updtsos.localPosition().x()); - localy.push_back(updtsos.localPosition().y()); - - }; - - if (align2d) { - fillAlignGrads(std::integral_constant()); - } - else { - fillAlignGrads(std::integral_constant()); - } -// fillAlignGrads(std::integral_constant()); - - ivalidhit++; - - } - -// std::cout << "hit " << ihit << " isvalid " << preciseHit->isValid() << std::endl; -// std::cout << "global position: " << updtsos.globalParameters().position() << std::endl; - //hit information - //FIXME consolidate this special cases into templated function(s) -// if (preciseHit->isValid()) { - } - - if (!valid) { - break; - } - - assert(parmidx == (nparsBfield + nparsEloss)); - assert(alignmentparmidx == nparsAlignment); - - //fake constraint on reference point parameters - if (dogen) { -// if (false) { - for (unsigned int i=0; i<5; ++i) { - gradfull[i] = 0.; - hessfull.row(i) *= 0.; - hessfull.col(i) *= 0.; - hessfull(i,i) = 1e6; - } -// //b field from reference point not consistently used in this case -// gradfull[nstateparms] = 0.; -// hessfull.row(nstateparms) *= 0.; -// hessfull.col(nstateparms) *= 0.; - } - - std::cout << "now do the expensive calculations and fill outputs" << std::endl; - - //now do the expensive calculations and fill outputs - auto const& dchisqdx = gradfull.head(nstateparms); - auto const& dchisqdparms = gradfull.tail(npars); - - auto const& d2chisqdx2 = hessfull.topLeftCorner(nstateparms, nstateparms); - auto const& d2chisqdxdparms = hessfull.topRightCorner(nstateparms, npars); - auto const& d2chisqdparms2 = hessfull.bottomRightCorner(npars, npars); - -// std::cout << "dchisqdx" << std::endl; -// std::cout << dchisqdx << std::endl; -// std::cout << "d2chisqdx2 diagonal" << std::endl; -// std::cout << d2chisqdx2.diagonal() << std::endl; -// std::cout << "d2chisqdx2" << std::endl; -// std::cout << d2chisqdx2 << std::endl; -// -// auto const& eigenvalues = d2chisqdx2.eigenvalues(); -// std::cout << "d2chisqdx2 eigenvalues" << std::endl; -// std::cout << eigenvalues << std::endl; - -// auto const& Cinvd = d2chisqdx2.ldlt(); - Cinvd.compute(d2chisqdx2); - - - if (islikelihood) { - const MatrixXd Cfull = Cinvd.solve(MatrixXd::Identity(nstateparms,nstateparms)); - - // add ln det terms to gradient and hessian - // MatrixXd dhessfulli; - // MatrixXd dhessfullj; - // VectorXd dgradfull; - // TODO should this cover correction parameter part of the matrix as well? - for (unsigned int ihit=0; ihit<(nhits-1); ++ihit) { - constexpr unsigned int localstateidx = 0; - const unsigned int fullstateidx = 3*ihit; - - auto const& Cblock = Cfull.block<8,8>(fullstateidx, fullstateidx); - - // dhessfulli = MatrixXd::Zero(nstateparms, nstateparms); - // dhessfullj = MatrixXd::Zero(nstateparms, nstateparms); - - //TODO fill correction parameter block as well - for (unsigned int i=0; i<8; ++i) { - gradfull[fullstateidx + i] += (Cblock*dhessv[ihit][i]).trace(); - for (unsigned int j=0; j<8; ++j) { - hessfull(fullstateidx + i, fullstateidx + j) += (-Cblock*dhessv[ihit][j]*Cblock*dhessv[ihit][i]).trace(); - } - } - - } - - Cinvd.compute(d2chisqdx2); - - } - - dxfull = -Cinvd.solve(dchisqdx); - - dxstate = statejac.leftCols(nstateparms)*dxfull; - -// const Vector5d dxRef = dx.head<5>(); -// // const Vector5d dxRef = -Cinvd.solve(dchisqdx).head<5>(); -// const Matrix5d Cinner = Cinvd.solve(MatrixXd::Identity(nstateparms,nstateparms)).topLeftCorner<5,5>(); - -// dxdparms = -Cinvd.solve(d2chisqdxdparms).transpose(); -// -// grad = dchisqdparms + dxdparms*dchisqdx; -// hess = d2chisqdparms2 + 2.*dxdparms*d2chisqdxdparms + dxdparms*d2chisqdx2*dxdparms.transpose(); -// -// std::cout << "dxfull" << std::endl; -// std::cout << dxfull << std::endl; -// std::cout << "errsq" << std::endl; -// std::cout << Cinvd.solve(MatrixXd::Identity(nstateparms,nstateparms)).diagonal() << std::endl; - - const Vector5d dxRef = dxstate.head<5>(); - const Matrix5d Cinner = (statejac.leftCols(nstateparms)*Cinvd.solve(MatrixXd::Identity(nstateparms,nstateparms))*statejac.leftCols(nstateparms).transpose()).topLeftCorner<5,5>(); - -// const Matrix5d Cinner = Cinvd.solve(MatrixXd::Identity(nstateparms,nstateparms)).topLeftCorner<5,5>(); - - if (debugprintout_) { - std::cout<< "dxRef" << std::endl; - std::cout<< dxRef << std::endl; - } - - //fill output with corrected state and covariance at reference point - refParms.fill(0.); - refCov.fill(0.); -// const AlgebraicVector5& refVec = track.parameters(); - CurvilinearTrajectoryParameters curvparms(refFts.position(), refFts.momentum(), refFts.charge()); - const AlgebraicVector5& refVec = curvparms.vector(); - -// std::cout << "refVec:" << std::endl; -// std::cout << refVec << std::endl; -// std::cout << "dxRef" << std::endl; -// std::cout << dxRef << std::endl; - - Map(refParms.data()) = (Map(refVec.Array()) + dxRef).cast(); - Map >(refCov.data()).triangularView() = (2.*Cinner).cast().triangularView(); - - if (iiter==0) { - refParms_iter0 = refParms; - refCov_iter0 = refCov; - } -// else if (iiter==2) { -// refParms_iter2 = refParms; -// refCov_iter2 = refCov; -// } - -// std::cout << "refParms" << std::endl; -// std::cout << Map(refParms.data()) << std::endl; - -// // gradv.clear(); -// jacrefv.clear(); -// -// // gradv.resize(npars,0.); -// jacrefv.resize(5*npars, 0.); -// -// nJacRef = 5*npars; -// // tree->SetBranchAddress("gradv", gradv.data()); -// tree->SetBranchAddress("jacrefv", jacrefv.data()); -// -// //eigen representation of the underlying vector storage -// // Map gradout(gradv.data(), npars); -// Map > jacrefout(jacrefv.data(), 5, npars); -// -// jacrefout = dxdparms.leftCols<5>().transpose().cast(); - - } - - if (!valid) { - return; - } - -// if (!nValidPixelHitsFinal) { -// continue; -// } - - std::cout << "now do the expensive calculations and fill outputs (again)" << std::endl; - - auto const& dchisqdx = gradfull.head(nstateparms); - auto const& dchisqdparms = gradfull.tail(npars); - - auto const& d2chisqdx2 = hessfull.topLeftCorner(nstateparms, nstateparms); - auto const& d2chisqdxdparms = hessfull.topRightCorner(nstateparms, npars); - auto const& d2chisqdparms2 = hessfull.bottomRightCorner(npars, npars); - - dxdparms = -Cinvd.solve(d2chisqdxdparms).transpose(); - -// if (debugprintout_) { -// std::cout << "dxrefdparms" << std::endl; -// std::cout << dxdparms.leftCols<5>() << std::endl; -// } - - grad = dchisqdparms + dxdparms*dchisqdx; - //TODO check the simplification -// hess = d2chisqdparms2 + 2.*dxdparms*d2chisqdxdparms + dxdparms*d2chisqdx2*dxdparms.transpose(); - hess = d2chisqdparms2 + dxdparms*d2chisqdxdparms; - -// const Vector5d dxRef = dxfull.head<5>(); -// const Matrix5d Cinner = Cinvd.solve(MatrixXd::Identity(nstateparms,nstateparms)).topLeftCorner<5,5>(); - -// std::cout << "dchisqdparms.head<6>()" << std::endl; -// std::cout << dchisqdparms.head<6>() << std::endl; -// -// std::cout << "grad.head<6>()" << std::endl; -// std::cout << grad.head<6>() << std::endl; -// -// std::cout << "d2chisqdparms2.topLeftCorner<6, 6>():" << std::endl; -// std::cout << d2chisqdparms2.topLeftCorner<6, 6>() << std::endl; -// std::cout << "hess.topLeftCorner<6, 6>():" << std::endl; -// std::cout << hess.topLeftCorner<6, 6>() << std::endl; -// -// std::cout << "dchisqdparms.segment<6>(nparsBfield+nparsEloss)" << std::endl; -// std::cout << dchisqdparms.segment<6>(nparsBfield+nparsEloss) << std::endl; -// -// std::cout << "grad.segment<6>(nparsBfield+nparsEloss)" << std::endl; -// std::cout << grad.segment<6>(nparsBfield+nparsEloss) << std::endl; -// -// std::cout << "d2chisqdparms2.block<6, 6>(nparsBfield+nparsEloss, nparsBfield+nparsEloss):" << std::endl; -// std::cout << d2chisqdparms2.block<6, 6>(nparsBfield+nparsEloss, nparsBfield+nparsEloss) << std::endl; -// std::cout << "hess.block<6, 6>(nparsBfield+nparsEloss, nparsBfield+nparsEloss):" << std::endl; -// std::cout << hess.block<6, 6>(nparsBfield+nparsEloss, nparsBfield+nparsEloss) << std::endl; -// -// std::cout << "d2chisqdparms2.bottomRightCorner<6, 6>():" << std::endl; -// std::cout << d2chisqdparms2.bottomRightCorner<6, 6>() << std::endl; -// std::cout << "hess.bottomRightCorner<6, 6>():" << std::endl; -// std::cout << hess.bottomRightCorner<6, 6>() << std::endl; - - gradv.clear(); - jacrefv.clear(); - - gradv.resize(npars,0.); - jacrefv.resize(5*npars, 0.); - - nJacRef = 5*npars; - if (fillTrackTree_ && fillGrads_) { - tree->SetBranchAddress("gradv", gradv.data()); - } - if (fillTrackTree_) { - tree->SetBranchAddress("jacrefv", jacrefv.data()); - } - - //eigen representation of the underlying vector storage - Map gradout(gradv.data(), npars); - Map > jacrefout(jacrefv.data(), 5, npars); - -// jacrefout = dxdparms.leftCols<5>().transpose().cast(); - jacrefout = ( (dxdparms*statejac.leftCols(nstateparms).transpose()).leftCols<5>().transpose() + statejac.topRightCorner(5, npars) ).cast(); - - gradout = grad.cast(); - - - rx.resize(2*nvalid); - Map > rxout(rx.data(), nvalid, 2); - rxout = rxfull; -// std::cout << "rx:" << std::endl; -// for (auto elem : rx) { -// std::cout << elem << " "; -// } -// std::cout << std::endl; -// std::cout << rx << std::endl; - - ry.resize(2*nvalid); - Map > ryout(ry.data(), nvalid, 2); - ryout = ryfull; - - deigx.resize(nvalid); - deigy.resize(nvalid); - - validdxeig = validdxeigjac*dxfull; - - for (unsigned int ivalid = 0; ivalid < nvalid; ++ivalid) { - deigx[ivalid] = validdxeig[2*ivalid]; - deigy[ivalid] = validdxeig[2*ivalid + 1]; - } - -// unsigned int ivalid = 0; -// for (unsigned int ihit = 0; ihit < nhits; ++ihit) { -// auto const& hit = hits[ihit]; -// if (hit->isValid()) { -// // if (ihit < (nhits-1)) { -// // std::cout << "ihit = " << ihit << ", ivalid = " << ivalid << std::endl; -// // std::cout << "dxfull.segment<3>(3*(ihit+1)):" << std::endl; -// // std::cout << dxfull.segment<3>(3*(ihit+1)) << std::endl; -// // std::cout << "dxstate.segment<5>(5*(ihit+1))" << std::endl; -// // std::cout << dxstate.segment<5>(5*(ihit+1)) << std::endl; -// // } -// const unsigned int dxidx = 3*(ihit + 1); -// dlocalx[ivalid] = dxfull[dxidx]; -// dlocaly[ivalid] = dxfull[dxidx + 1]; -// ++ivalid; -// } -// } - - - float refPt = dogen ? genpart->pt() : std::abs(1./refParms[0])*std::sin(M_PI_2 - refParms[1]); - - gradmax = 0.; - for (unsigned int i=0; igradmax) { - gradmax = absval; - } - } - - - hessmax = 0.; - for (unsigned int i=0; ihessmax) { - hessmax = absval; - } - - } - - } - -// if (gradmax < 1e5 && refPt > 5.5) { -// //fill aggregrate gradient and hessian -// for (unsigned int i=0; ihessmax) { -// hessmax = absval; -// } -// -// const std::pair key = std::make_pair(std::min(iidx,jidx), std::max(iidx,jidx)); -// -// auto it = hessaggsparse.find(key); -// if (it==hessaggsparse.end()) { -// hessaggsparse[key] = hess(i,j); -// } -// else { -// it->second += hess(i,j); -// } -// } -// } -// } - - - if (debugprintout_) { - const Matrix5d Cinner = (statejac*Cinvd.solve(MatrixXd::Identity(nstateparms,nstateparms))*statejac.transpose()).topLeftCorner<5,5>(); - std::cout << "hess debug" << std::endl; - std::cout << "track parms" << std::endl; -// std::cout << tkparms << std::endl; - // std::cout << "dxRef" << std::endl; - // std::cout << dxRef << std::endl; -// std::cout << "original cov" << std::endl; -// std::cout << track.covariance() << std::endl; - std::cout << "recomputed cov" << std::endl; - std::cout << 2.*Cinner << std::endl; - } - -// std::cout << "dxinner/dparms" << std::endl; -// std::cout << dxdparms.bottomRows<5>() << std::endl; -// std::cout << "grad" << std::endl; -// std::cout << grad << std::endl; -// std::cout << "hess diagonal" << std::endl; -// std::cout << hess.diagonal() << std::endl; -// std::cout << "hess0 diagonal" << std::endl; -// std::cout << d2chisqdparms2.diagonal() << std::endl; -// std::cout << "hess1 diagonal" << std::endl; -// std::cout << 2.*(dxdparms.transpose()*d2chisqdxdparms).diagonal() << std::endl; -// std::cout << "hess2 diagonal" << std::endl; -// std::cout << (dxdparms.transpose()*d2chisqdx2*dxdparms).diagonal() << std::endl; - - //fill packed hessian and indices - const unsigned int nsym = npars*(1+npars)/2; - hesspackedv.clear(); - hesspackedv.resize(nsym, 0.); - - nSym = nsym; - if (fillTrackTree_ && fillGrads_) { - tree->SetBranchAddress("hesspackedv", hesspackedv.data()); - } - - Map hesspacked(hesspackedv.data(), nsym); - const Map globalidx(globalidxv.data(), npars); - - unsigned int packedidx = 0; - for (unsigned int ipar = 0; ipar < npars; ++ipar) { - const unsigned int segmentsize = npars - ipar; - hesspacked.segment(packedidx, segmentsize) = hess.block<1, Dynamic>(ipar, ipar, 1, segmentsize).cast(); - packedidx += segmentsize; - } - - if (fillTrackTree_) { - tree->Fill(); - } - } -} - -DEFINE_FWK_MODULE(ResidualGlobalCorrectionMakerSimG4e); diff --git a/Analysis/HitAnalyzer/plugins/ResidualGlobalCorrectionMakerTwoTrack.cc b/Analysis/HitAnalyzer/plugins/ResidualGlobalCorrectionMakerTwoTrack.cc deleted file mode 100644 index 6ddf21ffdaec3..0000000000000 --- a/Analysis/HitAnalyzer/plugins/ResidualGlobalCorrectionMakerTwoTrack.cc +++ /dev/null @@ -1,2320 +0,0 @@ -#include "ResidualGlobalCorrectionMakerBase.h" -#include "MagneticFieldOffset.h" - -// required for Transient Tracks -#include "TrackingTools/TransientTrack/interface/TransientTrack.h" -#include "TrackingTools/TransientTrack/interface/TransientTrackBuilder.h" -#include "TrackingTools/Records/interface/TransientTrackRecord.h" -#include "MagneticField/Engine/interface/MagneticField.h" -// required for vtx fitting -#include "RecoVertex/KinematicFitPrimitives/interface/TransientTrackKinematicParticle.h" -#include "RecoVertex/KinematicFitPrimitives/interface/KinematicParticleFactoryFromTransientTrack.h" -#include "RecoVertex/KinematicFit/interface/KinematicParticleVertexFitter.h" -#include "RecoVertex/KinematicFit/interface/KinematicParticleFitter.h" -#include "RecoVertex/KinematicFit/interface/MassKinematicConstraint.h" -#include "RecoVertex/KinematicFit/interface/KinematicConstrainedVertexFitter.h" -#include "RecoVertex/KinematicFit/interface/TwoTrackMassKinematicConstraint.h" -#include "RecoVertex/KinematicFit/interface/MultiTrackMassKinematicConstraint.h" -#include "RecoVertex/KinematicFit/interface/MultiTrackPointingKinematicConstraint.h" -#include "RecoVertex/KinematicFit/interface/CombinedKinematicConstraint.h" - -#include "DataFormats/Math/interface/deltaR.h" - -#include "Math/Vector4Dfwd.h" - - -class ResidualGlobalCorrectionMakerTwoTrack : public ResidualGlobalCorrectionMakerBase -{ -public: - explicit ResidualGlobalCorrectionMakerTwoTrack(const edm::ParameterSet &); - ~ResidualGlobalCorrectionMakerTwoTrack() {} - -// static void fillDescriptions(edm::ConfigurationDescriptions &descriptions); - -private: - - Matrix massJacobian(const FreeTrajectoryState &state0, const FreeTrajectoryState &state1, double dmass) const; - - virtual void beginStream(edm::StreamID) override; - virtual void produce(edm::Event &, const edm::EventSetup &) override; - - bool doMassConstraint_; - double massConstraint_; - double massConstraintWidth_; - - float Jpsi_x; - float Jpsi_y; - float Jpsi_z; - float Jpsi_pt; - float Jpsi_eta; - float Jpsi_phi; - float Jpsi_mass; - - float Muplus_pt; - float Muplus_eta; - float Muplus_phi; - - float Muminus_pt; - float Muminus_eta; - float Muminus_phi; - - float Jpsikin_x; - float Jpsikin_y; - float Jpsikin_z; - float Jpsikin_pt; - float Jpsikin_eta; - float Jpsikin_phi; - float Jpsikin_mass; - - float Mupluskin_pt; - float Mupluskin_eta; - float Mupluskin_phi; - - float Muminuskin_pt; - float Muminuskin_eta; - float Muminuskin_phi; - - float Jpsigen_x; - float Jpsigen_y; - float Jpsigen_z; - float Jpsigen_pt; - float Jpsigen_eta; - float Jpsigen_phi; - float Jpsigen_mass; - - float Muplusgen_pt; - float Muplusgen_eta; - float Muplusgen_phi; - - float Muminusgen_pt; - float Muminusgen_eta; - float Muminusgen_phi; - - std::array Muplus_refParms; - std::array MuMinus_refParms; - - std::vector Muplus_jacRef; - std::vector Muminus_jacRef; - - unsigned int Muplus_nhits; - unsigned int Muplus_nvalid; - unsigned int Muplus_nvalidpixel; - - unsigned int Muminus_nhits; - unsigned int Muminus_nvalid; - unsigned int Muminus_nvalidpixel; - -// std::vector hessv; - - - -}; - - -ResidualGlobalCorrectionMakerTwoTrack::ResidualGlobalCorrectionMakerTwoTrack(const edm::ParameterSet &iConfig) : ResidualGlobalCorrectionMakerBase(iConfig) -{ - doMassConstraint_ = iConfig.getParameter("doMassConstraint"); - massConstraint_ = iConfig.getParameter("massConstraint"); - massConstraintWidth_ = iConfig.getParameter("massConstraintWidth"); -} - -void ResidualGlobalCorrectionMakerTwoTrack::beginStream(edm::StreamID streamid) -{ - ResidualGlobalCorrectionMakerBase::beginStream(streamid); - - if (fillTrackTree_) { - tree->Branch("Jpsi_x", &Jpsi_x); - tree->Branch("Jpsi_y", &Jpsi_y); - tree->Branch("Jpsi_z", &Jpsi_z); - tree->Branch("Jpsi_pt", &Jpsi_pt); - tree->Branch("Jpsi_eta", &Jpsi_eta); - tree->Branch("Jpsi_phi", &Jpsi_phi); - tree->Branch("Jpsi_mass", &Jpsi_mass); - - tree->Branch("Muplus_pt", &Muplus_pt); - tree->Branch("Muplus_eta", &Muplus_eta); - tree->Branch("Muplus_phi", &Muplus_phi); - - tree->Branch("Muminus_pt", &Muminus_pt); - tree->Branch("Muminus_eta", &Muminus_eta); - tree->Branch("Muminus_phi", &Muminus_phi); - - tree->Branch("Jpsikin_x", &Jpsikin_x); - tree->Branch("Jpsikin_y", &Jpsikin_y); - tree->Branch("Jpsikin_z", &Jpsikin_z); - tree->Branch("Jpsikin_pt", &Jpsikin_pt); - tree->Branch("Jpsikin_eta", &Jpsikin_eta); - tree->Branch("Jpsikin_phi", &Jpsikin_phi); - tree->Branch("Jpsikin_mass", &Jpsikin_mass); - - tree->Branch("Mupluskin_pt", &Mupluskin_pt); - tree->Branch("Mupluskin_eta", &Mupluskin_eta); - tree->Branch("Mupluskin_phi", &Mupluskin_phi); - - tree->Branch("Muminuskin_pt", &Muminuskin_pt); - tree->Branch("Muminuskin_eta", &Muminuskin_eta); - tree->Branch("Muminuskin_phi", &Muminuskin_phi); - - tree->Branch("Jpsigen_x", &Jpsigen_x); - tree->Branch("Jpsigen_y", &Jpsigen_y); - tree->Branch("Jpsigen_z", &Jpsigen_z); - tree->Branch("Jpsigen_pt", &Jpsigen_pt); - tree->Branch("Jpsigen_eta", &Jpsigen_eta); - tree->Branch("Jpsigen_phi", &Jpsigen_phi); - tree->Branch("Jpsigen_mass", &Jpsigen_mass); - - tree->Branch("Muplusgen_pt", &Muplusgen_pt); - tree->Branch("Muplusgen_eta", &Muplusgen_eta); - tree->Branch("Muplusgen_phi", &Muplusgen_phi); - - tree->Branch("Muminusgen_pt", &Muminusgen_pt); - tree->Branch("Muminusgen_eta", &Muminusgen_eta); - tree->Branch("Muminusgen_phi", &Muminusgen_phi); - - tree->Branch("Muplus_refParms", Muplus_refParms.data(), "Muplus_refParms[5]/F"); - tree->Branch("MuMinus_refParms", MuMinus_refParms.data(), "MuMinus_refParms[5]/F"); - -// tree->Branch("Muplus_jacRef", &Muplus_jacRef); -// tree->Branch("Muminus_jacRef", &Muminus_jacRef); - - tree->Branch("Muplus_nhits", &Muplus_nhits); - tree->Branch("Muplus_nvalid", &Muplus_nvalid); - tree->Branch("Muplus_nvalidpixel", &Muplus_nvalidpixel); - - tree->Branch("Muminus_nhits", &Muminus_nhits); - tree->Branch("Muminus_nvalid", &Muminus_nvalid); - tree->Branch("Muminus_nvalidpixel", &Muminus_nvalidpixel); - -// tree->Branch("hessv", &hessv); - - } -} - - -// ------------ method called for each event ------------ -void ResidualGlobalCorrectionMakerTwoTrack::produce(edm::Event &iEvent, const edm::EventSetup &iSetup) -{ - - const bool dogen = fitFromGenParms_; - - using namespace edm; - - Handle trackOrigH; - iEvent.getByToken(inputTrackOrig_, trackOrigH); - - - // loop over gen particles - - edm::ESHandle globalGeometry; - iSetup.get().get(globalGeometry); - - edm::ESHandle trackerTopology; - iSetup.get().get(trackerTopology); - - - edm::ESHandle ttrh; - iSetup.get().get("WithAngleAndTemplate",ttrh); - -// ESHandle thePropagator; -// iSetup.get().get("RungeKuttaTrackerPropagator", thePropagator); -// iSetup.get().get("PropagatorWithMaterial", thePropagator); -// iSetup.get().get("PropagatorWithMaterialParabolicMf", thePropagator); -// iSetup.get().get("Geant4ePropagator", thePropagator); -// const MagneticField* field = thePropagator->magneticField(); - - ESHandle fieldh; - iSetup.get().get("", fieldh); - std::unique_ptr fieldOffset = std::make_unique(&(*fieldh)); - - std::unique_ptr fPropagator = std::make_unique(alongMomentum, 0.105, fieldOffset.get(), 1.6, true, -1., true); - - const MagneticField* field = fPropagator->magneticField(); - - -// std::cout << "bfield orig prop type: " << typeid(*thePropagator->magneticField()).name() << std::endl; -// std::cout << "bfield orig type: " << typeid(*fieldh).name() << std::endl; -// std::cout << "bfield type: " << typeid(*field).name() << std::endl; - -// ESHandle theAnalyticPropagator; -// iSetup.get().get("PropagatorWithMaterial", theAnalyticPropagator); - - - Handle> genPartCollection; - if (doGen_) { - iEvent.getByToken(GenParticlesToken_, genPartCollection); - } - -// std::unique_ptr fPropagator(static_cast(thePropagator->clone())); -// fPropagator->setPropagationDirection(alongMomentum); - -// std::unique_ptr fAnalyticPropagator(static_cast(theAnalyticPropagator->clone())); -// fAnalyticPropagator->setPropagationDirection(alongMomentum); - - KFUpdator updator; - TkClonerImpl const& cloner = static_cast(ttrh.product())->cloner(); - - edm::ESHandle TTBuilder; - iSetup.get().get("TransientTrackBuilder", TTBuilder); - KinematicParticleFactoryFromTransientTrack pFactory; - - constexpr double mmu = 0.1056583745; - constexpr double mmuerr = 0.0000000024; - - VectorXd gradfull; - MatrixXd hessfull; - - VectorXd dxfull; - MatrixXd dxdparms; - VectorXd grad; - MatrixXd hess; - LDLT Cinvd; -// FullPivLU Cinvd; -// ColPivHouseholderQR Cinvd; - - std::array jacarr; - - std::array dxstatearr; - - // loop over combinatorics of track pairs - for (auto itrack = trackOrigH->begin(); itrack != trackOrigH->end(); ++itrack) { - const reco::TransientTrack itt = TTBuilder->build(*itrack); - for (auto jtrack = itrack + 1; jtrack != trackOrigH->end(); ++jtrack) { - const reco::TransientTrack jtt = TTBuilder->build(*jtrack); - - - const reco::GenParticle *mu0gen = nullptr; - const reco::GenParticle *mu1gen = nullptr; - - double massconstraintval = massConstraint_; - if (doGen_) { - for (auto const &genpart : *genPartCollection) { - if (genpart.status() != 1) { - continue; - } - if (std::abs(genpart.pdgId()) != 13) { - continue; - } - -// float dR0 = deltaR(genpart.phi(), itrack->phi(), genpart.eta(), itrack->eta()); - float dR0 = deltaR(genpart, *itrack); - if (dR0 < 0.1 && genpart.charge() == itrack->charge()) { - mu0gen = &genpart; - } - -// float dR1 = deltaR(genpart.phi(), jtrack->phi(), genpart.eta(), jtrack->eta()); - float dR1 = deltaR(genpart, *jtrack); - if (dR1 < 0.1 && genpart.charge() == jtrack->charge()) { - mu1gen = &genpart; - } - } - -// if (mu0gen != nullptr && mu1gen != nullptr) { -// auto const jpsigen = ROOT::Math::PtEtaPhiMVector(mu0gen->pt(), mu0gen->eta(), mu0gen->phi(), mmu) + -// ROOT::Math::PtEtaPhiMVector(mu1gen->pt(), mu1gen->eta(), mu1gen->phi(), mmu); -// -// massconstraintval = jpsigen.mass(); -// } -// else { -// continue; -// } - - } - -// std::cout << "massconstraintval = " << massconstraintval << std::endl; - - // common vertex fit - std::vector parts; - - float masserr = mmuerr; - float chisq = 0.; - float ndf = 0.; - parts.push_back(pFactory.particle(itt, mmu, chisq, ndf, masserr)); - parts.push_back(pFactory.particle(jtt, mmu, chisq, ndf, masserr)); - - RefCountedKinematicTree kinTree; - if (doMassConstraint_) { -// if (false) { -// TwoTrackMassKinematicConstraint constraint(massConstraint_); - TwoTrackMassKinematicConstraint constraint(massconstraintval); - KinematicConstrainedVertexFitter vtxFitter; - kinTree = vtxFitter.fit(parts, &constraint); - } - else { - KinematicParticleVertexFitter vtxFitter; - kinTree = vtxFitter.fit(parts); - } - - if (kinTree->isEmpty() || !kinTree->isConsistent()) { - continue; - } - - kinTree->movePointerToTheTop(); - RefCountedKinematicParticle dimu_kinfit = kinTree->currentParticle(); - const double m0 = dimu_kinfit->currentState().mass(); - - if (false) { - // debug output -// kinTree->movePointerToTheTop(); - -// RefCountedKinematicParticle dimu_kinfit = kinTree->currentParticle(); - RefCountedKinematicVertex dimu_vertex = kinTree->currentDecayVertex(); - - std::cout << dimu_kinfit->currentState().mass() << std::endl; - std::cout << dimu_vertex->position() << std::endl; - } - - const std::vector outparts = kinTree->finalStateParticles(); - std::array refftsarr = {{ outparts[0]->currentState().freeTrajectoryState(), - outparts[1]->currentState().freeTrajectoryState() }}; - - if (fitFromGenParms_ && mu0gen != nullptr && mu1gen != nullptr) { - GlobalPoint pos0(mu0gen->vertex().x(), mu0gen->vertex().y(), mu0gen->vertex().z()); - GlobalVector mom0(mu0gen->momentum().x(), mu0gen->momentum().y(), mu0gen->momentum().z()); - - GlobalPoint pos1(mu1gen->vertex().x(), mu1gen->vertex().y(), mu1gen->vertex().z()); - GlobalVector mom1(mu1gen->momentum().x(), mu1gen->momentum().y(), mu1gen->momentum().z()); - - refftsarr[0] = FreeTrajectoryState(pos0, mom0, mu0gen->charge(), field); - refftsarr[1] = FreeTrajectoryState(pos1, mom1, mu1gen->charge(), field); - } - - std::array hitsarr; - - // prepare hits - for (unsigned int id = 0; id < 2; ++id) { - const reco::Track &track = id == 0 ? *itrack : *jtrack; - auto &hits = hitsarr[id]; - hits.reserve(track.recHitsSize()); - - - for (auto it = track.recHitsBegin(); it != track.recHitsEnd(); ++it) { - const GeomDet* detectorG = globalGeometry->idToDet((*it)->geographicalId()); - const GluedGeomDet* detglued = dynamic_cast(detectorG); - - // split matched invalid hits - if (detglued != nullptr && !(*it)->isValid()) { - bool order = detglued->stereoDet()->surface().position().mag() > detglued->monoDet()->surface().position().mag(); - const GeomDetUnit* detinner = order ? detglued->monoDet() : detglued->stereoDet(); - const GeomDetUnit* detouter = order ? detglued->stereoDet() : detglued->monoDet(); - - hits.push_back(TrackingRecHit::RecHitPointer(new InvalidTrackingRecHit(*detinner, (*it)->type()))); - hits.push_back(TrackingRecHit::RecHitPointer(new InvalidTrackingRecHit(*detouter, (*it)->type()))); - } - else { - // apply hit quality criteria - const bool ispixel = GeomDetEnumerators::isTrackerPixel(detectorG->subDetector()); - bool hitquality = false; - if (applyHitQuality_ && (*it)->isValid()) { - const TrackerSingleRecHit* tkhit = dynamic_cast(*it); - assert(tkhit != nullptr); - - if (ispixel) { - const SiPixelRecHit *pixhit = dynamic_cast(tkhit); - const SiPixelCluster& cluster = *tkhit->cluster_pixel(); - assert(pixhit != nullptr); - - hitquality = !pixhit->isOnEdge() && cluster.sizeX() > 1; - } - else { - assert(tkhit->cluster_strip().isNonnull()); - const SiStripCluster& cluster = *tkhit->cluster_strip(); - const StripTopology* striptopology = dynamic_cast(&(detectorG->topology())); - assert(striptopology); - - const uint16_t firstStrip = cluster.firstStrip(); - const uint16_t lastStrip = cluster.firstStrip() + cluster.amplitudes().size() - 1; - const bool isOnEdge = firstStrip == 0 || lastStrip == (striptopology->nstrips() - 1); - - // if (isOnEdge) { - // std::cout << "strip hit isOnEdge" << std::endl; - // } - -// hitquality = !isOnEdge; - hitquality = true; - } - - } - else { - hitquality = true; - } - - - if (hitquality) { - hits.push_back((*it)->cloneForFit(*detectorG)); - } - else { - hits.push_back(TrackingRecHit::RecHitPointer(new InvalidTrackingRecHit(*detectorG, TrackingRecHit::inactive))); - } - } - } - } - - unsigned int nhits = 0; - unsigned int nvalid = 0; - unsigned int nvalidpixel = 0; - unsigned int nvalidalign2d = 0; - - std::array, 2> layerStatesarr; - - std::array nhitsarr = {{ 0, 0 }}; - std::array nvalidarr = {{ 0, 0 }}; - std::array nvalidpixelarr = {{ 0, 0 }}; - - // second loop to count hits - for (unsigned int id = 0; id < 2; ++id) { - auto const &hits = hitsarr[id]; - layerStatesarr[id].reserve(hits.size()); - for (auto const &hit : hits) { - ++nhits; - ++nhitsarr[id]; - if (hit->isValid()) { - ++nvalid; - ++nvalidarr[id]; - - const bool ispixel = GeomDetEnumerators::isTrackerPixel(hit->det()->subDetector()); - if (ispixel) { - ++nvalidpixel; - ++nvalidpixelarr[id]; - } - - - const bool align2d = detidparms.count(std::make_pair(1, hit->geographicalId())); - if (align2d) { - ++nvalidalign2d; - } - } - } - } - - - - const unsigned int nparsAlignment = 2*nvalid + nvalidalign2d; - const unsigned int nparsBfield = nhits; - const unsigned int nparsEloss = nhits - 2; - const unsigned int npars = nparsAlignment + nparsBfield + nparsEloss; - - const unsigned int nstateparms = 5 + 3*nhits - 2; - const unsigned int nparmsfull = nstateparms + npars; - - bool valid = true; - -// constexpr unsigned int niters = 1; - constexpr unsigned int niters = 10; - - for (unsigned int iiter=0; iiterSetBranchAddress("globalidxv", globalidxv.data()); - } - - std::array, 2> FdFprefarr; - std::array trackstateidxarr; - std::array trackparmidxarr; - - unsigned int trackstateidx = 3; - unsigned int parmidx = 0; - unsigned int alignmentparmidx = 0; - - double chisq0val = 0.; - - for (unsigned int id = 0; id < 2; ++id) { - // FreeTrajectoryState refFts = outparts[id]->currentState().freeTrajectoryState(); - FreeTrajectoryState &refFts = refftsarr[id]; - auto &hits = hitsarr[id]; - - - - const uint32_t gluedid0 = trackerTopology->glued(hits[0]->det()->geographicalId()); - const bool isglued0 = gluedid0 != 0; - const DetId parmdetid0 = isglued0 ? DetId(gluedid0) : hits[0]->geographicalId(); - const unsigned int bfieldidx = detidparms.at(std::make_pair(6, parmdetid0)); - fieldOffset->setOffset(corparms_[bfieldidx]); - - std::vector &layerStates = layerStatesarr[id]; - - const VectorXd &dxstate = dxstatearr[id]; - - trackstateidxarr[id] = trackstateidx; - trackparmidxarr[id] = parmidx; - - const unsigned int tracknhits = hits.size(); - - const unsigned int tracknstateparmspost = 5*(tracknhits+1); - - if (iiter > 0) { - //update current state from reference point state (errors not needed beyond first iteration) - JacobianCurvilinearToCartesian curv2cart(refFts.parameters()); - const AlgebraicMatrix65& jac = curv2cart.jacobian(); - const AlgebraicVector6 glob = refFts.parameters().vector(); - - const Matrix posupd = Map>(glob.Array()).head<3>() + dxfull.head<3>(); - - auto const& dxlocal = dxstate.head<5>(); - const Matrix momupd = Map>(glob.Array()).tail<3>() + Map>(jac.Array()).bottomRows<3>()*dxlocal; - - const GlobalPoint pos(posupd[0], posupd[1], posupd[2]); - const GlobalVector mom(momupd[0], momupd[1], momupd[2]); - const double charge = std::copysign(1., refFts.charge()/refFts.momentum().mag() + dxlocal[0]); - // std::cout << "before update: reffts:" << std::endl; - // std::cout << refFts.parameters().vector() << std::endl; - // std::cout << "charge " << refFts.charge() << std::endl; - refFts = FreeTrajectoryState(pos, mom, charge, field); - // std::cout << "after update: reffts:" << std::endl; - // std::cout << refFts.parameters().vector() << std::endl; - // std::cout << "charge " << refFts.charge() << std::endl; - // currentFts = refFts; - } - -// auto const &surface0 = *hits[0]->surface(); - auto const &surface0 = *surfacemap_.at(hits[0]->geographicalId()); - auto propresult = fPropagator->geometricalPropagator().propagateWithPath(refFts, surface0); - // auto propresult = fPropagator->geometricalPropagator().propagateWithPath(refFts, *beampipe); - if (!propresult.first.isValid()) { - std::cout << "Abort: Propagation of reference state Failed!" << std::endl; - valid = false; - break; - } - - // std::cout << "position on beampipe " << propresult.first.globalParameters().position() << std::endl; - - // cartesian to curvilinear jacobian, needed for cartesian parameterization of common vertex position - JacobianCartesianToCurvilinear cart2curvref(refFts.parameters()); - auto const &jacCart2Curvref = Map>(cart2curvref.jacobian().Array()); - -// const Matrix FdFpref = hybrid2curvTransportJacobian(refFts, propresult); - -// const Matrix FdFpref = hybrid2localTransportJacobian(refFts, propresult); - const Matrix FdFpref = hybrid2localTransportJacobian(refFts, propresult); - - // const Matrix FdFprefalt = curv2curvTransportJacobian(refFts, propresult); - - FdFprefarr[id] = FdFpref; - - // std::cout << "FdFpref:" << std::endl; - // std::cout << FdFpref << std::endl; - // std::cout << "FdFprefalt:" << std::endl; - // std::cout << FdFprefalt << std::endl; - - // const Matrix cartjac = cartesianToCartesianJacobian(refFts); - - - const Matrix dudvtx0 = FdFpref.block<2, 3>(3, 3); - const Matrix dudalph0inv = FdFpref.block<2, 2>(3, 1).inverse(); - const Matrix dudqop0 = FdFpref.block<2, 1>(3, 0); - const Matrix dudB = FdFpref.block<2, 1>(3, 6); - const Matrix du0dvtx0 = jacCart2Curvref.bottomLeftCorner<2,3>(); - - MatrixXd &jac = jacarr[id]; - jac = MatrixXd::Zero(tracknstateparmspost, nparmsfull); - - // dqop / dqop_0 - jac(0, trackstateidx) = 1.; - // d(lambda, phi) / dqop_0 - jac.block<2, 1>(1, trackstateidx) = -dudalph0inv*dudqop0; - // d(lambda, phi) / dvtx - jac.block<2, 3>(1, 0) = -dudalph0inv*dudvtx0; - // d(lambda, phi)_i/ d(dxy, dsz)_1 - jac.block<2, 2>(1, trackstateidx + 1) = dudalph0inv; - // d(lambda, phi) / dbeta - jac.block<2, 1>(1, nstateparms + parmidx) = -dudalph0inv*dudB; - //TODO avoid the need for this one - // d(dxy, dsz)/dvtx - jac.block<2, 3>(3, 0) = du0dvtx0; - -// Matrix FdFm = curv2curvTransportJacobian(refFts, propresult, true); -// Matrix FdFm = localTransportJacobian(refFts, propresult, true); - - Matrix FdFm; - - for (unsigned int ihit = 0; ihit < hits.size(); ++ihit) { - // std::cout << "ihit " << ihit << std::endl; - auto const& hit = hits[ihit]; - - const uint32_t gluedid = trackerTopology->glued(hit->det()->geographicalId()); - const bool isglued = gluedid != 0; - const DetId parmdetid = isglued ? DetId(gluedid) : hit->geographicalId(); - const GeomDet* parmDet = isglued ? globalGeometry->idToDet(parmdetid) : hit->det(); - const double xifraction = isglued ? hit->det()->surface().mediumProperties().xi()/parmDet->surface().mediumProperties().xi() : 1.; - - TrajectoryStateOnSurface updtsos = propresult.first; - - //apply measurement update if applicable - // std::cout << "constructing preciseHit" << std::endl; - auto const& preciseHit = hit->isValid() ? cloner.makeShared(hit, updtsos) : hit; - if (hit->isValid() && !preciseHit->isValid()) { - std::cout << "Abort: Failed updating hit" << std::endl; - valid = false; - break; - } - - // const uint32_t gluedid = trackerTopology->glued(preciseHit->det()->geographicalId()); - // const bool isglued = gluedid != 0; - // const DetId parmdetid = isglued ? DetId(gluedid) : preciseHit->geographicalId(); - // const bool align2d = detidparms.count(std::make_pair(1, parmdetid)); - // const GeomDet* parmDet = isglued ? globalGeometry->idToDet(parmdetid) : preciseHit->det(); - - const bool align2d = detidparms.count(std::make_pair(1, preciseHit->geographicalId())); - - - // compute convolution correction in local coordinates (BEFORE material effects are applied) - // const Matrix dxlocalconv = localPositionConvolution(updtsos); - - // curvilinear to local jacobian - JacobianCurvilinearToLocal curv2localm(updtsos.surface(), updtsos.localParameters(), *updtsos.magneticField()); - const AlgebraicMatrix55& curv2localjacm = curv2localm.jacobian(); -// const Matrix Hm = Map>(curv2localjacm.Array()); - - //energy loss jacobian - const Matrix EdE = materialEffectsJacobian(updtsos, fPropagator->materialEffectsUpdator()); -// const Matrix EdE = materialEffectsJacobianVar(updtsos, fPropagator->materialEffectsUpdator()); - -// const double xival = updtsos.surface().mediumProperties().xi(); - - //process noise jacobians - // const std::array, 5> dQs = processNoiseJacobians(updtsos, fPropagator->materialEffectsUpdator()); - - //TODO update code to allow doing this in one step with nominal update - //temporary tsos to extract process noise without loss of precision - TrajectoryStateOnSurface tmptsos(updtsos); - tmptsos.update(tmptsos.localParameters(), - LocalTrajectoryError(0.,0.,0.,0.,0.), - tmptsos.surface(), - tmptsos.magneticField(), - tmptsos.surfaceSide()); - - //apply the state update from the material effects - bool ok = fPropagator->materialEffectsUpdator().updateStateInPlace(tmptsos, alongMomentum); - if (!ok) { - std::cout << "Abort: material update failed" << std::endl; - valid = false; - break; - } - - const AlgebraicVector5 dxeloss = tmptsos.localParameters().vector() - updtsos.localParameters().vector(); - - // compute convolution effects - // const AlgebraicVector5 dlocalconv = localMSConvolution(updtsos, fPropagator->materialEffectsUpdator()); - - // const GlobalPoint updtsospos = updtsos.globalParameters().position(); - // std::cout << "before material update: " << updtsos.globalParameters().position() << " " << updtsos.globalParameters().momentum() << std::endl; - ok = fPropagator->materialEffectsUpdator().updateStateInPlace(updtsos, alongMomentum); - if (!ok) { - std::cout << "Abort: material update failed" << std::endl; - valid = false; - break; - } - // std::cout << "after material update: " << updtsos.globalParameters().position() << " " << updtsos.globalParameters().momentum() << std::endl; - - - // std::cout << "local parameters" << std::endl; - // std::cout << updtsos.localParameters().vector() << std::endl; - // std::cout << "dlocalconv" << std::endl; - // std::cout << dlocalconv << std::endl; - // - // // apply convolution effects - // const LocalTrajectoryParameters localupd(updtsos.localParameters().vector() + dlocalconv, - // updtsos.localParameters().pzSign()); - // updtsos.update(localupd, - // // updtsos.localError(), - // updtsos.surface(), - // updtsos.magneticField(), - // updtsos.surfaceSide()); - - //get the process noise matrix - AlgebraicMatrix55 const Qmat = tmptsos.localError().matrix(); - const Map>Q(Qmat.Array()); - // std::cout<< "Q" << std::endl; - // std::cout<< Q << std::endl; - - - // update state from previous iteration - //momentum kink residual - AlgebraicVector5 idx0(0., 0., 0., 0., 0.); - if (iiter==0) { - layerStates.push_back(updtsos); - } - else { - //current state from previous state on this layer - //save current parameters - TrajectoryStateOnSurface& oldtsos = layerStates[ihit]; - -// JacobianCurvilinearToLocal curv2localold(oldtsos.surface(), oldtsos.localParameters(), *oldtsos.magneticField()); -// const AlgebraicMatrix55& curv2localjacold = curv2localold.jacobian(); -// const Matrix Hold = Map>(curv2localjacold.Array()); - - const AlgebraicVector5 local = oldtsos.localParameters().vector(); - auto const& dxlocal = dxstate.segment<5>(5*(ihit+1)); -// auto const& dxlocal = Hold*dxstate.segment<5>(5*(ihit+1)); - const Matrix localupd = Map>(local.Array()) + dxlocal; - AlgebraicVector5 localvecupd(localupd[0],localupd[1],localupd[2],localupd[3],localupd[4]); - - idx0 = localvecupd - updtsos.localParameters().vector(); - - const LocalTrajectoryParameters localparms(localvecupd, oldtsos.localParameters().pzSign()); - - // std::cout << "before update: oldtsos:" << std::endl; - // std::cout << oldtsos.localParameters().vector() << std::endl; - oldtsos.update(localparms, oldtsos.surface(), field, oldtsos.surfaceSide()); - // std::cout << "after update: oldtsos:" << std::endl; - // std::cout << oldtsos.localParameters().vector() << std::endl; - updtsos = oldtsos; - - } - - // curvilinear to local jacobian - JacobianCurvilinearToLocal curv2localp(updtsos.surface(), updtsos.localParameters(), *updtsos.magneticField()); - const AlgebraicMatrix55& curv2localjacp = curv2localp.jacobian(); -// const Matrix Hp = Map>(curv2localjacp.Array()); - - if (ihit < (tracknhits-1)) { - - const uint32_t gluedidip1 = trackerTopology->glued(hits[ihit + 1]->det()->geographicalId()); - const bool isgluedip1 = gluedidip1 != 0; - const DetId parmdetidip1 = isgluedip1 ? DetId(gluedidip1) : hits[ihit + 1]->geographicalId(); - const unsigned int bfieldidx = detidparms.at(std::make_pair(6, parmdetidip1)); - fieldOffset->setOffset(corparms_[bfieldidx]); - - // std::cout << "EdE first hit:" << std::endl; - // std::cout << EdE << std::endl; - // - // std::cout << "xival = " << xival << std::endl; - -// AlgebraicVector5 idx0(0., 0., 0., 0., 0.); - const Vector5d dx0 = Map(idx0.Array()); - - -// auto const &surfaceip1 = *hits[ihit+1]->surface(); - auto const &surfaceip1 = *surfacemap_.at(hits[ihit+1]->geographicalId()); - propresult = fPropagator->geometricalPropagator().propagateWithPath(updtsos, surfaceip1); - if (!propresult.first.isValid()) { - std::cout << "Abort: Propagation Failed!" << std::endl; - valid = false; - break; - } - - //forward propagation jacobian -// const Matrix FdFp = curv2curvTransportJacobian(*updtsos.freeState(), propresult, false); - const Matrix FdFp = localTransportJacobian(updtsos, propresult, false); - - Matrix J = FdFp.block<2, 2>(3, 3); - // (du/dalphap)^-1 - Matrix Sinv = FdFp.block<2, 2>(3, 1).inverse(); - // du/dqopp - Matrix D = FdFp.block<2, 1>(3, 0); - // du/dBp - Matrix Bstate = FdFp.block<2, 1>(3, 5); - - const unsigned int jacstateidxout = 5*(ihit+1); - const unsigned int jacstateidxin = trackstateidx + 1 + 3*ihit; - - // qop_i - jac(jacstateidxout, jacstateidxin + 2) = 1.; - // dalpha_i/dqop_i - jac.block<2, 1>(jacstateidxout + 1, jacstateidxin + 2) = -Sinv*D; - // dalpha_i/du_i - jac.block<2, 2>(jacstateidxout + 1, jacstateidxin) = -Sinv*J; - // dalpha_i/du_(i+1) - jac.block<2, 2>(jacstateidxout + 1, jacstateidxin + 3) = Sinv; - // d(lambda, phi) / dbeta - jac.block<2, 1>(jacstateidxout + 1, nstateparms + parmidx) = -Sinv*Bstate; - // xlocal_i - jac(jacstateidxout + 3, jacstateidxin) = 1.; - // ylocal_i - jac(jacstateidxout + 4, jacstateidxin + 1) = 1.; - - if (ihit == 0) { - constexpr unsigned int nvtxstate = 3; - constexpr unsigned int nlocalstate = 6; - constexpr unsigned int nlocalbfield = 2; - constexpr unsigned int nlocaleloss = 1; - constexpr unsigned int nlocalparms = nlocalbfield + nlocaleloss; - - constexpr unsigned int nlocal = nvtxstate + nlocalstate + nlocalparms; - - constexpr unsigned int localvtxidx = 0; - constexpr unsigned int localstateidx = localvtxidx + nvtxstate; - constexpr unsigned int localparmidx = localstateidx + nlocalstate; - - constexpr unsigned int fullvtxidx = 0; - const unsigned int fullstateidx = trackstateidx; - const unsigned int fullparmidx = nstateparms + parmidx; - - using MSScalar = AANT; - - - // const Matrix Vm = jacCart2Curvref.bottomLeftCorner<2,3>().cast(); - // // du/dum - // const Matrix Jm = FdFm.block<2, 2>(3, 3).cast(); - // // (du/dalpham)^-1 - // const Matrix Sinvm = FdFm.block<2, 2>(3, 1).inverse().cast(); - // // du/dqopm - // const Matrix Dm = FdFm.block<2, 1>(3, 0).cast(); - // // du/dBm - // const Matrix Bm = FdFm.block<2, 1>(3, 5).cast(); - - - // individual pieces, now starting to cast to active scalars for autograd, - - // as in eq (3) of https://doi.org/10.1016/j.cpc.2011.03.017 - - const Matrix dudvtx0 = FdFpref.block<2, 3>(3, 3).cast(); - const Matrix dudalph0inv = FdFpref.block<2, 2>(3, 1).inverse().cast(); - const Matrix dudqop0 = FdFpref.block<2, 1>(3, 0).cast(); - const Matrix dudB = FdFpref.block<2, 1>(3, 6).cast(); - - const Matrix dalphadvtx0 = FdFpref.block<2, 3>(1, 3).cast(); - const Matrix dalphadalpha0 = FdFpref.block<2, 2>(1, 1).cast(); - const Matrix dalphadqop0 = FdFpref.block<2, 1>(1, 0).cast(); - const Matrix dalphadB = FdFpref.block<2, 1>(1, 6).cast(); - - // du/dup - const Matrix Jp = FdFp.block<2, 2>(3, 3).cast(); - // (du/dalphap)^-1 - const Matrix Sinvp = FdFp.block<2, 2>(3, 1).inverse().cast(); - // du/dqopp - const Matrix Dp = FdFp.block<2, 1>(3, 0).cast(); - // du/dBp - const Matrix Bp = FdFp.block<2, 1>(3, 5).cast(); - - const MSScalar Eqop(EdE(0,0)); - const Matrix Ealpha = EdE.block<1, 2>(0, 1).cast(); - const MSScalar dE(xifraction*EdE(0,5)); - - const MSScalar muE(dxeloss[0]); - - //energy loss inverse variance - const MSScalar invSigmaE(1./Q(0,0)); - - // multiple scattering inverse covariance - const Matrix Qinvms = Q.block<2,2>(1,1).inverse().cast(); - - - // initialize active scalars for common vertex parameters - Matrix dvtx = Matrix::Zero(); - for (unsigned int j=0; j du = Matrix::Zero(); - for (unsigned int j=0; j dup = Matrix::Zero(); - for (unsigned int j=0; j Halphalamphim = Hm.block<2,2>(1, 1).cast(); -// const Matrix Halphaum = Hm.block<2,2>(1, 3).cast(); - -// const Matrix Halphalamphip = Hp.block<2,2>(1, 1).cast(); -// const Matrix Halphaup = Hp.block<2,2>(1, 3).cast(); - - const Matrix dalpha0 = dx0.segment<2>(1).cast(); - - // const Matrix dum = Vm*dvtx; - // const Matrix dlamphim = Sinvm*(dum - Jm*du - Dm*dqopm - Bm*dbeta); - - const Matrix dlamphim = dalphadvtx0*dvtx + dalphadqop0*dqopm + dalphadB*dbeta + dalphadalpha0*dudalph0inv*(du - dudvtx0*dvtx - dudqop0*dqopm - dudB*dbeta); - const Matrix dlamphip = Sinvp*(dup - Jp*du - Dp*dqop - Bp*dbetap); - -// const Matrix dalpham = Halphalamphim*dlamphim + Halphaum*du; -// const Matrix dalphap = Halphalamphip*dlamphip + Halphaup*du; - - const Matrix dalpham = dlamphim; - const Matrix dalphap = dlamphip; - - const MSScalar deloss0(dx0[0]); - - const Matrix dms = dalpha0 + dalphap - dalpham; - const MSScalar chisqms = dms.transpose()*Qinvms*dms; - //energy loss term - - - const MSScalar deloss = deloss0 + dqop - Eqop*dqopm - (Ealpha*dalpham)[0] - dE*dxi; -// const MSScalar deloss = deloss0 + dqop - dqopm - dE*dxi; - const MSScalar chisqeloss = deloss*invSigmaE*deloss; - - const MSScalar chisq = chisqms + chisqeloss; - - chisq0val += chisq.value().value(); - - auto const& gradlocal = chisq.value().derivatives(); - //fill local hessian - Matrix hesslocal; - for (unsigned int j=0; j localsizes = {{ nvtxstate, nlocalstate, nlocalparms }}; - constexpr std::array localidxs = {{ localvtxidx, localstateidx, localparmidx }}; - const std::array fullidxs = {{ fullvtxidx, fullstateidx, fullparmidx }}; - - for (unsigned int iidx = 0; iidx < localidxs.size(); ++iidx) { - gradfull.segment(fullidxs[iidx], localsizes[iidx]) += gradlocal.segment(localidxs[iidx], localsizes[iidx]); - for (unsigned int jidx = 0; jidx < localidxs.size(); ++jidx) { - hessfull.block(fullidxs[iidx], fullidxs[jidx], localsizes[iidx], localsizes[jidx]) += hesslocal.block(localidxs[iidx], localidxs[jidx], localsizes[iidx], localsizes[jidx]); - } - } - - - // std::cout << "first hit gradlocal" << std::endl; - // std::cout << gradlocal << std::endl; - // - // std::cout << "first hit hesslocal" << std::endl; - // std::cout << hesslocal << std::endl; - - //Fill global grad and hess (upper triangular blocks only) - // gradfull.segment(fullvtxidx) += gradlocal.segment(localvtxidx); - // gradfull.segment(fullstateidx) += gradlocal.segment(localstateidx); - // gradfull.segment(fullparmidx) += gradlocal.segment(localparmidx); - // - // hessfull.block(fullvtxidx, fullvtxidx) += hesslocal.block(localvtxidx, localvtxidx); - // hessfull.block(fullvtxidx, fullstateidx) += hesslocal.block(localvtxidx, localstateidx); - // hessfull.block(fullvtxidx, fullparmidx) += hesslocal.block(localvtxidx, localparmidx); - // - // hessfull.block(fullstateidx, fullstateidx) += hesslocal.block(localstateidx, localstateidx); - // hessfull.block(fullstateidx, fullparmidx) += hesslocal.block(localstateidx, localparmidx); - // - // hessfull.block(fullparmidx, fullparmidx) += hesslocal.block(localparmidx, localparmidx); - // - // //extra part - // hessfull.block(fullstateidx, fullvtxidx) += hesslocal.block(localvtxidx, localstateidx).transpose(); - - - // std::cout << "first hit, parm idx = " << parmidx << std::endl; - - } - else { - //TODO statejac stuff - - constexpr unsigned int nlocalstate = 8; - constexpr unsigned int nlocalbfield = 2; - constexpr unsigned int nlocaleloss = 1; - constexpr unsigned int nlocalparms = nlocalbfield + nlocaleloss; - - constexpr unsigned int nlocal = nlocalstate + nlocalparms; - - constexpr unsigned int localstateidx = 0; - constexpr unsigned int localparmidx = localstateidx + nlocalstate; - - const unsigned int fullstateidx = trackstateidx + 1 + 3*(ihit - 1); - const unsigned int fullparmidx = nstateparms + parmidx; - - using MSScalar = AANT; - - - // individual pieces, now starting to cast to active scalars for autograd, - // as in eq (3) of https://doi.org/10.1016/j.cpc.2011.03.017 - // du/dum - const Matrix Jm = FdFm.block<2, 2>(3, 3).cast(); - // (du/dalpham)^-1 - const Matrix Sinvm = FdFm.block<2, 2>(3, 1).inverse().cast(); - // du/dqopm - const Matrix Dm = FdFm.block<2, 1>(3, 0).cast(); - // du/dBm - const Matrix Bm = FdFm.block<2, 1>(3, 5).cast(); - - // du/dup - const Matrix Jp = FdFp.block<2, 2>(3, 3).cast(); - // (du/dalphap)^-1 - const Matrix Sinvp = FdFp.block<2, 2>(3, 1).inverse().cast(); - // du/dqopp - const Matrix Dp = FdFp.block<2, 1>(3, 0).cast(); - // du/dBp - const Matrix Bp = FdFp.block<2, 1>(3, 5).cast(); - - const MSScalar Eqop(EdE(0,0)); - const Matrix Ealpha = EdE.block<1, 2>(0, 1).cast(); - const MSScalar dE(xifraction*EdE(0,5)); - - const MSScalar muE(dxeloss[0]); - - //energy loss inverse variance - const MSScalar invSigmaE(1./Q(0,0)); - - // multiple scattering inverse covariance - const Matrix Qinvms = Q.block<2,2>(1,1).inverse().cast(); - - // initialize active scalars for state parameters - Matrix dum = Matrix::Zero(); - //suppress gradients of reference point parameters when fitting with gen constraint - for (unsigned int j=0; j du = Matrix::Zero(); - for (unsigned int j=0; j dup = Matrix::Zero(); - for (unsigned int j=0; j Halphalamphim = Hm.block<2,2>(1, 1).cast(); -// Matrix Halphaum = Hm.block<2,2>(1, 3).cast(); - -// Matrix Halphalamphip = Hp.block<2,2>(1, 1).cast(); -// Matrix Halphaup = Hp.block<2,2>(1, 3).cast(); - - const Matrix dalpha0 = dx0.segment<2>(1).cast(); - - const Matrix dlamphim = Sinvm*(dum - Jm*du - Dm*dqopm - Bm*dbeta); - const Matrix dlamphip = Sinvp*(dup - Jp*du - Dp*dqop - Bp*dbetap); - -// const Matrix dalpham = Halphalamphim*dlamphim + Halphaum*du; -// const Matrix dalphap = Halphalamphip*dlamphip + Halphaup*du; - - const Matrix dalpham = dlamphim; - const Matrix dalphap = dlamphip; - - const MSScalar deloss0(dx0[0]); - - const Matrix dms = dalpha0 + dalphap - dalpham; - const MSScalar chisqms = dms.transpose()*Qinvms*dms; - //energy loss term - - - const MSScalar deloss = deloss0 + dqop - Eqop*dqopm - (Ealpha*dalpham)[0] - dE*dxi; -// const MSScalar deloss = deloss0 + dqop - dqopm - dE*dxi; - const MSScalar chisqeloss = deloss*invSigmaE*deloss; - - const MSScalar chisq = chisqms + chisqeloss; - - chisq0val += chisq.value().value(); - - auto const& gradlocal = chisq.value().derivatives(); - //fill local hessian - Matrix hesslocal; - for (unsigned int j=0; j localsizes = {{ nlocalstate, nlocalparms }}; - constexpr std::array localidxs = {{ localstateidx, localparmidx }}; - const std::array fullidxs = {{ fullstateidx, fullparmidx }}; - - for (unsigned int iidx = 0; iidx < localidxs.size(); ++iidx) { - gradfull.segment(fullidxs[iidx], localsizes[iidx]) += gradlocal.segment(localidxs[iidx], localsizes[iidx]); - for (unsigned int jidx = 0; jidx < localidxs.size(); ++jidx) { - hessfull.block(fullidxs[iidx], fullidxs[jidx], localsizes[iidx], localsizes[jidx]) += hesslocal.block(localidxs[iidx], localidxs[jidx], localsizes[iidx], localsizes[jidx]); - } - } - - // //fill global gradient - // gradfull.segment(fullstateidx) += gradlocal.head(); - // gradfull.segment(fullparmidx) += gradlocal.segment(localparmidx); - // - // //fill global hessian (upper triangular blocks only) - // hessfull.block(fullstateidx, fullstateidx) += hesslocal.topLeftCorner(); - // hessfull.block(fullstateidx, fullparmidx) += hesslocal.topRightCorner(); - // hessfull.block(fullparmidx, fullparmidx) += hesslocal.bottomRightCorner(); - - // std::cout << "intermediate hit, parm idx = " << parmidx << std::endl; - - } - - const unsigned int bfieldglobalidx = detidparms.at(std::make_pair(6, parmdetid)); - globalidxv[parmidx] = bfieldglobalidx; - parmidx++; - - const unsigned int elossglobalidx = detidparms.at(std::make_pair(7, parmdetid)); - globalidxv[parmidx] = elossglobalidx; - parmidx++; - - //backwards propagation jacobian (local to local) to be used at the next layer -// FdFm = curv2curvTransportJacobian(*updtsos.freeState(), propresult, true); - FdFm = localTransportJacobian(updtsos, propresult, true); - } - else { - // std::cout << "last hit, parm idx = " << parmidx << std::endl; - - // special case for last hit, assume zero scattering angle and nominal energy loss on last layer - Matrix J = FdFm.block<2, 2>(3, 3); - // (du/dalphap)^-1 - Matrix Sinv = FdFm.block<2, 2>(3, 1).inverse(); - // du/dqopp - Matrix D = FdFm.block<2, 1>(3, 0); - // du/dBp - Matrix Bstate = FdFm.block<2, 1>(3, 5); - - const unsigned int jacstateidxout = 5*(ihit+1); - const unsigned int jacstateidxin = trackstateidx + 1 + 3*ihit; - - // qop_i - //FIXME this should account for the energy loss, but only needed for outer momentum estimate which is not currently used - jac(jacstateidxout, jacstateidxin - 1) = 1.; - - // dalpha_i/dqop_i - jac.block<2, 1>(jacstateidxout + 1, jacstateidxin - 1) = -Sinv*D; - // dalpha_i/du_i - jac.block<2, 2>(jacstateidxout + 1, jacstateidxin) = -Sinv*J; - // dalpha_i/du_(i-1) - jac.block<2, 2>(jacstateidxout + 1, jacstateidxin - 3) = Sinv; - // d(lambda, phi) / dbeta - jac.block<2, 1>(jacstateidxout + 1, nstateparms + parmidx) = -Sinv*Bstate; - // xlocal_i - jac(jacstateidxout + 3, jacstateidxin) = 1.; - // ylocal_i - jac(jacstateidxout + 4, jacstateidxin + 1) = 1.; - - const unsigned int bfieldglobalidx = detidparms.at(std::make_pair(6, parmdetid)); - globalidxv[parmidx] = bfieldglobalidx; - parmidx++; - } - - if (preciseHit->isValid()) { - - auto fillAlignGrads = [&](auto Nalign) { - constexpr unsigned int nlocalstate = 2; - constexpr unsigned int localstateidx = 0; - constexpr unsigned int localalignmentidx = nlocalstate; - constexpr unsigned int localparmidx = localalignmentidx; - - // abusing implicit template argument to pass - // a template value via std::integral_constant - constexpr unsigned int nlocalalignment = Nalign(); - constexpr unsigned int nlocalparms = nlocalalignment; - constexpr unsigned int nlocal = nlocalstate + nlocalparms; - - // std::cout << "idx = " << id << ", ihit = " << ihit << ", alignmentparmidx = " << alignmentparmidx << ", nlocalalignment = " << nlocalalignment << std::endl; - - using AlignScalar = AANT; - - const unsigned int fullstateidx = trackstateidx + 1 + 3*ihit; - const unsigned int fullparmidx = nstateparms + nparsBfield + nparsEloss + alignmentparmidx; - - const bool ispixel = GeomDetEnumerators::isTrackerPixel(preciseHit->det()->subDetector()); - - //TODO add hit validation stuff - //TODO add simhit stuff - -// Matrix Hu = Hp.bottomRightCorner<2,2>().cast(); - - Matrix dy0; - Matrix Vinv; - // rotation from module to strip coordinates - // Matrix R; - Matrix2d R; - if (preciseHit->dimension() == 1) { - dy0[0] = AlignScalar(preciseHit->localPosition().x() - updtsos.localPosition().x()); - dy0[1] = AlignScalar(0.); - - // bool simvalid = false; - // for (auto const& simhith : simHits) { - // for (const PSimHit& simHit : *simhith) { - // if (simHit.detUnitId() == preciseHit->geographicalId()) { - // - // dy0[0] = AlignScalar(simHit.localPosition().x() - updtsos.localPosition().x()); - // - // simvalid = true; - // break; - // } - // } - // if (simvalid) { - // break; - // } - // } - - Vinv = Matrix::Zero(); - Vinv(0,0) = 1./preciseHit->localPositionError().xx(); - - // R = Matrix::Identity(); - R = Matrix2d::Identity(); - } - else { - // 2d hit - Matrix2d iV; - iV << preciseHit->localPositionError().xx(), preciseHit->localPositionError().xy(), - preciseHit->localPositionError().xy(), preciseHit->localPositionError().yy(); - if (ispixel) { - //take 2d hit as-is for pixels - dy0[0] = AlignScalar(preciseHit->localPosition().x() - updtsos.localPosition().x()); - dy0[1] = AlignScalar(preciseHit->localPosition().y() - updtsos.localPosition().y()); - - Vinv = iV.inverse().cast(); - //FIXME various temporary hacks; - - // dy0[1] = AlignScalar(0.); - // Vinv = Matrix::Zero(); - // Vinv(0,0) = 1./preciseHit->localPositionError().xx(); - - // if (GeomDetEnumerators::isEndcap(preciseHit->det()->subDetector())) { - // if (GeomDetEnumerators::isBarrel(preciseHit->det()->subDetector())) { - // PXBDetId detidtest(preciseHit->det()->geographicalId()); - // int layertest = detidtest.layer(); - // - // if (layertest > 1) { - // Vinv = Matrix::Zero(); - // } - // - // // Vinv = Matrix::Zero(); - // // dy0[0] = AlignScalar(0.); - // // dy0[1] = AlignScalar(0.); - // } - - // bool simvalid = false; - // for (auto const& simhith : simHits) { - // for (const PSimHit& simHit : *simhith) { - // if (simHit.detUnitId() == preciseHit->geographicalId()) { - // - // if (GeomDetEnumerators::isBarrel(preciseHit->det()->subDetector())) { - // dy0[0] = AlignScalar(simHit.localPosition().x() - updtsos.localPosition().x()); - // dy0[1] = AlignScalar(simHit.localPosition().y() - updtsos.localPosition().y()); - // } - // - // // dy0[1] = AlignScalar(0.); - // - // simvalid = true; - // break; - // } - // } - // if (simvalid) { - // break; - // } - // } - - - // R = Matrix::Identity(); - R = Matrix2d::Identity(); - } - else { - // diagonalize and take only smallest eigenvalue for 2d hits in strip wedge modules, - // since the constraint parallel to the strip is spurious - SelfAdjointEigenSolver eigensolver(iV); - // const Matrix2d& v = eigensolver.eigenvectors(); - R = eigensolver.eigenvectors().transpose(); - if (R(0,0) < 0.) { - R.row(0) *= -1.; - } - if (R(1,1) <0.) { - R.row(1) *= -1.; - } - - Matrix dy0local; - dy0local[0] = preciseHit->localPosition().x() - updtsos.localPosition().x(); - dy0local[1] = preciseHit->localPosition().y() - updtsos.localPosition().y(); - - // bool simvalid = false; - // for (auto const& simhith : simHits) { - // for (const PSimHit& simHit : *simhith) { - // if (simHit.detUnitId() == preciseHit->geographicalId()) { - // - // dy0local[0] = simHit.localPosition().x() - updtsos.localPosition().x(); - // dy0local[1] = simHit.localPosition().y() - updtsos.localPosition().y(); - // - // simvalid = true; - // break; - // } - // } - // if (simvalid) { - // break; - // } - // } - - const Matrix dy0eig = R*dy0local; - - //TODO deal properly with rotations (rotate back to module local coords?) - dy0[0] = AlignScalar(dy0eig[0]); - dy0[1] = AlignScalar(0.); - - Vinv = Matrix::Zero(); - Vinv(0,0) = AlignScalar(1./eigensolver.eigenvalues()[0]); - - // R = v.transpose().cast(); - - } - } - - // rxfull.row(ivalidhit) = R.row(0).cast(); - // ryfull.row(ivalidhit) = R.row(1).cast(); - - // validdxeigjac.block<2,2>(2*ivalidhit, 3*(ihit+1)) = R*Hp.bottomRightCorner<2,2>(); - - const Matrix Ralign = R.cast(); - - Matrix dx = Matrix::Zero(); - for (unsigned int j=0; j dalpha = Matrix::Zero(); - // order in which to use parameters, especially relevant in case nlocalalignment < 6 - constexpr std::array alphaidxs = {{5, 0, 1, 2, 3, 4}}; - for (unsigned int idim=0; idim A = Matrix::Zero(); - - - // dx/dx - A(0,0) = AlignScalar(1.); - // dy/dy - A(1,1) = AlignScalar(1.); - // dx/dz - A(0,2) = updtsos.localParameters().dxdz(); - // dy/dz - A(1,2) = updtsos.localParameters().dydz(); - // dx/dtheta_x - A(0,3) = -updtsos.localPosition().y()*updtsos.localParameters().dxdz(); - // dy/dtheta_x - A(1,3) = -updtsos.localPosition().y()*updtsos.localParameters().dydz(); - // dx/dtheta_y - A(0,4) = -updtsos.localPosition().x()*updtsos.localParameters().dxdz(); - // dy/dtheta_y - A(1,4) = -updtsos.localPosition().x()*updtsos.localParameters().dydz(); - // dx/dtheta_z - A(0,5) = -updtsos.localPosition().y(); - // dy/dtheta_z - A(1,5) = updtsos.localPosition().x(); - - - // std::cout << "strip local z shift gradient: " << (Ralign*A.col(2))[0].value().value() << std::endl; - - // rotation from alignment basis to module local coordinates - // Matrix A; - // if (isglued) { - // const GlobalVector modx = preciseHit->det()->surface().toGlobal(LocalVector(1.,0.,0.)); - // const GlobalVector mody = preciseHit->det()->surface().toGlobal(LocalVector(0.,1.,0.)); - // - // const GlobalVector gluedx = parmDet->surface().toGlobal(LocalVector(1.,0.,0.)); - // const GlobalVector gluedy = parmDet->surface().toGlobal(LocalVector(0.,1.,0.)); - // - // A(0,0) = AlignScalar(modx.dot(gluedx)); - // A(0,1) = AlignScalar(modx.dot(gluedy)); - // A(1,0) = AlignScalar(mody.dot(gluedx)); - // A(1,1) = AlignScalar(mody.dot(gluedy)); - // } - // else { - // A = Matrix::Identity(); - // } - // - // Matrix dh = dy0 - R*Hu*dx - R*A*dalpha; - - - double thetaincidence = std::asin(1./std::sqrt(std::pow(updtsos.localParameters().dxdz(),2) + std::pow(updtsos.localParameters().dydz(),2) + 1.)); - - // bool morehitquality = applyHitQuality_ ? thetaincidence > 0.25 : true; - bool morehitquality = true; - - if (morehitquality) { - nValidHitsFinal++; - if (ispixel) { - nValidPixelHitsFinal++; - } - } - else { - Vinv = Matrix::Zero(); - } - -// Matrix dh = dy0 - Ralign*Hu*dx - Ralign*A*dalpha; - Matrix dh = dy0 - Ralign*dx - Ralign*A*dalpha; - AlignScalar chisq = dh.transpose()*Vinv*dh; - - chisq0val += chisq.value().value(); - - auto const& gradlocal = chisq.value().derivatives(); - //fill local hessian - Matrix hesslocal; - for (unsigned int j=0; j localsizes = {{ nlocalstate, nlocalparms }}; - constexpr std::array localidxs = {{ localstateidx, localparmidx }}; - const std::array fullidxs = {{ fullstateidx, fullparmidx }}; - - for (unsigned int iidx = 0; iidx < localidxs.size(); ++iidx) { - gradfull.segment(fullidxs[iidx], localsizes[iidx]) += gradlocal.segment(localidxs[iidx], localsizes[iidx]); - for (unsigned int jidx = 0; jidx < localidxs.size(); ++jidx) { - hessfull.block(fullidxs[iidx], fullidxs[jidx], localsizes[iidx], localsizes[jidx]) += hesslocal.block(localidxs[iidx], localidxs[jidx], localsizes[iidx], localsizes[jidx]); - } - } - - // Matrix gradloctest0; - // Matrix gradloctest1; - // Matrix gradloctest2; - - // std::cout << "nlocalalignment: " << nlocalalignment << " nlocal: " << nlocal << std::endl; - // std::cout << "gradlocal type: " << typeid(gradlocal).name() << std::endl; - // std::cout << "gradloctest0 type: " << typeid(gradloctest0).name() << std::endl; - // std::cout << "gradloctest1 type: " << typeid(gradloctest1).name() << std::endl; - // std::cout << "gradloctest2 type: " << typeid(gradloctest2).name() << std::endl; - // - // std::cout << "nhits: " << nhits << " nvalid: " << nvalid << " nvalidalign2d: " << nvalidalign2d << " ihit: " << ihit << std::endl; - // std::cout << "gradfull.size(): " << gradfull.size() << " nlocalstate: " << nlocalstate << " fullstateidx: " << fullstateidx << " nlocalparms: " << nlocalparms << " fullparmidx: " << fullparmidx << std::endl; - - // FIXME the templated block functions don't work here for some reason - //fill global gradient - // gradfull.segment(fullstateidx) += gradlocal.head(nlocalstate); - // gradfull.segment(fullparmidx) += gradlocal.segment(localparmidx, nlocalparms); - // - // //fill global hessian (upper triangular blocks only) - // hessfull.block(fullstateidx, fullstateidx) += hesslocal.topLeftCorner(nlocalstate,nlocalstate); - // hessfull.block(fullstateidx, fullparmidx) += hesslocal.topRightCorner(nlocalstate, nlocalparms); - // hessfull.block(fullparmidx, fullparmidx) += hesslocal.bottomRightCorner(nlocalparms, nlocalparms); - - for (unsigned int idim=0; idimgeographicalId())); - globalidxv[nparsBfield + nparsEloss + alignmentparmidx] = xglobalidx; - alignmentparmidx++; - if (alphaidxs[idim]==0) { - hitidxv.push_back(xglobalidx); - } - } - }; - - - if (align2d) { - fillAlignGrads(std::integral_constant()); - } - else { - fillAlignGrads(std::integral_constant()); - } - - } - - } - - if (!valid) { - break; - } - - trackstateidx += 3*tracknhits; - } - - if (!valid) { - break; - } - - // MatrixXd massjac; - - // add mass constraint to gbl fit - if (doMassConstraint_) { - // if (false) { - constexpr unsigned int nvtxstate = 6; - constexpr unsigned int nlocalstate = 3; - constexpr unsigned int nlocalparms0 = 1; - constexpr unsigned int nlocalparms1 = 1; - - constexpr unsigned int nlocal = nvtxstate + nlocalstate + nlocalparms0 + nlocalparms1; - - constexpr unsigned int localvtxidx = 0; - constexpr unsigned int localstateidx = localvtxidx + nvtxstate; - constexpr unsigned int localparmidx0 = localstateidx + nlocalstate; - constexpr unsigned int localparmidx1 = localparmidx0 + nlocalparms0; - - constexpr unsigned int fullvtxidx = 0; - const unsigned int fullstateidx = trackstateidxarr[1]; - const unsigned int fullparmidx0 = nstateparms + trackparmidxarr[0]; - const unsigned int fullparmidx1 = nstateparms + trackparmidxarr[1]; - - using MScalar = AANT; - - //TODO optimize to avoid recomputation of FTS - // const FreeTrajectoryState refFts0 = outparts[0]->currentState().freeTrajectoryState(); - // const FreeTrajectoryState refFts1 = outparts[1]->currentState().freeTrajectoryState(); - - const FreeTrajectoryState &refFts0 = refftsarr[0]; - const FreeTrajectoryState &refFts1 = refftsarr[1]; - - const ROOT::Math::PxPyPzMVector mom0(refFts0.momentum().x(), - refFts0.momentum().y(), - refFts0.momentum().z(), - mmu); - - const ROOT::Math::PxPyPzMVector mom1(refFts1.momentum().x(), - refFts1.momentum().y(), - refFts1.momentum().z(), - mmu); - - const double massval = (mom0 + mom1).mass(); - - const Matrix &FdFpref0 = FdFprefarr[0]; - const Matrix &FdFpref1 = FdFprefarr[1]; - - JacobianCurvilinearToCartesian curv2cartref0(refFts0.parameters()); - auto const &jacCurv2Cartref0 = Map>(curv2cartref0.jacobian().Array()); - - JacobianCurvilinearToCartesian curv2cartref1(refFts1.parameters()); - auto const &jacCurv2Cartref1 = Map>(curv2cartref1.jacobian().Array()); - - // const Matrix cartjac0 = cartesianToCartesianJacobian(refFts0); - // const Matrix cartjac1 = cartesianToCartesianJacobian(refFts1); - - const Matrix m2jac = massJacobian(refFts0, refFts1, mmu); - - const Matrix dudvtx0_0 = FdFpref0.block<2, 3>(3, 3).cast(); - const Matrix dudalph0inv_0 = FdFpref0.block<2, 2>(3, 1).inverse().cast(); - const Matrix dudqop0_0 = FdFpref0.block<2, 1>(3, 0).cast(); - const Matrix dudB_0 = FdFpref0.block<2, 1>(3, 6).cast(); - - const Matrix dudvtx0_1 = FdFpref1.block<2, 3>(3, 3).cast(); - const Matrix dudalph0inv_1 = FdFpref1.block<2, 2>(3, 1).inverse().cast(); - const Matrix dudqop0_1 = FdFpref1.block<2, 1>(3, 0).cast(); - const Matrix dudB_1 = FdFpref1.block<2, 1>(3, 6).cast(); - - // massjac = (m2jac.leftCols<3>()*(jacCurv2Cartref0*jacarr[0]).bottomRows<3>() + m2jac.rightCols<3>()*(jacCurv2Cartref1*jacarr[1]).bottomRows<3>()).leftCols(nstateparms); - - // initialize active scalars for common vertex parameters - Matrix dvtx = Matrix::Zero(); - for (unsigned int j=0; j du0 = Matrix::Zero(); - for (unsigned int j=0; j du1 = Matrix::Zero(); - for (unsigned int j=0; j dlamphi0 = dudalph0inv_0*(du0 - dudvtx0_0*dvtx - dudqop0_0*dqop0 - dudB_0*dbeta0); - const Matrix dlamphi1 = dudalph0inv_1*(du1 - dudvtx0_1*dvtx - dudqop0_1*dqop1 - dudB_1*dbeta1); - - const Matrix dmom0 = jacCurv2Cartref0.block<3, 1>(3, 0).cast()*dqop0 + jacCurv2Cartref0.block<3, 2>(3, 1).cast()*dlamphi0; - const Matrix dmom1 = jacCurv2Cartref1.block<3, 1>(3, 0).cast()*dqop1 + jacCurv2Cartref1.block<3, 2>(3, 1).cast()*dlamphi1; - - // resonance width - // const MScalar invSigmaMsq(0.25/massConstraint_/massConstraint_/massConstraintWidth_/massConstraintWidth_); - // const MScalar dmsq0 = MScalar(m0*m0 - massConstraint_*massConstraint_); - - const MScalar invSigmaMsq(1./massConstraintWidth_/massConstraintWidth_); - const MScalar dmsq0 = MScalar(massval - massconstraintval); -// const MScalar dmsq0 = MScalar(massval - massConstraint_); -// const MScalar dmsq0 = MScalar(0.); - // const MScalar dmsq0 = MScalar(m0 - massConstraint_); - - - // std::cout << "invSigmaMsq = " << invSigmaMsq.value().value() << std::endl; - - const MScalar dmsqtrack0 = (m2jac.leftCols<3>().cast()*dmom0)[0]; - const MScalar dmsqtrack1 = (m2jac.rightCols<3>().cast()*dmom1)[0]; - - const MScalar dmsq = dmsq0 + dmsqtrack0 + dmsqtrack1; - - // const MScalar chisq = dmsq*invSigmaMsq*dmsq; - const MScalar chisq = invSigmaMsq*dmsq*dmsq; - - chisq0val += chisq.value().value(); - - auto const& gradlocal = chisq.value().derivatives(); - //fill local hessian - Matrix hesslocal; - for (unsigned int j=0; j localsizes = {{ nvtxstate, nlocalstate, nlocalparms0, nlocalparms1 }}; - constexpr std::array localidxs = {{ localvtxidx, localstateidx, localparmidx0, localparmidx1 }}; - const std::array fullidxs = {{ fullvtxidx, fullstateidx, fullparmidx0, fullparmidx1 }}; - - for (unsigned int iidx = 0; iidx < localidxs.size(); ++iidx) { - gradfull.segment(fullidxs[iidx], localsizes[iidx]) += gradlocal.segment(localidxs[iidx], localsizes[iidx]); - for (unsigned int jidx = 0; jidx < localidxs.size(); ++jidx) { - hessfull.block(fullidxs[iidx], fullidxs[jidx], localsizes[iidx], localsizes[jidx]) += hesslocal.block(localidxs[iidx], localidxs[jidx], localsizes[iidx], localsizes[jidx]); - } - } - - // //Fill global grad and hess (upper triangular blocks only) - // gradfull.segment(fullvtxidx) += gradlocal.segment(localvtxidx); - // gradfull.segment(fullstateidx) += gradlocal.segment(localstateidx); - // gradfull.segment(fullparmidx0) += gradlocal.segment(localparmidx0); - // gradfull.segment(fullparmidx1) += gradlocal.segment(localparmidx1); - // - // hessfull.block(fullvtxidx, fullvtxidx) += hesslocal.block(localvtxidx, localvtxidx); - // hessfull.block(fullvtxidx, fullstateidx) += hesslocal.block(localvtxidx, localstateidx); - // hessfull.block(fullvtxidx, fullparmidx0) += hesslocal.block(localvtxidx, localparmidx0); - // hessfull.block(fullvtxidx, fullparmidx1) += hesslocal.block(localvtxidx, localparmidx1); - // - // hessfull.block(fullstateidx, fullstateidx) += hesslocal.block(localstateidx, localstateidx); - // hessfull.block(fullstateidx, fullparmidx0) += hesslocal.block(localstateidx, localparmidx0); - // hessfull.block(fullstateidx, fullparmidx1) += hesslocal.block(localstateidx, localparmidx1); - // - // hessfull.block(fullparmidx0, fullparmidx0) += hesslocal.block(localparmidx0, localparmidx0); - // hessfull.block(fullparmidx0, fullparmidx1) += hesslocal.block(localparmidx0, localparmidx1); - // - // hessfull.block(fullparmidx1, fullparmidx1) += hesslocal.block(localparmidx1, localparmidx1); - // - // //extra part - // hessfull.block(fullstateidx, fullvtxidx) += hesslocal.block(localvtxidx, localstateidx).transpose(); - - } - - - - // std::cout << nhits << std::endl; - // std::cout << nvalid << std::endl; - // std::cout << nvalidalign2d << std::endl; - // std::cout << nparsAlignment << std::endl; - // std::cout << alignmentparmidx << std::endl; - // - // std::cout << nparsBfield << std::endl; - // std::cout << nparsEloss << std::endl; - // std::cout << parmidx << std::endl; - - assert(trackstateidx == nstateparms); - assert(parmidx == (nparsBfield + nparsEloss)); - assert(alignmentparmidx == nparsAlignment); - - // if (nhits != nvalid) { - // continue; - // } - - auto freezeparm = [&](unsigned int idx) { - gradfull[idx] = 0.; - hessfull.row(idx) *= 0.; - hessfull.col(idx) *= 0.; - hessfull(idx,idx) = 1e6; - }; - - if (fitFromGenParms_) { - for (unsigned int i=0; i<3; ++i) { - freezeparm(i); - } - for (unsigned int id = 0; id < 2; ++id) { - freezeparm(nstateparms + trackparmidxarr[id]); - for (unsigned int i=0; i<3; ++i) { - freezeparm(trackstateidxarr[id] + i); - } - } - } - - //now do the expensive calculations and fill outputs - - //symmetrize the matrix (previous block operations do not guarantee that the needed blocks are filled) - //TODO handle this more efficiently? - // hessfull.triangularView() = hessfull.triangularView().transpose(); - - // for (unsigned int i=0; i<3; ++i) { - // gradfull[i] = 0.; - // hessfull.row(i) *= 0.; - // hessfull.col(i) *= 0.; - // hessfull(i,i) = 1e6; - // } - - // for (auto trackstateidx : trackstateidxarr) { - // for (unsigned int i = trackstateidx; i < (trackstateidx + 1); ++i) { - // gradfull[i] = 0.; - // hessfull.row(i) *= 0.; - // hessfull.col(i) *= 0.; - // hessfull(i,i) = 1e6; - // } - // } - - // { - // unsigned int i = trackstateidxarr[1]; - // gradfull[i] = 0.; - // hessfull.row(i) *= 0.; - // hessfull.col(i) *= 0.; - // hessfull(i,i) = 1e6; - // } - // - - // std::cout << "gradfull:" << std::endl; - // std::cout << gradfull << std::endl; - // - // std::cout << "gradfull.head(nstateparms):" << std::endl; - // std::cout << gradfull.head(nstateparms) << std::endl; - // - // std::cout << "gradfull.tail(npars):" << std::endl; - // std::cout << gradfull.tail(npars) << std::endl; - // - // std::cout << "hessfull.diagonal():" << std::endl; - // std::cout << hessfull.diagonal() << std::endl; - - auto const& dchisqdx = gradfull.head(nstateparms); - auto const& dchisqdparms = gradfull.tail(npars); - - auto const& d2chisqdx2 = hessfull.topLeftCorner(nstateparms, nstateparms); - auto const& d2chisqdxdparms = hessfull.topRightCorner(nstateparms, npars); - auto const& d2chisqdparms2 = hessfull.bottomRightCorner(npars, npars); - - - - Cinvd.compute(d2chisqdx2); - - dxfull = -Cinvd.solve(dchisqdx); - dxdparms = -Cinvd.solve(d2chisqdxdparms).transpose(); - - for (unsigned int id = 0; id < 2; ++id) { - dxstatearr[id] = jacarr[id].leftCols(nstateparms)*dxfull; - } - -// dxdparms = -Cinvd.solve(d2chisqdxdparms).transpose(); - - // if (debugprintout_) { - // std::cout << "dxrefdparms" << std::endl; - // std::cout << dxdparms.leftCols<5>() << std::endl; - // } - -// grad = dchisqdparms + dxdparms*dchisqdx; - //TODO check the simplification - // hess = d2chisqdparms2 + 2.*dxdparms*d2chisqdxdparms + dxdparms*d2chisqdx2*dxdparms.transpose(); -// hess = d2chisqdparms2 + dxdparms*d2chisqdxdparms; - - const Matrix deltachisq = dchisqdx.transpose()*dxfull + 0.5*dxfull.transpose()*d2chisqdx2*dxfull; - -// std::cout << "iiter = " << iiter << ", deltachisq = " << deltachisq[0] << std::endl; -// -// SelfAdjointEigenSolver es(d2chisqdx2, EigenvaluesOnly); -// const double condition = es.eigenvalues()[nstateparms-1]/es.eigenvalues()[0]; -// std::cout << "eigenvalues:" << std::endl; -// std::cout << es.eigenvalues().transpose() << std::endl; -// std::cout << "condition: " << condition << std::endl; - - chisqval = chisq0val + deltachisq[0]; - - ndof = 3*(nhits - 2) + nvalid + nvalidalign2d - nstateparms; - - if (doMassConstraint_) { - ++ndof; - } - - // std::cout << "dchisqdparms.head<6>()" << std::endl; - // std::cout << dchisqdparms.head<6>() << std::endl; - // - // std::cout << "grad.head<6>()" << std::endl; - // std::cout << grad.head<6>() << std::endl; - // - // std::cout << "d2chisqdparms2.topLeftCorner<6, 6>():" << std::endl; - // std::cout << d2chisqdparms2.topLeftCorner<6, 6>() << std::endl; - // std::cout << "hess.topLeftCorner<6, 6>():" << std::endl; - // std::cout << hess.topLeftCorner<6, 6>() << std::endl; - // - // std::cout << "dchisqdparms.segment<6>(nparsBfield+nparsEloss)" << std::endl; - // std::cout << dchisqdparms.segment<6>(nparsBfield+nparsEloss) << std::endl; - // - // std::cout << "grad.segment<6>(nparsBfield+nparsEloss)" << std::endl; - // std::cout << grad.segment<6>(nparsBfield+nparsEloss) << std::endl; - // - // std::cout << "d2chisqdparms2.block<6, 6>(nparsBfield+nparsEloss, nparsBfield+nparsEloss):" << std::endl; - // std::cout << d2chisqdparms2.block<6, 6>(nparsBfield+nparsEloss, nparsBfield+nparsEloss) << std::endl; - // std::cout << "hess.block<6, 6>(nparsBfield+nparsEloss, nparsBfield+nparsEloss):" << std::endl; - // std::cout << hess.block<6, 6>(nparsBfield+nparsEloss, nparsBfield+nparsEloss) << std::endl; - // // - // - // std::cout << "d2chisqdparms2.block<6, 6>(trackparmidxarr[1], trackparmidxarr[1]):" << std::endl; - // std::cout << d2chisqdparms2.block<6, 6>(trackparmidxarr[1], trackparmidxarr[1]) << std::endl; - // std::cout << "hess.block<6, 6>(trackparmidxarr[1], trackparmidxarr[1]):" << std::endl; - // std::cout << hess.block<6, 6>(trackparmidxarr[1], trackparmidxarr[1]) << std::endl; - // - // std::cout << "d2chisqdparms2.bottomRightCorner<6, 6>():" << std::endl; - // std::cout << d2chisqdparms2.bottomRightCorner<6, 6>() << std::endl; - // std::cout << "hess.bottomRightCorner<6, 6>():" << std::endl; - // std::cout << hess.bottomRightCorner<6, 6>() << std::endl; - - // const double - // // const double corxi0plusminus = hess(1, trackparmidxarr[1] + 1)/std::sqrt(hess(1,1)*hess(trackparmidxarr[1] + 1, trackparmidxarr[1] + 1)); - // // const double corxi1plusminus = hess(3, trackparmidxarr[1] + 3)/std::sqrt(hess(3,3)*hess(trackparmidxarr[1] + 3, trackparmidxarr[1] + 3)); - // - // const double cor01plus = hess(1, 3)/std::sqrt(hess(1, 1)*hess(3, 3)); - // // const double cor01minus = hess(trackparmidxarr[1] + 1, trackparmidxarr[1] + 3)/std::sqrt(hess(trackparmidxarr[1] + 1, trackparmidxarr[1] + 1)*hess(trackparmidxarr[1] + 3, trackparmidxarr[1] + 3)); - // - // const double cor12plus = hess(3, 5)/std::sqrt(hess(3, 3)*hess(5, 5)); - // // const double cor12minus = hess(trackparmidxarr[1] + 3, trackparmidxarr[1] + 5)/std::sqrt(hess(trackparmidxarr[1] + 3, trackparmidxarr[1] + 3)*hess(trackparmidxarr[1] + 5, trackparmidxarr[1] + 5)); - // - // // std::cout << "corxi0plusminus = " << corxi0plusminus << std::endl; - // // std::cout << "corxi1plusminus = " << corxi1plusminus << std::endl; - // std::cout << "cor01plus = " << cor01plus << std::endl; - // // std::cout << "cor01minus = " << cor01minus << std::endl; - // std::cout << "cor12plus = " << cor12plus << std::endl; - // // std::cout << "cor12minus = " << cor12minus << std::endl; - - // std::cout << "hess(1, 1)" << std::endl; - // std::cout << hess(1, 1) << std::endl; - // std::cout << "hess(trackparmidxarr[1] + 1, trackparmidxarr[1] + 1)" << std::endl; - // std::cout << hess(trackparmidxarr[1] + 1, trackparmidxarr[1] + 1) << std::endl; - // std::cout << "hess(1, trackparmidxarr[1] + 1)" << std::endl; - // std::cout << hess(1, trackparmidxarr[1] + 1) << std::endl; - - // compute final kinematics - - kinTree->movePointerToTheTop(); - RefCountedKinematicVertex dimu_vertex = kinTree->currentDecayVertex(); - - Jpsikin_x = dimu_vertex->position().x(); - Jpsikin_y = dimu_vertex->position().y(); - Jpsikin_z = dimu_vertex->position().z(); - - // apply the GBL fit results to the vertex position - Vector3d vtxpos; - vtxpos << dimu_vertex->position().x(), - dimu_vertex->position().y(), - dimu_vertex->position().z(); - - vtxpos += dxfull.head<3>(); - - Jpsi_x = vtxpos[0]; - Jpsi_y = vtxpos[1]; - Jpsi_z = vtxpos[2]; - - std::array muarr; - std::array mucurvarr; - std::array muchargearr; - - // std::cout << dimu_vertex->position() << std::endl; - - // apply the GBL fit results to the muon kinematics - for (unsigned int id = 0; id < 2; ++id) { - // auto const &refFts = outparts[id]->currentState().freeTrajectoryState(); - auto const &refFts = refftsarr[id]; - auto const &jac = jacarr[id]; - - // std::cout << refFts.position() << std::endl; - - JacobianCurvilinearToCartesian curv2cart(refFts.parameters()); - const AlgebraicMatrix65& jaccurv2cart = curv2cart.jacobian(); - const AlgebraicVector6 glob = refFts.parameters().vector(); - - // const Matrix dxcurv = jac.leftCols(nstateparms)*dxfull; - const Matrix dxcurv = jac.topLeftCorner(5, nstateparms)*dxfull; - - // const Matrix dxcurv = Matrix::Zero(); - - // std::cout << "id = " << id << ", dxcurv = " << dxcurv << std::endl; - - const Matrix globupd = Map>(glob.Array()) + Map>(jaccurv2cart.Array())*dxcurv; - - double charge = std::copysign(1.0, refFts.charge()/refFts.momentum().mag() + dxcurv[0]); - - muarr[id] = ROOT::Math::PxPyPzMVector(globupd[3], globupd[4], globupd[5], mmu); - muchargearr[id] = charge; - - auto &refParms = mucurvarr[id]; - CurvilinearTrajectoryParameters curvparms(refFts.position(), refFts.momentum(), refFts.charge()); - refParms << curvparms.Qbp(), curvparms.lambda(), curvparms.phi(), curvparms.xT(), curvparms.yT(); - refParms += dxcurv; - - } - - if ( (muchargearr[0] + muchargearr[1]) != 0) { - continue; - } - - const unsigned int idxplus = muchargearr[0] > 0 ? 0 : 1; - const unsigned int idxminus = muchargearr[0] > 0 ? 1 : 0; - - Muplus_pt = muarr[idxplus].pt(); - Muplus_eta = muarr[idxplus].eta(); - Muplus_phi = muarr[idxplus].phi(); - - Muminus_pt = muarr[idxminus].pt(); - Muminus_eta = muarr[idxminus].eta(); - Muminus_phi = muarr[idxminus].phi(); - - Mupluskin_pt = outparts[idxplus]->currentState().globalMomentum().perp(); - Mupluskin_eta = outparts[idxplus]->currentState().globalMomentum().eta(); - Mupluskin_phi = outparts[idxplus]->currentState().globalMomentum().phi(); - - Muminuskin_pt = outparts[idxminus]->currentState().globalMomentum().perp(); - Muminuskin_eta = outparts[idxminus]->currentState().globalMomentum().eta(); - Muminuskin_phi = outparts[idxminus]->currentState().globalMomentum().phi(); - - // std::cout << "Muplus pt, eta, phi = " << Muplus_pt << ", " << Muplus_eta << ", " << Muplus_phi << std::endl; - // std::cout << "Muminus pt, eta, phi = " << Muminus_pt << ", " << Muminus_eta << ", " << Muminus_phi << std::endl; - - Map>(Muplus_refParms.data()) = mucurvarr[idxplus].cast(); - Map>(MuMinus_refParms.data()) = mucurvarr[idxminus].cast(); - - Muplus_jacRef.resize(5*npars); - Map>(Muplus_jacRef.data(), 5, npars) = (jacarr[idxplus].topLeftCorner(5, nstateparms)*dxdparms.transpose() + jacarr[idxplus].topRightCorner(5, npars)).cast(); - - Muminus_jacRef.resize(5*npars); - Map>(Muminus_jacRef.data(), 5, npars) = (jacarr[idxminus].topLeftCorner(5, nstateparms)*dxdparms.transpose() + jacarr[idxminus].topRightCorner(5, npars)).cast(); - - auto const jpsimom = muarr[0] + muarr[1]; - - Jpsi_pt = jpsimom.pt(); - Jpsi_eta = jpsimom.eta(); - Jpsi_phi = jpsimom.phi(); - Jpsi_mass = jpsimom.mass(); - - Muplus_nhits = nhitsarr[idxplus]; - Muplus_nvalid = nvalidarr[idxplus]; - Muplus_nvalidpixel = nvalidpixelarr[idxplus]; - - Muminus_nhits = nhitsarr[idxminus]; - Muminus_nvalid = nvalidarr[idxminus]; - Muminus_nvalidpixel = nvalidpixelarr[idxminus]; - - const ROOT::Math::PxPyPzMVector mompluskin(outparts[idxplus]->currentState().globalMomentum().x(), - outparts[idxplus]->currentState().globalMomentum().y(), - outparts[idxplus]->currentState().globalMomentum().z(), - mmu); - - const ROOT::Math::PxPyPzMVector momminuskin(outparts[idxminus]->currentState().globalMomentum().x(), - outparts[idxminus]->currentState().globalMomentum().y(), - outparts[idxminus]->currentState().globalMomentum().z(), - mmu); - - auto const jpsimomkin = mompluskin + momminuskin; - - Jpsikin_pt = jpsimomkin.pt(); - Jpsikin_eta = jpsimomkin.eta(); - Jpsikin_phi = jpsimomkin.phi(); - Jpsikin_mass = jpsimomkin.mass(); - - const reco::GenParticle *muplusgen = nullptr; - const reco::GenParticle *muminusgen = nullptr; - - if (doGen_) { - for (auto const &genpart : *genPartCollection) { - if (genpart.status() != 1) { - continue; - } - if (std::abs(genpart.pdgId()) != 13) { - continue; - } - -// float dRplus = deltaR(genpart.phi(), muarr[idxplus].phi(), genpart.eta(), muarr[idxplus].eta()); - float dRplus = deltaR(genpart, muarr[idxplus]); - if (dRplus < 0.1 && genpart.charge() > 0) { - muplusgen = &genpart; - } - -// float dRminus = deltaR(genpart.phi(), muarr[idxminus].phi(), genpart.eta(), muarr[idxminus].eta()); - float dRminus = deltaR(genpart, muarr[idxminus]); - if (dRminus < 0.1 && genpart.charge() < 0) { - muminusgen = &genpart; - } - } - } - - if (muplusgen != nullptr) { - Muplusgen_pt = muplusgen->pt(); - Muplusgen_eta = muplusgen->eta(); - Muplusgen_phi = muplusgen->phi(); - } - else { - Muplusgen_pt = -99.; - Muplusgen_eta = -99.; - Muplusgen_phi = -99.; - } - - if (muminusgen != nullptr) { - Muminusgen_pt = muminusgen->pt(); - Muminusgen_eta = muminusgen->eta(); - Muminusgen_phi = muminusgen->phi(); - } - else { - Muminusgen_pt = -99.; - Muminusgen_eta = -99.; - Muminusgen_phi = -99.; - } - - if (muplusgen != nullptr && muminusgen != nullptr) { - auto const jpsigen = ROOT::Math::PtEtaPhiMVector(muplusgen->pt(), muplusgen->eta(), muplusgen->phi(), mmu) + - ROOT::Math::PtEtaPhiMVector(muminusgen->pt(), muminusgen->eta(), muminusgen->phi(), mmu); - - Jpsigen_pt = jpsigen.pt(); - Jpsigen_eta = jpsigen.eta(); - Jpsigen_phi = jpsigen.phi(); - Jpsigen_mass = jpsigen.mass(); - - Jpsigen_x = muplusgen->vx(); - Jpsigen_y = muplusgen->vy(); - Jpsigen_z = muplusgen->vz(); - } - else { - Jpsigen_pt = -99.; - Jpsigen_eta = -99.; - Jpsigen_phi = -99.; - Jpsigen_mass = -99.; - - Jpsigen_x = -99.; - Jpsigen_y = -99.; - Jpsigen_z = -99.; - } - - - // const Vector5d dxRef = dxfull.head<5>(); - // const Matrix5d Cinner = Cinvd.solve(MatrixXd::Identity(nstateparms,nstateparms)).topLeftCorner<5,5>(); - - - niter = iiter + 1; - edmval = -deltachisq[0]; - -// std::cout << "iiter = " << iiter << " edmval = " << edmval << std::endl; - - if (iiter > 1 && std::abs(deltachisq[0])<1e-3) { - break; - } - - } - - if (!valid) { - continue; - } - - auto const& dchisqdx = gradfull.head(nstateparms); - auto const& dchisqdparms = gradfull.tail(npars); - - auto const& d2chisqdx2 = hessfull.topLeftCorner(nstateparms, nstateparms); - auto const& d2chisqdxdparms = hessfull.topRightCorner(nstateparms, npars); - auto const& d2chisqdparms2 = hessfull.bottomRightCorner(npars, npars); - - - // if (debugprintout_) { - // std::cout << "dxrefdparms" << std::endl; - // std::cout << dxdparms.leftCols<5>() << std::endl; - // } - - grad = dchisqdparms + dxdparms*dchisqdx; - //TODO check the simplification - // hess = d2chisqdparms2 + 2.*dxdparms*d2chisqdxdparms + dxdparms*d2chisqdx2*dxdparms.transpose(); - hess = d2chisqdparms2 + dxdparms*d2chisqdxdparms; - -// for (unsigned int iparm = 0; iparm < npars; ++iparm) { -// if (detidparmsrev[globalidxv[iparm]].first != 7) { -// hess.row(iparm) *= 0.; -// hess.col(iparm) *= 0.; -// hess(iparm, iparm) = 1e6; -// } -// } - -// SelfAdjointEigenSolver es(hess, EigenvaluesOnly); -// const double condition = es.eigenvalues()[nstateparms-1]/es.eigenvalues()[0]; -// std::cout << "hess eigenvalues:" << std::endl; -// std::cout << es.eigenvalues().transpose() << std::endl; -// std::cout << "condition: " << condition << std::endl; - -// std::cout << "hess diagonal:" << std::endl; -// std::cout << hess.diagonal().transpose() << std::endl; -// -// assert(es.eigenvalues()[0] > -1e-5); -// assert(hess.diagonal().minCoeff() > 0.); - - nParms = npars; - - gradv.clear(); - gradv.resize(npars,0.); - - if (fillTrackTree_ && fillGrads_) { - tree->SetBranchAddress("gradv", gradv.data()); - } - - //eigen representation of the underlying vector storage - Map gradout(gradv.data(), npars); - - gradout = grad.cast(); - - - - gradmax = 0.; - for (unsigned int i=0; igradmax) { - gradmax = absval; - } - } - - - hessmax = 0.; - for (unsigned int i=0; ihessmax) { - hessmax = absval; - } - - } - - } - - //fill packed hessian and indices - const unsigned int nsym = npars*(1+npars)/2; - hesspackedv.clear(); - hesspackedv.resize(nsym, 0.); - - nSym = nsym; - if (fillTrackTree_ && fillGrads_) { - tree->SetBranchAddress("hesspackedv", hesspackedv.data()); - } - - Map hesspacked(hesspackedv.data(), nsym); - const Map globalidx(globalidxv.data(), npars); - - unsigned int packedidx = 0; - for (unsigned int ipar = 0; ipar < npars; ++ipar) { - const unsigned int segmentsize = npars - ipar; - hesspacked.segment(packedidx, segmentsize) = hess.block<1, Dynamic>(ipar, ipar, 1, segmentsize).cast(); - packedidx += segmentsize; - } - -// hessv.resize(npars*npars); -// Map>(hessv.data(), npars, npars) = hess.cast(); - - if (fillTrackTree_) { - tree->Fill(); - } - - - - - -// const Matrix3d covvtx = Cinvd.solve(MatrixXd::Identity(nstateparms,nstateparms)).topLeftCorner<3,3>(); -// -// const double covqop0 = Cinvd.solve(MatrixXd::Identity(nstateparms,nstateparms))(trackstateidxarr[0], trackstateidxarr[0]); -// const double covqop1 = Cinvd.solve(MatrixXd::Identity(nstateparms,nstateparms))(trackstateidxarr[1], trackstateidxarr[1]); -// -// const double covqop0kin = outparts[0]->currentState().freeTrajectoryState().curvilinearError().matrix()(0,0); -// const double covqop1kin = outparts[1]->currentState().freeTrajectoryState().curvilinearError().matrix()(0,0); -// -// // Matrix covmass = 2.*massjac*Cinvd.solve(MatrixXd::Identity(nstateparms,nstateparms))*massjac.transpose(); -// -// // const VectorXd cinvrow0 = Cinvd.solve(MatrixXd::Identity(nstateparms,nstateparms)).row(0).head(nstateparms); -// // -// std::cout << "kinfit covariance:" << std::endl; -// std::cout << dimu_vertex->error().matrix() << std::endl; -// -// std::cout << "GBL covariance:" << std::endl; -// std::cout << 2.*covvtx << std::endl; -// -// std::cout << "kinfit qop0 covariance:" << std::endl; -// std::cout << covqop0kin << std::endl; -// -// std::cout << "GBL qop0 covariance:" << std::endl; -// std::cout << 2.*covqop0 << std::endl; -// -// std::cout << "kinfit qop1 covariance:" << std::endl; -// std::cout << covqop1kin << std::endl; -// -// std::cout << "GBL qop1 covariance:" << std::endl; -// std::cout << 2.*covqop1 << std::endl; - -// std::cout << "dqop0 beamline" << std::endl; -// std::cout << dxfull[trackstateidxarr[0]] << std::endl; -// std::cout << "dqop0 first layer" << std::endl; -// std::cout << dxfull[trackstateidxarr[0]+3] << std::endl; -// std::cout << "dqop0 second layer" << std::endl; -// std::cout << dxfull[trackstateidxarr[0]+6] << std::endl; -// -// std::cout << "dqop1 beamline" << std::endl; -// std::cout << dxfull[trackstateidxarr[1]] << std::endl; -// std::cout << "dqop1 first layer" << std::endl; -// std::cout << dxfull[trackstateidxarr[1]+3] << std::endl; -// std::cout << "dqop1 second layer" << std::endl; -// std::cout << dxfull[trackstateidxarr[1]+6] << std::endl; -// -// std::cout << "sigmam" << std::endl; -// std::cout << std::sqrt(covmass[0]) << std::endl; - -// -// std::cout << "cinvrow0" << std::endl; -// std::cout << cinvrow0 << std::endl; - - //TODO restore statejac stuff -// dxstate = statejac*dxfull; -// const Vector5d dxRef = dxstate.head<5>(); -// const Matrix5d Cinner = (statejac*Cinvd.solve(MatrixXd::Identity(nstateparms,nstateparms))*statejac.transpose()).topLeftCorner<5,5>(); - - //TODO fill outputs - - } - } - -} - -Matrix ResidualGlobalCorrectionMakerTwoTrack::massJacobian(const FreeTrajectoryState &state0, const FreeTrajectoryState &state1, double dmass) const { - Matrix res = Matrix::Zero(); - - const double e0 = std::sqrt(state0.momentum().mag2() + dmass*dmass); - const double e1 = std::sqrt(state1.momentum().mag2() + dmass*dmass); - - // dm^2/dp0x - res(0, 0) = 2.*e1*state0.momentum().x()/e0 - 2.*state1.momentum().x(); - // dm^2/dp0y - res(0, 1) = 2.*e1*state0.momentum().y()/e0 - 2.*state1.momentum().y(); - // dm^2/dp0z - res(0, 2) = 2.*e1*state0.momentum().z()/e0 - 2.*state1.momentum().z(); - - // d^m/dp1x - res(0, 3) = 2.*e0*state1.momentum().x()/e1 - 2.*state0.momentum().x(); - // d^m/dp1y - res(0, 4) = 2.*e0*state1.momentum().y()/e1 - 2.*state0.momentum().y(); - // d^m/dp1z - res(0, 5) = 2.*e0*state1.momentum().z()/e1 - 2.*state0.momentum().z(); - - const double m = std::sqrt(2.*dmass*dmass + 2.*e0*e1 - 2.*state0.momentum().x()*state1.momentum().x() - 2.*state0.momentum().y()*state1.momentum().y() - 2.*state0.momentum().z()*state1.momentum().z()); - - res *= 0.5/m; - - return res; -} - - -DEFINE_FWK_MODULE(ResidualGlobalCorrectionMakerTwoTrack); diff --git a/Analysis/HitAnalyzer/plugins/ResidualGlobalCorrectionMakerTwoTrackG4e.cc b/Analysis/HitAnalyzer/plugins/ResidualGlobalCorrectionMakerTwoTrackG4e.cc index 5a85f716c2d4e..dab889636584d 100644 --- a/Analysis/HitAnalyzer/plugins/ResidualGlobalCorrectionMakerTwoTrackG4e.cc +++ b/Analysis/HitAnalyzer/plugins/ResidualGlobalCorrectionMakerTwoTrackG4e.cc @@ -24,6 +24,8 @@ #include "TrackPropagation/Geant4e/interface/Geant4ePropagator.h" +#include "FWCore/Common/interface/TriggerNames.h" + class ResidualGlobalCorrectionMakerTwoTrackG4e : public ResidualGlobalCorrectionMakerBase { @@ -34,16 +36,16 @@ class ResidualGlobalCorrectionMakerTwoTrackG4e : public ResidualGlobalCorrection // static void fillDescriptions(edm::ConfigurationDescriptions &descriptions); private: - - Matrix massJacobian(const FreeTrajectoryState &state0, const FreeTrajectoryState &state1, double dmass) const; - + virtual void beginStream(edm::StreamID) override; virtual void produce(edm::Event &, const edm::EventSetup &) override; + bool doVtxConstraint_; bool doMassConstraint_; double massConstraint_; double massConstraintWidth_; + float Jpsi_d; float Jpsi_x; float Jpsi_y; float Jpsi_z; @@ -78,6 +80,7 @@ class ResidualGlobalCorrectionMakerTwoTrackG4e : public ResidualGlobalCorrection float Muminuskin_eta; float Muminuskin_phi; + float Jpsicons_d; float Jpsicons_x; float Jpsicons_y; float Jpsicons_z; @@ -143,6 +146,9 @@ class ResidualGlobalCorrectionMakerTwoTrackG4e : public ResidualGlobalCorrection unsigned int Muminus_nvalidpixel; unsigned int Muminus_nmatchedvalid; unsigned int Muminus_nambiguousmatchedvalid; + + bool Muplus_highpurity; + bool Muminus_highpurity; bool Muplus_isMuon; bool Muplus_muonLoose; @@ -164,6 +170,15 @@ class ResidualGlobalCorrectionMakerTwoTrackG4e : public ResidualGlobalCorrection bool Muminus_muonIsStandalone; bool Muminus_muonInnerTrackBest; + float edmval_cons0; + int niter_cons0; + + float dmassconvval = 0.; + float dinvmasssqconvval = 0.; + + float dmassconvval_cons0 = 0.; + float dinvmasssqconvval_cons0 = 0.; + // std::vector hessv; @@ -173,6 +188,7 @@ class ResidualGlobalCorrectionMakerTwoTrackG4e : public ResidualGlobalCorrection ResidualGlobalCorrectionMakerTwoTrackG4e::ResidualGlobalCorrectionMakerTwoTrackG4e(const edm::ParameterSet &iConfig) : ResidualGlobalCorrectionMakerBase(iConfig) { + doVtxConstraint_ = iConfig.getParameter("doVtxConstraint"); doMassConstraint_ = iConfig.getParameter("doMassConstraint"); massConstraint_ = iConfig.getParameter("massConstraint"); massConstraintWidth_ = iConfig.getParameter("massConstraintWidth"); @@ -183,6 +199,7 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::beginStream(edm::StreamID streami ResidualGlobalCorrectionMakerBase::beginStream(streamid); if (fillTrackTree_) { + tree->Branch("Jpsi_d", &Jpsi_d); tree->Branch("Jpsi_x", &Jpsi_x); tree->Branch("Jpsi_y", &Jpsi_y); tree->Branch("Jpsi_z", &Jpsi_z); @@ -217,7 +234,7 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::beginStream(edm::StreamID streami tree->Branch("Muminuskin_eta", &Muminuskin_eta); tree->Branch("Muminuskin_phi", &Muminuskin_phi); - + tree->Branch("Jpsicons_d", &Jpsicons_d); tree->Branch("Jpsicons_x", &Jpsicons_x); tree->Branch("Jpsicons_y", &Jpsicons_y); tree->Branch("Jpsicons_z", &Jpsicons_z); @@ -269,8 +286,10 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::beginStream(edm::StreamID streami tree->Branch("Muplus_refParms", Muplus_refParms.data(), "Muplus_refParms[3]/F"); tree->Branch("Muminus_refParms", Muminus_refParms.data(), "Muminus_refParms[3]/F"); - tree->Branch("Muplus_jacRef", &Muplus_jacRef); - tree->Branch("Muminus_jacRef", &Muminus_jacRef); + if (fillJac_) { + tree->Branch("Muplus_jacRef", &Muplus_jacRef); + tree->Branch("Muminus_jacRef", &Muminus_jacRef); + } tree->Branch("Muplus_nhits", &Muplus_nhits); tree->Branch("Muplus_nvalid", &Muplus_nvalid); @@ -284,6 +303,9 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::beginStream(edm::StreamID streami tree->Branch("Muminus_nmatchedvalid", &Muminus_nmatchedvalid); tree->Branch("Muminus_nambiguousmatchedvalid", &Muminus_nambiguousmatchedvalid); + tree->Branch("Muplus_highpurity", &Muplus_highpurity); + tree->Branch("Muminus_highpurity", &Muminus_highpurity); + tree->Branch("Muplus_isMuon", &Muplus_isMuon); tree->Branch("Muplus_muonLoose", &Muplus_muonLoose); tree->Branch("Muplus_muonMedium", &Muplus_muonMedium); @@ -304,6 +326,14 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::beginStream(edm::StreamID streami tree->Branch("Muminus_muonIsStandalone", &Muminus_muonIsStandalone); tree->Branch("Muminus_muonInnerTrackBest", &Muminus_muonInnerTrackBest); + tree->Branch("edmval_cons0", &edmval_cons0); + tree->Branch("niter_cons0", &niter_cons0); + + tree->Branch("dmassconvval", &dmassconvval); + tree->Branch("dinvmasssqconvval", &dinvmasssqconvval); + tree->Branch("dmassconvval_cons0", &dmassconvval_cons0); + tree->Branch("dinvmasssqconvval_cons0", &dinvmasssqconvval_cons0); + // tree->Branch("hessv", &hessv); } @@ -315,6 +345,8 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::produce(edm::Event &iEvent, const { const bool dogen = fitFromGenParms_; + + constexpr bool dolocalupdate = false; using namespace edm; @@ -342,11 +374,15 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::produce(edm::Event &iEvent, const // Handle> genPartCollection; Handle> genPartCollection; + Handle genXyz0; Handle genEventInfo; Handle> genPartBarcodes; + Handle> pileupSummary; if (doGen_) { iEvent.getByToken(GenParticlesToken_, genPartCollection); + iEvent.getByToken(genXyz0Token_, genXyz0); iEvent.getByToken(genEventInfoToken_, genEventInfo); + iEvent.getByToken(pileupSummaryToken_, pileupSummary); } std::vector>> simHits(inputSimHits_.size()); @@ -363,6 +399,11 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::produce(edm::Event &iEvent, const if (doMuons_) { iEvent.getByToken(inputMuons_, muons); } + + Handle triggerResults; + if (doTrigger_) { + iEvent.getByToken(inputTriggerResults_, triggerResults); + } KFUpdator updator; TkClonerImpl const& cloner = static_cast(ttrh.product())->cloner(); @@ -385,6 +426,8 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::produce(edm::Event &iEvent, const VectorXd grad; MatrixXd hess; LDLT Cinvd; +// MatrixXd covstate; + Matrix covrefmom; // FullPivLU Cinvd; // ColPivHouseholderQR Cinvd; @@ -397,6 +440,66 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::produce(edm::Event &iEvent, const genweight = 1.; if (doGen_) { genweight = genEventInfo->weight(); + + Pileup_nPU = pileupSummary->front().getPU_NumInteractions(); + Pileup_nTrueInt = pileupSummary->front().getTrueNumInteractions(); + } + + // trigger bits + if (doTrigger_) { + auto const &triggerNames = iEvent.triggerNames(*triggerResults); + + if (triggerNames.parameterSetID() != triggerNamesId_) { + //trigger menu changed, update list of trigger path idxs + + triggerIdxs_.clear(); + + for (auto const &trigger : triggers_) { + const std::string basename = trigger + "_v"; +// std::cout << "basename = " << basename << std::endl; + std::size_t idx = triggerNames.size(); + for (std::size_t itrig = 0; itrig < triggerNames.size(); ++itrig) { +// auto findres = triggerNames.triggerName(itrig).find(basename); +// std::cout << triggerNames.triggerName(itrig) << " findres = " << findres << std::endl; + if (triggerNames.triggerName(itrig).find(basename) == 0) { + idx = itrig; + break; + } + } + triggerIdxs_.push_back(idx); + } + +// for (std::size_t itrig = 0; itrig < triggerIdxs_.size(); ++itrig) { +// std::cout << "itrig = " << itrig << ", idx = " << triggerIdxs_[itrig] << std::endl; +// } + + triggerNamesId_ = triggerNames.parameterSetID(); + } + + + + + + // set trigger decision bits + for (std::size_t itrig = 0; itrig < triggerIdxs_.size(); ++itrig) { + const std::size_t idx = triggerIdxs_[itrig]; +// if (idx < triggerResults->size()) { +// std::cout << "itrig = " << itrig << " idx = " << idx << " accept = " << triggerResults->accept(idx) << std::endl; +// } + triggerDecisions_[itrig] = idx < triggerResults->size() ? triggerResults->accept(idx) : false; + } + + +// for (unsigned int i = 0; i < triggerNames.size(); ++i) { +// std::cout << i << " " << triggerNames.triggerName(i) << " accept = " << triggerResults->accept(i) << std::endl; +// } +// auto const idx = iEvent.triggerNames(*triggerResults).triggerIndex("HLT_Mu7p5_Track2_Jpsi"); +// auto const idx = triggerNames.triggerIndex("HLT_Mu7p5_Track3p5_Jpsi_v4"); +// auto const idx2 = triggerNames.triggerIndex("HLT_eawgawe"); +// std::cout << "trigger names size = " << triggerNames.size() << std::endl; +// std::cout << "trigger index = " << idx << std::endl; +// std::cout << "trigger index2 = " << idx2 << std::endl; + } // loop over combinatorics of track pairs @@ -404,6 +507,27 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::produce(edm::Event &iEvent, const if (itrack->isLooper()) { continue; } + + const reco::Candidate *mu0gen = nullptr; + if (doGen_ && !doSim_) { + for (auto const &genpart : *genPartCollection) { + if (genpart.status() != 1) { + continue; + } + if (std::abs(genpart.pdgId()) != 13) { + continue; + } + + float dR0 = deltaR(genpart, *itrack); + if (dR0 < 0.1 && genpart.charge() == itrack->charge()) { + mu0gen = &genpart; + } + } + } + + if (requireGen_ && mu0gen == nullptr) { + continue; + } const reco::TransientTrack itt = TTBuilder->build(*itrack); @@ -429,25 +553,6 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::produce(edm::Event &iEvent, const continue; } - const reco::TransientTrack jtt = TTBuilder->build(*jtrack); - - const reco::Muon *matchedmuon1 = nullptr; - if (doMuons_) { - for (auto const &muon : *muons) { - if (muon.bestTrack()->algo() == jtrack->algo()) { - if ( (muon.bestTrack()->momentum() - jtrack->momentum()).mag2() < 1e-3 ) { - matchedmuon1 = &muon; - } - } - else if (muon.innerTrack().isNonnull() && muon.innerTrack()->algo() == jtrack->algo()) { - if ( (muon.innerTrack()->momentum() - jtrack->momentum()).mag2() < 1e-3 ) { - matchedmuon1 = &muon; - } - } - } - } - - const reco::Candidate *mu0gen = nullptr; const reco::Candidate *mu1gen = nullptr; double massconstraintval = massConstraint_; @@ -460,29 +565,35 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::produce(edm::Event &iEvent, const continue; } -// float dR0 = deltaR(genpart.phi(), itrack->phi(), genpart.eta(), itrack->eta()); - float dR0 = deltaR(genpart, *itrack); - if (dR0 < 0.1 && genpart.charge() == itrack->charge()) { - mu0gen = &genpart; - } - -// float dR1 = deltaR(genpart.phi(), jtrack->phi(), genpart.eta(), jtrack->eta()); float dR1 = deltaR(genpart, *jtrack); if (dR1 < 0.1 && genpart.charge() == jtrack->charge()) { mu1gen = &genpart; } } -// if (mu0gen != nullptr && mu1gen != nullptr) { -// auto const jpsigen = ROOT::Math::PtEtaPhiMVector(mu0gen->pt(), mu0gen->eta(), mu0gen->phi(), mmu) + -// ROOT::Math::PtEtaPhiMVector(mu1gen->pt(), mu1gen->eta(), mu1gen->phi(), mmu); -// -// massconstraintval = jpsigen.mass(); -// } -// else { -// continue; -// } - + } + + if (requireGen_ && mu1gen == nullptr) { + continue; + } + + + const reco::TransientTrack jtt = TTBuilder->build(*jtrack); + + const reco::Muon *matchedmuon1 = nullptr; + if (doMuons_) { + for (auto const &muon : *muons) { + if (muon.bestTrack()->algo() == jtrack->algo()) { + if ( (muon.bestTrack()->momentum() - jtrack->momentum()).mag2() < 1e-3 ) { + matchedmuon1 = &muon; + } + } + else if (muon.innerTrack().isNonnull() && muon.innerTrack()->algo() == jtrack->algo()) { + if ( (muon.innerTrack()->momentum() - jtrack->momentum()).mag2() < 1e-3 ) { + matchedmuon1 = &muon; + } + } + } } // std::cout << "massconstraintval = " << massconstraintval << std::endl; @@ -506,7 +617,17 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::produce(edm::Event &iEvent, const // split matched invalid hits if (detglued != nullptr && !(*it)->isValid()) { - bool order = detglued->stereoDet()->surface().position().mag() > detglued->monoDet()->surface().position().mag(); +// bool order = detglued->stereoDet()->surface().position().mag() > detglued->monoDet()->surface().position().mag(); + + const auto stereopos = detglued->stereoDet()->surface().position(); + const auto monopos = detglued->monoDet()->surface().position(); + + const Eigen::Vector3d stereoposv(stereopos.x(), stereopos.y(), stereopos.z()); + const Eigen::Vector3d monoposv(monopos.x(), monopos.y(), monopos.z()); + const Eigen::Vector3d trackmomv(track.momentum().x(), track.momentum().y(), track.momentum().z()); + + bool order = (stereoposv - monoposv).dot(trackmomv) > 0.; + const GeomDetUnit* detinner = order ? detglued->monoDet() : detglued->stereoDet(); const GeomDetUnit* detouter = order ? detglued->stereoDet() : detglued->monoDet(); @@ -535,6 +656,7 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::produce(edm::Event &iEvent, const // } hitquality = !pixhit->isOnEdge() && cluster.sizeX() > 1; +// hitquality = !pixhit->isOnEdge() && cluster.sizeX() > 1 && pixhit->qBin() < 2; // hitquality = !pixhit->isOnEdge() && cluster.sizeX() > 1 && cluster.sizeY() > 1; } else { @@ -583,12 +705,22 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::produce(edm::Event &iEvent, const std::array nmatchedvalidarr = {{ 0, 0 }}; std::array nambiguousmatchedvalidarr = {{ 0, 0 }}; + const std::array highpurityarr = {{ itrack->quality(reco::TrackBase::highPurity), + jtrack->quality(reco::TrackBase::highPurity) }}; + // second loop to count hits for (unsigned int id = 0; id < 2; ++id) { std::unordered_map trackidmap; auto const &hits = hitsarr[id]; // layerStatesarr[id].reserve(hits.size()); for (auto const &hit : hits) { + + const uint32_t gluedid = trackerTopology->glued(hit->geographicalId()); + const bool isglued = gluedid != 0; + const DetId parmdetid = isglued ? DetId(gluedid) : hit->geographicalId(); + + const DetId aligndetid = alignGlued_ ? parmdetid : hit->geographicalId(); + ++nhits; ++nhitsarr[id]; if (hit->isValid()) { @@ -602,7 +734,7 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::produce(edm::Event &iEvent, const } - const bool align2d = detidparms.count(std::make_pair(1, hit->geographicalId())); + const bool align2d = detidparms.count(std::make_pair(1, aligndetid)); if (align2d) { ++nvalidalign2d; } @@ -691,13 +823,15 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::produce(edm::Event &iEvent, const const CurvilinearTrajectoryError nullerr(null55); - const unsigned int nparsAlignment = 2*nvalid + nvalidalign2d; +// const unsigned int nparsAlignment = 2*nvalid + nvalidalign2d; +// const unsigned int nparsAlignment = 6*nvalid; + const unsigned int nparsAlignment = 5*nvalid + nvalidalign2d; const unsigned int nparsBfield = nhits; const unsigned int nparsEloss = nhits; // const unsigned int nparsEloss = nhits + 2; const unsigned int npars = nparsAlignment + nparsBfield + nparsEloss; - const unsigned int nstateparms = 9 + 5*nhits; + const unsigned int nstateparms = 10 + 5*nhits; const unsigned int nparmsfull = nstateparms + npars; @@ -721,11 +855,10 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::produce(edm::Event &iEvent, const parts.push_back(pFactory.particle(jtt, mmu, chisq, ndf, masserr)); RefCountedKinematicTree kinTree; - // if (icons == 1) { if (icons > 0) { - // if (false) { - // TwoTrackMassKinematicConstraint constraint(massConstraint_); - TwoTrackMassKinematicConstraint constraint(massconstraintval); +// double kinconstraintval = 1./std::sqrt(massconstraintval); + double kinconstraintval = massconstraintval; + TwoTrackMassKinematicConstraint constraint(kinconstraintval); KinematicConstrainedVertexFitter vtxFitter; kinTree = vtxFitter.fit(parts, &constraint); } @@ -809,9 +942,13 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::produce(edm::Event &iEvent, const // constexpr unsigned int niters = 1; // constexpr unsigned int niters = 3; // constexpr unsigned int niters = 5; - constexpr unsigned int niters = 10; +// constexpr unsigned int niters = 10; -// const unsigned int niters = icons == 0 ? 3 : 1; +// constexpr unsigned int niters = 1; + const unsigned int niters = (dogen && !dolocalupdate) ? 1 : 10; + + +// const unsigned int niters = icons == 0 ? 10 : 1; for (unsigned int iiter=0; iiter trackstateidxarr; std::array trackparmidxarr; - unsigned int trackstateidx = 3; + unsigned int trackstateidx = 10; unsigned int parmidx = 0; unsigned int alignmentparmidx = 0; double chisq0val = 0.; + if (iiter > 0) { + //update current state from reference point state + const Matrix statepca = twoTrackCart2pca(refftsarr[0], refftsarr[1]); + const Matrix statepcaupd = statepca + dxfull.head<10>(); + + refftsarr = twoTrackPca2cart(statepcaupd); + } + // const bool firsthitshared = hitsarr[0][0]->sharesInput(&(*hitsarr[1][0]), TrackingRecHit::some); // std::cout << "firsthitshared = " << firsthitshared << std::endl; double dbetavalref = 0.; + std::array dbetavalrefarr; for (unsigned int id = 0; id < 2; ++id) { auto &hits = hitsarr[id]; auto const& hit = hits[0]; @@ -854,9 +1000,12 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::produce(edm::Event &iEvent, const const unsigned int bfieldglobalidx = detidparms.at(std::make_pair(6, parmdetid)); const double dbetaval = corparms_[bfieldglobalidx]; - dbetavalref += 0.5*dbetaval; + dbetavalref += 0.5*dbetaval; + dbetavalrefarr[id] = dbetaval; } + const Matrix twotrackpca2curvref = twoTrackPca2curvJacobianD(refftsarr[0], refftsarr[1], field, dbetavalrefarr[0], dbetavalrefarr[1]); + for (unsigned int id = 0; id < 2; ++id) { // FreeTrajectoryState refFts = outparts[id]->currentState().freeTrajectoryState(); @@ -871,72 +1020,6 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::produce(edm::Event &iEvent, const trackparmidxarr[id] = parmidx; const unsigned int tracknhits = hits.size(); - -// if (iiter > 0 || icons > 0) { - if (iiter > 0) { - //update current state from reference point state (errors not needed beyond first iteration) - - const double qbp = refFts[6]/refFts.segment<3>(3).norm(); - const double lam = std::atan(refFts[5]/std::sqrt(refFts[3]*refFts[3] + refFts[4]*refFts[4])); - const double phi = std::atan2(refFts[4], refFts[3]); - - const double qbpupd = qbp + dxfull(trackstateidx); - const double lamupd = lam + dxfull(trackstateidx + 1); - const double phiupd = phi + dxfull(trackstateidx + 2); - - const double charge = std::copysign(1., qbpupd); - const double pupd = std::abs(1./qbpupd); - - const double pxupd = pupd*std::cos(lamupd)*std::cos(phiupd); - const double pyupd = pupd*std::cos(lamupd)*std::sin(phiupd); - const double pzupd = pupd*std::sin(lamupd); - - refFts.head<3>() += dxfull.head<3>(); - refFts[3] = pxupd; - refFts[4] = pyupd; - refFts[5] = pzupd; - refFts[6] = charge; - } - -// // initialize with zero uncertainty -// refFts = FreeTrajectoryState(refFts.parameters(), nullerr); -// -// const ROOT::Math::PxPyPzMVector momtmp(refFts[3], refFts[4], refFts[5], mmu); -// const Matrix Felossadhoc = elossAdHocJacobianD(refFts, mmu); -// const unsigned int etaphiidx = hetaphi->FindFixBin(momtmp.eta(), momtmp.phi()); -// -// // std::cout << "refFts:" << std::endl; -// // std::cout << refFts.position() << std::endl; -// // std::cout << refFts.momentum() << std::endl; -// // std::cout << refFts.charge() << std::endl; -// -// // auto const &surface0 = *hits[0]->surface(); -// auto const &surface0 = *surfacemap_.at(hits[0]->geographicalId()); -// // auto propresult = fPropagator->geometricalPropagator().propagateWithPath(refFts, surface0); -// // auto propresult = fPropagator->geometricalPropagator().propagateWithPath(refFts, *beampipe); -// // auto const propresultref = g4prop->propagateGenericWithJacobian(refFts, surface0); -// auto const propresultref = g4prop->propagateGenericWithJacobianAlt(refFts, surface0); -// if (!std::get<0>(propresultref).isValid()) { -// std::cout << "Abort: Propagation of reference state Failed!" << std::endl; -// valid = false; -// break; -// } -// TrajectoryStateOnSurface updtsos = std::get<0>(propresultref); -// -// const Matrix hybrid2curvref = hybrid2curvJacobian(refFts); -// -// // JacobianCartesianToCurvilinear cart2curvref(refFts.parameters()); -// // auto const &jacCart2CurvRef = Map>(cart2curvref.jacobian().Array()); -// -// Matrix FdFm = Map>(std::get<1>(propresultref).Array()); -// -// FdFmrefarr[id] = FdFm; -// -// double dEdxlast = std::get<3>(propresultref); - - - - const Matrix hybrid2curvref = hybrid2curvJacobianD(refFts, field, dbetavalref); Matrix updtsos = refFts; @@ -944,17 +1027,13 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::produce(edm::Event &iEvent, const if (bsConstraint_) { // apply beamspot constraint // TODO add residual corrections for beamspot parameters? + // TODO impose constraints on individual tracks when not applying common vertex constraint? constexpr unsigned int nlocalvtx = 3; - constexpr unsigned int nlocal = nlocalvtx; - constexpr unsigned int localvtxidx = 0; - - constexpr unsigned int fullvtxidx = 0; - - using BSScalar = AANT; - + constexpr unsigned int fullvtxidx = 7; + const double sigb1 = bsH->BeamWidthX(); const double sigb2 = bsH->BeamWidthY(); const double sigb3 = bsH->sigmaZ(); @@ -984,35 +1063,19 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::produce(edm::Event &iEvent, const covBS(2,1) = covBS(1,2) = dydz*(varb3-varb2)-dxdz*covBS(1,0); covBS(2,2) = varb3; - // std::cout << "covBS:" << std::endl; - // std::cout << covBS << std::endl; - - Matrix dvtx = Matrix::Zero(); - for (unsigned int j=0; j dbs0; - dbs0[0] = BSScalar(refFts[0] - x0); - dbs0[1] = BSScalar(refFts[1] - y0); - dbs0[2] = BSScalar(refFts[2] - z0); - - // std::cout << "dposition / d(qop, lambda, phi) (should be 0?):" << std::endl; - // std::cout << Map>(jac.Array()).topLeftCorner<3,3>() << std::endl; - - const Matrix covBSinv = covBS.inverse().cast(); - - const Matrix dbs = dbs0 + dvtx; - const BSScalar chisq = dbs.transpose()*covBSinv*dbs; - - chisq0val += chisq.value().value(); - - auto const& gradlocal = chisq.value().derivatives(); - //fill local hessian - Matrix hesslocal; - for (unsigned int j=0; j dbs0; + dbs0[0] = refFts[0] - x0; + dbs0[1] = refFts[1] - y0; + dbs0[2] = refFts[2] - z0; + + const Matrix Fbs = Matrix::Identity(); + const Matrix covBSinv = covBS.inverse(); + + const double bschisq = dbs0.transpose()*covBSinv*dbs0; + const Matrix gradlocal = 2.*Fbs.transpose()*covBSinv*dbs0; + const Matrix hesslocal = 2.*Fbs.transpose()*covBSinv*Fbs; + + chisq0val += bschisq; //fill global gradient gradfull.segment(fullvtxidx) += gradlocal.head(); @@ -1031,7 +1094,8 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::produce(edm::Event &iEvent, const const bool isglued = gluedid != 0; const DetId parmdetid = isglued ? DetId(gluedid) : hit->geographicalId(); const GeomDet* parmDet = isglued ? globalGeometry->idToDet(parmdetid) : hit->det(); - const double xifraction = isglued ? hit->det()->surface().mediumProperties().xi()/parmDet->surface().mediumProperties().xi() : 1.; + + const DetId aligndetid = alignGlued_ ? parmdetid : hit->geographicalId(); const unsigned int bfieldglobalidx = detidparms.at(std::make_pair(6, parmdetid)); const unsigned int elossglobalidx = detidparms.at(std::make_pair(7, parmdetid)); @@ -1039,37 +1103,8 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::produce(edm::Event &iEvent, const const double dbetaval = corparms_[bfieldglobalidx]; const double dxival = corparms_[elossglobalidx]; -// if (ihit > 0) { -// if (std::abs(updtsos.globalMomentum().eta()) > 4.0) { -// std::cout << "WARNING: Invalid state!!!" << std::endl; -// valid = false; -// break; -// } -// -// -// // auto const &surfaceip1 = *hits[ihit+1]->surface(); -// // auto const &surface = *hit->surface(); -// // const Plane &surface = *hit->surface(); -// auto const &surface = *surfacemap_.at(hit->geographicalId()); -// // auto propresult = thePropagator->propagateWithPath(updtsos, surface); -// // auto const propresult = g4prop->propagateGenericWithJacobian(*updtsos.freeState(), surface); -// auto const propresult = g4prop->propagateGenericWithJacobianAlt(*updtsos.freeState(), surface); -// // propresult = fPropagator->geometricalPropagator().propagateWithPath(updtsos, *hits[ihit+1]->surface()); -// if (!std::get<0>(propresult).isValid()) { -// std::cout << "Abort: Propagation Failed!" << std::endl; -// valid = false; -// break; -// } -// -// FdFm = Map>(std::get<1>(propresult).Array()); -// // FdFm = localTransportJacobian(updtsos, propresult, false); -// updtsos = std::get<0>(propresult); -// -// dEdxlast = std::get<3>(propresult); -// } - - const GloballyPositioned &surface = surfacemapD_.at(hit->geographicalId()); + auto propresult = g4prop->propagateGenericWithJacobianAltD(updtsos, surface, dbetaval, dxival); if (!std::get<0>(propresult)) { std::cout << "Abort: Propagation Failed!" << std::endl; @@ -1077,381 +1112,181 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::produce(edm::Event &iEvent, const break; } -// auto propresultalt = g4prop->propagateGenericWithJacobianAltD(updtsos, surface, 10.); -// const Matrix Qcurvalt = std::get<2>(propresultalt); updtsos = std::get<1>(propresult); const Matrix Qcurv = std::get<2>(propresult); const Matrix FdFm = std::get<3>(propresult); const double dEdxlast = std::get<4>(propresult); - -// std::cout << "Qcurv" << std::endl; -// std::cout << Qcurv << std::endl; -// std::cout << "Qcurvalt" << std::endl; -// std::cout << Qcurvalt << std::endl; - - - // compute convolution correction in local coordinates (BEFORE material effects are applied) - // const Matrix dxlocalconv = localPositionConvolution(updtsos); - - // curvilinear to local jacobian -// JacobianCurvilinearToLocal curv2localm(updtsos.surface(), updtsos.localParameters(), *updtsos.magneticField()); -// const AlgebraicMatrix55& curv2localjacm = curv2localm.jacobian(); -// const Matrix Hm = Map>(curv2localjacm.Array()); -// const Matrix Hm = curv2localJacobianAlt(updtsos); -// const Matrix Hm = curv2localJacobianAlteloss(updtsos, dEdxlast, mmu); const Matrix Hm = curv2localJacobianAltelossD(updtsos, field, surface, dEdxlast, mmu, dbetaval); - //get the process noise matrix -// AlgebraicMatrix55 const Qmat = updtsos.localError().matrix(); -// const Map>Q(Qmat.Array()); + const Matrix localparmsprop = globalToLocal(updtsos, surface); + + Matrix localparms = localparmsprop; - const Matrix Q = Hm*Qcurv*Hm.transpose(); + Matrix dx0 = Matrix::Zero(); + if (dolocalupdate) { + if (iiter==0) { + layerStates.push_back(updtsos); + } + else { + //current state from previous state on this layer + //save current parameters + + Matrix& oldtsos = layerStates[ihit]; + const Matrix Hold = curv2localJacobianAltelossD(oldtsos, field, surface, dEdxlast, mmu, dbetaval); + const Matrix dxlocal = Hold*dxfull.segment<5>(trackstateidx + 5*ihit); + localparms = globalToLocal(oldtsos, surface); - Matrix localparmsprop; - const Point3DBase posprop(updtsos[0], updtsos[1], updtsos[2]); - const Vector3DBase momprop(updtsos[3], updtsos[4], updtsos[5]); - - const Point3DBase localpos = surface.toLocal(posprop); - const Vector3DBase localmom = surface.toLocal(momprop); - - localparmsprop[0] = updtsos[6]/updtsos.segment<3>(3).norm(); - localparmsprop[1] = localmom.x()/localmom.z(); - localparmsprop[2] = localmom.y()/localmom.z(); - localparmsprop[3] = localpos.x(); - localparmsprop[4] = localpos.y(); - - Matrix localparms = localparmsprop; + localparms.head<5>() += dxlocal; - Matrix idx0 = Matrix::Zero(); - if (iiter==0) { - // if (true) { - layerStates.push_back(updtsos); - // layerStatesStart.push_back(updtsos); - + oldtsos = localToGlobal(localparms, surface); + + updtsos = oldtsos; + dx0 = (localparms - localparmsprop).head<5>(); + } } - else { - //current state from previous state on this layer - //save current parameters - - Matrix& oldtsos = layerStates[ihit]; - const Matrix Hold = curv2localJacobianAltelossD(oldtsos, field, surface, dEdxlast, mmu, dbetaval); -// const Matrix dxlocal = Hold*dxfull.segment<5>(5*(ihit+1)); - const Matrix dxlocal = Hold*dxfull.segment<5>(trackstateidx + 3 + 5*ihit); - - const Point3DBase pos(oldtsos[0], oldtsos[1], oldtsos[2]); - const Point3DBase localpos = surface.toLocal(pos); - - const Point3DBase localposupd(localpos.x() + dxlocal[3], localpos.y() + dxlocal[4], localpos.z()); - const Point3DBase posupd = surface.toGlobal(localposupd); - - - const Vector3DBase mom(oldtsos[3], oldtsos[4], oldtsos[5]); - const Vector3DBase localmom = surface.toLocal(mom); - - const double dxdz = localmom.x()/localmom.z(); - const double dydz = localmom.y()/localmom.z(); - - - - const double dxdzupd = dxdz + dxlocal[1]; - const double dydzupd = dydz + dxlocal[2]; - - const double qop = oldtsos[6]/oldtsos.segment<3>(3).norm(); - const double qopupd = qop + dxlocal[0]; - - const double pupd = std::abs(1./qopupd); - const double charge = std::copysign(1., qopupd); - - const double signpz = std::copysign(1., localmom.z()); - const double localmomfact = signpz/std::sqrt(1. + dxdzupd*dxdzupd + dydzupd*dydzupd); - const Vector3DBase localmomupd(pupd*dxdzupd*localmomfact, pupd*dydzupd*localmomfact, pupd*localmomfact); - const Vector3DBase momupd = surface.toGlobal(localmomupd); - - oldtsos[0] = posupd.x(); - oldtsos[1] = posupd.y(); - oldtsos[2] = posupd.z(); - oldtsos[3] = momupd.x(); - oldtsos[4] = momupd.y(); - oldtsos[5] = momupd.z(); - oldtsos[6] = charge; - - updtsos = oldtsos; - - localparms[0] = qopupd; - localparms[1] = dxdzupd; - localparms[2] = dydzupd; - localparms[3] = localposupd.x(); - localparms[4] = localposupd.y(); - - idx0 = localparms - localparmsprop; - + + // curvilinear to local jacobian + // const Matrix &Hp = dolocalupdate ? curv2localJacobianAltelossD(updtsos, field, surface, dEdxlast, mmu, dbetaval) : Hm; + Matrix Hp = Hm; + if (dolocalupdate) { + Hp = curv2localJacobianAltelossD(updtsos, field, surface, dEdxlast, mmu, dbetaval); } - -// // update state from previous iteration -// //momentum kink residual -// AlgebraicVector5 idx0(0., 0., 0., 0., 0.); -// // if (iiter==0 && icons==0) { -// if (iiter==0) { -// updtsos.update(updtsos.localParameters(), -// LocalTrajectoryError(0.,0.,0.,0.,0.), -// updtsos.surface(), -// updtsos.magneticField(), -// updtsos.surfaceSide()); -// layerStates.push_back(updtsos); -// } -// else { -// //current state from previous state on this layer -// //save current parameters -// TrajectoryStateOnSurface& oldtsos = layerStates[ihit]; -// -// // JacobianCurvilinearToLocal curv2localold(oldtsos.surface(), oldtsos.localParameters(), *oldtsos.magneticField()); -// // const AlgebraicMatrix55& curv2localjacold = curv2localold.jacobian(); -// // const Matrix Hold = Map>(curv2localjacold.Array()); -// // const Matrix Hold = curv2localJacobianAlt(oldtsos); -// const Matrix Hold = curv2localJacobianAlteloss(oldtsos, dEdxlast, mmu); -// -// -// const AlgebraicVector5 local = oldtsos.localParameters().vector(); -// -// auto const& dxlocal = Hold*dxfull.segment<5>(trackstateidx + 3 + 5*ihit); -// const Matrix localupd = Map>(local.Array()) + dxlocal; -// AlgebraicVector5 localvecupd(localupd[0],localupd[1],localupd[2],localupd[3],localupd[4]); -// -// idx0 = localvecupd - updtsos.localParameters().vector(); -// -// const LocalTrajectoryParameters localparms(localvecupd, oldtsos.localParameters().pzSign()); -// -// // std::cout << "before update: oldtsos:" << std::endl; -// // std::cout << oldtsos.localParameters().vector() << std::endl; -// // oldtsos.update(localparms, oldtsos.surface(), field, oldtsos.surfaceSide()); -// oldtsos.update(localparms, LocalTrajectoryError(0.,0.,0.,0.,0.), oldtsos.surface(), field, oldtsos.surfaceSide()); -// // std::cout << "after update: oldtsos:" << std::endl; -// // std::cout << oldtsos.localParameters().vector() << std::endl; -// updtsos = oldtsos; -// -// } - - //apply measurement update if applicable - // std::cout << "constructing preciseHit" << std::endl; -// auto const& preciseHit = hit->isValid() ? cloner.makeShared(hit, updtsos) : hit; + // const Matrix &Q = dolocalupdate ? Hm*Qcurv*Hm.transpose() : Qcurv; - const GlobalPoint pos(updtsos[0], updtsos[1], updtsos[2]); - const GlobalVector mom(updtsos[3], updtsos[4], updtsos[5]); - const GlobalTrajectoryParameters glob(pos, mom, updtsos[6], field); - const TrajectoryStateOnSurface tsostmp(glob, *hit->surface()); + Matrix Q = Qcurv; + if (dolocalupdate) { + Q = Hm*Qcurv*Hm.transpose(); + } - auto const& preciseHit = hit->isValid() ? cloner.makeShared(hit, tsostmp) : hit; + const Matrix Qinv = Q.inverse(); - if (hit->isValid() && !preciseHit->isValid()) { - std::cout << "Abort: Failed updating hit" << std::endl; - valid = false; - break; - } - - // const uint32_t gluedid = trackerTopology->glued(preciseHit->det()->geographicalId()); - // const bool isglued = gluedid != 0; - // const DetId parmdetid = isglued ? DetId(gluedid) : preciseHit->geographicalId(); - // const bool align2d = detidparms.count(std::make_pair(1, parmdetid)); - // const GeomDet* parmDet = isglued ? globalGeometry->idToDet(parmdetid) : preciseHit->det(); - - const bool align2d = detidparms.count(std::make_pair(1, preciseHit->geographicalId())); - - // curvilinear to local jacobian -// JacobianCurvilinearToLocal curv2localp(updtsos.surface(), updtsos.localParameters(), *updtsos.magneticField()); -// const AlgebraicMatrix55& curv2localjacp = curv2localp.jacobian(); -// const Matrix Hp = Map>(curv2localjacp.Array()); -// const Matrix Hp = curv2localJacobianAlt(updtsos); -// const Matrix Hp = curv2localJacobianAlteloss(updtsos, dEdxlast, mmu); - const Matrix Hp = curv2localJacobianAltelossD(updtsos, field, surface, dEdxlast, mmu, dbetaval); - - // const Matrix Hpalt = curv2localJacobianAlt(updtsos); - // - // std::cout << "Hp" << std::endl; - // std::cout << Hp << std::endl; - // std::cout << "Hpalt" << std::endl; - // std::cout << Hpalt << std::endl; - - - if (true) { - // std::cout << "EdE first hit:" << std::endl; - // std::cout << EdE << std::endl; - // - // std::cout << "xival = " << xival << std::endl; - - // AlgebraicVector5 idx0(0., 0., 0., 0., 0.); -// const Vector5d dx0 = Map(idx0.Array()); - const Vector5d dx0 = idx0; - - if (ihit == 0) { - constexpr unsigned int nvtxstate = 3; - constexpr unsigned int nlocalstate = 8; - constexpr unsigned int nlocalbfield = 1; - constexpr unsigned int nlocaleloss = 1; - constexpr unsigned int nlocalparms = nlocalbfield + nlocaleloss; - - constexpr unsigned int nlocal = nvtxstate + nlocalstate + nlocalparms; - - constexpr unsigned int localvtxidx = 0; - constexpr unsigned int localstateidx = localvtxidx + nvtxstate; - constexpr unsigned int localparmidx = localstateidx + nlocalstate; - - constexpr unsigned int fullvtxidx = 0; - const unsigned int fullstateidx = trackstateidx; - const unsigned int fullparmidx = nstateparms + parmidx; - - using MSScalar = AANT; - - Matrix Fvtx = (FdFm.leftCols<5>()*hybrid2curvref.rightCols<3>()).cast(); - Matrix Fmom = (FdFm.leftCols<5>()*hybrid2curvref.leftCols<3>()).cast(); - - // Matrix Fvtx = (FdFm.leftCols<5>()*jacCart2CurvRef.leftCols<3>()).cast(); - // Matrix Fmom = FdFm.leftCols<3>().cast(); - - Matrix Fb = FdFm.col(5).cast(); - Matrix Fxi = FdFm.col(6).cast(); - - Matrix Hmstate = Hm.cast(); - Matrix Hpstate = Hp.cast(); - - Matrix Qinv = Q.inverse().cast(); - - // initialize active scalars for common vertex parameters - Matrix dvtx = Matrix::Zero(); - for (unsigned int j=0; j dmom = Matrix::Zero(); - for (unsigned int j=0; j du = Matrix::Zero(); - for (unsigned int j=0; j dprop = dx0.cast() + Hpstate*du - Hmstate*Fvtx*dvtx - Hmstate*Fmom*dmom - Hmstate*Fb*dbeta - Hmstate*Fxi*dxi; - const MSScalar chisq = dprop.transpose()*Qinv*dprop; - - chisq0val += chisq.value().value(); - - auto const& gradlocal = chisq.value().derivatives(); - //fill local hessian - Matrix hesslocal; - for (unsigned int j=0; j localsizes = {{ nvtxstate, nlocalstate, nlocalparms }}; - constexpr std::array localidxs = {{ localvtxidx, localstateidx, localparmidx }}; - const std::array fullidxs = {{ fullvtxidx, fullstateidx, fullparmidx }}; - - for (unsigned int iidx = 0; iidx < localidxs.size(); ++iidx) { - gradfull.segment(fullidxs[iidx], localsizes[iidx]) += gradlocal.segment(localidxs[iidx], localsizes[iidx]); - for (unsigned int jidx = 0; jidx < localidxs.size(); ++jidx) { - hessfull.block(fullidxs[iidx], fullidxs[jidx], localsizes[iidx], localsizes[jidx]) += hesslocal.block(localidxs[iidx], localidxs[jidx], localsizes[iidx], localsizes[jidx]); - } - } - + if (ihit == 0) { + constexpr unsigned int nvtxstate = 10; + constexpr unsigned int nlocalstate = 5; + constexpr unsigned int nlocalbfield = 1; + constexpr unsigned int nlocaleloss = 1; + constexpr unsigned int nlocalparms = nlocalbfield + nlocaleloss; + + constexpr unsigned int nlocal = nvtxstate + nlocalstate + nlocalparms; + + constexpr unsigned int localvtxidx = 0; + constexpr unsigned int localstateidx = localvtxidx + nvtxstate; + constexpr unsigned int localparmidx = localstateidx + nlocalstate; + + constexpr unsigned int fullvtxidx = 0; + const unsigned int fullstateidx = trackstateidx; + const unsigned int fullparmidx = nstateparms + parmidx; + + const unsigned int vtxjacidx = 5*id; + + Matrix Fprop; + if (dolocalupdate) { + Fprop.middleCols(localvtxidx) = -Hm*FdFm.leftCols<5>()*twotrackpca2curvref.middleRows<5>(vtxjacidx); + Fprop.middleCols(localstateidx) = Hp; + Fprop.middleCols(localparmidx) = -Hm*FdFm.rightCols<2>(); } else { - //TODO statejac stuff - - constexpr unsigned int nlocalstate = 10; - constexpr unsigned int nlocalbfield = 1; - constexpr unsigned int nlocaleloss = 1; - constexpr unsigned int nlocalparms = nlocalbfield + nlocaleloss; - - constexpr unsigned int nlocal = nlocalstate + nlocalparms; - - constexpr unsigned int localstateidx = 0; - constexpr unsigned int localparmidx = localstateidx + nlocalstate; - - const unsigned int fullstateidx = trackstateidx + 3 + 5*(ihit - 1); - const unsigned int fullparmidx = nstateparms + parmidx; - - using MSScalar = AANT; + Fprop.middleCols(localvtxidx) = -FdFm.leftCols<5>()*twotrackpca2curvref.middleRows<5>(vtxjacidx); + Fprop.middleCols(localstateidx) = Matrix::Identity(); + Fprop.middleCols(localparmidx) = -FdFm.rightCols<2>(); + } - Matrix Fstate = FdFm.leftCols<5>().cast(); - Matrix Fb = FdFm.col(5).cast(); - Matrix Fxi = FdFm.col(6).cast(); - - Matrix Hmstate = Hm.cast(); - Matrix Hpstate = Hp.cast(); - - Matrix Qinv = Q.inverse().cast(); - - // initialize active scalars for state parameters - Matrix dum = Matrix::Zero(); - //suppress gradients of reference point parameters when fitting with gen constraint - for (unsigned int j=0; j propgrad = 2.*Fprop.transpose()*Qinv*dx0; + const Matrix prophess = 2.*Fprop.transpose()*Qinv*Fprop; - Matrix du = Matrix::Zero(); - for (unsigned int j=0; j dprop = dx0.cast() + Hpstate*du - Hmstate*Fstate*dum - Hmstate*Fb*dbeta - Hmstate*Fxi*dxi; - const MSScalar chisq = dprop.transpose()*Qinv*dprop; - - chisq0val += chisq.value().value(); - - auto const& gradlocal = chisq.value().derivatives(); - //fill local hessian - Matrix hesslocal; - for (unsigned int j=0; j localsizes = {{ nvtxstate, nlocalstate, nlocalparms }}; + constexpr std::array localidxs = {{ localvtxidx, localstateidx, localparmidx }}; + const std::array fullidxs = {{ fullvtxidx, fullstateidx, fullparmidx }}; + + chisq0val += propchisq; + + for (unsigned int iidx = 0; iidx < localidxs.size(); ++iidx) { + gradfull.segment(fullidxs[iidx], localsizes[iidx]) += propgrad.segment(localidxs[iidx], localsizes[iidx]); + for (unsigned int jidx = 0; jidx < localidxs.size(); ++jidx) { + hessfull.block(fullidxs[iidx], fullidxs[jidx], localsizes[iidx], localsizes[jidx]) += prophess.block(localidxs[iidx], localidxs[jidx], localsizes[iidx], localsizes[jidx]); } - - constexpr std::array localsizes = {{ nlocalstate, nlocalparms }}; - constexpr std::array localidxs = {{ localstateidx, localparmidx }}; - const std::array fullidxs = {{ fullstateidx, fullparmidx }}; - - for (unsigned int iidx = 0; iidx < localidxs.size(); ++iidx) { - gradfull.segment(fullidxs[iidx], localsizes[iidx]) += gradlocal.segment(localidxs[iidx], localsizes[iidx]); - for (unsigned int jidx = 0; jidx < localidxs.size(); ++jidx) { - hessfull.block(fullidxs[iidx], fullidxs[jidx], localsizes[iidx], localsizes[jidx]) += hesslocal.block(localidxs[iidx], localidxs[jidx], localsizes[iidx], localsizes[jidx]); - } + } + } + else { + constexpr unsigned int nlocalstate = 10; + constexpr unsigned int nlocalbfield = 1; + constexpr unsigned int nlocaleloss = 1; + constexpr unsigned int nlocalparms = nlocalbfield + nlocaleloss; + + constexpr unsigned int nlocal = nlocalstate + nlocalparms; + + constexpr unsigned int localstateidx = 0; + constexpr unsigned int localparmidx = localstateidx + nlocalstate; + + const unsigned int fullstateidx = trackstateidx + 5*(ihit - 1); + const unsigned int fullparmidx = nstateparms + parmidx; + + Matrix Fprop; + if (dolocalupdate) { + Fprop.leftCols<5>() = -Hm*FdFm.leftCols<5>(); + Fprop.middleCols<5>(5) = Hp; + Fprop.middleCols(localparmidx) = -Hm*FdFm.rightCols<2>(); + } + else { + Fprop.leftCols<5>() = -FdFm.leftCols<5>(); + Fprop.middleCols<5>(5) = Matrix::Identity(); + Fprop.middleCols(localparmidx) = -FdFm.rightCols<2>(); + } + + const double propchisq = dx0.transpose()*Qinv*dx0; + const Matrix propgrad = 2.*Fprop.transpose()*Qinv*dx0; + const Matrix prophess = 2.*Fprop.transpose()*Qinv*Fprop; + + constexpr std::array localsizes = {{ nlocalstate, nlocalparms }}; + constexpr std::array localidxs = {{ localstateidx, localparmidx }}; + const std::array fullidxs = {{ fullstateidx, fullparmidx }}; + + chisq0val += propchisq; + + for (unsigned int iidx = 0; iidx < localidxs.size(); ++iidx) { + gradfull.segment(fullidxs[iidx], localsizes[iidx]) += propgrad.segment(localidxs[iidx], localsizes[iidx]); + for (unsigned int jidx = 0; jidx < localidxs.size(); ++jidx) { + hessfull.block(fullidxs[iidx], fullidxs[jidx], localsizes[iidx], localsizes[jidx]) += prophess.block(localidxs[iidx], localidxs[jidx], localsizes[iidx], localsizes[jidx]); } - } - -// const unsigned int bfieldglobalidx = detidparms.at(std::make_pair(6, parmdetid)); - globalidxv[parmidx] = bfieldglobalidx; - parmidx++; - -// const unsigned int elossglobalidx = detidparms.at(std::make_pair(7, parmdetid)); - globalidxv[parmidx] = elossglobalidx; - parmidx++; } - - if (preciseHit->isValid()) { + + globalidxv[parmidx] = bfieldglobalidx; + parmidx++; + + globalidxv[parmidx] = elossglobalidx; + parmidx++; + + if (hit->isValid()) { + + //apply measurement update if applicable + LocalTrajectoryParameters locparm(localparms[0], + localparms[1], + localparms[2], + localparms[3], + localparms[4], + localparms[5]); + const TrajectoryStateOnSurface tsostmp(locparm, *hit->surface(), field); + + auto const& preciseHit = cloner.makeShared(hit, tsostmp); + + if (!preciseHit->isValid()) { + std::cout << "Abort: Failed updating hit" << std::endl; + valid = false; + break; + } + + const bool align2d = detidparms.count(std::make_pair(1, aligndetid)); + + const Matrix &Rglued = rgluemap_.at(preciseHit->geographicalId()); + const GloballyPositioned &surfaceglued = surfacemapD_.at(parmdetid); auto fillAlignGrads = [&](auto Nalign) { constexpr unsigned int nlocalstate = 2; @@ -1465,246 +1300,191 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::produce(edm::Event &iEvent, const constexpr unsigned int nlocalparms = nlocalalignment; constexpr unsigned int nlocal = nlocalstate + nlocalparms; - // std::cout << "idx = " << id << ", ihit = " << ihit << ", alignmentparmidx = " << alignmentparmidx << ", nlocalalignment = " << nlocalalignment << std::endl; - - using AlignScalar = AANT; - - const unsigned int fullstateidx = trackstateidx + 3 + 5*ihit + 3; + const unsigned int fullstateidx = trackstateidx + 5*ihit + 3; const unsigned int fullparmidx = nstateparms + nparsBfield + nparsEloss + alignmentparmidx; const bool ispixel = GeomDetEnumerators::isTrackerPixel(preciseHit->det()->subDetector()); - //TODO add hit validation stuff - //TODO add simhit stuff - -// const bool hit1d = preciseHit->dimension() == 1 || ispixel; const bool hit1d = preciseHit->dimension() == 1; - - Matrix Hu = Hp.bottomRightCorner<2,2>().cast(); - Matrix dy0; - Matrix Vinv; + const Matrix Hu = Hp.bottomRightCorner<2,2>(); + + Matrix dy0; + Matrix Vinv; // rotation from module to strip coordinates - // Matrix R; Matrix2d R; -// if (preciseHit->dimension() == 1) { + + const double lxcor = localparms[3]; + const double lycor = localparms[4]; + + + const Topology &topology = preciseHit->det()->topology(); + + // undo deformation correction + const LocalPoint lpnull(0., 0.); + const MeasurementPoint mpnull = topology.measurementPosition(lpnull); + const Topology::LocalTrackPred pred(tsostmp.localParameters().vector()); + + auto const defcorr = topology.localPosition(mpnull, pred) - topology.localPosition(mpnull); + + const double hitx = preciseHit->localPosition().x() - defcorr.x(); + const double hity = preciseHit->localPosition().y() - defcorr.y(); + + double lyoffset = 0.; + double hitphival = -99.; + double localphival = -99.; + if (hit1d) { - dy0[0] = AlignScalar(preciseHit->localPosition().x() - localparms[3]); - dy0[1] = AlignScalar(0.); - - // bool simvalid = false; - // for (auto const& simhith : simHits) { - // for (const PSimHit& simHit : *simhith) { - // if (simHit.detUnitId() == preciseHit->geographicalId()) { - // - // dy0[0] = AlignScalar(simHit.localPosition().x() - updtsos.localPosition().x()); - // - // simvalid = true; - // break; - // } - // } - // if (simvalid) { - // break; - // } - // } - - Vinv = Matrix::Zero(); + const ProxyStripTopology *proxytopology = dynamic_cast(&(preciseHit->det()->topology())); + + dy0[0] = hitx - lxcor; + dy0[1] = hity - lycor; + + const double striplength = proxytopology->stripLength(); + const double yerr2 = striplength*striplength/12.; + + Vinv = Matrix::Zero(); Vinv(0,0) = 1./preciseHit->localPositionError().xx(); - - // R = Matrix::Identity(); + // Vinv(1,1) = 1./yerr2; + R = Matrix2d::Identity(); + + + // std::cout << "1d hit, original x = " << preciseHit->localPosition().x() << " y = " << preciseHit->localPosition().y() << " corrected x = " << hitx << " y = " << hity << std::endl; } else { // 2d hit + // assert(align2d); + Matrix2d iV; iV << preciseHit->localPositionError().xx(), preciseHit->localPositionError().xy(), preciseHit->localPositionError().xy(), preciseHit->localPositionError().yy(); if (ispixel) { -// if (true) { - //take 2d hit as-is for pixels - dy0[0] = AlignScalar(preciseHit->localPosition().x() - localparms[3]); - dy0[1] = AlignScalar(preciseHit->localPosition().y() - localparms[4]); - - Vinv = iV.inverse().cast(); - //FIXME various temporary hacks; - - // dy0[1] = AlignScalar(0.); - // Vinv = Matrix::Zero(); - // Vinv(0,0) = 1./preciseHit->localPositionError().xx(); - - // if (GeomDetEnumerators::isEndcap(preciseHit->det()->subDetector())) { - // if (GeomDetEnumerators::isBarrel(preciseHit->det()->subDetector())) { - // PXBDetId detidtest(preciseHit->det()->geographicalId()); - // int layertest = detidtest.layer(); - // - // if (layertest > 1) { - // Vinv = Matrix::Zero(); - // } - // - // // Vinv = Matrix::Zero(); - // // dy0[0] = AlignScalar(0.); - // // dy0[1] = AlignScalar(0.); - // } - - // bool simvalid = false; - // for (auto const& simhith : simHits) { - // for (const PSimHit& simHit : *simhith) { - // if (simHit.detUnitId() == preciseHit->geographicalId()) { - // - // if (GeomDetEnumerators::isBarrel(preciseHit->det()->subDetector())) { - // dy0[0] = AlignScalar(simHit.localPosition().x() - updtsos.localPosition().x()); - // dy0[1] = AlignScalar(simHit.localPosition().y() - updtsos.localPosition().y()); - // } - // - // // dy0[1] = AlignScalar(0.); - // - // simvalid = true; - // break; - // } - // } - // if (simvalid) { - // break; - // } - // } - - - // R = Matrix::Identity(); + + dy0[0] = hitx - lxcor; + dy0[1] = hity - lycor; + + Vinv = iV.inverse(); + R = Matrix2d::Identity(); } else { - // diagonalize and take only smallest eigenvalue for 2d hits in strip wedge modules, - // since the constraint parallel to the strip is spurious - SelfAdjointEigenSolver eigensolver(iV); - // const Matrix2d& v = eigensolver.eigenvectors(); - R = eigensolver.eigenvectors().transpose(); - if (R(0,0) < 0.) { - R.row(0) *= -1.; - } - if (R(1,1) <0.) { - R.row(1) *= -1.; - } - - Matrix dy0local; - dy0local[0] = preciseHit->localPosition().x() - localparms[3]; - dy0local[1] = preciseHit->localPosition().y() - localparms[4]; - - // bool simvalid = false; - // for (auto const& simhith : simHits) { - // for (const PSimHit& simHit : *simhith) { - // if (simHit.detUnitId() == preciseHit->geographicalId()) { - // - // dy0local[0] = simHit.localPosition().x() - updtsos.localPosition().x(); - // dy0local[1] = simHit.localPosition().y() - updtsos.localPosition().y(); - // - // simvalid = true; - // break; - // } - // } - // if (simvalid) { - // break; - // } - // } - - const Matrix dy0eig = R*dy0local; - - //TODO deal properly with rotations (rotate back to module local coords?) - dy0[0] = AlignScalar(dy0eig[0]); - dy0[1] = AlignScalar(0.); - - Vinv = Matrix::Zero(); - Vinv(0,0) = AlignScalar(1./eigensolver.eigenvalues()[0]); - - // R = v.transpose().cast(); - + // transform to polar coordinates to end the madness + //TODO handle the module deformations consistently here (currently equivalent to dropping/undoing deformation correction) + + // std::cout << "wedge\n" << std::endl; + + const ProxyStripTopology *proxytopology = dynamic_cast(&(preciseHit->det()->topology())); + + const TkRadialStripTopology *radialtopology = dynamic_cast(&proxytopology->specificTopology()); + + const double rdir = radialtopology->yAxisOrientation(); + const double radius = radialtopology->originToIntersection(); + + const double phihit = rdir*std::atan2(hitx, rdir*hity + radius); + const double rhohit = std::sqrt(hitx*hitx + std::pow(rdir*hity + radius, 2)); + + // invert original calculation of covariance matrix to extract variance on polar angle + const double detHeight = radialtopology->detHeight(); + const double radsigma = detHeight*detHeight/12.; + + const double t1 = std::tan(phihit); + const double t2 = t1*t1; + + const double tt = preciseHit->localPositionError().xx() - t2*radsigma; + + const double phierr2 = tt / std::pow(radialtopology->centreToIntersection(), 2); + + const double striplength = detHeight * std::sqrt(1. + std::pow( hitx/(rdir*hity + radius), 2) ); + + const double rhoerr2 = striplength*striplength/12.; + + + // std::cout << "rhohit = " << rhohit << " rhobar = " << rhobar << " rhoerr2lin = " << rhoerr2lin << " rhoerr2 = " << rhoerr2 << std::endl; + + // TODO apply (inverse) corrections for module deformations here? (take into account for jacobian?) + const double phistate = rdir*std::atan2(lxcor, rdir*lycor + radius); + const double rhostate = std::sqrt(lxcor*lxcor + std::pow(rdir*lycor + radius, 2)); + + Vinv = Matrix::Zero(); + Vinv(0, 0) = 1./phierr2; + // Vinv(1, 1) = 1./rhoerr2lin; + + // jacobian from localx-localy to localphi-localrho + R = Matrix2d::Zero(); + + const double yp = rdir*lycor + radius; + const double invden = 1./(lxcor*lxcor + yp*yp); + + // dphi / dx + R(0, 0) = rdir*yp*invden; + // dphi / dy + R(0, 1) = -lxcor*invden; + // drho / dx + R(1, 0) = lxcor/rhostate; + // drho / dy + R(1, 1) = rdir*(rdir*lycor + radius)/rhostate; + + + dy0[0] = phihit - phistate; + dy0[1] = rhohit - rhostate; + + // std::cout << "wedge hit, original x = " << preciseHit->localPosition().x() << " y = " << preciseHit->localPosition().y() << " corrected x = " << hitx << " y = " << hity << std::endl; + } } - - // rxfull.row(ivalidhit) = R.row(0).cast(); - // ryfull.row(ivalidhit) = R.row(1).cast(); - - // validdxeigjac.block<2,2>(2*ivalidhit, 3*(ihit+1)) = R*Hp.bottomRightCorner<2,2>(); - - const Matrix Ralign = R.cast(); - - Matrix dx = Matrix::Zero(); - for (unsigned int j=0; j dalpha = Matrix::Zero(); - // order in which to use parameters, especially relevant in case nlocalalignment < 6 - constexpr std::array alphaidxs = {{5, 0, 1, 2, 3, 4}}; - -// for (unsigned int idim=0; idimgeographicalId())); -// dalpha[alphaidxs[idim]] = AlignScalar(corparms_[xglobalidx]); -// } - - for (unsigned int idim=0; idim A = Matrix::Zero(); - - const double localqopval = localparms[0]; - const double localdxdzval = localparms[1]; - const double localdydzval = localparms[2]; - const double localxval = localparms[3]; - const double localyval = localparms[4]; - + Matrix Aval = Matrix::Zero(); + + // const Matrix &localparmsalign = alignGlued_ ? globalToLocal(updtsos, surfaceglued) : localparms; + + Matrix localparmsalign = localparms; + if (alignGlued_) { + localparmsalign = globalToLocal(updtsos, surfaceglued); + } + + const double localqopval = localparmsalign[0]; + const double localdxdzval = localparmsalign[1]; + const double localdydzval = localparmsalign[2]; + const double localxval = localparmsalign[3]; + const double localyval = localparmsalign[4]; + + //standard case + // dx/dx - A(0,0) = AlignScalar(1.); + Aval(0,0) = -1.; // dy/dy - A(1,1) = AlignScalar(1.); + Aval(1,1) = -1.; // dx/dz - A(0,2) = localdxdzval; + Aval(0,2) = localdxdzval; // dy/dz - A(1,2) = localdydzval; + Aval(1,2) = localdydzval; // dx/dtheta_x - A(0,3) = -localyval*localdxdzval; + Aval(0,3) = localyval*localdxdzval; // dy/dtheta_x - A(1,3) = -localyval*localdydzval; + Aval(1,3) = localyval*localdydzval; // dx/dtheta_y - A(0,4) = -localxval*localdxdzval; + Aval(0,4) = -localxval*localdxdzval; // dy/dtheta_y - A(1,4) = -localxval*localdydzval; + Aval(1,4) = -localxval*localdydzval; // dx/dtheta_z - A(0,5) = -localyval; + Aval(0,5) = localyval; // dy/dtheta_z - A(1,5) = localxval; - - - // std::cout << "strip local z shift gradient: " << (Ralign*A.col(2))[0].value().value() << std::endl; - - // rotation from alignment basis to module local coordinates - // Matrix A; - // if (isglued) { - // const GlobalVector modx = preciseHit->det()->surface().toGlobal(LocalVector(1.,0.,0.)); - // const GlobalVector mody = preciseHit->det()->surface().toGlobal(LocalVector(0.,1.,0.)); - // - // const GlobalVector gluedx = parmDet->surface().toGlobal(LocalVector(1.,0.,0.)); - // const GlobalVector gluedy = parmDet->surface().toGlobal(LocalVector(0.,1.,0.)); - // - // A(0,0) = AlignScalar(modx.dot(gluedx)); - // A(0,1) = AlignScalar(modx.dot(gluedy)); - // A(1,0) = AlignScalar(mody.dot(gluedx)); - // A(1,1) = AlignScalar(mody.dot(gluedy)); - // } - // else { - // A = Matrix::Identity(); - // } - // - // Matrix dh = dy0 - R*Hu*dx - R*A*dalpha; - - + Aval(1,5) = -localxval; + + // const Matrix &A = alignGlued_ ? Rglued*Aval : Aval; + + Matrix A = Aval; + if (alignGlued_) { + A = Rglued*Aval; + } + double thetaincidence = std::asin(1./std::sqrt(std::pow(localdxdzval,2) + std::pow(localdydzval,2) + 1.)); - // bool morehitquality = applyHitQuality_ ? thetaincidence > 0.25 : true; bool morehitquality = true; - + if (morehitquality) { nValidHitsFinal++; if (ispixel) { @@ -1712,59 +1492,38 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::produce(edm::Event &iEvent, const } } else { - Vinv = Matrix::Zero(); + Vinv = Matrix::Zero(); } - Matrix dh = dy0 - Ralign*Hu*dx - Ralign*A*dalpha; - AlignScalar chisq = dh.transpose()*Vinv*dh; - -// std::cout << "icons = " << icons << " iiter = " << iiter << " ihit = " << ihit << " chisq = " << chisq.value().value() << std::endl; - - chisq0val += chisq.value().value(); + constexpr std::array alphaidxs = {{0, 2, 3, 4, 5, 1}}; - auto const& gradlocal = chisq.value().derivatives(); - //fill local hessian - Matrix hesslocal; - for (unsigned int j=0; j Fhit; + //TODO figure out why templated version doesn't work here (gcc bug?) + Fhit.leftCols(2) = -R*Hu; + + for (unsigned int ialign = 0; ialign < nlocalalignment; ++ialign) { + Fhit.col(ialign + 2) = -R*A.col(alphaidxs[ialign]); } - + + const double hitchisq = dy0.transpose()*Vinv*dy0; + const Matrix hitgrad = 2.*Fhit.transpose()*Vinv*dy0; + const Matrix hithess = 2.*Fhit.transpose()*Vinv*Fhit; + constexpr std::array localsizes = {{ nlocalstate, nlocalparms }}; constexpr std::array localidxs = {{ localstateidx, localparmidx }}; const std::array fullidxs = {{ fullstateidx, fullparmidx }}; - + + chisq0val += hitchisq; + for (unsigned int iidx = 0; iidx < localidxs.size(); ++iidx) { - gradfull.segment(fullidxs[iidx], localsizes[iidx]) += gradlocal.segment(localidxs[iidx], localsizes[iidx]); + gradfull.segment(fullidxs[iidx], localsizes[iidx]) += hitgrad.segment(localidxs[iidx], localsizes[iidx]); for (unsigned int jidx = 0; jidx < localidxs.size(); ++jidx) { - hessfull.block(fullidxs[iidx], fullidxs[jidx], localsizes[iidx], localsizes[jidx]) += hesslocal.block(localidxs[iidx], localidxs[jidx], localsizes[iidx], localsizes[jidx]); + hessfull.block(fullidxs[iidx], fullidxs[jidx], localsizes[iidx], localsizes[jidx]) += hithess.block(localidxs[iidx], localidxs[jidx], localsizes[iidx], localsizes[jidx]); } } - // Matrix gradloctest0; - // Matrix gradloctest1; - // Matrix gradloctest2; - - // std::cout << "nlocalalignment: " << nlocalalignment << " nlocal: " << nlocal << std::endl; - // std::cout << "gradlocal type: " << typeid(gradlocal).name() << std::endl; - // std::cout << "gradloctest0 type: " << typeid(gradloctest0).name() << std::endl; - // std::cout << "gradloctest1 type: " << typeid(gradloctest1).name() << std::endl; - // std::cout << "gradloctest2 type: " << typeid(gradloctest2).name() << std::endl; - // - // std::cout << "nhits: " << nhits << " nvalid: " << nvalid << " nvalidalign2d: " << nvalidalign2d << " ihit: " << ihit << std::endl; - // std::cout << "gradfull.size(): " << gradfull.size() << " nlocalstate: " << nlocalstate << " fullstateidx: " << fullstateidx << " nlocalparms: " << nlocalparms << " fullparmidx: " << fullparmidx << std::endl; - - // FIXME the templated block functions don't work here for some reason - //fill global gradient - // gradfull.segment(fullstateidx) += gradlocal.head(nlocalstate); - // gradfull.segment(fullparmidx) += gradlocal.segment(localparmidx, nlocalparms); - // - // //fill global hessian (upper triangular blocks only) - // hessfull.block(fullstateidx, fullstateidx) += hesslocal.topLeftCorner(nlocalstate,nlocalstate); - // hessfull.block(fullstateidx, fullparmidx) += hesslocal.topRightCorner(nlocalstate, nlocalparms); - // hessfull.block(fullparmidx, fullparmidx) += hesslocal.bottomRightCorner(nlocalparms, nlocalparms); - for (unsigned int idim=0; idimgeographicalId())); + const unsigned int xglobalidx = detidparms.at(std::make_pair(alphaidxs[idim], aligndetid)); globalidxv[nparsBfield + nparsEloss + alignmentparmidx] = xglobalidx; alignmentparmidx++; if (alphaidxs[idim]==0) { @@ -1772,24 +1531,21 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::produce(edm::Event &iEvent, const } } }; - - + if (align2d) { - fillAlignGrads(std::integral_constant()); + fillAlignGrads(std::integral_constant()); } else { - fillAlignGrads(std::integral_constant()); + fillAlignGrads(std::integral_constant()); } - } - } if (!valid) { break; } - trackstateidx += 3 + 5*tracknhits; + trackstateidx += 5*tracknhits; } if (!valid) { @@ -1798,10 +1554,28 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::produce(edm::Event &iEvent, const // MatrixXd massjac; + const Matrix &refFts0 = refftsarr[0]; + const Matrix &refFts1 = refftsarr[1]; + + const Matrix mhess = massHessianAltD(refFts0, refFts1, mmu); + const Matrix mhessinvsq = massinvsqHessianAltD(refFts0, refFts1, mmu); + + const double dmassconv = iiter > 0 ? 0.5*(mhess*covrefmom).trace() : 0.; + const double dmassconvinvsq = iiter > 0 ? 0.5*(mhessinvsq*covrefmom).trace() : 0.; + + + dmassconvval = dmassconv; + dinvmasssqconvval = dmassconvinvsq; + + if (icons == 0) { + dmassconvval_cons0 = dmassconv; + dinvmasssqconvval_cons0 = dmassconvinvsq; + } + // add mass constraint to gbl fit -// if (icons == 1 && iiter > 3) { if (icons > 0) { -// if (false) { + //TODO simplify this to treat the 6 parameters contiguously (now that they are contiguous in the original vector) + constexpr unsigned int nlocalstate0 = 3; constexpr unsigned int nlocalstate1 = 3; @@ -1810,20 +1584,11 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::produce(edm::Event &iEvent, const constexpr unsigned int localstateidx0 = 0; constexpr unsigned int localstateidx1 = localstateidx0 + nlocalstate0; - const unsigned int fullstateidx0 = trackstateidxarr[0]; - const unsigned int fullstateidx1 = trackstateidxarr[1]; + const unsigned int fullstateidx0 = 0; + const unsigned int fullstateidx1 = 3; using MScalar = AANT; - //TODO optimize to avoid recomputation of FTS - // const FreeTrajectoryState refFts0 = outparts[0]->currentState().freeTrajectoryState(); - // const FreeTrajectoryState refFts1 = outparts[1]->currentState().freeTrajectoryState(); - -// const FreeTrajectoryState &refFts0 = refftsarr[0]; -// const FreeTrajectoryState &refFts1 = refftsarr[1]; - - const Matrix &refFts0 = refftsarr[0]; - const Matrix &refFts1 = refftsarr[1]; const ROOT::Math::PxPyPzMVector mom0(refFts0[3], refFts0[4], @@ -1837,115 +1602,25 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::produce(edm::Event &iEvent, const const double massval = (mom0 + mom1).mass(); - // const double mrval = 1./massval/massval; - - // const double mrconstraintval = 1./massconstraintval/massconstraintval; - - // const double - - const Matrix &FdFmref0 = FdFmrefarr[0]; - const Matrix &FdFmref1 = FdFmrefarr[1]; - -// JacobianCurvilinearToCartesian curv2cartref0(refFts0.parameters()); -// auto const &jacCurv2Cartref0 = Map>(curv2cartref0.jacobian().Array()); -// -// JacobianCurvilinearToCartesian curv2cartref1(refFts1.parameters()); -// auto const &jacCurv2Cartref1 = Map>(curv2cartref1.jacobian().Array()); - - // const Matrix m2jac = massJacobian(refFts0, refFts1, mmu); - - // std::cout << "massval = " << massval << std::endl; - const Matrix mjacalt = massJacobianAltD(refFts0, refFts1, mmu); - -// const Matrix mjacalt = (1./massval)*massJacobianAlt(refFts0, refFts1, mmu); - - - // const Matrix mjacalt = mrJacobian(refFts0, refFts1, mmu); - - // const Matrix mjac0 = m2jac.leftCols<3>()*jacCurv2Cartref0.bottomLeftCorner<3, 3>(); - // const Matrix mjac1 = m2jac.rightCols<3>()*jacCurv2Cartref1.bottomLeftCorner<3, 3>(); - - // const Matrix mjacalt0 = mjacalt.leftCols<3>(); - // const Matrix mjacalt1 = mjacalt.rightCols<3>(); - - // std::cout << "mjac0" << std::endl; - // std::cout << mjac0 << std::endl; - // std::cout << "mjacalt0" << std::endl; - // std::cout << mjacalt0 << std::endl; - // - // std::cout << "mjac1" << std::endl; - // std::cout << mjac1 << std::endl; - // std::cout << "mjacalt1" << std::endl; - // std::cout << mjacalt1 << std::endl; - - // initialize active scalars for common vertex parameters -// Matrix dvtx = Matrix::Zero(); -// for (unsigned int j=0; j dmomcurv0 = Matrix::Zero(); - for (unsigned int j=0; j dmomcurv1 = Matrix::Zero(); - for (unsigned int j=0; j dmom0 = jacCurv2Cartref0.bottomLeftCorner<3, 3>().cast()*dmomcurv0; - // const Matrix dmom1 = jacCurv2Cartref1.bottomLeftCorner<3, 3>().cast()*dmomcurv1; - - // resonance width - // const MScalar invSigmaMsq(0.25/massConstraint_/massConstraint_/massConstraintWidth_/massConstraintWidth_); - // const MScalar dmsq0 = MScalar(m0*m0 - massConstraint_*massConstraint_); - - const MScalar invSigmaMsq(1./massConstraintWidth_/massConstraintWidth_); - const MScalar dmsq0 = MScalar(massval - massconstraintval); +// const double dmsq0 = massval - massconstraintval; + const double dmsq0 = massval - massconstraintval - dmassconv; - -// const double sigmalnm = massConstraintWidth_/massconstraintval; -// const MScalar invSigmaMsq(1./sigmalnm/sigmalnm); -// const MScalar dmsq0 = MScalar(std::log(massval) - std::log(massconstraintval)); - - // const MScalar invSigmaMsq(0.25*std::pow(massval, 6)/massConstraintWidth_/massConstraintWidth_); - // const MScalar dmsq0 = MScalar(mrval - mrconstraintval); - // const MScalar dmsq0 = MScalar(massval - massConstraint_); - // const MScalar dmsq0 = MScalar(0.); - // const MScalar dmsq0 = MScalar(m0 - massConstraint_); - - - // std::cout << "invSigmaMsq = " << invSigmaMsq.value().value() << std::endl; - - // const MScalar dmsqtrack0 = (m2jac.leftCols<3>().cast()*dmom0)[0]; - // const MScalar dmsqtrack1 = (m2jac.rightCols<3>().cast()*dmom1)[0]; - - const MScalar dmsqtrack0 = (mjacalt.leftCols<3>().cast()*dmomcurv0)[0]; - const MScalar dmsqtrack1 = (mjacalt.rightCols<3>().cast()*dmomcurv1)[0]; - - const MScalar dmsq = dmsq0 + dmsqtrack0 + dmsqtrack1; - - // const MScalar chisq = dmsq*invSigmaMsq*dmsq; - const MScalar chisq = invSigmaMsq*dmsq*dmsq; - - chisq0val += chisq.value().value(); - - auto const& gradlocal = chisq.value().derivatives(); - //fill local hessian - Matrix hesslocal; - for (unsigned int j=0; j &Fmass = mjacalt; + + const double invSigmaMsq = 1./massConstraintWidth_/massConstraintWidth_; + + const double masschisq = dmsq0*dmsq0*invSigmaMsq; + const Matrix gradlocal = 2.*Fmass.transpose()*invSigmaMsq*dmsq0; + const Matrix hesslocal = 2.*Fmass.transpose()*invSigmaMsq*Fmass; constexpr std::array localsizes = {{ nlocalstate0, nlocalstate0 }}; constexpr std::array localidxs = {{ localstateidx0, localstateidx1 }}; const std::array fullidxs = {{ fullstateidx0, fullstateidx1 }}; + chisq0val += masschisq; + for (unsigned int iidx = 0; iidx < localidxs.size(); ++iidx) { gradfull.segment(fullidxs[iidx], localsizes[iidx]) += gradlocal.segment(localidxs[iidx], localsizes[iidx]); for (unsigned int jidx = 0; jidx < localidxs.size(); ++jidx) { @@ -1985,14 +1660,14 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::produce(edm::Event &iEvent, const }; if (fitFromGenParms_) { - for (unsigned int i=0; i<3; ++i) { + for (unsigned int i=0; i<10; ++i) { freezeparm(i); } - for (unsigned int id = 0; id < 2; ++id) { - for (unsigned int i=0; i<3; ++i) { - freezeparm(trackstateidxarr[id] + i); - } - } + } + + if (doVtxConstraint_) { + // freeze track pca distance to 0 to impose common vertex constraint + freezeparm(6); } // if (fitFromGenParms_) { @@ -2098,11 +1773,16 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::produce(edm::Event &iEvent, const chisqvalold = chisq0val + deltachisq[0]; - ndof = 5*nhits + nvalid + nvalidalign2d - nstateparms; +// ndof = 5*nhits + nvalid + nvalidalign2d - nstateparms; + ndof = 5*nhits + nvalid + nvalidpixel - nstateparms; if (bsConstraint_) { ndof += 3; } + + if (doVtxConstraint_) { + ++ndof; + } if (icons == 1) { ++ndof; @@ -2184,17 +1864,26 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::produce(edm::Event &iEvent, const } // apply the GBL fit results to the vertex position - const Vector3d vtxpos = refftsarr[0].head<3>() + dxfull.head<3>(); - + const Matrix statepca = twoTrackCart2pca(refftsarr[0], refftsarr[1]); + const Matrix statepcaupd = statepca + dxfull.head<10>(); + +// std::cout << "statepcaupd d = " << statepcaupd[6] << std::endl; + + const bool firstplus = statepcaupd[0] > 0.; + if (icons == 0) { - Jpsi_x = vtxpos[0]; - Jpsi_y = vtxpos[1]; - Jpsi_z = vtxpos[2]; + // define sign of d wrt charge of tracks + Jpsi_d = firstplus ? statepcaupd[6] : -statepcaupd[6]; + Jpsi_x = statepcaupd[7]; + Jpsi_y = statepcaupd[8]; + Jpsi_z = statepcaupd[9]; } else { - Jpsicons_x = vtxpos[0]; - Jpsicons_y = vtxpos[1]; - Jpsicons_z = vtxpos[2]; + // define sign of d wrt charge of tracks + Jpsicons_d = firstplus ? statepcaupd[6] : -statepcaupd[6]; + Jpsicons_x = statepcaupd[7]; + Jpsicons_y = statepcaupd[8]; + Jpsicons_z = statepcaupd[9]; } std::array muarr; @@ -2206,16 +1895,11 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::produce(edm::Event &iEvent, const // apply the GBL fit results to the muon kinematics for (unsigned int id = 0; id < 2; ++id) { const Matrix &refFts = refftsarr[id]; - const unsigned int trackstateidx = trackstateidxarr[id]; + const unsigned int trackstateidx = 3*id; - - const double qbp = refFts[6]/refFts.segment<3>(3).norm(); - const double lam = std::atan(refFts[5]/std::sqrt(refFts[3]*refFts[3] + refFts[4]*refFts[4])); - const double phi = std::atan2(refFts[4], refFts[3]); - - const double qbpupd = qbp + dxfull(trackstateidx); - const double lamupd = lam + dxfull(trackstateidx + 1); - const double phiupd = phi + dxfull(trackstateidx + 2); + const double qbpupd = statepcaupd[trackstateidx]; + const double lamupd = statepcaupd[trackstateidx + 1]; + const double phiupd = statepcaupd[trackstateidx + 2]; const double charge = std::copysign(1., qbpupd); const double pupd = std::abs(1./qbpupd); @@ -2328,8 +2012,26 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::produce(edm::Event &iEvent, const Muminuskincons_phi = outparts[idxminus]->currentState().globalMomentum().phi(); } - // std::cout << "Muplus pt, eta, phi = " << Muplus_pt << ", " << Muplus_eta << ", " << Muplus_phi << std::endl; - // std::cout << "Muminus pt, eta, phi = " << Muminus_pt << ", " << Muminus_eta << ", " << Muminus_phi << std::endl; +// std::cout << "Muplus pt, eta, phi = " << Muplus_pt << ", " << Muplus_eta << ", " << Muplus_phi << std::endl; +// std::cout << "Muminus pt, eta, phi = " << Muminus_pt << ", " << Muminus_eta << ", " << Muminus_phi << std::endl; + + + MatrixXd covstate = 2.*Cinvd.solve(MatrixXd::Identity(nstateparms,nstateparms)); + + covrefmom = covstate.topLeftCorner<6, 6>(); + + +// Matrix covrefmom; +// covrefmom = Matrix::Zero(); +// +// constexpr std::array localidxs = {{ 0, 3 }}; +// const std::array globalidxs = {{ trackstateidxarr[0], trackstateidxarr[1] }}; +// +// for (unsigned int iidx = 0; iidx < localidxs.size(); ++iidx) { +// for (unsigned int jidx = 0; jidx < localidxs.size(); ++jidx) { +// covrefmom.block<3, 3>(localidxs[iidx], localidxs[jidx]) = covstate.block<3, 3>(globalidxs[iidx], globalidxs[jidx]); +// } +// } if (icons == 0) { @@ -2346,18 +2048,18 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::produce(edm::Event &iEvent, const // Map>(Muminus_jacRef.data(), 3, npars) = dxdparms.block(0, trackstateidxarr[idxminus], npars, 3).transpose().cast(); - MatrixXd covstate = 2.*Cinvd.solve(MatrixXd::Identity(nstateparms,nstateparms)); - - Matrix covrefmom; +// MatrixXd covstate = 2.*Cinvd.solve(MatrixXd::Identity(nstateparms,nstateparms)); - constexpr std::array localidxs = {{ 0, 3 }}; - const std::array globalidxs = {{ trackstateidxarr[0], trackstateidxarr[1] }}; - - for (unsigned int iidx = 0; iidx < localidxs.size(); ++iidx) { - for (unsigned int jidx = 0; jidx < localidxs.size(); ++jidx) { - covrefmom.block<3, 3>(localidxs[iidx], localidxs[jidx]) = covstate.block<3, 3>(globalidxs[iidx], globalidxs[jidx]); - } - } +// Matrix covrefmom; +// +// constexpr std::array localidxs = {{ 0, 3 }}; +// const std::array globalidxs = {{ trackstateidxarr[0], trackstateidxarr[1] }}; +// +// for (unsigned int iidx = 0; iidx < localidxs.size(); ++iidx) { +// for (unsigned int jidx = 0; jidx < localidxs.size(); ++jidx) { +// covrefmom.block<3, 3>(localidxs[iidx], localidxs[jidx]) = covstate.block<3, 3>(globalidxs[iidx], globalidxs[jidx]); +// } +// } @@ -2411,6 +2113,9 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::produce(edm::Event &iEvent, const Muminus_nmatchedvalid = nmatchedvalidarr[idxminus]; Muminus_nambiguousmatchedvalid = nambiguousmatchedvalidarr[idxminus]; + Muplus_highpurity = highpurityarr[idxplus]; + Muminus_highpurity = highpurityarr[idxminus]; + const ROOT::Math::PxPyPzMVector mompluskin(outparts[idxplus]->currentState().globalMomentum().x(), outparts[idxplus]->currentState().globalMomentum().y(), outparts[idxplus]->currentState().globalMomentum().z(), @@ -2496,6 +2201,8 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::produce(edm::Event &iEvent, const Jpsigen_x = muplusgen->vx(); Jpsigen_y = muplusgen->vy(); Jpsigen_z = muplusgen->vz(); + + genl3d = std::sqrt((muplusgen->vertex() - *genXyz0).mag2()); } else { Jpsigen_pt = -99.; @@ -2506,6 +2213,8 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::produce(edm::Event &iEvent, const Jpsigen_x = -99.; Jpsigen_y = -99.; Jpsigen_z = -99.; + + genl3d = -99.; } @@ -2566,6 +2275,26 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::produce(edm::Event &iEvent, const niter = iiter + 1; edmval = -deltachisq[0]; + + const Matrix dxRef = dxfull.head<10>(); + const Matrix covref = covstate.topLeftCorner<10, 10>(); + + const Matrix hessref = covref.inverse(); + + const double deltachisqref = -0.5*dxRef.transpose()*hessref*dxRef; + + edmvalref = -deltachisqref; + + if (std::isnan(edmval) || std::isinf(edmval)) { + std::cout << "WARNING: invalid parameter update!!!" << " edmval = " << edmval << " deltachisqval = " << deltachisqval << std::endl; + valid = false; + break; + } + + if (icons == 0) { + edmval_cons0 = edmval; + niter_cons0 = niter; + } // std::cout << "icons = " << icons << " iiter = " << iiter << " edmval = " << edmval << " deltachisqval = " << deltachisqval << " chisqval = " << chisqval << std::endl; // std::cout << "dxvtx" << std::endl; @@ -2594,7 +2323,14 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::produce(edm::Event &iEvent, const // break; // } - if (edmval < 1e-5) { +// if (iiter > 0 && edmval < 1e-5) { +// break; +// } + + if (iiter > 0 && dolocalupdate && edmval < 1e-5) { + break; + } + else if (iiter > 0 && !dolocalupdate && edmvalref < 1e-5) { break; } @@ -2654,13 +2390,24 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::produce(edm::Event &iEvent, const const unsigned int idxplus = muchargearr[0] > 0 ? 0 : 1; const unsigned int idxminus = muchargearr[0] > 0 ? 1 : 0; + const unsigned int trackstateidxplus = 3*idxplus; + const unsigned int trackstateidxminus = 3*idxminus; + Muplus_jacRef.resize(3*nparsfinal); - Map>(Muplus_jacRef.data(), 3, nparsfinal) = dxdparms.block(0, trackstateidxarr[idxplus], nparsfinal, 3).transpose().cast(); + Map>(Muplus_jacRef.data(), 3, nparsfinal) = dxdparms.block(0, trackstateidxplus, nparsfinal, 3).transpose().cast(); Muminus_jacRef.resize(3*nparsfinal); - Map>(Muminus_jacRef.data(), 3, nparsfinal) = dxdparms.block(0, trackstateidxarr[idxminus], nparsfinal, 3).transpose().cast(); + Map>(Muminus_jacRef.data(), 3, nparsfinal) = dxdparms.block(0, trackstateidxminus, nparsfinal, 3).transpose().cast(); + + if (false) { + std::cout << "Muplus pt eta phi = " << Muplus_pt << " " << Muplus_eta << " " << Muplus_phi << std::endl; + std::cout << "Muminus pt eta phi = " << Muminus_pt << " " << Muminus_eta << " " << Muminus_phi << std::endl; + + std::cout << "Muplus_jacref:\n" << Map>(Muplus_jacRef.data(), 3, nparsfinal) << std::endl; + std::cout << "Muminus_jacref:\n" << Map>(Muminus_jacRef.data(), 3, nparsfinal) << std::endl; + } } - + } if (!valid) { @@ -2868,7 +2615,7 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::produce(edm::Event &iEvent, const // // std::cout << "sigmam" << std::endl; // std::cout << std::sqrt(covmass[0]) << std::endl; - + // // std::cout << "cinvrow0" << std::endl; // std::cout << cinvrow0 << std::endl; @@ -2885,32 +2632,5 @@ void ResidualGlobalCorrectionMakerTwoTrackG4e::produce(edm::Event &iEvent, const } -Matrix ResidualGlobalCorrectionMakerTwoTrackG4e::massJacobian(const FreeTrajectoryState &state0, const FreeTrajectoryState &state1, double dmass) const { - Matrix res = Matrix::Zero(); - - const double e0 = std::sqrt(state0.momentum().mag2() + dmass*dmass); - const double e1 = std::sqrt(state1.momentum().mag2() + dmass*dmass); - - // dm^2/dp0x - res(0, 0) = 2.*e1*state0.momentum().x()/e0 - 2.*state1.momentum().x(); - // dm^2/dp0y - res(0, 1) = 2.*e1*state0.momentum().y()/e0 - 2.*state1.momentum().y(); - // dm^2/dp0z - res(0, 2) = 2.*e1*state0.momentum().z()/e0 - 2.*state1.momentum().z(); - - // d^m/dp1x - res(0, 3) = 2.*e0*state1.momentum().x()/e1 - 2.*state0.momentum().x(); - // d^m/dp1y - res(0, 4) = 2.*e0*state1.momentum().y()/e1 - 2.*state0.momentum().y(); - // d^m/dp1z - res(0, 5) = 2.*e0*state1.momentum().z()/e1 - 2.*state0.momentum().z(); - - const double m = std::sqrt(2.*dmass*dmass + 2.*e0*e1 - 2.*state0.momentum().x()*state1.momentum().x() - 2.*state0.momentum().y()*state1.momentum().y() - 2.*state0.momentum().z()*state1.momentum().z()); - - res *= 0.5/m; - - return res; -} - DEFINE_FWK_MODULE(ResidualGlobalCorrectionMakerTwoTrackG4e); diff --git a/MagneticField/ParametrizedEngine/src/ParametrizedMagneticFieldFactory.cc b/MagneticField/ParametrizedEngine/src/ParametrizedMagneticFieldFactory.cc index 97a1778df0940..e5cb1d566ef9e 100644 --- a/MagneticField/ParametrizedEngine/src/ParametrizedMagneticFieldFactory.cc +++ b/MagneticField/ParametrizedEngine/src/ParametrizedMagneticFieldFactory.cc @@ -10,6 +10,7 @@ #include "OAEParametrizedMagneticField.h" #include "ParabolicParametrizedMagneticField.h" #include "PolyFit2DParametrizedMagneticField.h" +#include "PolyFit3DParametrizedMagneticField.h" #include "FWCore/Utilities/interface/Exception.h" @@ -32,7 +33,8 @@ ParametrizedMagneticFieldFactory::get(string version, const ParameterSet& parame return result; } else if (version=="PolyFit3D") { // V. Maroussov polynomial fit to mapping data - throw cms::Exception("InvalidParameter")<<"PolyFit3D is not supported anymore"; + std::auto_ptr result( new PolyFit3DParametrizedMagneticField(parameters)); + return result; } else if (version=="Parabolic"){ // FIXME implement configurable parameters to be passed to ctor // vector params = parameters.getParameter("parameters"); diff --git a/MagneticField/ParametrizedEngine/src/PolyFit3DParametrizedMagneticField.cc b/MagneticField/ParametrizedEngine/src/PolyFit3DParametrizedMagneticField.cc new file mode 100644 index 0000000000000..15ba721ff075f --- /dev/null +++ b/MagneticField/ParametrizedEngine/src/PolyFit3DParametrizedMagneticField.cc @@ -0,0 +1,92 @@ +/** \file + * + * \author N. Amapane + */ + +#include "PolyFit3DParametrizedMagneticField.h" +#include +#include + +#include "BFit3D.h" + + +using namespace std; +using namespace magfieldparam; + +PolyFit3DParametrizedMagneticField::PolyFit3DParametrizedMagneticField(double bVal) : + theParam(new BFit3D()) +{ + theParam->SetField(bVal); +} + + +PolyFit3DParametrizedMagneticField::PolyFit3DParametrizedMagneticField(const edm::ParameterSet& parameters) : theParam(new BFit3D()) { + theParam->SetField(parameters.getParameter("BValue")); + + // Additional options (documentation by Vassili): + + // By default, the package accepts signed value of "r". That means, + // one can cross r=0 and orientation of the coordinate "orts" + // e_r and e_phi will not be flipped over. + // In other words for an r<0 the e_r points inward, in the direction r=0. + // This is a "natural" mode. However, the default behavior may be + // changed by the call + + // theParam->UseSignedRad(false); + + // In that case with crossing of r=0 e_r and e_phi will be flipped in + // such a way that e_r always points outward. In other words instead of + // (r<0, phi) the (abs(r), phi+PI) will be used in this mode. + + // The expansion coefficients for a nominal field in between measurement + // field values (2.0T, 3.5T, 3.8T and 4.0T) by default are calculated by + // means of a linear piecewise interpolation. Another provided + // interpolation mode is cubic spline. This mode can be switched + // on by the call: + + // theParam->UseSpline(true); + + // From practical point of view the use of spline interpolation doesn't + // change much, but it makes the coefficients' behavior a bit more + // physical at very low/high field values. +} + + +PolyFit3DParametrizedMagneticField::~PolyFit3DParametrizedMagneticField() { + delete theParam; +} + + +GlobalVector +PolyFit3DParametrizedMagneticField::inTesla(const GlobalPoint& gp) const { + + if (isDefined(gp)) { + return inTeslaUnchecked(gp); + } else { + edm::LogWarning("MagneticField|FieldOutsideValidity") << " Point " << gp << " is outside the validity region of PolyFit3DParametrizedMagneticField"; + return GlobalVector(); + } +} + +GlobalVector +PolyFit3DParametrizedMagneticField::inTeslaUnchecked(const GlobalPoint& gp) const { + double Br, Bz, Bphi; + theParam->GetField(gp.perp()/100., gp.z()/100., gp.phi(), + Br, Bz, Bphi); + + double cosphi = cos(gp.phi()); + double sinphi = sin(gp.phi()); + + return GlobalVector(Br*cosphi - Bphi*sinphi, + Br*sinphi + Bphi*cosphi, + Bz); +} + +bool +PolyFit3DParametrizedMagneticField::isDefined(const GlobalPoint& gp) const { + double z = fabs(gp.z()); + double r = gp.perp(); + //"rectangle" |z|<3.5, r<1.9 _except_ the "corners" |z|+2.5*r>6.7, everything in meters + if (z>350. || r>190 || z+2.5*r>670.) return false; + return true; +} diff --git a/MagneticField/ParametrizedEngine/src/PolyFit3DParametrizedMagneticField.h b/MagneticField/ParametrizedEngine/src/PolyFit3DParametrizedMagneticField.h new file mode 100644 index 0000000000000..be3c57217ca81 --- /dev/null +++ b/MagneticField/ParametrizedEngine/src/PolyFit3DParametrizedMagneticField.h @@ -0,0 +1,40 @@ +#ifndef PolyFit3DParametrizedMagneticField_h +#define PolyFit3DParametrizedMagneticField_h + +/** \class PolyFit3DParametrizedMagneticField + * + * Magnetic Field engine wrapper for V. Maroussov's 3D parametrization + * of the MT data. + * + * \author N. Amapane + */ + +#include "MagneticField/Engine/interface/MagneticField.h" + +namespace edm { class ParameterSet; } +namespace magfieldparam { class BFit3D; } + + +class PolyFit3DParametrizedMagneticField : public MagneticField { + public: + /// Constructor. Fitted bVal for the nominal currents are: + /// 2.0216; 3.5162; 3.8114; 4.01242188708911 + PolyFit3DParametrizedMagneticField(double bVal = 3.8114); + + /// Constructor. Parameters taken from a PSet + PolyFit3DParametrizedMagneticField(const edm::ParameterSet& parameters); + + /// Destructor + virtual ~PolyFit3DParametrizedMagneticField(); + + GlobalVector inTesla (const GlobalPoint& gp) const; + + GlobalVector inTeslaUnchecked (const GlobalPoint& gp) const; + + bool isDefined(const GlobalPoint& gp) const; + + private: + magfieldparam::BFit3D* theParam; +}; +#endif + diff --git a/PhysicsTools/NanoAOD/interface/FlattenedValueMapVectorTableProducer.h b/PhysicsTools/NanoAOD/interface/FlattenedValueMapVectorTableProducer.h index e3f8881ed784a..b8f329a365594 100644 --- a/PhysicsTools/NanoAOD/interface/FlattenedValueMapVectorTableProducer.h +++ b/PhysicsTools/NanoAOD/interface/FlattenedValueMapVectorTableProducer.h @@ -84,10 +84,10 @@ class FlattenedValueMapVectorTableProducer : public edm::stream::EDProducer<> { iEvent.getByToken(intVecMaps_[i], vmap); const auto& results = readVals(*vmap, objs, sizes); - auto intsizetab = std::make_unique(objs.size(), this->name_+intNames_[i]+"_Counts", false, false); + auto intsizetab = std::make_unique(objs.size(), this->name_ + "_" + intNames_[i]+"_Counts", false, false); intsizetab->template addColumn("", sizes, "Number of entries per object", nanoaod::FlatTable::IntColumn, -1); intsizetab->setDoc(doc_); - auto intvectab = std::make_unique(results.size(), this->name_+intNames_[i], false, false); + auto intvectab = std::make_unique(results.size(), this->name_+ "_" + intNames_[i], false, false); intvectab->template addColumn("Vals", results, intDocs_[i], nanoaod::FlatTable::IntColumn, intPrecisions_[i]); intvectab->setDoc(doc_); @@ -100,10 +100,10 @@ class FlattenedValueMapVectorTableProducer : public edm::stream::EDProducer<> { iEvent.getByToken(floatVecMaps_[i], vmap); const auto& results = readVals(*vmap, objs, sizes); - auto floatsizetab = std::make_unique(objs.size(), this->name_+floatNames_[i]+"_Counts", false, false); + auto floatsizetab = std::make_unique(objs.size(), this->name_ + "_" + floatNames_[i]+"_Counts", false, false); floatsizetab->template addColumn("", sizes, "Number of entries per object", nanoaod::FlatTable::IntColumn, -1); floatsizetab->setDoc(doc_); - auto floatvectab = std::make_unique(results.size(), this->name_+floatNames_[i], false, false); + auto floatvectab = std::make_unique(results.size(), this->name_ + "_" + floatNames_[i], false, false); floatvectab->template addColumn("Vals", results, floatDocs_[i], nanoaod::FlatTable::FloatColumn, floatPrecisions_[i]); floatvectab->setDoc(doc_); diff --git a/PhysicsTools/NanoAOD/python/muons_cff.py b/PhysicsTools/NanoAOD/python/muons_cff.py index 617fdf11d776e..dfa93683724e3 100644 --- a/PhysicsTools/NanoAOD/python/muons_cff.py +++ b/PhysicsTools/NanoAOD/python/muons_cff.py @@ -128,36 +128,48 @@ trackrefit = cms.EDProducer('ResidualGlobalCorrectionMakerG4e', src = cms.InputTag("tracksfrommuons"), fitFromGenParms = cms.bool(False), + fitFromSimParms = cms.bool(False), fillTrackTree = cms.bool(False), fillGrads = cms.bool(False), + fillJac = cms.bool(False), fillRunTree = cms.bool(False), doGen = cms.bool(False), doSim = cms.bool(False), + requireGen = cms.bool(False), doMuons = cms.bool(False), doMuonAssoc = cms.bool(True), + doTrigger = cms.bool(False), + doRes = cms.bool(False), bsConstraint = cms.bool(False), applyHitQuality = cms.bool(True), - corFile = cms.string(""), + useIdealGeometry = cms.bool(False), + corFiles = cms.vstring(), ) -trackrefitbs = cms.EDProducer('ResidualGlobalCorrectionMakerG4e', +trackrefitideal = cms.EDProducer('ResidualGlobalCorrectionMakerG4e', src = cms.InputTag("tracksfrommuons"), fitFromGenParms = cms.bool(False), + fitFromSimParms = cms.bool(False), fillTrackTree = cms.bool(False), fillGrads = cms.bool(False), + fillJac = cms.bool(False), fillRunTree = cms.bool(False), doGen = cms.bool(False), doSim = cms.bool(False), + requireGen = cms.bool(False), doMuons = cms.bool(False), doMuonAssoc = cms.bool(True), - bsConstraint = cms.bool(True), + doTrigger = cms.bool(False), + doRes = cms.bool(False), + bsConstraint = cms.bool(False), applyHitQuality = cms.bool(True), - corFile = cms.string(""), + useIdealGeometry = cms.bool(True), + corFiles = cms.vstring(), ) mergedGlobalIdxs = cms.EDProducer("GlobalIdxProducer", src0 = cms.InputTag("trackrefit", "globalIdxs"), - src1 = cms.InputTag("trackrefitbs", "globalIdxs") + src1 = cms.InputTag("trackrefitideal", "globalIdxs") ) muonTable = cms.EDProducer("SimpleCandidateFlatTableProducer", @@ -229,28 +241,50 @@ cvhPhi = ExtVar(cms.InputTag("trackrefit:corPhi"), float, doc="Refitted track phi", precision=12), cvhCharge = ExtVar(cms.InputTag("trackrefit:corCharge"), int, doc="Refitted track charge"), cvhEdmval = ExtVar(cms.InputTag("trackrefit:edmval"), float, doc="Refitted estimated distance to minimum", precision=10), - cvhbsPt = ExtVar(cms.InputTag("trackrefitbs:corPt"), float, doc="Refitted track pt (with bs constraint)", precision=-1), - cvhbsEta = ExtVar(cms.InputTag("trackrefitbs:corEta"), float, doc="Refitted track eta (with bs constraint)", precision=12), - cvhbsPhi = ExtVar(cms.InputTag("trackrefitbs:corPhi"), float, doc="Refitted track phi (with bs constraint)", precision=12), - cvhbsCharge = ExtVar(cms.InputTag("trackrefitbs:corCharge"), int, doc="Refitted track charge (with bs constraint)"), - cvhbsEdmval = ExtVar(cms.InputTag("trackrefitbs:edmval"), float, doc="Refitted estimated distance to minimum (with bs constraint)", precision=10), + ), +) + +muonIdealTable = cms.EDProducer("SimpleCandidateFlatTableProducer", + src = muonTable.src, + cut = muonTable.cut, + name = muonTable.name, + singleton = cms.bool(False), # the number of entries is variable + extension = cms.bool(True), + variables = cms.PSet(), + externalVariables = cms.PSet( + cvhidealPt = ExtVar(cms.InputTag("trackrefitideal:corPt"), float, doc="Refitted track pt (with ideal geometry)", precision=-1), + cvhidealEta = ExtVar(cms.InputTag("trackrefitideal:corEta"), float, doc="Refitted track eta (with ideal geometry)", precision=12), + cvhidealPhi = ExtVar(cms.InputTag("trackrefitideal:corPhi"), float, doc="Refitted track phi (with ideal geometry)", precision=12), + cvhidealCharge = ExtVar(cms.InputTag("trackrefitideal:corCharge"), int, doc="Refitted track charge (with ideal geometry)"), + cvhidealEdmval = ExtVar(cms.InputTag("trackrefitideal:edmval"), float, doc="Refitted estimated distance to minimum (with ideal geometry)", precision=10), ), ) muonExternalVecVarsTable = cms.EDProducer("FlattenedCandValueMapVectorTableProducer", - name = cms.string(muonTable.name.value()+"_cvh"), + name = cms.string(muonTable.name.value()), src = muonTable.src, cut = muonTable.cut, doc = muonTable.doc, variables = cms.PSet( # can you declare a max number of bits here? technically for the moment this needs 16 bits, but might eventually need 17 or 18 - mergedGlobalIdxs = ExtVar(cms.InputTag("mergedGlobalIdxs"), "std::vector", doc="Indices for correction parameters"), + cvhmergedGlobalIdxs = ExtVar(cms.InputTag("trackrefit:globalIdxs"), "std::vector", doc="Indices for correction parameters"), # optimal precision tbd, but presumably can work the same way as for scalar floats - JacRef = ExtVar(cms.InputTag("trackrefit:jacRef"), "std::vector", doc="jacobian for corrections", precision = 12), - MomCov = ExtVar(cms.InputTag("trackrefit:momCov"), "std::vector", doc="covariance matrix for qop, lambda, phi", precision = 12), + cvhJacRef = ExtVar(cms.InputTag("trackrefit:jacRef"), "std::vector", doc="jacobian for corrections", precision = 12), + cvhMomCov = ExtVar(cms.InputTag("trackrefit:momCov"), "std::vector", doc="covariance matrix for qop, lambda, phi", precision = 12), + ) +) + +muonIdealExternalVecVarsTable = cms.EDProducer("FlattenedCandValueMapVectorTableProducer", + name = cms.string(muonTable.name.value()), + src = muonTable.src, + cut = muonTable.cut, + doc = muonTable.doc, + variables = cms.PSet( + # can you declare a max number of bits here? technically for the moment this needs 16 bits, but might eventually need 17 or 18 + cvhmergedGlobalIdxs = ExtVar(cms.InputTag("mergedGlobalIdxs"), "std::vector", doc="Indices for correction parameters"), # optimal precision tbd, but presumably can work the same way as for scalar floats - bsJacRef = ExtVar(cms.InputTag("trackrefitbs:jacRef"), "std::vector", doc="Jacobian for corrections (with bs constraint)", precision = 12), - bsMomCov = ExtVar(cms.InputTag("trackrefitbs:momCov"), "std::vector", doc="covariance matrix for qop, lambda, phi (with bs constraint)", precision = 12), + cvhidealJacRef = ExtVar(cms.InputTag("trackrefitideal:jacRef"), "std::vector", doc="jacobian for corrections (with ideal geometry)", precision = 12), + cvhidealMomCov = ExtVar(cms.InputTag("trackrefitideal:momCov"), "std::vector", doc="covariance matrix for qop, lambda, phi (with ideal geometry)", precision = 12), ) ) @@ -258,8 +292,6 @@ for modifier in run2_miniAOD_80XLegacy, run2_nanoAOD_94X2016, run2_nanoAOD_94XMiniAODv1, run2_nanoAOD_94XMiniAODv2, run2_nanoAOD_LowPU: modifier.toModify(muonTable.variables, puppiIsoId = None, softMva = None) -run2_nanoAOD_LowPU.toModify(muonTable, externalVariables = cms.PSet()) - run2_nanoAOD_102Xv1.toModify(muonTable.variables, puppiIsoId = None) (run2_nanoAOD_106Xv1 & ~run2_nanoAOD_devel).toModify(muonTable.variables, isStandalone = None) @@ -285,9 +317,13 @@ docString = cms.string("MC matching to status==1 muons"), ) -muonSequence = cms.Sequence(slimmedMuonsUpdated+isoForMu + ptRatioRelForMu + slimmedMuonsWithUserData + finalMuons + finalLooseMuons ) -muonMC = cms.Sequence(muonsMCMatchForTable + muonMCTable) -muonTables = cms.Sequence(muonFSRphotons + muonFSRassociation + muonMVATTH + muonMVALowPt + geopro + tracksfrommuons + trackrefit + trackrefitbs + mergedGlobalIdxs + muonTable + muonExternalVecVarsTable + fsrTable) +muonSequence = cms.Sequence(slimmedMuonsUpdated+isoForMu + ptRatioRelForMu + slimmedMuonsWithUserData + finalMuons + finalLooseMuons) +muonMC = cms.Sequence(trackrefitideal + mergedGlobalIdxs + muonsMCMatchForTable + muonMCTable + muonIdealTable + muonIdealExternalVecVarsTable) +muonTables = cms.Sequence(muonFSRphotons + muonFSRassociation + muonMVATTH + muonMVALowPt + geopro + tracksfrommuons + trackrefit + muonTable + muonExternalVecVarsTable + fsrTable) + +# remove track refit stuff for low pu +run2_nanoAOD_LowPU.toReplaceWith(muonTables, muonTables.copyAndExclude([geopro, tracksfrommuons, trackrefit, muonExternalVecVarsTable])) +run2_nanoAOD_LowPU.toReplaceWith(muonMC, muonMC.copyAndExclude([trackrefitideal, mergedGlobalIdxs, muonIdealTable, muonIdealExternalVecVarsTable])) +run2_nanoAOD_LowPU.toModify(muonTable, externalVariables = cms.PSet()) -run2_nanoAOD_LowPU.toReplaceWith(muonTables, muonTables.copyAndExclude([geopro, tracksfrommuons, trackrefit, trackrefitbs, mergedGlobalIdxs, muonExternalVecVarsTable])) diff --git a/PhysicsTools/NanoAOD/python/nano_cff.py b/PhysicsTools/NanoAOD/python/nano_cff.py index ca8d33ed2bb92..18b2857f5f4c0 100644 --- a/PhysicsTools/NanoAOD/python/nano_cff.py +++ b/PhysicsTools/NanoAOD/python/nano_cff.py @@ -443,6 +443,21 @@ def nanoAOD_customizeData(process): process = nanoAOD_recalibrateMETs(process,isData=True) for modifier in run2_nanoAOD_94XMiniAODv1, run2_nanoAOD_94XMiniAODv2: modifier.toModify(process, lambda p: nanoAOD_runMETfixEE2017(p,isData=True)) + + # load geometry needed by g4e propagator + process.GlobalTag.toGet = cms.VPSet( + cms.PSet( + record = cms.string("GeometryFileRcd"),tag = cms.string("XMLFILE_Geometry_2016_81YV1_Extended2016_mc"),label = cms.untracked.string("Extended"), + ), + ) + + # load 3d field map and use it for g4e propagator + from MagneticField.ParametrizedEngine.parametrizedMagneticField_PolyFit3D_cfi import ParametrizedMagneticFieldProducer as PolyFit3DMagneticFieldProducer + process.PolyFit3DMagneticFieldProducer = PolyFit3DMagneticFieldProducer + fieldlabel = "PolyFit3DMf" + process.PolyFit3DMagneticFieldProducer.label = fieldlabel + process.Geant4ePropagator.MagneticFieldLabel = fieldlabel + return process def nanoAOD_customizeMC(process): @@ -450,6 +465,8 @@ def nanoAOD_customizeMC(process): process = nanoAOD_recalibrateMETs(process,isData=False) for modifier in run2_nanoAOD_94XMiniAODv1, run2_nanoAOD_94XMiniAODv2: modifier.toModify(process, lambda p: nanoAOD_runMETfixEE2017(p,isData=False)) + # remove duplicate branch with incomplete content *TODO* make this more elegant + del process.muonExternalVecVarsTable.variables.cvhmergedGlobalIdxs return process ###Customizations needed for Wmass analysis diff --git a/SimG4Core/GeometryProducer/interface/GeometryProducer.h b/SimG4Core/GeometryProducer/interface/GeometryProducer.h index cd309b22645d0..df799048a03dc 100644 --- a/SimG4Core/GeometryProducer/interface/GeometryProducer.h +++ b/SimG4Core/GeometryProducer/interface/GeometryProducer.h @@ -26,6 +26,7 @@ class SimProducer; class DDDWorld; class G4RunManagerKernel; class SimTrackManager; +class DDCompactView; class GeometryProducer : public edm::one::EDProducer { public: @@ -46,9 +47,7 @@ class GeometryProducer : public edm::one::EDProducer> m_watchers; std::vector> m_producers; @@ -58,7 +57,12 @@ class GeometryProducer : public edm::one::EDProducer m_sensTkDets; std::vector m_sensCaloDets; edm::ParameterSet m_p; + + mutable const DDCompactView *m_pDD; + bool m_firstRun; + bool m_pUseMagneticField; + bool m_pUseSensitiveDetectors; }; #endif diff --git a/SimG4Core/GeometryProducer/src/GeometryProducer.cc b/SimG4Core/GeometryProducer/src/GeometryProducer.cc index 00cc0d9017473..fbd6dc4d238cb 100644 --- a/SimG4Core/GeometryProducer/src/GeometryProducer.cc +++ b/SimG4Core/GeometryProducer/src/GeometryProducer.cc @@ -59,12 +59,13 @@ static void createWatchers(const edm::ParameterSet &iP, GeometryProducer::GeometryProducer(edm::ParameterSet const &p) : m_kernel(nullptr), - m_pUseMagneticField(p.getParameter("UseMagneticField")), m_pField(p.getParameter("MagneticField")), - m_pUseSensitiveDetectors(p.getParameter("UseSensitiveDetectors")), m_attach(nullptr), m_p(p), - m_firstRun(true) { + m_pDD(nullptr), + m_firstRun(true), + m_pUseMagneticField(p.getParameter("UseMagneticField")), + m_pUseSensitiveDetectors(p.getParameter("UseSensitiveDetectors")) { // Look for an outside SimActivityRegistry // this is used by the visualization code edm::Service otherRegistry; @@ -110,21 +111,31 @@ void GeometryProducer::produce(edm::Event &e, const edm::EventSetup &es) { return; m_firstRun = false; - edm::LogInfo("GeometryProducer") << "Producing G4 Geom"; + edm::LogVerbatim("GeometryProducer") << "Producing G4 Geom"; m_kernel = G4RunManagerKernel::GetRunManagerKernel(); if (m_kernel == nullptr) m_kernel = new G4RunManagerKernel(); - edm::LogInfo("GeometryProducer") << " GeometryProducer initializing "; + edm::LogVerbatim("GeometryProducer") << " GeometryProducer initializing "; // DDDWorld: get the DDCV from the ES and use it to build the World + edm::ESTransientHandle pDD; es.get().get(pDD); + m_pDD = pDD.product(); + + SensitiveDetectorCatalog catalog; + G4LogicalVolumeToDDLogicalPartMap map; + + const DDDWorld *dddworld = new DDDWorld(&(*pDD), map, catalog, false); + G4VPhysicalVolume *world = dddworld->GetWorldVolumeForWorker(); + + if (nullptr != world) + edm::LogVerbatim("GeometryProducer") << " World Volume: " << world->GetName(); + m_kernel->DefineWorldVolume(world, true); - G4LogicalVolumeToDDLogicalPartMap map_; - SensitiveDetectorCatalog catalog_; - const DDDWorld *world = new DDDWorld(&(*pDD), map_, catalog_, false); - m_registry.dddWorldSignal_(world); + m_registry.dddWorldSignal_(dddworld); + edm::LogVerbatim("GeometryProducer") << " Magnetic field initialisation"; updateMagneticField(es); if (m_pUseSensitiveDetectors) { @@ -135,7 +146,7 @@ void GeometryProducer::produce(edm::Event &e, const edm::EventSetup &es) { m_attach = new AttachSD; { std::pair, std::vector> sensDets = - m_attach->create((*pDD), catalog_, m_p, m_trackManager.get(), m_registry); + m_attach->create((*m_pDD), catalog, m_p, m_trackManager.get(), m_registry); m_sensTkDets.swap(sensDets.first); m_sensCaloDets.swap(sensDets.second); diff --git a/TrackPropagation/Geant4e/interface/G4TablesForExtrapolatorCustom.h b/TrackPropagation/Geant4e/interface/G4TablesForExtrapolatorCustom.h index 7544affb485b9..ebb232db445b0 100644 --- a/TrackPropagation/Geant4e/interface/G4TablesForExtrapolatorCustom.h +++ b/TrackPropagation/Geant4e/interface/G4TablesForExtrapolatorCustom.h @@ -76,7 +76,7 @@ class G4TablesForExtrapolatorCustom { public: - explicit G4TablesForExtrapolatorCustom(G4int verb, G4int bins, G4double e1, G4double e2); + explicit G4TablesForExtrapolatorCustom(G4int verb, G4int bins, G4double e1, G4double e2, G4bool iononly = false); ~G4TablesForExtrapolatorCustom(); @@ -140,6 +140,8 @@ class G4TablesForExtrapolatorCustom G4double charge2; G4bool splineFlag; + + G4bool ionOnly; }; //....oooOO0OOooo........oooOO0OOooo........oooOO0OOooo........oooOO0OOooo.... diff --git a/TrackPropagation/Geant4e/interface/G4UniversalFluctuationForExtrapolator.hh b/TrackPropagation/Geant4e/interface/G4UniversalFluctuationForExtrapolator.hh new file mode 100644 index 0000000000000..afefb612f145f --- /dev/null +++ b/TrackPropagation/Geant4e/interface/G4UniversalFluctuationForExtrapolator.hh @@ -0,0 +1,242 @@ +// +// ******************************************************************** +// * License and Disclaimer * +// * * +// * The Geant4 software is copyright of the Copyright Holders of * +// * the Geant4 Collaboration. It is provided under the terms and * +// * conditions of the Geant4 Software License, included in the file * +// * LICENSE and available at http://cern.ch/geant4/license . These * +// * include a list of copyright holders. * +// * * +// * Neither the authors of this software system, nor their employing * +// * institutes,nor the agencies providing financial support for this * +// * work make any representation or warranty, express or implied, * +// * regarding this software system or assume any liability for its * +// * use. Please see the license in the file LICENSE and URL above * +// * for the full disclaimer and the limitation of liability. * +// * * +// * This code implementation is the result of the scientific and * +// * technical work of the GEANT4 collaboration. * +// * By using, copying, modifying or distributing the software (or * +// * any work based on the software) you agree to acknowledge its * +// * use in resulting scientific publications, and indicate your * +// * acceptance of all terms of the Geant4 Software license. * +// ******************************************************************** +// +// $Id$ +// +// ------------------------------------------------------------------- +// +// GEANT4 Class header file +// +// +// File name: G4UniversalFluctuationForExtrapolator +// +// Author: V.Ivanchenko make a class with the Laszlo Urban model +// +// Creation date: 03.01.2002 +// +// Modifications: +// +// +// Class Description: +// +// Implementation of energy loss fluctuations + +// ------------------------------------------------------------------- +// + +#ifndef G4UniversalFluctuationForExtrapolator_h +#define G4UniversalFluctuationForExtrapolator_h 1 + + +#include "G4VEmFluctuationModel.hh" +#include "G4ParticleDefinition.hh" +#include "G4Poisson.hh" +#include +#include "TrackPropagation/Geant4e/interface/G4TablesForExtrapolatorCustom.h" + +class G4UniversalFluctuationForExtrapolator : public G4VEmFluctuationModel +{ + +public: + + explicit G4UniversalFluctuationForExtrapolator(const G4String& nam = "UniFluc"); + + virtual ~G4UniversalFluctuationForExtrapolator(); + + virtual G4double SampleFluctuations(const G4Material*, + const G4DynamicParticle*, + G4double, + G4double, + G4double); + + virtual G4double SampleFluctuations2(const G4Material*, + const G4DynamicParticle*, + G4double, + G4double, + G4double, + G4double); + + virtual G4double SampleFluctuations(const G4MaterialCutsCouple*, + const G4DynamicParticle*, + G4double, + G4double, + G4double) override { return 0.; } + + virtual G4double Dispersion(const G4Material*, + const G4DynamicParticle*, + G4double, + G4double) override; + + virtual void InitialiseMe(const G4ParticleDefinition*) final; + + // Initialisation prestep + virtual void SetParticleAndCharge(const G4ParticleDefinition*, + G4double q2) final; + +private: + + inline void AddExcitation(G4double a, G4double e, G4double& eav, + G4double& eloss, G4double& esig2, G4double& esig2tot); + + inline void SampleGauss(G4double eav, G4double esig2, + G4double& eloss, G4double& esig2tot); + + inline void AddExcitation2(CLHEP::HepRandomEngine* rndm, + G4double a, G4double e, G4double& eav, + G4double& eloss, G4double& esig2); + + inline void SampleGauss2(CLHEP::HepRandomEngine* rndm, + G4double eav, G4double esig2, + G4double& eloss); + + // hide assignment operator + G4UniversalFluctuationForExtrapolator & operator=(const G4UniversalFluctuationForExtrapolator &right) = delete; + G4UniversalFluctuationForExtrapolator(const G4UniversalFluctuationForExtrapolator&) = delete; + + const G4ParticleDefinition* particle; + const G4Material* lastMaterial; + + G4double particleMass; + + // Derived quantities + G4double m_Inv_particleMass; + G4double m_massrate; + G4double chargeSquare; + + // data members to speed up the fluctuation calculation + G4double ipotFluct; + G4double electronDensity; + + G4double f1Fluct; + G4double f2Fluct; + G4double e1Fluct; + G4double e2Fluct; + G4double e1LogFluct; + G4double e2LogFluct; + G4double ipotLogFluct; + G4double e0; + G4double esmall; + + G4double e1,e2; + + G4double minNumberInteractionsBohr; + G4double minLoss; + G4double nmaxCont; + G4double rate,a0,fw; + + G4int sizearray; + G4double* rndmarray; + + G4TablesForExtrapolatorCustom *tables = nullptr; + const G4PhysicsTable *table = nullptr; + G4double massratio = 1.; + G4double charge2ratio = 1.; + +}; + +//....oooOO0OOooo........oooOO0OOooo........oooOO0OOooo........oooOO0OOooo...... + + +inline void +G4UniversalFluctuationForExtrapolator::AddExcitation(G4double ax, G4double ex, G4double& eav, + G4double& eloss, G4double& esig2, G4double& esig2tot) +{ + if(ax > nmaxCont) { + eav += ax*ex; + esig2 += ax*ex*ex; + } else { +// G4int p = G4Poisson(ax); + G4double p = ax; + if(p > 0) { +// eloss += ((p + 1) - 2.*rndm->flat())*ex; + eloss += ((p + 1) - 1.)*ex; + esig2tot += (ax + 1./3.)*ex*ex; + } + } +} + +inline void +G4UniversalFluctuationForExtrapolator::SampleGauss(G4double eav, G4double esig2, + G4double& eloss, G4double &esig2tot) +{ + G4double x = eav; + G4double sig = std::sqrt(esig2); + if(eav < 0.25*sig) { +// x += (2.*rndm->flat() - 1.)*eav; + x += (1. - 1.)*eav; + esig2tot += eav*eav/3.; + } else { +// do { +// // x = G4RandGauss::shoot(rndm, eav, sig); +// } while (x < 0.0 || x > 2*eav); + x = eav; + // variance of truncated gaussian +// esig2tot += sig*sig; + const double alpha = -eav/sig; + const double beta = eav/sig; + const double z = 0.5*(std::erf(beta/std::sqrt(2.)) - std::erf(alpha/std::sqrt(2.))); + const double phialpha = 1./std::sqrt(2.*M_PI)*std::exp(-0.5*alpha*alpha); + const double phibeta = 1./std::sqrt(2.*M_PI)*std::exp(-0.5*beta*beta); + esig2tot += sig*sig*(1. + (alpha*phialpha - beta*phibeta)/z - (phialpha - phibeta)*(phialpha - phibeta)/z/z); + + // Loop checking, 23-Feb-2016, Vladimir Ivanchenko + } + eloss += x; +} + +inline void +G4UniversalFluctuationForExtrapolator::AddExcitation2(CLHEP::HepRandomEngine* rndm, + G4double ax, G4double ex, G4double& eav, + G4double& eloss, G4double& esig2) +{ + if(ax > nmaxCont) { + eav += ax*ex; + esig2 += ax*ex*ex; + } else { + G4int p = G4Poisson(ax); + if(p > 0) { eloss += ((p + 1) - 2.*rndm->flat())*ex; } + } +} + +inline void +G4UniversalFluctuationForExtrapolator::SampleGauss2(CLHEP::HepRandomEngine* rndm, + G4double eav, G4double esig2, + G4double& eloss) +{ + G4double x = eav; + G4double sig = std::sqrt(esig2); + if(eav < 0.25*sig) { + x += (2.*rndm->flat() - 1.)*eav; + } else { + do { + x = G4RandGauss::shoot(rndm, eav, sig); + } while (x < 0.0 || x > 2*eav); + // Loop checking, 23-Feb-2016, Vladimir Ivanchenko + } + eloss += x; +} + +#endif + diff --git a/TrackPropagation/Geant4e/interface/G4WentzelVIModelCustom.hh b/TrackPropagation/Geant4e/interface/G4WentzelVIModelCustom.hh new file mode 100644 index 0000000000000..c068db58e1cba --- /dev/null +++ b/TrackPropagation/Geant4e/interface/G4WentzelVIModelCustom.hh @@ -0,0 +1,287 @@ +// +// ******************************************************************** +// * License and Disclaimer * +// * * +// * The Geant4 software is copyright of the Copyright Holders of * +// * the Geant4 Collaboration. It is provided under the terms and * +// * conditions of the Geant4 Software License, included in the file * +// * LICENSE and available at http://cern.ch/geant4/license . These * +// * include a list of copyright holders. * +// * * +// * Neither the authors of this software system, nor their employing * +// * institutes,nor the agencies providing financial support for this * +// * work make any representation or warranty, express or implied, * +// * regarding this software system or assume any liability for its * +// * use. Please see the license in the file LICENSE and URL above * +// * for the full disclaimer and the limitation of liability. * +// * * +// * This code implementation is the result of the scientific and * +// * technical work of the GEANT4 collaboration. * +// * By using, copying, modifying or distributing the software (or * +// * any work based on the software) you agree to acknowledge its * +// * use in resulting scientific publications, and indicate your * +// * acceptance of all terms of the Geant4 Software license. * +// ******************************************************************** +// +// $Id$ +// +// ------------------------------------------------------------------- +// +// +// GEANT4 Class header file +// +// +// File name: G4WentzelVIModelCustom +// +// Author: V.Ivanchenko +// +// Creation date: 09.04.2008 from G4MuMscModel +// +// Modifications: +// 27-05-2010 V.Ivanchenko added G4WentzelOKandVIxSection class to +// compute cross sections and sample scattering angle +// +// Class Description: +// +// Implementation of the model of multiple scattering based on +// G.Wentzel, Z. Phys. 40 (1927) 590. +// H.W.Lewis, Phys Rev 78 (1950) 526. +// J.M. Fernandez-Varea et al., NIM B73 (1993) 447. +// L.Urban, CERN-OPEN-2006-077. + +// ------------------------------------------------------------------- +// + +#ifndef G4WentzelVIModelCustom_h +#define G4WentzelVIModelCustom_h 1 + +//....oooOO0OOooo........oooOO0OOooo........oooOO0OOooo........oooOO0OOooo...... + +#include "G4VMscModel.hh" +#include "G4MaterialCutsCouple.hh" +#include "G4WentzelOKandVIxSection.hh" + +//....oooOO0OOooo........oooOO0OOooo........oooOO0OOooo........oooOO0OOooo...... + +class G4WentzelVIModelCustom : public G4VMscModel +{ + +public: + + explicit G4WentzelVIModelCustom(G4bool comb=true, const G4String& nam = "WentzelVIUni"); + + virtual ~G4WentzelVIModelCustom(); + + virtual void Initialise(const G4ParticleDefinition*, + const G4DataVector&) override; + + virtual void InitialiseLocal(const G4ParticleDefinition*, + G4VEmModel* masterModel) override; + + virtual void StartTracking(G4Track*) override; + + virtual G4double ComputeCrossSectionPerAtom(const G4ParticleDefinition*, + G4double KineticEnergy, + G4double AtomicNumber, + G4double AtomicWeight=0., + G4double cut = DBL_MAX, + G4double emax= DBL_MAX) override; + + virtual G4ThreeVector& SampleScattering(const G4ThreeVector&, + G4double safety) override; + + G4ThreeVector SampleScatteringTest(const G4ThreeVector&, + G4double safety); + + double ProjectedVariance() const; + + virtual G4double + ComputeTruePathLengthLimit(const G4Track& track, + G4double& currentMinimalStep) override; + + virtual G4double ComputeGeomPathLength(G4double truePathLength) override; + + virtual G4double ComputeTrueStepLength(G4double geomStepLength) override; + + // defines low energy limit on energy transfer to atomic electron + inline void SetFixedCut(G4double); + + // low energy limit on energy transfer to atomic electron + inline G4double GetFixedCut() const; + + // access to cross section class + inline void SetWVICrossSection(G4WentzelOKandVIxSection*); + + inline G4WentzelOKandVIxSection* GetWVICrossSection(); + + inline void SetUseSecondMoment(G4bool); + + inline G4bool UseSecondMoment() const; + + inline G4PhysicsTable* GetSecondMomentTable(); + + inline G4double SecondMoment(const G4ParticleDefinition*, + const G4MaterialCutsCouple*, + G4double kineticEnergy); + + void SetSingleScatteringFactor(G4double); + + void DefineMaterial(const G4MaterialCutsCouple*); + +protected: + + G4double ComputeTransportXSectionPerVolume(G4double cosTheta); + + inline void SetupParticle(const G4ParticleDefinition*); + +private: + + G4double ComputeSecondMoment(const G4ParticleDefinition*, + G4double kineticEnergy); + + // hide assignment operator + G4WentzelVIModelCustom & operator=(const G4WentzelVIModelCustom &right) = delete; + G4WentzelVIModelCustom(const G4WentzelVIModelCustom&) = delete; + +protected: + + G4WentzelOKandVIxSection* wokvi; + + G4double tlimitminfix; + G4double ssFactor; + G4double invssFactor; + + // cache kinematics + G4double preKinEnergy; + G4double tPathLength; + G4double zPathLength; + G4double lambdaeff; + G4double currentRange; + G4double cosTetMaxNuc; + + // cache material + G4int currentMaterialIndex; + const G4MaterialCutsCouple* currentCouple; + const G4Material* currentMaterial; + + const G4ParticleDefinition* particle; + G4ParticleChangeForMSC* fParticleChange; + const G4DataVector* currentCuts; + + G4double invsqrt12; + G4double fixedCut; + + // cache kinematics + G4double effKinEnergy; + + // single scattering parameters + G4double cosThetaMin; + G4double cosThetaMax; + + G4PhysicsTable* fSecondMoments; + size_t idx2; + + // data for single scattering mode + G4int minNCollisions; + G4int nelments; + std::vector xsecn; + std::vector prob; + + G4double xtsec; + G4double numlimit; + + // projectile + G4double lowEnergyLimit; + + // flags + G4bool singleScatteringMode; + G4bool isCombined; + G4bool useSecondMoment; +}; + +//....oooOO0OOooo........oooOO0OOooo........oooOO0OOooo........oooOO0OOooo...... +//....oooOO0OOooo........oooOO0OOooo........oooOO0OOooo........oooOO0OOooo...... + +inline void G4WentzelVIModelCustom::SetupParticle(const G4ParticleDefinition* p) +{ + // Initialise mass and charge + if(p != particle) { + particle = p; + wokvi->SetupParticle(p); + } +} + +//....oooOO0OOooo........oooOO0OOooo........oooOO0OOooo........oooOO0OOooo.... + +inline void G4WentzelVIModelCustom::SetFixedCut(G4double val) +{ + fixedCut = val; +} + +//....oooOO0OOooo........oooOO0OOooo........oooOO0OOooo........oooOO0OOooo.... + +inline G4double G4WentzelVIModelCustom::GetFixedCut() const +{ + return fixedCut; +} + +//....oooOO0OOooo........oooOO0OOooo........oooOO0OOooo........oooOO0OOooo.... + +inline void G4WentzelVIModelCustom::SetWVICrossSection(G4WentzelOKandVIxSection* ptr) +{ + if(ptr != wokvi) { + delete wokvi; + wokvi = ptr; + } +} + +//....oooOO0OOooo........oooOO0OOooo........oooOO0OOooo........oooOO0OOooo.... + +inline G4WentzelOKandVIxSection* G4WentzelVIModelCustom::GetWVICrossSection() +{ + return wokvi; +} + +//....oooOO0OOooo........oooOO0OOooo........oooOO0OOooo........oooOO0OOooo.... + +inline void G4WentzelVIModelCustom::SetUseSecondMoment(G4bool val) +{ + useSecondMoment = val; +} + +//....oooOO0OOooo........oooOO0OOooo........oooOO0OOooo........oooOO0OOooo.... + +inline G4bool G4WentzelVIModelCustom::UseSecondMoment() const +{ + return useSecondMoment; +} + +//....oooOO0OOooo........oooOO0OOooo........oooOO0OOooo........oooOO0OOooo.... + +inline G4PhysicsTable* G4WentzelVIModelCustom::GetSecondMomentTable() +{ + return fSecondMoments; +} + +//....oooOO0OOooo........oooOO0OOooo........oooOO0OOooo........oooOO0OOooo.... + +inline G4double +G4WentzelVIModelCustom::SecondMoment(const G4ParticleDefinition* part, + const G4MaterialCutsCouple* couple, + G4double ekin) +{ + G4double x = 0.0; + if(useSecondMoment) { + DefineMaterial(couple); + x = (fSecondMoments) ? + (*fSecondMoments)[(*theDensityIdx)[currentMaterialIndex]]->Value(ekin, idx2) + *(*theDensityFactor)[currentMaterialIndex]/(ekin*ekin) + : ComputeSecondMoment(part, ekin); + } + return x; +} + +//....oooOO0OOooo........oooOO0OOooo........oooOO0OOooo........oooOO0OOooo...... + +#endif + diff --git a/TrackPropagation/Geant4e/interface/Geant4ePropagator.h b/TrackPropagation/Geant4e/interface/Geant4ePropagator.h index f7a380073ba91..b7c8b5403d192 100644 --- a/TrackPropagation/Geant4e/interface/Geant4ePropagator.h +++ b/TrackPropagation/Geant4e/interface/Geant4ePropagator.h @@ -14,6 +14,8 @@ #include +#include "TrackPropagation/Geant4e/interface/G4UniversalFluctuationForExtrapolator.hh" +#include "TrackPropagation/Geant4e/interface/G4WentzelVIModelCustom.hh" /** Propagator based on the Geant4e package. Uses the Propagator class * in the TrackingTools/GeomPropagators package to define the interface. @@ -83,16 +85,13 @@ class Geant4ePropagator : public Propagator { Geant4ePropagator *clone() const override { return new Geant4ePropagator(*this); } const MagneticField *magneticField() const override { return theField; } - - std::tuple propagateGenericWithJacobian(const FreeTrajectoryState &ftsStart, - const Plane &pDest) const; - - std::tuple propagateGenericWithJacobianAlt(const FreeTrajectoryState &ftsStart, - const Plane &pDest) const; - std::tuple, Eigen::Matrix, Eigen::Matrix, double, Eigen::Matrix> propagateGenericWithJacobianAltD(const Eigen::Matrix &ftsStart, - const GloballyPositioned &pDest, double dBz = 0., double dxi = 0., double pforced = -1.) const; - + std::tuple, Eigen::Matrix, Eigen::Matrix, double, Eigen::Matrix, Eigen::Matrix, double, double> propagateGenericWithJacobianAltD(const Eigen::Matrix &ftsStart, + const GloballyPositioned &pDest, double dBz = 0., double dxi = 0., + double dms = 0., double dioni = 0., double pforced = -1.) const; + + static void CalculateEffectiveZandA(const G4Material* mate, G4double& effZ, G4double& effA); + private: typedef std::pair TsosPP; typedef std::pair> ErrorTargetPair; @@ -179,32 +178,15 @@ class Geant4ePropagator : public Propagator { const SurfaceType &pDest) const; Eigen::Matrix PropagateErrorMSC( const G4Track* aTrack, double pforced = -1. ) const; - - double computeErrorIoni(const G4Track* aTrack, double pforced = -1.) const; - void CalculateEffectiveZandA(const G4Material* mate, G4double& effZ, G4double& effA) const; - - AlgebraicMatrix55 curv2localJacobianAlt(const GlobalTrajectoryParameters &globalSource, const Surface &surface) const; - - AlgebraicMatrix55 curv2localJacobianAlteloss(const GlobalTrajectoryParameters &globalSource, const Surface &surface, double dEdx, double mass) const; - - Eigen::Matrix transportJacobian(const FreeTrajectoryState &start, double s, double dEdx, double mass) const; - - Eigen::Matrix transportJacobianBz(const FreeTrajectoryState &start, double s, double dEdx, double mass) const; - - Eigen::Matrix transportJacobianBzD(const Eigen::Matrix &start, double s, double dEdx, double mass, double dBz) const; - - Eigen::Matrix transportJacobianD(const Eigen::Matrix &start, double s, double dEdx, double mass) const; + std::pair computeLandau(const G4Track* aTrack) const; + double computeErrorIoni(const G4Track* aTrack, double pforced = -1.) const; + + Eigen::Matrix transportJacobianBzD(const Eigen::Matrix &start, double s, double dEdx, double mass, double dBz) const; - - Eigen::Matrix transportJacobianBzAdvanced(const FreeTrajectoryState &start, double s, double dEdx, double mass) const; - - Eigen::Matrix curv2cartJacobianAlt(const FreeTrajectoryState &state) const; - - Eigen::Matrix transportResult(const FreeTrajectoryState &start, double s, double dEdx, double mass) const; - - Eigen::Matrix transportResultD(const Eigen::Matrix &start, double s, double dEdx, double mass, double bzoffset) const; + G4UniversalFluctuationForExtrapolator *fluct = nullptr; + G4WentzelVIModelCustom *msmodel = nullptr; }; diff --git a/TrackPropagation/Geant4e/plugins/GeantPropagatorESProducer.cc b/TrackPropagation/Geant4e/plugins/GeantPropagatorESProducer.cc index 6608d9cbd5cb0..4a80c3b9ec7aa 100644 --- a/TrackPropagation/Geant4e/plugins/GeantPropagatorESProducer.cc +++ b/TrackPropagation/Geant4e/plugins/GeantPropagatorESProducer.cc @@ -17,6 +17,7 @@ GeantPropagatorESProducer::GeantPropagatorESProducer(const edm::ParameterSet &p) std::string myname = p.getParameter("ComponentName"); pset_ = p; plimit_ = pset_.getParameter("PropagationPtotLimit"); + fieldlabel_ = pset_.getParameter("MagneticFieldLabel"); setWhatProduced(this, myname); } @@ -24,7 +25,7 @@ GeantPropagatorESProducer::~GeantPropagatorESProducer() {} std::unique_ptr GeantPropagatorESProducer::produce(const TrackingComponentsRecord &iRecord) { ESHandle magfield; - iRecord.getRecord().get(magfield); + iRecord.getRecord().get(fieldlabel_, magfield); std::string pdir = pset_.getParameter("PropagationDirection"); std::string particleName = pset_.getParameter("ParticleName"); diff --git a/TrackPropagation/Geant4e/plugins/GeantPropagatorESProducer.h b/TrackPropagation/Geant4e/plugins/GeantPropagatorESProducer.h index 1530f992bfbc6..6f07f483cfb42 100644 --- a/TrackPropagation/Geant4e/plugins/GeantPropagatorESProducer.h +++ b/TrackPropagation/Geant4e/plugins/GeantPropagatorESProducer.h @@ -24,6 +24,7 @@ class GeantPropagatorESProducer : public edm::ESProducer { private: edm::ParameterSet pset_; double plimit_; + std::string fieldlabel_; }; #endif diff --git a/TrackPropagation/Geant4e/python/Geant4ePropagator_cfi.py b/TrackPropagation/Geant4e/python/Geant4ePropagator_cfi.py index b75ccd398ebae..e882b335b6490 100644 --- a/TrackPropagation/Geant4e/python/Geant4ePropagator_cfi.py +++ b/TrackPropagation/Geant4e/python/Geant4ePropagator_cfi.py @@ -8,5 +8,6 @@ ComponentName = cms.string("Geant4ePropagator"), PropagationDirection=cms.string("alongMomentum"), ParticleName=cms.string("mu"), - PropagationPtotLimit = cms.double(1.0) ## GeV/c + PropagationPtotLimit = cms.double(0.5), ## GeV/c + MagneticFieldLabel = cms.string("") ) diff --git a/TrackPropagation/Geant4e/src/G4TablesForExtrapolatorCustom.cc b/TrackPropagation/Geant4e/src/G4TablesForExtrapolatorCustom.cc index 3813002dfb0e9..be7c35a13515b 100644 --- a/TrackPropagation/Geant4e/src/G4TablesForExtrapolatorCustom.cc +++ b/TrackPropagation/Geant4e/src/G4TablesForExtrapolatorCustom.cc @@ -48,6 +48,7 @@ //....oooOO0OOooo........oooOO0OOooo........oooOO0OOooo........oooOO0OOooo.... #include "TrackPropagation/Geant4e/interface/G4TablesForExtrapolatorCustom.h" +#include "TrackPropagation/Geant4e/interface/Geant4ePropagator.h" #include "G4PhysicalConstants.hh" #include "G4SystemOfUnits.hh" #include "G4PhysicsLogVector.hh" @@ -69,12 +70,14 @@ #include "G4ProductionCuts.hh" #include "G4LossTableBuilder.hh" #include "G4WentzelVIModel.hh" +#include "G4ProductionCutsTable.hh" +#include "G4RunManagerKernel.hh" //....oooOO0OOooo........oooOO0OOooo........oooOO0OOooo........oooOO0OOooo.... G4TablesForExtrapolatorCustom::G4TablesForExtrapolatorCustom(G4int verb, G4int bins, - G4double e1, G4double e2) - : verbose(verb), nbins(bins), emin(e1), emax(e2) + G4double e1, G4double e2, G4bool iononly) + : verbose(verb), nbins(bins), emin(e1), emax(e2), ionOnly(iononly) { Initialisation(); } @@ -325,10 +328,12 @@ G4TablesForExtrapolatorCustom::ComputeMuonDEDX(const G4ParticleDefinition* part, G4PhysicsTable* table) { G4BetheBlochModel* ioni = new G4BetheBlochModel(); -// G4MuBetheBlochModel* ionialt = new G4MuBetheBlochModel(); + G4MuBetheBlochModel* ionialt = new G4MuBetheBlochModel(); +// G4MuBetheBlochModel* ioni = new G4MuBetheBlochModel(); G4MuPairProductionModel* pair = new G4MuPairProductionModel(); G4MuBremsstrahlungModel* brem = new G4MuBremsstrahlungModel(); ioni->Initialise(part, cuts); + ionialt->Initialise(part, cuts); pair->Initialise(part, cuts); brem->Initialise(part, cuts); @@ -338,12 +343,28 @@ G4TablesForExtrapolatorCustom::ComputeMuonDEDX(const G4ParticleDefinition* part, const G4MaterialTable* mtable = G4Material::GetMaterialTable(); + +// G4ProductionCutsTable::GetProductionCutsTable()->UpdateCoupleTable(G4RunManagerKernel::GetRunManagerKernel()->GetCurrentWorld()); + +// const G4ProductionCutsTable* theCoupleTable= +// G4ProductionCutsTable::GetProductionCutsTable(); + +// std::cout << "couple table size = " << theCoupleTable->GetTableSize() << std::endl; + +// const G4DataVector *theCuts = +// static_cast(theCoupleTable->GetEnergyCutsVector(1)); + if(0GetParticleName() << G4endl; } + const double mass = 0.1056583745; + const double massev = mass*1e9; + G4double eMass = 0.51099906 / GeV; + G4double massRatio = eMass / mass; + for(G4int i=0; iGetIndex(); +// const G4double cut = (*theCuts)[icouple]; +// const G4double cut = 0.; + +// int icouple = -1; +// G4double cut = 0.; +// for (unsigned int j=0; jGetTableSize(); ++j) { +// if (theCoupleTable->GetMaterialCutsCouple(j)->GetMaterial() == mat) { +// icouple = j; +// // cut = (*theCuts)[j]; +// cut = theCoupleTable->GetMaterialCutsCouple(j)->GetProductionCuts()->GetProductionCut("e-"); +// break; +// } +// } +// +// std::cout << "i= " << i << " mat= " << mat->GetName() << " icouple = " << icouple << " cut = " << cut << std::endl; +// std::cout << "i= " << i << " mat= " << mat->GetName() << std::endl; // // std::cout << "material = " << mat->GetName() << std::endl; // // std::cout << "computing dedxmin" << std::endl; @@ -377,16 +416,88 @@ G4TablesForExtrapolatorCustom::ComputeMuonDEDX(const G4ParticleDefinition* part, // std::cout << mat->GetName() << " dedxmin = " << dedxionimin << " emin = " << emin << " dedxioniminalt = " << dedxioniminalt << " eminalt = " << eminalt << std::endl; + +// const double cut = 0.121694; + G4double effZ, effA; + Geant4ePropagator::CalculateEffectiveZandA(mat, effZ, effA); + G4double I = 16.*pow(effZ,0.9); + const double f2 = effZ <= 2. ? 0. : 2./effZ; + const double f1 = 1. - f2; + const double e2 = 10.*effZ*effZ; + const double e1 = pow(I/pow(e2,f2),1./f1); + const double r = 0.4; + for(G4int j=0; j<=nbins; j++) { G4double e = aVector->Energy(j); + + + G4double pgev = e / GeV; + G4double Etot = sqrt(pgev*pgev + mass*mass); + G4double beta = pgev/Etot; + G4double gamma = Etot / mass; + G4double eta = beta * gamma; + G4double etasq = eta * eta; + G4double F1 = 2 * eMass * etasq; + G4double F2 = 1. + 2. * massRatio * gamma + massRatio * massRatio; + G4double Emax = 1.E+6 * F1 / F2; // now in keV + + + const double emaxev = Emax*1e3; // keV -> eV + + + const double sigma1partial = f1*(log(2.*massev*beta*beta*gamma*gamma/e1) - beta*beta)/e1/(log(2.*massev*beta*beta*gamma*gamma/I) - beta*beta)*(1.-r); + + const double sigma2partial = f1*(log(2.*massev*beta*beta*gamma*gamma/e2) - beta*beta)/e2/(log(2.*massev*beta*beta*gamma*gamma/I) - beta*beta)*(1.-r); + + const double sigma3partial = emaxev/I/(emaxev+I)/log((emaxev+I)/I)*r; + + const double e3med = I/(1. - 0.5*emaxev/(emaxev + I)); + const double e3mean = I*(emaxev +I)*log((emaxev+I)/I)/emaxev; + const double e3mode = I; + + const double emed = sigma1partial*e1 + sigma2partial*e2 + sigma3partial*e3med; + const double emean = sigma1partial*e1 + sigma2partial*e2 + sigma3partial*e3mean; + const double emode = sigma1partial*e1 + sigma2partial*e2 + sigma3partial*e3mode; + + const double dedxratio = emed/emean; + const double moderatio = emode/emean; + const double alpha = 0.996; +// const double alpha = 0.999; + + const double ealpha = I/(1. - alpha*emaxev/(emaxev + I)); +// const double ealphamev = ealpha*1e-6; + // G4double dedx = std::min(ioni->ComputeDEDX(couple,part,e,e), dedxionimin) + // pair->ComputeDEDX(couple,part,e,e) + // brem->ComputeDEDX(couple,part,e,e); - G4double dedx = ioni->ComputeDEDX(couple,part,e,e) + - pair->ComputeDEDX(couple,part,e,e) + +// G4double dedx = ioni->ComputeDEDX(couple,part,e,cut) + +// pair->ComputeDEDX(couple,part,e,e) + +// brem->ComputeDEDX(couple,part,e,e); +// const double cut = i == 18 ? 0.121694 : e; // energy cut only for silicon for now +// const double cut = e; +// const double cut = 100.; +// const double cut = ealphamev; + const double dedxioni = e > 1000 ? ionialt->ComputeDEDX(couple,part,e,e) : ioni->ComputeDEDX(couple,part,e,e); +// const double dedxioni = ioni->ComputeDEDX(couple,part,e,cut); +// const double dedxioni = ioni->ComputeDEDX(couple,part,e,e); +// const double dedxioni = dedxratio*ioni->ComputeDEDX(couple,part,e,e); +// const double dedxioni = ioni->ComputeDEDX(couple,part,e,ealpha); + G4double dedx = ionOnly ? dedxioni : dedxioni + pair->ComputeDEDX(couple,part,e,e) + brem->ComputeDEDX(couple,part,e,e); +// G4double dedx = dedxioni; +// const double dedxionicut = ioni->ComputeDEDX(couple,part,e,cut); +// const double dedxioni = ioni->ComputeDEDX(couple,part,e,e); +// G4double dedx = ioni->ComputeDEDX(couple,part,e,e) + +// pair->ComputeDEDX(couple,part,e,e) + +// brem->ComputeDEDX(couple,part,e,e); +// std::cout << "material = " << mat->GetName() << " e = " << e << " cut = " << cut << " dedxioni = " << dedxioni << " dedx = " << dedx << std::endl; +// std::cout << "material = " << mat->GetName() << " e = " << e << " dedxratio = " << dedxratio << " moderatio = " << moderatio << " dedxioni = " << dedxioni << " dedx = " << dedx << std::endl; + + +// std::cout << "e = " << e << " dedxionicut = " << dedxionicut << " dedxioni = " << dedxioni << " dedx = " << dedx << std::endl; + // dedx *= 1e-6; // dedx *= 1e-12; aVector->PutValue(j,dedx); @@ -402,6 +513,7 @@ G4TablesForExtrapolatorCustom::ComputeMuonDEDX(const G4ParticleDefinition* part, if(splineFlag) { aVector->FillSecondDerivatives(); } } delete ioni; + delete ionialt; } //....oooOO0OOooo........oooOO0OOooo........oooOO0OOooo........oooOO0OOooo.... diff --git a/TrackPropagation/Geant4e/src/G4UniversalFluctuationForExtrapolator.cc b/TrackPropagation/Geant4e/src/G4UniversalFluctuationForExtrapolator.cc new file mode 100644 index 0000000000000..5150e52b0fdd3 --- /dev/null +++ b/TrackPropagation/Geant4e/src/G4UniversalFluctuationForExtrapolator.cc @@ -0,0 +1,606 @@ +// +// ******************************************************************** +// * License and Disclaimer * +// * * +// * The Geant4 software is copyright of the Copyright Holders of * +// * the Geant4 Collaboration. It is provided under the terms and * +// * conditions of the Geant4 Software License, included in the file * +// * LICENSE and available at http://cern.ch/geant4/license . These * +// * include a list of copyright holders. * +// * * +// * Neither the authors of this software system, nor their employing * +// * institutes,nor the agencies providing financial support for this * +// * work make any representation or warranty, express or implied, * +// * regarding this software system or assume any liability for its * +// * use. Please see the license in the file LICENSE and URL above * +// * for the full disclaimer and the limitation of liability. * +// * * +// * This code implementation is the result of the scientific and * +// * technical work of the GEANT4 collaboration. * +// * By using, copying, modifying or distributing the software (or * +// * any work based on the software) you agree to acknowledge its * +// * use in resulting scientific publications, and indicate your * +// * acceptance of all terms of the Geant4 Software license. * +// ******************************************************************** +// +// $Id$ +// +// ------------------------------------------------------------------- +// +// GEANT4 Class file +// +// +// File name: G4UniversalFluctuationForExtrapolator +// +// Author: V. Ivanchenko for Laszlo Urban +// +// Creation date: 03.01.2002 +// +// Modifications: +// +// + +//....oooOO0OOooo........oooOO0OOooo........oooOO0OOooo........oooOO0OOooo...... +//....oooOO0OOooo........oooOO0OOooo........oooOO0OOooo........oooOO0OOooo...... + +#include "TrackPropagation/Geant4e/interface/G4UniversalFluctuationForExtrapolator.hh" +#include "G4PhysicalConstants.hh" +#include "G4SystemOfUnits.hh" +#include "Randomize.hh" +#include "G4Poisson.hh" +#include "G4Step.hh" +#include "G4Material.hh" +#include "G4MaterialCutsCouple.hh" +#include "G4DynamicParticle.hh" +#include "G4ParticleDefinition.hh" +#include "G4Log.hh" +#include "G4Exp.hh" +#include "G4Electron.hh" +#include "G4Positron.hh" +#include "G4Proton.hh" +#include "G4MuonPlus.hh" +#include "G4MuonMinus.hh" + +//....oooOO0OOooo........oooOO0OOooo........oooOO0OOooo........oooOO0OOooo...... + +using namespace std; + +G4UniversalFluctuationForExtrapolator::G4UniversalFluctuationForExtrapolator(const G4String& nam) + :G4VEmFluctuationModel(nam), + particle(nullptr), + minNumberInteractionsBohr(10.0), + minLoss(10.*eV), + nmaxCont(16.), + rate(0.56), + a0(50.), + fw(4.00) +{ + lastMaterial = nullptr; + particleMass = chargeSquare = ipotFluct = electronDensity = f1Fluct = f2Fluct + = e1Fluct = e2Fluct = e1LogFluct = e2LogFluct = ipotLogFluct = e0 = esmall + = e1 = e2 = 0.0; + m_Inv_particleMass = m_massrate = DBL_MAX; + sizearray = 30; + rndmarray = new G4double[30]; + tables = new G4TablesForExtrapolatorCustom(0, 70, 1.*MeV, 10.*TeV, true); +} + +//....oooOO0OOooo........oooOO0OOooo........oooOO0OOooo........oooOO0OOooo...... + +G4UniversalFluctuationForExtrapolator::~G4UniversalFluctuationForExtrapolator() +{ + delete [] rndmarray; + delete tables; +} + +//....oooOO0OOooo........oooOO0OOooo........oooOO0OOooo........oooOO0OOooo...... + +void G4UniversalFluctuationForExtrapolator::InitialiseMe(const G4ParticleDefinition* part) +{ + particle = part; + particleMass = part->GetPDGMass(); + G4double q = part->GetPDGCharge()/eplus; + + // Derived quantities + m_Inv_particleMass = 1.0 / particleMass; + m_massrate = electron_mass_c2 * m_Inv_particleMass ; + chargeSquare = q*q; +} + +//....oooOO0OOooo........oooOO0OOooo........oooOO0OOooo........oooOO0OOooo...... + + +G4double +G4UniversalFluctuationForExtrapolator::SampleFluctuations2(const G4Material* material, + const G4DynamicParticle* dp, + G4double tmax, + G4double length, + G4double ekin, + G4double eloss) +{ + + // Calculate actual loss from the mean loss. + // The model used to get the fluctuations is essentially the same + // as in Glandz in Geant3 (Cern program library W5013, phys332). + // L. Urban et al. NIM A362, p.416 (1995) and Geant4 Physics Reference Manual + + // shortcut for very small loss or from a step nearly equal to the range + // (out of validity of the model) + // +// G4double meanLoss = averageLoss; +// const G4PhysicsTable *table = tables->GetPhysicsTable(fDedxMuon); + size_t idx = 0; + G4double dedx = ((*table)[material->GetIndex()])->Value(massratio*ekin, idx)*charge2ratio; + G4double meanLoss = length*dedx; +// std::cout << "meanLoss = " << meanLoss << std::endl; +// G4double tkin = dp->GetKineticEnergy(); + G4double tkin = ekin; + + // std::cout << "eloss = " << eloss << " meanLoss = " << meanLoss << std::endl; + + // const double extraloss = eloss - meanLoss; + const double extraloss = 0.; + + if (meanLoss < minLoss) { return meanLoss + extraloss; } + + if(dp->GetDefinition() != particle) { InitialiseMe(dp->GetDefinition()); } + + CLHEP::HepRandomEngine* rndmEngineF = G4Random::getTheEngine(); + + G4double tau = tkin * m_Inv_particleMass; + G4double gam = tau + 1.0; + G4double gam2 = gam*gam; + G4double beta2 = tau*(tau + 2.0)/gam2; + + G4double loss(0.), siga(0.); + + // const G4Material* material = couple->GetMaterial(); + + // Gaussian regime + // for heavy particles only and conditions + // for Gauusian fluct. has been changed + // + if ((particleMass > electron_mass_c2) && + (meanLoss >= minNumberInteractionsBohr*tmax)) + { + G4double tmaxkine = 2.*electron_mass_c2*beta2*gam2/ + (1.+m_massrate*(2.*gam+m_massrate)) ; + if (tmaxkine <= 2.*tmax) + { + electronDensity = material->GetElectronDensity(); + siga = sqrt((1.0/beta2 - 0.5) * twopi_mc2_rcl2 * tmax * length + * electronDensity * chargeSquare); + + G4double sn = meanLoss/siga; + + // thick target case + if (sn >= 2.0) { + + G4double twomeanLoss = meanLoss + meanLoss; + do { + loss = G4RandGauss::shoot(rndmEngineF,meanLoss,siga); + // Loop checking, 03-Aug-2015, Vladimir Ivanchenko + } while (0.0 > loss || twomeanLoss < loss); + + // Gamma distribution + } else { + + G4double neff = sn*sn; + loss = meanLoss*G4RandGamma::shoot(rndmEngineF,neff,1.0)/neff; + } + //G4cout << "Gauss: " << loss << G4endl; + return loss + extraloss; + } + } + + // Glandz regime : initialisation + // + if (material != lastMaterial) { + f1Fluct = material->GetIonisation()->GetF1fluct(); + f2Fluct = material->GetIonisation()->GetF2fluct(); + e1Fluct = material->GetIonisation()->GetEnergy1fluct(); + e2Fluct = material->GetIonisation()->GetEnergy2fluct(); + e1LogFluct = material->GetIonisation()->GetLogEnergy1fluct(); + e2LogFluct = material->GetIonisation()->GetLogEnergy2fluct(); + ipotFluct = material->GetIonisation()->GetMeanExcitationEnergy(); + ipotLogFluct = material->GetIonisation()->GetLogMeanExcEnergy(); + e0 = material->GetIonisation()->GetEnergy0fluct(); + esmall = 0.5*sqrt(e0*ipotFluct); + lastMaterial = material; + } + + // very small step or low-density material + if(tmax <= e0) { return meanLoss + extraloss; } + + // width correction for small cuts + G4double scaling = std::min(1.+0.5*CLHEP::keV/tmax,1.50); + meanLoss /= scaling; + + G4double a1(0.0), a2(0.0), a3(0.0); + + loss = 0.0; + + e1 = e1Fluct; + e2 = e2Fluct; + + if(tmax > ipotFluct) { + G4double w2 = G4Log(2.*electron_mass_c2*beta2*gam2)-beta2; + + if(w2 > ipotLogFluct) { + if(w2 > e2LogFluct) { + G4double C = meanLoss*(1.-rate)/(w2-ipotLogFluct); + a1 = C*f1Fluct*(w2-e1LogFluct)/e1Fluct; + a2 = C*f2Fluct*(w2-e2LogFluct)/e2Fluct; + } else { + a1 = meanLoss*(1.-rate)/e1; + } + if(a1 < a0) { + G4double fwnow = 0.5+(fw-0.5)*sqrt(a1/a0); + a1 /= fwnow; + e1 *= fwnow; + } else { + a1 /= fw; + e1 = fw*e1Fluct; + } + } + } + + G4double w1 = tmax/e0; + if(tmax > e0) { + a3 = rate*meanLoss*(tmax-e0)/(e0*tmax*G4Log(w1)); + if(a1+a2 <= 0.) { + a3 /= rate; + } + } + //'nearly' Gaussian fluctuation if a1>nmaxCont&&a2>nmaxCont&&a3>nmaxCont + G4double emean = 0.; + G4double sig2e = 0.; + + // excitation of type 1 + if(a1 > 0.0) { AddExcitation2(rndmEngineF, a1, e1, emean, loss, sig2e); } + + // excitation of type 2 + if(a2 > 0.0) { AddExcitation2(rndmEngineF, a2, e2, emean, loss, sig2e); } + + if(sig2e > 0.0) { SampleGauss2(rndmEngineF, emean, sig2e, loss); } + + // ionisation + if(a3 > 0.) { + emean = 0.; + sig2e = 0.; + G4double p3 = a3; + G4double alfa = 1.; + if(a3 > nmaxCont) + { + alfa = w1*(nmaxCont+a3)/(w1*nmaxCont+a3); + G4double alfa1 = alfa*G4Log(alfa)/(alfa-1.); + G4double namean = a3*w1*(alfa-1.)/((w1-1.)*alfa); + emean += namean*e0*alfa1; + sig2e += e0*e0*namean*(alfa-alfa1*alfa1); + p3 = a3-namean; + } + + G4double w2 = alfa*e0; + if(tmax > w2) { + G4double w = (tmax-w2)/tmax; + G4int nnb = G4Poisson(p3); + if(nnb > 0) { + if(nnb > sizearray) { + sizearray = nnb; + delete [] rndmarray; + rndmarray = new G4double[nnb]; + } + rndmEngineF->flatArray(nnb, rndmarray); + for (G4int k=0; k 0.0) { SampleGauss2(rndmEngineF, emean, sig2e, loss); } + } + + loss *= scaling; + + return loss + extraloss; + +} + +G4double +G4UniversalFluctuationForExtrapolator::SampleFluctuations(const G4Material* material, + const G4DynamicParticle* dp, + G4double tmax, + G4double length, + G4double ekin) +{ + // Calculate actual loss from the mean loss. + // The model used to get the fluctuations is essentially the same + // as in Glandz in Geant3 (Cern program library W5013, phys332). + // L. Urban et al. NIM A362, p.416 (1995) and Geant4 Physics Reference Manual + + // shortcut for very small loss or from a step nearly equal to the range + // (out of validity of the model) + // +// G4double meanLoss = averageLoss; +// const G4PhysicsTable *table = tables->GetPhysicsTable(fDedxMuon); + size_t idx = 0; + G4double dedx = ((*table)[material->GetIndex()])->Value(massratio*ekin, idx)*charge2ratio; + G4double meanLoss = length*dedx; +// std::cout << "meanLoss = " << meanLoss << std::endl; +// G4double tkin = dp->GetKineticEnergy(); + G4double tkin = ekin; + //G4cout<< "Emean= "<< meanLoss<< " tmax= "<< tmax<< " L= "<GetDefinition() != particle) { InitialiseMe(dp->GetDefinition()); } + +// CLHEP::HepRandomEngine* rndmEngineF = G4Random::getTheEngine(); + + G4double tau = tkin * m_Inv_particleMass; + G4double gam = tau + 1.0; + G4double gam2 = gam*gam; + G4double beta2 = tau*(tau + 2.0)/gam2; + + G4double loss(0.), siga(0.); + +// const G4Material* material = couple->GetMaterial(); + + // Gaussian regime + // for heavy particles only and conditions + // for Gauusian fluct. has been changed + // + if ((particleMass > electron_mass_c2) && + (meanLoss >= minNumberInteractionsBohr*tmax)) + { + G4double tmaxkine = 2.*electron_mass_c2*beta2*gam2/ + (1.+m_massrate*(2.*gam+m_massrate)) ; + if (tmaxkine <= 2.*tmax) + { + electronDensity = material->GetElectronDensity(); + siga = sqrt((1.0/beta2 - 0.5) * twopi_mc2_rcl2 * tmax * length + * electronDensity * chargeSquare); + + G4double sn = meanLoss/siga; + + // thick target case + if (sn >= 2.0) { + +// std::cout << "gaussian case\n"; + return siga*siga; + + G4double twomeanLoss = meanLoss + meanLoss; + do { +// loss = G4RandGauss::shoot(rndmEngineF,meanLoss,siga); + loss = meanLoss; + // Loop checking, 03-Aug-2015, Vladimir Ivanchenko + } while (0.0 > loss || twomeanLoss < loss); + + // Gamma distribution + } else { + +// std::cout << "gamma case\n"; + G4double neff = sn*sn; + return meanLoss*meanLoss/neff; + loss = meanLoss; +// loss = meanLoss*G4RandGamm:a:shoot(rndmEngineF,neff,1.0)/neff; + } + //G4cout << "Gauss: " << loss << G4endl; + return loss; + } + } + + // Glandz regime : initialisation + // + if (material != lastMaterial) { + f1Fluct = material->GetIonisation()->GetF1fluct(); + f2Fluct = material->GetIonisation()->GetF2fluct(); + e1Fluct = material->GetIonisation()->GetEnergy1fluct(); + e2Fluct = material->GetIonisation()->GetEnergy2fluct(); + e1LogFluct = material->GetIonisation()->GetLogEnergy1fluct(); + e2LogFluct = material->GetIonisation()->GetLogEnergy2fluct(); + ipotFluct = material->GetIonisation()->GetMeanExcitationEnergy(); + ipotLogFluct = material->GetIonisation()->GetLogMeanExcEnergy(); + e0 = material->GetIonisation()->GetEnergy0fluct(); + esmall = 0.5*sqrt(e0*ipotFluct); + lastMaterial = material; + } + + // very small step or low-density material + if(tmax <= e0) { return 0.; } + + // width correction for small cuts + G4double scaling = std::min(1.+0.5*CLHEP::keV/tmax,1.50); + meanLoss /= scaling; + +// std::cout << "urban model: scaling = " << scaling << std::endl; + + G4double a1(0.0), a2(0.0), a3(0.0); + + loss = 0.0; + + e1 = e1Fluct; + e2 = e2Fluct; + + if(tmax > ipotFluct) { + G4double w2 = G4Log(2.*electron_mass_c2*beta2*gam2)-beta2; + + if(w2 > ipotLogFluct) { + if(w2 > e2LogFluct) { + G4double C = meanLoss*(1.-rate)/(w2-ipotLogFluct); + a1 = C*f1Fluct*(w2-e1LogFluct)/e1Fluct; + a2 = C*f2Fluct*(w2-e2LogFluct)/e2Fluct; + } else { + a1 = meanLoss*(1.-rate)/e1; + } + if(a1 < a0) { + G4double fwnow = 0.5+(fw-0.5)*sqrt(a1/a0); + a1 /= fwnow; + e1 *= fwnow; + } else { + a1 /= fw; + e1 = fw*e1Fluct; + } + } + } + + G4double w1 = tmax/e0; + if(tmax > e0) { + a3 = rate*meanLoss*(tmax-e0)/(e0*tmax*G4Log(w1)); + if(a1+a2 <= 0.) { + a3 /= rate; + } + } + //'nearly' Gaussian fluctuation if a1>nmaxCont&&a2>nmaxCont&&a3>nmaxCont + G4double emean = 0.; + G4double sig2e = 0.; + G4double esig2tot = 0.; + + // excitation of type 1 + if(a1 > 0.0) { AddExcitation(a1, e1, emean, loss, sig2e, esig2tot); } + + // excitation of type 2 + if(a2 > 0.0) { AddExcitation(a2, e2, emean, loss, sig2e, esig2tot); } + + if(sig2e > 0.0) { SampleGauss(emean, sig2e, loss, esig2tot); } + + // ionisation + if(a3 > 0.) { + emean = 0.; + sig2e = 0.; + G4double p3 = a3; + G4double alfa = 1.; + if(a3 > nmaxCont) + { + alfa = w1*(nmaxCont+a3)/(w1*nmaxCont+a3); + G4double alfa1 = alfa*G4Log(alfa)/(alfa-1.); + G4double namean = a3*w1*(alfa-1.)/((w1-1.)*alfa); + emean += namean*e0*alfa1; + sig2e += e0*e0*namean*(alfa-alfa1*alfa1); + p3 = a3-namean; + } + + G4double w2 = alfa*e0; + if(tmax > w2) { + G4double w = (tmax-w2)/tmax; +// G4int nnb = G4Poisson(p3); +// if(nnb > 0) { +// if(nnb > sizearray) { +// sizearray = nnb; +// delete [] rndmarray; +// rndmarray = new G4double[nnb]; +// } +// rndmEngineF->flatArray(nnb, rndmarray); +// for (G4int k=0; kGetPhysicsTable(fDedxMuon); + massratio = 1.; + charge2ratio = 1.; + } + else { + // scaled energy loss from proton tables + table = tables->GetPhysicsTable(fDedxProton); + massratio = proton_mass_c2/particleMass; + charge2ratio = part->GetPDGCharge()*part->GetPDGCharge(); + } +} + +//....oooOO0OOooo........oooOO0OOooo........oooOO0OOooo........oooOO0OOooo...... diff --git a/TrackPropagation/Geant4e/src/G4WentzelVIModelCustom.cc b/TrackPropagation/Geant4e/src/G4WentzelVIModelCustom.cc new file mode 100644 index 0000000000000..792b32012d4ef --- /dev/null +++ b/TrackPropagation/Geant4e/src/G4WentzelVIModelCustom.cc @@ -0,0 +1,1042 @@ +// +// ******************************************************************** +// * License and Disclaimer * +// * * +// * The Geant4 software is copyright of the Copyright Holders of * +// * the Geant4 Collaboration. It is provided under the terms and * +// * conditions of the Geant4 Software License, included in the file * +// * LICENSE and available at http://cern.ch/geant4/license . These * +// * include a list of copyright holders. * +// * * +// * Neither the authors of this software system, nor their employing * +// * institutes,nor the agencies providing financial support for this * +// * work make any representation or warranty, express or implied, * +// * regarding this software system or assume any liability for its * +// * use. Please see the license in the file LICENSE and URL above * +// * for the full disclaimer and the limitation of liability. * +// * * +// * This code implementation is the result of the scientific and * +// * technical work of the GEANT4 collaboration. * +// * By using, copying, modifying or distributing the software (or * +// * any work based on the software) you agree to acknowledge its * +// * use in resulting scientific publications, and indicate your * +// * acceptance of all terms of the Geant4 Software license. * +// ******************************************************************** +// +// $Id$ +// +// ------------------------------------------------------------------- +// +// GEANT4 Class file +// +// +// File name: G4WentzelVIModelCustom +// +// Author: V.Ivanchenko +// +// Creation date: 09.04.2008 from G4MuMscModel +// +// Modifications: +// 27-05-2010 V.Ivanchenko added G4WentzelOKandVIxSection class to +// compute cross sections and sample scattering angle +// +// +// Class Description: +// +// Implementation of the model of multiple scattering based on +// G.Wentzel, Z. Phys. 40 (1927) 590. +// H.W.Lewis, Phys Rev 78 (1950) 526. +// J.M. Fernandez-Varea et al., NIM B73 (1993) 447. +// L.Urban, CERN-OPEN-2006-077. + +// ------------------------------------------------------------------- +// + +//....oooOO0OOooo........oooOO0OOooo........oooOO0OOooo........oooOO0OOooo...... +//....oooOO0OOooo........oooOO0OOooo........oooOO0OOooo........oooOO0OOooo...... + +#include "TrackPropagation/Geant4e/interface//G4WentzelVIModelCustom.hh" +#include "G4PhysicalConstants.hh" +#include "G4SystemOfUnits.hh" +#include "Randomize.hh" +#include "G4ParticleChangeForMSC.hh" +#include "G4PhysicsTableHelper.hh" +#include "G4ElementVector.hh" +#include "G4ProductionCutsTable.hh" +#include "G4EmParameters.hh" +#include "G4Log.hh" +#include "G4Exp.hh" + +//....oooOO0OOooo........oooOO0OOooo........oooOO0OOooo........oooOO0OOooo...... + +using namespace std; + +G4WentzelVIModelCustom::G4WentzelVIModelCustom(G4bool comb, const G4String& nam) + : G4VMscModel(nam), + ssFactor(1.05), + invssFactor(1.0), + currentCouple(nullptr), + cosThetaMin(1.0), + cosThetaMax(-1.0), + fSecondMoments(nullptr), + idx2(0), + numlimit(0.1), + singleScatteringMode(false), + isCombined(comb), + useSecondMoment(false) +{ + SetSingleScatteringFactor(1.25); + invsqrt12 = 1./sqrt(12.); + tlimitminfix = 1.e-6*mm; + lowEnergyLimit = 1.0*eV; + particle = nullptr; + nelments = 5; + xsecn.resize(nelments); + prob.resize(nelments); + wokvi = new G4WentzelOKandVIxSection(isCombined); + fixedCut = -1.0; + + minNCollisions = 10; + + preKinEnergy = effKinEnergy = tPathLength = zPathLength = lambdaeff + = currentRange = xtsec = cosTetMaxNuc = 0.0; + currentMaterialIndex = 0; + + fParticleChange = nullptr; + currentCuts = nullptr; + currentMaterial = nullptr; +} + +//....oooOO0OOooo........oooOO0OOooo........oooOO0OOooo........oooOO0OOooo...... + +G4WentzelVIModelCustom::~G4WentzelVIModelCustom() +{ + delete wokvi; + if(fSecondMoments && IsMaster()) { + delete fSecondMoments; + fSecondMoments = nullptr; + } +} + +//....oooOO0OOooo........oooOO0OOooo........oooOO0OOooo........oooOO0OOooo...... + +void G4WentzelVIModelCustom::Initialise(const G4ParticleDefinition* p, + const G4DataVector& cuts) +{ + // reset parameters + SetupParticle(p); + currentRange = 0.0; + + if(isCombined) { + G4double tet = PolarAngleLimit(); + if(tet <= 0.0) { cosThetaMax = 1.0; } + else if(tet < CLHEP::pi) { cosThetaMax = cos(tet); } + } + //G4cout << "G4WentzelVIModelCustom::Initialise " << p->GetParticleName() + // << " " << this << " " << wokvi << G4endl; + + wokvi->Initialise(p, cosThetaMax); + /* + G4cout << "G4WentzelVIModelCustom: " << particle->GetParticleName() + << " 1-cos(ThetaLimit)= " << 1 - cosThetaMax + << " SingScatFactor= " << ssFactor + << G4endl; + */ + currentCuts = &cuts; + + // set values of some data members + fParticleChange = GetParticleChangeForMSC(p); + + // build second moment table only if transport table is build + G4PhysicsTable* table = GetCrossSectionTable(); + if(useSecondMoment && IsMaster() && table) { + + //G4cout << "### G4WentzelVIModelCustom::Initialise: build 2nd moment table " + // << table << G4endl; + fSecondMoments = + G4PhysicsTableHelper::PreparePhysicsTable(fSecondMoments); + // Access to materials + const G4ProductionCutsTable* theCoupleTable = + G4ProductionCutsTable::GetProductionCutsTable(); + size_t numOfCouples = theCoupleTable->GetTableSize(); + + G4bool splineFlag = true; + G4PhysicsVector* aVector = nullptr; + G4PhysicsVector* bVector = nullptr; + G4double emin = std::max(LowEnergyLimit(), LowEnergyActivationLimit()); + G4double emax = std::min(HighEnergyLimit(), HighEnergyActivationLimit()); + if(emin < emax) { + size_t n = G4EmParameters::Instance()->NumberOfBinsPerDecade() + *G4lrint(std::log10(emax/emin)); + if(n < 3) { n = 3; } + + for(size_t i=0; iSetupKinematic(kinEnergy, currentMaterial); + if(cosTetMaxNuc < 1.0) { + G4double cut = (0.0 < fixedCut) ? fixedCut : cutEnergy; + G4double cost = wokvi->SetupTarget(G4lrint(Z), cut); + cross = wokvi->ComputeTransportCrossSectionPerAtom(cost); + /* + if(p->GetParticleName() == "e-") + G4cout << "G4WentzelVIModelCustom::CS: Z= " << G4int(Z) << " e(MeV)= "<GetParticleName() << G4endl; + */ + } + return cross; +} + +//....oooOO0OOooo........oooOO0OOooo........oooOO0OOooo........oooOO0OOooo...... + +void G4WentzelVIModelCustom::StartTracking(G4Track* track) +{ + /* + G4cout << "G4WentzelVIModelCustom::StartTracking " << track << " " << this << " " + << track->GetParticleDefinition()->GetParticleName() + << " workvi: " << wokvi << G4endl; + */ + SetupParticle(track->GetParticleDefinition()); +} + +//....oooOO0OOooo........oooOO0OOooo........oooOO0OOooo........oooOO0OOooo...... + +G4double G4WentzelVIModelCustom::ComputeTruePathLengthLimit( + const G4Track& track, + G4double& currentMinimalStep) +{ + G4double tlimit = currentMinimalStep; + const G4DynamicParticle* dp = track.GetDynamicParticle(); + G4StepPoint* sp = track.GetStep()->GetPreStepPoint(); + G4StepStatus stepStatus = sp->GetStepStatus(); + singleScatteringMode = false; + + //G4cout << "G4WentzelVIModelCustom::ComputeTruePathLengthLimit stepStatus= " + // << stepStatus << " " << track.GetDefinition()->GetParticleName() + // << G4endl; + + // initialisation for each step, lambda may be computed from scratch + preKinEnergy = dp->GetKineticEnergy(); + effKinEnergy = preKinEnergy; + DefineMaterial(track.GetMaterialCutsCouple()); + lambdaeff = GetTransportMeanFreePath(particle,preKinEnergy); + currentRange = GetRange(particle,preKinEnergy,currentCouple); + cosTetMaxNuc = wokvi->SetupKinematic(preKinEnergy, currentMaterial); + + //G4cout << "lambdaeff= " << lambdaeff << " Range= " << currentRange + // << " tlimit= " << tlimit << " 1-cost= " << 1 - cosTetMaxNuc << G4endl; + + // extra check for abnormal situation + // this check needed to run MSC with eIoni and eBrem inactivated + if(tlimit > currentRange) { tlimit = currentRange; } + + // stop here if small range particle + if(tlimit < tlimitminfix) { + return ConvertTrueToGeom(tlimit, currentMinimalStep); + } + + // pre step + G4double presafety = sp->GetSafety(); + // far from geometry boundary + if(currentRange < presafety) { + return ConvertTrueToGeom(tlimit, currentMinimalStep); + } + + // compute presafety again if presafety <= 0 and no boundary + // i.e. when it is needed for optimization purposes + if(stepStatus != fGeomBoundary && presafety < tlimitminfix) { + presafety = ComputeSafety(sp->GetPosition(), tlimit); + if(currentRange < presafety) { + return ConvertTrueToGeom(tlimit, currentMinimalStep); + } + } + /* + G4cout << "e(MeV)= " << preKinEnergy/MeV + << " " << particle->GetParticleName() + << " CurLimit(mm)= " << tlimit/mm <<" safety(mm)= " << presafety/mm + << " R(mm)= " <GetParticleName() << G4endl; + + // ignore scattering for zero step length and energy below the limit + if(preKinEnergy < lowEnergyLimit || tPathLength <= 0.0) + { return fDisplacement; } + + G4double invlambda = 0.0; + if(lambdaeff < DBL_MAX) { invlambda = 0.5/lambdaeff; } + + // use average kinetic energy over the step + G4double cut = (*currentCuts)[currentMaterialIndex]; + if(fixedCut > 0.0) { cut = fixedCut; } + /* + G4cout <<"SampleScat: E0(MeV)= "<< preKinEnergy/MeV + << " Leff= " << lambdaeff <<" sig0(1/mm)= " << xtsec + << " xmsc= " << tPathLength*invlambda + << " safety= " << safety << G4endl; + */ + // step limit due msc + G4int nMscSteps = 1; + G4double x0 = tPathLength; + G4double z0 = x0*invlambda; + //G4double zzz = 0.0; + G4double prob2 = 0.0; + + CLHEP::HepRandomEngine* rndmEngine = G4Random::getTheEngine(); + + // large scattering angle case - two step approach + if(!singleScatteringMode) { + static const G4double zzmin = 0.05; + if(useSecondMoment) { + G4double z1 = invlambda*invlambda; + G4double z2 = SecondMoment(particle, currentCouple, effKinEnergy); + prob2 = (z2 - z1)/(1.5*z1 - z2); + } + // if(z0 > zzmin && safety > tlimitminfix) { + if(z0 > zzmin) { + x0 *= 0.5; + z0 *= 0.5; + nMscSteps = 2; + } + //if(z0 > zzmin) { zzz = G4Exp(-1.0/z0); } + G4double zzz = 0.0; + if(z0 > zzmin) { + zzz = G4Exp(-1.0/z0); + z0 += zzz; + prob2 *= (1 + zzz); + } + prob2 /= (1 + prob2); + } + + // step limit due to single scattering + G4double x1 = 2*tPathLength; + if(0.0 < xtsec) { x1 = -G4Log(rndmEngine->flat())/xtsec; } + + // no scattering case + if(singleScatteringMode && x1 > tPathLength) + { return fDisplacement; } + + const G4ElementVector* theElementVector = + currentMaterial->GetElementVector(); + G4int nelm = currentMaterial->GetNumberOfElements(); + + // geometry + G4double sint, cost, phi; + G4ThreeVector temp(0.0,0.0,1.0); + + // current position and direction relative to the end point + // because of magnetic field geometry is computed relatively to the + // end point of the step + G4ThreeVector dir(0.0,0.0,1.0); + fDisplacement.set(0.0,0.0,-zPathLength); + + G4double mscfac = zPathLength/tPathLength; + + // start a loop + G4double x2 = x0; + G4double step, z; + G4bool singleScat; + /* + G4cout << "Start of the loop x1(mm)= " << x1 << " x2(mm)= " << x2 + << " 1-cost1= " << 1 - cosThetaMin << " SSmode= " << singleScatteringMode + << " xtsec= " << xtsec << " Nst= " << nMscSteps << G4endl; + */ + do { + + //G4cout << "# x1(mm)= "<< x1<< " x2(mm)= "<< x2 << G4endl; + // single scattering case + if(singleScatteringMode && x1 > x2) { + fDisplacement += x2*mscfac*dir; + break; + } + + // what is next single of multiple? + if(x1 <= x2) { + step = x1; + singleScat = true; + } else { + step = x2; + singleScat = false; + } + + //G4cout << "# step(mm)= "<< step<< " singlScat= "<< singleScat << G4endl; + + // new position + fDisplacement += step*mscfac*dir; + + if(singleScat) { + + // select element + G4int i = 0; + if(nelm > 1) { + G4double qsec = rndmEngine->flat()*xtsec; + for (; i= qsec) { break; } } + } + G4double cosTetM = + wokvi->SetupTarget((*theElementVector)[i]->GetZasInt(), cut); + //G4cout << "!!! " << cosThetaMin << " " << cosTetM << " " + // << prob[i] << G4endl; + temp = wokvi->SampleSingleScattering(cosThetaMin, cosTetM, prob[i]); + + // direction is changed + temp.rotateUz(dir); + dir = temp; + //G4cout << dir << G4endl; + + // new proposed step length + x2 -= step; + x1 = -G4Log(rndmEngine->flat())/xtsec; + + // multiple scattering + } else { + --nMscSteps; + x1 -= step; + x2 = x0; + + // sample z in interval 0 - 1 + G4bool isFirst = true; + if(prob2 > 0.0 && rndmEngine->flat() < prob2) { isFirst = false; } + do { + //z = -z0*G4Log(1.0 - (1.0 - zzz)*rndmEngine->flat()); + if(isFirst) { z = -G4Log(rndmEngine->flat()); } + else { z = G4RandGamma::shoot(rndmEngine, 2.0, 2.0); } + z *= z0; + // Loop checking, 03-Aug-2015, Vladimir Ivanchenko + } while(z > 1.0); + + cost = 1.0 - 2.0*z/*factCM*/; + if(cost > 1.0) { cost = 1.0; } + else if(cost < -1.0) { cost =-1.0; } + sint = sqrt((1.0 - cost)*(1.0 + cost)); + phi = twopi*rndmEngine->flat(); + G4double vx1 = sint*cos(phi); + G4double vy1 = sint*sin(phi); + + // lateral displacement + if (latDisplasment) { + G4double rms = invsqrt12*sqrt(2*z0); + G4double r = x0*mscfac; + G4double dx = r*(0.5*vx1 + rms*G4RandGauss::shoot(rndmEngine,0.0,1.0)); + G4double dy = r*(0.5*vy1 + rms*G4RandGauss::shoot(rndmEngine,0.0,1.0)); + G4double d = r*r - dx*dx - dy*dy; + + // change position + if(d >= 0.0) { + temp.set(dx,dy,sqrt(d) - r); + temp.rotateUz(dir); + fDisplacement += temp; + } + } + // change direction + temp.set(vx1,vy1,cost); + temp.rotateUz(dir); + dir = temp; + } + // Loop checking, 03-Aug-2015, Vladimir Ivanchenko + } while (0 < nMscSteps); + + dir.rotateUz(oldDirection); + + //G4cout<<"G4WentzelVIModelCustom sampling is done 1-cost= "<< 1.-dir.z()<ProposeMomentumDirection(dir); + + // lateral displacement + fDisplacement.rotateUz(oldDirection); + + /* + G4cout << " r(mm)= " << fDisplacement.mag() + << " safety= " << safety + << " trueStep(mm)= " << tPathLength + << " geomStep(mm)= " << zPathLength + << " x= " << fDisplacement.x() + << " y= " << fDisplacement.y() + << " z= " << fDisplacement.z() + << G4endl; + */ + + //G4cout<< "G4WentzelVIModelCustom::SampleScattering end NewDir= " << dir<< G4endl; + return fDisplacement; +// return dir; +} + +G4ThreeVector +G4WentzelVIModelCustom::SampleScatteringTest(const G4ThreeVector& oldDirection, + G4double /*safety*/) +{ + +// std::cout << "tPathLength = " << tPathLength << " zPathLength = " << zPathLength << std::endl; + + + fDisplacement.set(0.0,0.0,0.0); + //G4cout << "!##! G4WentzelVIModelCustom::SampleScattering for " + // << particle->GetParticleName() << G4endl; + + // ignore scattering for zero step length and energy below the limit + if(preKinEnergy < lowEnergyLimit || tPathLength <= 0.0) + { return fDisplacement; } + + G4double invlambda = 0.0; + if(lambdaeff < DBL_MAX) { invlambda = 0.5/lambdaeff; } + + // use average kinetic energy over the step + G4double cut = (*currentCuts)[currentMaterialIndex]; + if(fixedCut > 0.0) { cut = fixedCut; } + /* + G4cout <<"SampleScat: E0(MeV)= "<< preKinEnergy/MeV + << " Leff= " << lambdaeff <<" sig0(1/mm)= " << xtsec + << " xmsc= " << tPathLength*invlambda + << " safety= " << safety << G4endl; + */ + // step limit due msc + G4int nMscSteps = 1; + G4double x0 = tPathLength; + G4double z0 = x0*invlambda; + //G4double zzz = 0.0; + G4double prob2 = 0.0; + + CLHEP::HepRandomEngine* rndmEngine = G4Random::getTheEngine(); + + // large scattering angle case - two step approach + if(!singleScatteringMode) { + static const G4double zzmin = 0.05; + if(useSecondMoment) { + G4double z1 = invlambda*invlambda; + G4double z2 = SecondMoment(particle, currentCouple, effKinEnergy); + prob2 = (z2 - z1)/(1.5*z1 - z2); + } + // if(z0 > zzmin && safety > tlimitminfix) { + if(z0 > zzmin) { + x0 *= 0.5; + z0 *= 0.5; + nMscSteps = 2; + } + //if(z0 > zzmin) { zzz = G4Exp(-1.0/z0); } + G4double zzz = 0.0; + if(z0 > zzmin) { + zzz = G4Exp(-1.0/z0); + z0 += zzz; + prob2 *= (1 + zzz); + } + prob2 /= (1 + prob2); + } + +// std::cout << "singleScatteringMode = " << singleScatteringMode << " useSecondMoment = " << useSecondMoment << " nMscSteps = " << nMscSteps << std::endl; + + // step limit due to single scattering + G4double x1 = 2*tPathLength; + if(0.0 < xtsec) { x1 = -G4Log(rndmEngine->flat())/xtsec; } + + // no scattering case + if(singleScatteringMode && x1 > tPathLength) + { return fDisplacement; } + + const G4ElementVector* theElementVector = + currentMaterial->GetElementVector(); + G4int nelm = currentMaterial->GetNumberOfElements(); + + // geometry + G4double sint, cost, phi; + G4ThreeVector temp(0.0,0.0,1.0); + + // current position and direction relative to the end point + // because of magnetic field geometry is computed relatively to the + // end point of the step + G4ThreeVector dir(0.0,0.0,1.0); + fDisplacement.set(0.0,0.0,-zPathLength); + + G4double mscfac = zPathLength/tPathLength; + + // start a loop + G4double x2 = x0; + G4double step, z; + G4bool singleScat; + /* + G4cout << "Start of the loop x1(mm)= " << x1 << " x2(mm)= " << x2 + << " 1-cost1= " << 1 - cosThetaMin << " SSmode= " << singleScatteringMode + << " xtsec= " << xtsec << " Nst= " << nMscSteps << G4endl; + */ + do { + + //G4cout << "# x1(mm)= "<< x1<< " x2(mm)= "<< x2 << G4endl; + // single scattering case + if(singleScatteringMode && x1 > x2) { + fDisplacement += x2*mscfac*dir; + break; + } + + // what is next single of multiple? + if(x1 <= x2) { + step = x1; + singleScat = true; + } else { + step = x2; + singleScat = false; + } + + //G4cout << "# step(mm)= "<< step<< " singlScat= "<< singleScat << G4endl; + + // new position + fDisplacement += step*mscfac*dir; + +// std::cout << "singleScat = " << singleScat << std::endl; + + if(singleScat) { + + // select element + G4int i = 0; + if(nelm > 1) { + G4double qsec = rndmEngine->flat()*xtsec; + for (; i= qsec) { break; } } + } + G4double cosTetM = + wokvi->SetupTarget((*theElementVector)[i]->GetZasInt(), cut); + //G4cout << "!!! " << cosThetaMin << " " << cosTetM << " " + // << prob[i] << G4endl; + temp = wokvi->SampleSingleScattering(cosThetaMin, cosTetM, prob[i]); + + // direction is changed + temp.rotateUz(dir); + dir = temp; + //G4cout << dir << G4endl; + + // new proposed step length + x2 -= step; + x1 = -G4Log(rndmEngine->flat())/xtsec; + + // multiple scattering + } else { + --nMscSteps; + x1 -= step; + x2 = x0; + + // sample z in interval 0 - 1 + G4bool isFirst = true; + if(prob2 > 0.0 && rndmEngine->flat() < prob2) { isFirst = false; } + do { +// std::cout << "isFirst = " << isFirst << std::endl; + //z = -z0*G4Log(1.0 - (1.0 - zzz)*rndmEngine->flat()); + if(isFirst) { z = -G4Log(rndmEngine->flat()); } + else { z = G4RandGamma::shoot(rndmEngine, 2.0, 2.0); } + z *= z0; + // Loop checking, 03-Aug-2015, Vladimir Ivanchenko + } while(z > 1.0); + +// std::cout << "lambdaeff = " << lambdaeff << " tPathLength = " << tPathLength << " z0 = " << z0 << " z = " << z << std::endl; + + cost = 1.0 - 2.0*z/*factCM*/; + if(cost > 1.0) { cost = 1.0; } + else if(cost < -1.0) { cost =-1.0; } + sint = sqrt((1.0 - cost)*(1.0 + cost)); + phi = twopi*rndmEngine->flat(); + G4double vx1 = sint*cos(phi); + G4double vy1 = sint*sin(phi); + + // lateral displacement + if (latDisplasment) { + G4double rms = invsqrt12*sqrt(2*z0); + G4double r = x0*mscfac; + G4double dx = r*(0.5*vx1 + rms*G4RandGauss::shoot(rndmEngine,0.0,1.0)); + G4double dy = r*(0.5*vy1 + rms*G4RandGauss::shoot(rndmEngine,0.0,1.0)); + G4double d = r*r - dx*dx - dy*dy; + + // change position + if(d >= 0.0) { + temp.set(dx,dy,sqrt(d) - r); + temp.rotateUz(dir); + fDisplacement += temp; + } + } + // change direction + temp.set(vx1,vy1,cost); + temp.rotateUz(dir); + dir = temp; + } + // Loop checking, 03-Aug-2015, Vladimir Ivanchenko + } while (0 < nMscSteps); + + dir.rotateUz(oldDirection); + + //G4cout<<"G4WentzelVIModelCustom sampling is done 1-cost= "<< 1.-dir.z()<ProposeMomentumDirection(dir); + + // lateral displacement + fDisplacement.rotateUz(oldDirection); + + /* + G4cout << " r(mm)= " << fDisplacement.mag() + << " safety= " << safety + << " trueStep(mm)= " << tPathLength + << " geomStep(mm)= " << zPathLength + << " x= " << fDisplacement.x() + << " y= " << fDisplacement.y() + << " z= " << fDisplacement.z() + << G4endl; + */ + + //G4cout<< "G4WentzelVIModelCustom::SampleScattering end NewDir= " << dir<< G4endl; +// return fDisplacement; + return dir; +} + +double G4WentzelVIModelCustom::ProjectedVariance() const { +// return 1.5*tPathLength*tPathLength/lambdaeff/lambdaeff; + std::cout << "tPathLength = " << tPathLength << " lambdeff = " << lambdaeff << std::endl; + return tPathLength/lambdaeff; +} + +//....oooOO0OOooo........oooOO0OOooo........oooOO0OOooo........oooOO0OOooo...... + +G4double G4WentzelVIModelCustom::ComputeTransportXSectionPerVolume(G4double cosTheta) +{ + // prepare recomputation of x-sections + const G4ElementVector* theElementVector = currentMaterial->GetElementVector(); + const G4double* theAtomNumDensityVector = + currentMaterial->GetVecNbOfAtomsPerVolume(); + G4int nelm = currentMaterial->GetNumberOfElements(); + if(nelm > nelments) { + nelments = nelm; + xsecn.resize(nelm); + prob.resize(nelm); + } + + // check consistency + xtsec = 0.0; + if(cosTetMaxNuc >= cosTheta) { return 0.0; } + + G4double cut = (*currentCuts)[currentMaterialIndex]; + if(fixedCut > 0.0) { cut = fixedCut; } + + // loop over elements + G4double xs = 0.0; + for (G4int i=0; iSetupTarget((*theElementVector)[i]->GetZasInt(), cut); + G4double density = theAtomNumDensityVector[i]; + + G4double esec = 0.0; + if(costm < cosTheta) { + + // recompute the transport x-section + if(1.0 > cosTheta) { + xs += density*wokvi->ComputeTransportCrossSectionPerAtom(cosTheta); + } + // recompute the total x-section + G4double nucsec = wokvi->ComputeNuclearCrossSection(cosTheta, costm); + esec = wokvi->ComputeElectronCrossSection(cosTheta, costm); + nucsec += esec; + if(nucsec > 0.0) { esec /= nucsec; } + xtsec += nucsec*density; + } + xsecn[i] = xtsec; + prob[i] = esec; + //G4cout << i << " xs= " << xs << " xtsec= " << xtsec + // << " 1-cosTheta= " << 1-cosTheta + // << " 1-cosTetMaxNuc2= " <<1-cosTetMaxNuc2<< G4endl; + } + + //G4cout << "ComputeXS result: xsec(1/mm)= " << xs + // << " txsec(1/mm)= " << xtsec <SetupKinematic(kinEnergy, currentMaterial); + + if(cosTetMaxNuc >= 1.0) { return xs; } + + const G4ElementVector* theElementVector = currentMaterial->GetElementVector(); + const G4double* theAtomNumDensityVector = + currentMaterial->GetVecNbOfAtomsPerVolume(); + G4int nelm = currentMaterial->GetNumberOfElements(); + + G4double cut = (*currentCuts)[currentMaterialIndex]; + if(fixedCut > 0.0) { cut = fixedCut; } + + // loop over elements + for (G4int i=0; iSetupTarget((*theElementVector)[i]->GetZasInt(), cut); + xs += theAtomNumDensityVector[i] + *wokvi->ComputeSecondTransportMoment(costm); + } + return xs; +} + +//....oooOO0OOooo........oooOO0OOooo........oooOO0OOooo........oooOO0OOooo.... + +void G4WentzelVIModelCustom::SetSingleScatteringFactor(G4double val) +{ + if(val > 0.05) { + ssFactor = val; + invssFactor = 1.0/(val - 0.05); + } +} + +//....oooOO0OOooo........oooOO0OOooo........oooOO0OOooo........oooOO0OOooo...... diff --git a/TrackPropagation/Geant4e/src/Geant4ePropagator.cc b/TrackPropagation/Geant4e/src/Geant4ePropagator.cc index ab2add81b596c..8e5b2cc9ccad9 100644 --- a/TrackPropagation/Geant4e/src/Geant4ePropagator.cc +++ b/TrackPropagation/Geant4e/src/Geant4ePropagator.cc @@ -31,6 +31,9 @@ #include "G4TransportationManager.hh" #include "G4Tubs.hh" #include "G4UImanager.hh" +#include "G4RunManagerKernel.hh" +#include "G4PathFinder.hh" +#include "G4ErrorPropagationNavigator.hh" // CLHEP #include "CLHEP/Units/GlobalSystemOfUnits.h" @@ -42,6 +45,9 @@ #include "SimG4Core/MagneticField/interface/Field.h" +#include "Math/ProbFuncMathCore.h" +#include "Math/QuantFuncMathCore.h" + /** Constructor. */ @@ -60,6 +66,27 @@ Geant4ePropagator::Geant4ePropagator(const MagneticField *field, // has to be called here, doing it later will not load the G4 physics list // properly when using the G4 ES Producer. Reason: unclear ensureGeant4eIsInitilized(true); + + const G4ParticleDefinition *partdef = G4ParticleTable::GetParticleTable()->FindParticle(generateParticleName(1)); + + fluct = new G4UniversalFluctuationForExtrapolator(); + fluct->SetParticleAndCharge(partdef, 1.); + + if (false) { + //FIXME memory leak + G4DataVector *cuts = new G4DataVector(G4Material::GetNumberOfMaterials(), DBL_MAX); + msmodel = new G4WentzelVIModelCustom(); + msmodel->Initialise(partdef, *cuts); + } + + if (false) { + const G4MaterialTable *mattable = G4Material::GetMaterialTable(); + + for (const G4Material *mat : *mattable) { + std::cout << "name = " << mat->GetName() << " density = " << mat->GetDensity() / mg * mole << " radlen = " << mat->GetRadlen() << std::endl; + } + } + } /** Destructor. @@ -70,6 +97,7 @@ Geant4ePropagator::~Geant4ePropagator() { // don't close the g4 Geometry here, because the propagator might have been // cloned // but there is only one, globally-shared Geometry + delete fluct; } // @@ -87,6 +115,8 @@ void Geant4ePropagator::ensureGeant4eIsInitilized(bool forceInit) const { //G4UImanager::GetUIpointer()->ApplyCommand("/exerror/setField -10. kilogauss"); +// G4ProductionCutsTable::GetProductionCutsTable()->UpdateCoupleTable(G4RunManagerKernel::GetRunManagerKernel()->GetCurrentWorld()); + theG4eManager->SetUserInitialization(new G4ErrorPhysicsListCustom()); theG4eManager->InitGeant4e(); @@ -292,6 +322,14 @@ std::pair Geant4ePropagator::propagateGeneric( // //* Set the target surface + const G4Field *field = G4TransportationManager::GetTransportationManager()->GetFieldManager()->GetDetectorField(); +// //FIXME check thread safety of this +// sim::Field *cmsField = const_cast(static_cast(field)); +// +// // cmsField->SetOffset(0., 0., dBz); +// // cmsField->SetMaterialOffset(dxi); +// cmsField->SetMaterialOffset(std::log(1e-10)); + ErrorTargetPair g4eTarget_center = transformToG4SurfaceTarget(pDest, false); // * Get the starting point and direction and convert them to @@ -462,32 +500,59 @@ std::pair Geant4ePropagator::propagateGeneric( return TsosPP(TrajectoryStateOnSurface(tParsDest, curvError, pDest, side), finalPathLength); } -std::tuple Geant4ePropagator::propagateGenericWithJacobian(const FreeTrajectoryState &ftsStart, - const Plane &pDest) const { - - auto retDefault = []() { - return std::tuple(); + std::tuple, Eigen::Matrix, Eigen::Matrix, double, Eigen::Matrix, Eigen::Matrix, double, double> Geant4ePropagator::propagateGenericWithJacobianAltD(const Eigen::Matrix &ftsStart, + const GloballyPositioned &pDest, double dBz, double dxi, double dms, double dioni, double pforced) const { + + using namespace Eigen; + + + const G4Field *field = G4TransportationManager::GetTransportationManager()->GetFieldManager()->GetDetectorField(); + //FIXME check thread safety of this + sim::Field *cmsField = const_cast(static_cast(field)); + + cmsField->SetOffset(0., 0., dBz); + cmsField->SetMaterialOffset(dxi); +// cmsField->SetMaterialOffset(std::log(1e-6)); +// cmsField->SetMaterialOffset(std::log(1e-10)); + + auto retDefault = [cmsField]() { + cmsField->SetOffset(0., 0., 0.); + cmsField->SetMaterialOffset(0.); +// return std::tuple(); + return std::tuple, Matrix, Matrix, double, Matrix, Matrix, double, double>(false, Matrix::Zero(), Matrix::Zero(), Matrix::Zero(), 0., Matrix::Zero(), Matrix::Zero(), 0., 0.); }; + + + +// std::cout << "start propagation" << std::endl; + /////////////////////////////// // Construct the target surface // //* Set the target surface - ErrorTargetPair g4eTarget_center = transformToG4SurfaceTarget(pDest, false); + //TODO fix precision of this + ErrorTargetPair g4eTarget_center = transformToG4SurfaceTargetD(pDest, false); + CLHEP::Hep3Vector g4InitPos(ftsStart[0]*cm, ftsStart[1]*cm, ftsStart[2]*cm); + CLHEP::Hep3Vector g4InitMom(ftsStart[3]*GeV, ftsStart[4]*GeV, ftsStart[5]*GeV); + // * Get the starting point and direction and convert them to // CLHEP::Hep3Vector // for G4. CMS uses cm and GeV while Geant4 uses mm and MeV - GlobalPoint cmsInitPos = ftsStart.position(); - GlobalVector cmsInitMom = ftsStart.momentum(); + GlobalPoint cmsInitPos(ftsStart[0], ftsStart[1], ftsStart[2]); + GlobalVector cmsInitMom(ftsStart[3], ftsStart[4], ftsStart[5]); bool flipped = false; if (propagationDirection() == oppositeToMomentum) { // flip the momentum vector as Geant4 will not do this // on it's own in a backward propagation cmsInitMom = -cmsInitMom; + g4InitMom = -g4InitMom; flipped = true; } + + const double charge = ftsStart[6]; // Set the mode of propagation according to the propagation direction G4ErrorMode mode = G4ErrorMode_PropForwards; @@ -495,13 +560,27 @@ std::tupleGetFieldValue(posarr, fieldval); +// +// std::cout << "cmsInitPos:" << std::endl; +// std::cout << cmsInitPos << std::endl; +// std::cout << "fieldval " << fieldval[0] << " " << fieldval[1] << " " << fieldval[2] << std::endl; + +// CLHEP::Hep3Vector g4InitPos = TrackPropagation::globalPointToHep3Vector(cmsInitPos); +// CLHEP::Hep3Vector g4InitMom = TrackPropagation::globalVectorToHep3Vector(cmsInitMom * GeV); + - debugReportTrackState("intitial", cmsInitPos, g4InitPos, cmsInitMom, g4InitMom, pDest); + +// debugReportTrackState("intitial", cmsInitPos, g4InitPos, cmsInitMom, g4InitMom, pDest); // Set the mode of propagation according to the propagation direction // G4ErrorMode mode = G4ErrorMode_PropForwards; @@ -512,16 +591,16 @@ std::tupleSetStage(G4ErrorStage_Deflation); } - G4ErrorFreeTrajState g4eTrajState(generateParticleName(ftsStart.charge()), g4InitPos, g4InitMom, g4error); + G4ErrorFreeTrajState g4eTrajState(generateParticleName(charge), g4InitPos, g4InitMom, g4error); LogDebug("Geant4e") << "G4e - Traj. State: " << (g4eTrajState); ////////////////////////////// @@ -550,27 +629,90 @@ std::tupleInitTrackPropagation(); + // re-initialize navigator to avoid mismatches and/or segfaults +// G4TransportationManager::GetTransportationManager()->GetPropagatorInField()->GetNavigatorForPropagating()->LocateGlobalPointAndSetup(g4InitPos, &g4InitMom); +// theG4eManager->GetErrorPropagationNavigator()->LocateGlobalPointAndSetup(g4InitPos, &g4InitMom); + theG4eManager->GetErrorPropagationNavigator()->LocateGlobalPointAndSetup(g4InitPos, &g4InitMom, false, false); +// std::cout << "pathfinder = " << G4PathFinder::GetInstance() << std::endl; +// G4PathFinder::GetInstance()->Locate(g4InitPos, g4InitMom, false); + +// std::cout << "pathfinder nav0 = " << G4PathFinder::GetInstance()->GetNavigator(0) << " prop navigator = " << theG4eManager->GetErrorPropagationNavigator() << std::endl; + // initial jacobian is the identity matrix for the state components, // and 0 for b-field and material variations - G4ErrorMatrix jac(5, 7); - for (unsigned int i = 0; i < 5; ++i) { - for (unsigned int j = 0; j < 7; ++j) { - jac(i+1, j+1) = i == j ? 1. : 0.; - } - } +// G4ErrorMatrix jac(5, 7); +// for (unsigned int i = 0; i < 5; ++i) { +// for (unsigned int j = 0; j < 7; ++j) { +// jac(i+1, j+1) = i == j ? 1. : 0.; +// } +// } + + Matrix jac; + jac.leftCols<5>() = Matrix::Identity(); + jac.rightCols<2>() = Matrix::Zero(); const G4ErrorTrajErr errnull(5, 0); - G4ErrorTrajErr g4errorEnd = g4eTrajState.GetError(); - G4ErrorTrajErr dQ(5, 0); +// const G4ErrorTrajErr g4errorEnd = g4eTrajState.GetError(); +// const AlgebraicMatrix55 errstart = ftsStart.curvilinearError().matrix(); + Matrix g4errorEnd = Matrix::Zero(); + +// Matrix ftsEnd = ftsStart; + + +// G4ErrorTrajErr dQ(5, 0); + Matrix dQ = Matrix::Zero(); + Matrix dQ2 = Matrix::Zero(); + + double dEdxlast = 0.; +// double masslast = 0.; + + Matrix dErrorDxLast = Matrix::Zero(); + + bool firstStep = true; + double RItotal = 0.; + double deltaTotal = 0.; + double wTotal = 0.; bool continuePropagation = true; while (continuePropagation) { +// if (iterations > 0) { +// G4PathFinder::GetInstance()->ReLocate(g4eTrajState.GetPosition()); +// } + + // re-initialize navigator to avoid mismatches and/or segfaults + theG4eManager->GetErrorPropagationNavigator()->LocateGlobalPointWithinVolume(g4eTrajState.GetPosition()); +// G4PathFinder::GetInstance()->ReLocate(g4eTrajState.GetPosition()); +// const G4ThreeVector momstep = g4eTrajState.GetMomentum(); +// theG4eManager->GetErrorPropagationNavigator()->LocateGlobalPointAndSetup(g4eTrajState.GetPosition(), &momstep, false, false); +// theG4eManager->GetErrorPropagationNavigator() +// G4PathFinder::GetInstance()->Locate(g4InitPos, g4InitMom, false); + + // brute force re-initialization at every step to be safe +// const G4ThreeVector momstep = g4eTrajState.GetMomentum(); +// theG4eManager->GetErrorPropagationNavigator()->LocateGlobalPointAndSetup(g4eTrajState.GetPosition(), &momstep, false, false); + + iterations++; LogDebug("Geant4e") << std::endl << "step count " << iterations << " step length " << finalPathLength; +// const GlobalPoint pos = TrackPropagation::hepPoint3DToGlobalPoint(g4eTrajState.GetPosition()); +// const GlobalVector mom = TrackPropagation::hep3VectorToGlobalVector(g4eTrajState.GetMomentum() / GeV); +// constexpr double mass = 0.1056583745; + +// const FreeTrajectoryState statepre(pos, mom, ftsStart.charge(), &ftsStart.parameters().magneticField()); + + Matrix statepre; + statepre[0] = g4eTrajState.GetPosition().x()/cm; + statepre[1] = g4eTrajState.GetPosition().y()/cm; + statepre[2] = g4eTrajState.GetPosition().z()/cm; + statepre[3] = g4eTrajState.GetMomentum().x()/GeV; + statepre[4] = g4eTrajState.GetMomentum().y()/GeV; + statepre[5] = g4eTrajState.GetMomentum().z()/GeV; + statepre[6] = charge; + //set the error matrix to null to disentangle MS and ionization contributions g4eTrajState.SetError(errnull); const int ierr = theG4eManager->PropagateOneStep(&g4eTrajState, mode); @@ -580,3090 +722,1172 @@ std::tupleGetStepLength()); + const double thisPathLength = TrackPropagation::g4doubleToCmsDouble(g4eTrajState.GetG4Track()->GetStepLength()); - const G4ErrorMatrix &transferMatrix = g4eTrajState.GetTransfMat(); + const double pPre = g4eTrajState.GetMomentum().mag()/GeV; + const double ePre = g4eTrajState.GetG4Track()->GetStep()->GetPreStepPoint()->GetTotalEnergy() / GeV; + const double ePost = g4eTrajState.GetG4Track()->GetStep()->GetPostStepPoint()->GetTotalEnergy() / GeV; + const double dEdx = (ePost - ePre)/thisPathLength; + const double mass = g4eTrajState.GetG4Track()->GetDynamicParticle()->GetMass() / GeV; + + if (std::abs(thisPathLength) > 0.) { + dEdxlast = dEdx; +// masslast = mass; + } - // transport contribution to error - g4errorEnd = g4errorEnd.similarity(transferMatrix).T(); - dQ = dQ.similarity(transferMatrix).T(); +// std::cout << "thisPathLength = " << thisPathLength << std::endl; +// const Matrix transportJac = transportJacobian(statepre, thisPathLength, dEdx, mass); + const Matrix transportJac = transportJacobianBzD(statepre, thisPathLength, dEdx, mass, dBz); - // MS and ionization contributions to error +// const Matrix transportJacAlt = transportJacobianD(statepre, thisPathLength, dEdx, mass); - G4ErrorTrajErr errMSI = g4eTrajState.GetError(); +// const double dh = 1e-4; + +// const Matrix dest = transportResultD(statepre, thisPathLength, dEdx, mass, 0.); +// const Matrix destalt = transportResultD(statepre, thisPathLength, dEdx, mass, dh); - // recompute ionization contribution (without truncation for the moment) - errMSI(0+1, 0+1) = computeErrorIoni(g4eTrajState.GetG4Track()); +// const Matrix ddest = (destalt-dest)/dh; +// +// Matrix statepost; +// statepost[0] = g4eTrajState.GetPosition().x()/cm; +// statepost[1] = g4eTrajState.GetPosition().y()/cm; +// statepost[2] = g4eTrajState.GetPosition().z()/cm; +// statepost[3] = g4eTrajState.GetMomentum().x()/GeV; +// statepost[4] = g4eTrajState.GetMomentum().y()/GeV; +// statepost[5] = g4eTrajState.GetMomentum().z()/GeV; +// statepost[6] = charge; +// +// const Matrix Wpost = statepost.segment<3>(3).normalized(); +// const Matrix khat(0., 0., 1.); +// +// const Matrix Upost = khat.cross(Wpost).normalized(); +// // const Matrix Upost = Upostpre.normalized(); +// +// const double ddxt = ddest.head<3>().dot(Upost); - g4errorEnd += errMSI; +// std::cout << - // transport + nominal energy loss contribution to jacobian - jac = transferMatrix*jac; - //TODO assess relevance of approximations (does the order matter? position/momentum before or after step?) - // local b-field contribution to jacobian - const GlobalPoint pos = TrackPropagation::hepPoint3DToGlobalPoint(g4eTrajState.GetPosition()); - const GlobalVector mom = TrackPropagation::hep3VectorToGlobalVector(g4eTrajState.GetMomentum() / GeV); - const GlobalVector momPre = TrackPropagation::hep3VectorToGlobalVector(g4eTrajState.GetG4Track()->GetStep()->GetPreStepPoint()->GetMomentum() / GeV); - const double B = theField->inTesla(pos).mag(); - const double pinv = 1./mom.mag(); - for (unsigned int i = 1; i < 5; ++i) { - jac(i+1, 5+1) += transferMatrix(i+1, 0+1)*pinv/B; - } +// const Matrix transportJac = transportJacobianBzAdvanced(statepre, thisPathLength, dEdx, mass); - // local material contribution to jacobian - const double ePre = g4eTrajState.GetG4Track()->GetStep()->GetPreStepPoint()->GetTotalEnergy() / GeV; - const double ePost = g4eTrajState.GetG4Track()->GetStep()->GetPostStepPoint()->GetTotalEnergy() / GeV; - const double dE = ePost - ePre; - -// std::cout << "ePost = " << ePost << " p = " << mom.mag() << std::endl; +// const Matrix transportJacAlt = transportJacobianBz(statepre, thisPathLength, dEdx, mass); + +// std::cout << "transportJac" << std::endl; +// std::cout << transportJac << std::endl; +// // std::cout << "transportJacAlt" << std::endl; +// // std::cout << transportJacAlt << std::endl; +// std::cout << "ddest numerical" << std::endl; +// std::cout << ddest.transpose() << std::endl; +// std::cout << "ddxt jac = " << transportJac(3, 5) << " ddxt numerical = " << ddxt << std::endl; + +// bool isnan = false; +// for (unsigned int i=0; i()*g4errorEnd*transportJac.leftCols<5>().transpose()).eval(); + dQ = (transportJac.leftCols<5>()*dQ*transportJac.leftCols<5>().transpose()).eval(); + dQ2 = (transportJac.leftCols<5>()*dQ2*transportJac.leftCols<5>().transpose()).eval(); - const double pPre = momPre.mag(); - const double betaPre = pPre/ePre; - const double mass = g4eTrajState.GetG4Track()->GetDynamicParticle()->GetMass() / GeV; +// const G4ErrorMatrix &transferMatrix = g4eTrajState.GetTransfMat(); - G4ErrorTrajErr errMS = errMSI; - errMS(0+1, 0+1) = 0.; -// std::cout << "ppre = " << pPre << " betaPre = " << betaPre << " mass = " << mass << " ePre = " << ePre << std::endl; - dQ += 2.*pPre*(betaPre*betaPre + 2.*mass*mass/ePre/ePre)*errMS; -// dQ += 2.*pPre*(betaPre*betaPre + 2.*mass*mass/ePre/ePre)*errMS; + // transport contribution to error +// g4errorEnd = g4errorEnd.similarity(transferMatrix).T(); +// dQ = dQ.similarity(transferMatrix).T(); -// std::cout << "dE = " << dE << std::endl; + // MS and ionization contributions to error +// G4ErrorTrajErr errMSI = g4eTrajState.GetError(); +// +// // recompute ionization contribution (without truncation for the moment) +// errMSI(0+1, 0+1) = computeErrorIoni(g4eTrajState.GetG4Track()); +// +// Matrix errMSIout = Matrix::Zero(); +// +// // std::cout << "errMSI" << std::endl; +// // std::cout << errMSI << std::endl; +// +// for (unsigned int i = 0; i < 5; i++) { +// for (unsigned int j = 0; j < 5; j++) { +// double w = 1.; +// if (i==0) { +// w *= charge; +// } +// if (j==0) { +// w *= charge; +// } +// errMSIout(i, j) += w*errMSI(i+1, j+1); +// } +// } -// std::cout << "jac" << std::endl; -// std::cout << jac << std::endl; + Matrix errMSIout = PropagateErrorMSC(g4eTrajState.GetG4Track(), pforced); - LogDebug("Geant4e") << "step Length was " << thisPathLength << " cm, current global position: " - << TrackPropagation::hepPoint3DToGlobalPoint(g4eTrajState.GetPosition()) << std::endl; + // std::cout << "dxi = " << dxi << " drad = " << drad << std::endl; - finalPathLength += thisPathLength; + // scaling only affects MS + const double msfact = std::exp(dms); + // std::cout << "dms = " << dms << " msfact = " << msfact << std::endl; + + errMSIout *= msfact; - // if (std::fabs(finalPathLength) > 10000.0f) - if (std::fabs(finalPathLength) > 200.0f) { - LogDebug("Geant4e") << "ERROR: Quitting propagation: path length mega large" << std::endl; - theG4eManager->GetPropagator()->InvokePostUserTrackingAction(g4eTrajState.GetG4Track()); - continuePropagation = false; - LogDebug("Geant4e") << "WARNING: Quitting propagation: max path length " - "exceeded, returning invalid state" - << std::endl; + const G4Material* mate = g4eTrajState.GetG4Track()->GetVolume()->GetLogicalVolume()->GetMaterial(); + const double X0 = mate->GetRadlen() / cm; + RItotal += msfact*thisPathLength/X0; - // reached maximum path length, bail out - return retDefault(); - } + // DDtotal += errMSIout(1, 1); - if (theG4eManager->GetPropagator()->CheckIfLastStep(g4eTrajState.GetG4Track())) { - theG4eManager->GetPropagator()->InvokePostUserTrackingAction(g4eTrajState.GetG4Track()); - continuePropagation = false; - } - } +// std::cout << "radfact = " << radfact << std::endl; - // CMSSW Tracking convention, backward propagations have negative path length - if (propagationDirection() == oppositeToMomentum) - finalPathLength = -finalPathLength; +// errMSIout *= radfact; - // store the correct location for the hit on the RECO surface - LogDebug("Geant4e") << "Position on the RECO surface" << g4eTrajState.GetPosition() << std::endl; - finalRecoPos = g4eTrajState.GetPosition(); + if (false) { + // XItotal += computeXi(g4eTrajState.GetG4Track()); + const std::pair landau = computeLandau(g4eTrajState.GetG4Track()); - theG4eManager->EventTermination(); + // G4double dedxsqurban = 1e-6*fluct->SampleFluctuations(mate, aTrack->GetDynamicParticle(), Emaxmev, stepLengthmm, ekinmev); - LogDebug("Geant4e") << "Final position of the Track :" << g4eTrajState.GetPosition() << std::endl; + // std::cout << "material = " << mate->GetName() << " thisPathLength = " << thisPathLength << " dEdx = " << dEdx << " dEdx*thisPathLength = " << dEdx*thisPathLength << " delta = " << landau.first << " w = " << landau.second << std::endl; - ////////////////////////////// - // Retrieve the state in the end from Geant4e, convert them to CMS vectors - // and points, and build global trajectory parameters. - // CMS uses cm and GeV while Geant4 uses mm and MeV - // - const HepGeom::Vector3D momEnd = g4eTrajState.GetMomentum(); + deltaTotal += landau.first; + wTotal += landau.second; + + // const double kd = ROOT::Math::landau_pdf(ROOT::Math::landau_quantile(0.5)) + constexpr double kd = 0.13017714; + const double c = landau.second; - // use the hit on the the RECO plane as the final position to be d'accor with - // the RecHit measurements - const GlobalPoint posEndGV = TrackPropagation::hepPoint3DToGlobalPoint(finalRecoPos); - GlobalVector momEndGV = TrackPropagation::hep3VectorToGlobalVector(momEnd) / GeV; + const double ascale = 2.*c/M_PI; - debugReportTrackState("final", posEndGV, finalRecoPos, momEndGV, momEnd, pDest); + constexpr double alphalandau = 0.996; + const double varE = ascale*ascale*ROOT::Math::landau_xm2(ROOT::Math::landau_quantile(alphalandau)); - // Get the error covariance matrix from Geant4e. It comes in curvilinear - // coordinates so use the appropiate CMS class -// G4ErrorTrajErr g4errorEnd = g4eTrajState.GetError(); - CurvilinearTrajectoryError curvError( - TrackPropagation::g4ErrorTrajErrToAlgebraicSymMatrix55(g4errorEnd, ftsStart.charge())); + // const double varE = 2.*c*c/M_PI/M_PI/M_PI/kd/kd; - if (mode == G4ErrorMode_PropBackwards) { - GlobalTrajectoryParameters endParm( - posEndGV, momEndGV, ftsStart.parameters().charge(), &ftsStart.parameters().magneticField()); + // Etot = aTrack->GetTotalEnergy() / GeV; - // flip the momentum direction because it has been flipped before running - // G4's backwards prop - momEndGV = -momEndGV; - } + const double pPre6 = std::pow(pPre, 6); + // Apply it to error + const double varqop = ePre * ePre * varE / pPre6; - LogDebug("Geant4e") << "G4e - Error matrix after propagation: " << std::endl << g4errorEnd; + } - LogDebug("Geant4e") << "CMS - Error matrix after propagation: " << std::endl << curvError.matrix(); + + const double ionifact = std::exp(dioni); + + // std::cout << "dioni = " << dioni << " ionifact = " << ionifact << std::endl; -// GlobalTrajectoryParameters tParsDest(posEndGV, momEndGV, ftsStart.charge(), theField); - -// auto const localf = pDest.toLocal(posEndGV); -// auto const locald = pDest.toLocal(posEndGV); - -// std::cout << "localf = " << localf << std::endl; -// std::cout << "locald = " << locald << std::endl; -// std::cout << "diff = " << localf-locald << std::endl; - - auto const localpos = pDest.toLocal(posEndGV); - auto const localmom = pDest.toLocal(momEndGV); - - const LocalPoint localposcor(localpos.x() - localpos.z()*localmom.x()/localmom.z(), localpos.y() - localpos.z()*localmom.y()/localmom.z(), 0.); - - const LocalTrajectoryParameters localparms(localposcor, localmom, ftsStart.charge()); - - const GlobalPoint posEndcor = pDest.toGlobal(localposcor); - - GlobalTrajectoryParameters tParsDest(posEndcor, momEndGV, ftsStart.charge(), theField); - + +// errMSIout(0, 0) = varqop; + errMSIout(0, 0) = ionifact*computeErrorIoni(g4eTrajState.GetG4Track(), pforced); + + // std::cout << "ionifact = " << ionifact << " errMSIout(0, 0) = " << errMSIout(0, 0) << std::endl; + - SurfaceSideDefinition::SurfaceSide side; + // separate scaling for ionization and MS +// const double xifact = std::exp(dxi); +// const double radfact = std::exp(drad); - side = propagationDirection() == alongMomentum ? SurfaceSideDefinition::afterSurface - : SurfaceSideDefinition::beforeSurface; +// errMSIout(0, 0) *= radfact; +// errMSIout.bottomRightCorner<4, 4>() *= xifact; + + g4errorEnd += errMSIout; + + - AlgebraicMatrix57 jacfinal; - // convert from 1/p to q/p - for (unsigned int i = 0; i < 5; ++i) { - for (unsigned int j = 0; j < 7; ++j) { - jacfinal(i, j) = jac(i+1, j+1); - if (i==0) { - jacfinal(i, j) *= ftsStart.charge(); - } - if (j==0) { - jacfinal(i, j) *= ftsStart.charge(); - } - } - } - - AlgebraicMatrix55 dQfinal; - for (unsigned int i = 0; i < 5; ++i) { - for (unsigned int j = 0; j < 5; ++j) { - dQfinal(i, j) = ftsStart.charge()*dQ(i+1, j+1); + + if (std::abs(thisPathLength) > 0.) { + dErrorDxLast = errMSIout/thisPathLength; } - } - -// JacobianCurvilinearToLocal curv2Loc(pDest, localparms, tParsDest, *theField); - const AlgebraicMatrix55 curv2Loc = curv2localJacobianAlt(tParsDest, pDest); - auto const localerr = ROOT::Math::Similarity(curv2Loc, curvError.matrix()); + + // transport + nominal energy loss contribution to jacobian +// jac = transferMatrix*jac; + jac = (transportJac.leftCols<5>()*jac).eval(); + //TODO assess relevance of approximations (does the order matter? position/momentum before or after step?) + //b-field and material contributions to jacobian + jac.rightCols<2>() += transportJac.rightCols<2>(); + + +// const double betaPre = pPre/ePre; +// G4ErrorTrajErr errMS = errMSI; +// errMS(0+1, 0+1) = 0.; +// // std::cout << "ppre = " << pPre << " betaPre = " << betaPre << " mass = " << mass << " ePre = " << ePre << std::endl; +// // G4ErrorTrajErr dQpre = 2.*pPre*(betaPre*betaPre + 2.*mass*mass/ePre/ePre)*errMS; +// G4ErrorTrajErr dQpre = (2.*pPre + 4.*mass*mass/pPre)*betaPre*betaPre*errMS; +// +// for (unsigned int i = 0; i < 5; i++) { +// for (unsigned int j = 0; j < 5; j++) { +// double w = charge; +// if (i==0) { +// w *= charge; +// } +// if (j==0) { +// w *= charge; +// } +// w *= jac(0,0); +// dQ(i, j) += w*dQpre(i+1, j+1); +// } +// } + +// Matrix errMS = errMSIout; +// errMS(0,0) = 0.; + +// Matrix errIoni = Matrix::Zero(); +// errIoni(0,0) = errMSIout(0,0); +// +// const double q = charge; +// const double qop = charge/pPre; +// const double eMass = 510.99906e-6; +// +// const double x0 = std::pow(q, 2); +// const double x1 = std::pow(mass, 2)*std::pow(qop, 2); +// const double x2 = 1./(qop*(x0 + x1)); +// const Matrix dQms = 2.*errMS*x2*(x0 + 2.*x1); +// const Matrix dQion = errIoni*x2*(4.0*x0 + 10.0*x1); -// return std::tuple(TrajectoryStateOnSurface(tParsDest, curvError, pDest, side), jacfinal, dQfinal); - - return std::tuple(TrajectoryStateOnSurface(localparms, localerr, pDest, theField, side), jacfinal, dQfinal); -} -std::tuple Geant4ePropagator::propagateGenericWithJacobianAlt(const FreeTrajectoryState &ftsStart, - const Plane &pDest) const { - - using namespace Eigen; - - auto retDefault = []() { - return std::tuple(); - }; - -// std::cout << "start propagation" << std::endl; - - /////////////////////////////// - // Construct the target surface - // - //* Set the target surface +// std::cout << "dQionNorm" << std::endl; +// std::cout << dQion/errIoni(0,0) << std::endl; - ErrorTargetPair g4eTarget_center = transformToG4SurfaceTarget(pDest, false); + +// const double dQmsNorm = 2*x0*(x1 + 2*x4)/(x1 + x4); +// std::cout << "dQmsNorm = " << dQmsNorm << std::endl; - // * Get the starting point and direction and convert them to - // CLHEP::Hep3Vector - // for G4. CMS uses cm and GeV while Geant4 uses mm and MeV - GlobalPoint cmsInitPos = ftsStart.position(); - GlobalVector cmsInitMom = ftsStart.momentum(); - bool flipped = false; - if (propagationDirection() == oppositeToMomentum) { - // flip the momentum vector as Geant4 will not do this - // on it's own in a backward propagation - cmsInitMom = -cmsInitMom; - flipped = true; - } +// std::cout << "errMSIout" << std::endl; +// std::cout << errMSIout << std::endl; +// +// std::cout << "dQms" << std::endl; +// std::cout << dQms << std::endl; +// +// std::cout << "dQion" << std::endl; +// std::cout << dQion << std::endl; + + - // Set the mode of propagation according to the propagation direction - G4ErrorMode mode = G4ErrorMode_PropForwards; - if (!configurePropagation(mode, pDest, cmsInitPos, cmsInitMom)) - return retDefault(); - // re-check propagation direction chosen in case of AnyDirection - if (mode == G4ErrorMode_PropBackwards && !flipped) - cmsInitMom = -cmsInitMom; - CLHEP::Hep3Vector g4InitPos = TrackPropagation::globalPointToHep3Vector(cmsInitPos); - CLHEP::Hep3Vector g4InitMom = TrackPropagation::globalVectorToHep3Vector(cmsInitMom * GeV); + +// dQ += 0.*jac(0,0)*dQion; +// dQ += jac(0,0)*dQms; + + + Matrix errMS = errMSIout; + errMS(0,0) = 0.; - debugReportTrackState("intitial", cmsInitPos, g4InitPos, cmsInitMom, g4InitMom, pDest); + Matrix errI = errMSIout; + errI.bottomRightCorner<4, 4>() *= 0.; + + dQ += errMS; + dQ2 += errI; + + + + + + LogDebug("Geant4e") << "step Length was " << thisPathLength << " cm, current global position: " + << TrackPropagation::hepPoint3DToGlobalPoint(g4eTrajState.GetPosition()) << std::endl; - // Set the mode of propagation according to the propagation direction - // G4ErrorMode mode = G4ErrorMode_PropForwards; + finalPathLength += thisPathLength; - // if (!configurePropagation(mode, pDest, cmsInitPos, cmsInitMom)) - // return TsosPP(TrajectoryStateOnSurface(), 0.0f); + // if (std::fabs(finalPathLength) > 10000.0f) + if (std::fabs(finalPathLength) > 200.0f) { + LogDebug("Geant4e") << "ERROR: Quitting propagation: path length mega large" << std::endl; + theG4eManager->GetPropagator()->InvokePostUserTrackingAction(g4eTrajState.GetG4Track()); + continuePropagation = false; + LogDebug("Geant4e") << "WARNING: Quitting propagation: max path length " + "exceeded, returning invalid state" + << std::endl; - /////////////////////////////// - // Set the error and trajectories, and finally propagate - // - G4ErrorTrajErr g4error(5, 1); - if (ftsStart.hasError()) { - CurvilinearTrajectoryError initErr; - initErr = ftsStart.curvilinearError(); - g4error = TrackPropagation::algebraicSymMatrix55ToG4ErrorTrajErr(initErr, ftsStart.charge()); - LogDebug("Geant4e") << "CMS - Error matrix: " << std::endl << initErr.matrix(); - } else { - LogDebug("Geant4e") << "No error matrix available" << std::endl; - return retDefault(); + // reached maximum path length, bail out + return retDefault(); + } + + if (theG4eManager->GetPropagator()->CheckIfLastStep(g4eTrajState.GetG4Track())) { + theG4eManager->GetPropagator()->InvokePostUserTrackingAction(g4eTrajState.GetG4Track()); + continuePropagation = false; + } } - LogDebug("Geant4e") << "G4e - Error matrix: " << std::endl << g4error; + constexpr double kd = 0.13017714; + const double c = wTotal; - // in CMSSW, the state errors are deflated when performing the backward - // propagation - if (mode == G4ErrorMode_PropForwards) { - G4ErrorPropagatorData::GetErrorPropagatorData()->SetStage(G4ErrorStage_Inflation); - } else if (mode == G4ErrorMode_PropBackwards) { - G4ErrorPropagatorData::GetErrorPropagatorData()->SetStage(G4ErrorStage_Deflation); - } + const double varE = 2.*c*c/M_PI/M_PI/M_PI/kd/kd; - G4ErrorFreeTrajState g4eTrajState(generateParticleName(ftsStart.charge()), g4InitPos, g4InitMom, g4error); - LogDebug("Geant4e") << "G4e - Traj. State: " << (g4eTrajState); + // Etot = aTrack->GetTotalEnergy() / GeV; - ////////////////////////////// - // Propagate - int iterations = 0; - double finalPathLength = 0; + const double eFinal = g4eTrajState.GetG4Track()->GetTotalEnergy() / GeV; + const double pFinal = g4eTrajState.GetMomentum().mag() / GeV; + const double p6 = std::pow(pFinal, 6); + // Apply it to error + const double varqop = eFinal * eFinal * varE / p6; - HepGeom::Point3D finalRecoPos; + // const double varqopratio = std::sqrt(varqop)*pFinal; + // const double varqopratio = std::sqrt(g4errorEnd(0, 0))*pFinal; + // std::cout << "varqopratio = " << varqopratio << std::endl; - G4ErrorPropagatorData::GetErrorPropagatorData()->SetMode(mode); + // std::cout << "g4errorEnd before:\n" << g4errorEnd << std::endl; - theG4eData->SetTarget(g4eTarget_center.second.get()); - LogDebug("Geant4e") << "Running Propagation to the RECO surface" << std::endl; + // const double qopscale = std::sqrt(varqop/g4errorEnd(0, 0)); + // g4errorEnd.row(0) *= qopscale; + // g4errorEnd.col(0) *= qopscale; - theG4eManager->InitTrackPropagation(); - - // initial jacobian is the identity matrix for the state components, - // and 0 for b-field and material variations -// G4ErrorMatrix jac(5, 7); -// for (unsigned int i = 0; i < 5; ++i) { -// for (unsigned int j = 0; j < 7; ++j) { -// jac(i+1, j+1) = i == j ? 1. : 0.; -// } + // const double varqopdummyfact = 1e-7; + // const double varqopdummy = 1e-7*1e-7/pFinal/pFinal; + // g4errorEnd(0, 0) += varqopdummy; + // g4errorEnd.row(0) *= 0.; + // g4errorEnd.col(0) *= 0.; + // g4errorEnd(0, 0) = varqopdummy; + + // std::cout << "g4errorEnd after:\n" << g4errorEnd << std::endl; + + // std::cout << "qopscale = " << qopscale << std::endl; + +// std::cout << "g4errorEnd(0, 0) = " << g4errorEnd(0, 0) << " varqop = " << varqop << std::endl; + + // adjust multiple scattering for core width + // G4double DDold = 1.8496E-4*RI*(charge/pBeta * charge/pBeta ); + // G4double DDcore = 0.0136*0.0136*RI*(charge/pBeta * charge/pBeta )*std::pow(1. + 0.038*std::log(RI), 2); + // g4errorEnd *= std::pow(1. + 0.038*std::log(RItotal), 2); + + +// std::cout << "propgationDirection = " << propagationDirection() << " finalPathLength = " << finalPathLength << std::endl; +// if (finalPathLength < 0.) { +// throw std::runtime_error("negative path length!"); // } - Matrix jac; - jac.leftCols<5>() = Matrix::Identity(); - jac.rightCols<2>() = Matrix::Zero(); - - const G4ErrorTrajErr errnull(5, 0); + // CMSSW Tracking convention, backward propagations have negative path length + if (propagationDirection() == oppositeToMomentum) + finalPathLength = -finalPathLength; + +// std::cout << "finalPathLength = " << finalPathLength << std::endl; + // store the correct location for the hit on the RECO surface + LogDebug("Geant4e") << "Position on the RECO surface" << g4eTrajState.GetPosition() << std::endl; + finalRecoPos = g4eTrajState.GetPosition(); + + theG4eManager->EventTermination(); + + LogDebug("Geant4e") << "Final position of the Track :" << g4eTrajState.GetPosition() << std::endl; + + ////////////////////////////// + // Retrieve the state in the end from Geant4e, convert them to CMS vectors + // and points, and build global trajectory parameters. + // CMS uses cm and GeV while Geant4 uses mm and MeV + // -// const G4ErrorTrajErr g4errorEnd = g4eTrajState.GetError(); - const AlgebraicMatrix55 errstart = ftsStart.curvilinearError().matrix(); - Matrix g4errorEnd = Map>(errstart.Array()); + Matrix ftsEnd; + ftsEnd[0] = g4eTrajState.GetPosition().x()/cm; + ftsEnd[1] = g4eTrajState.GetPosition().y()/cm; + ftsEnd[2] = g4eTrajState.GetPosition().z()/cm; + ftsEnd[3] = g4eTrajState.GetMomentum().x()/GeV; + ftsEnd[4] = g4eTrajState.GetMomentum().y()/GeV; + ftsEnd[5] = g4eTrajState.GetMomentum().z()/GeV; + ftsEnd[6] = charge; + cmsField->SetOffset(0., 0., 0.); + cmsField->SetMaterialOffset(0.); - + return std::tuple, Matrix, Matrix, double, Matrix, Matrix, double, double>(true, ftsEnd, g4errorEnd, jac, dEdxlast, dQ, dQ2, deltaTotal, wTotal); - G4ErrorTrajErr dQ(5, 0); +// return std::tuple(TrajectoryStateOnSurface(localparms, localerr, pDest, theField, side), jacfinal, dQfinal, dEdxlast); +} - double dEdxlast = 0.; - double masslast = 0.; +// +//////////////////////////////////////////////////////////////////////////// +// + +/** The methods propagateWithPath() are identical to the corresponding + * methods propagate() in what concerns the resulting + * TrajectoryStateOnSurface, but they provide in addition the + * exact path length along the trajectory. + */ + +std::pair Geant4ePropagator::propagateWithPath(const FreeTrajectoryState &ftsStart, + const Plane &pDest) const { + // Finally build the pair<...> that needs to be returned where the second + // parameter is the exact path length. Currently calculated with a stepping + // action that adds up the length of every step + return propagateGeneric(ftsStart, pDest); +} + +std::pair Geant4ePropagator::propagateWithPath(const FreeTrajectoryState &ftsStart, + const Cylinder &cDest) const { + // Finally build the pair<...> that needs to be returned where the second + // parameter is the exact path length. + return propagateGeneric(ftsStart, cDest); +} + +std::pair Geant4ePropagator::propagateWithPath( + const TrajectoryStateOnSurface &tsosStart, const Plane &pDest) const { + // Finally build the pair<...> that needs to be returned where the second + // parameter is the exact path length. + const FreeTrajectoryState ftsStart = *tsosStart.freeState(); + return propagateGeneric(ftsStart, pDest); +} + +std::pair Geant4ePropagator::propagateWithPath( + const TrajectoryStateOnSurface &tsosStart, const Cylinder &cDest) const { + const FreeTrajectoryState ftsStart = *tsosStart.freeState(); + // Finally build the pair<...> that needs to be returned where the second + // parameter is the exact path length. + return propagateGeneric(ftsStart, cDest); +} + +void Geant4ePropagator::debugReportPlaneSetup(GlobalPoint const &posPlane, + HepGeom::Point3D const &surfPos, + GlobalVector const &normalPlane, + HepGeom::Normal3D const &surfNorm, + const Plane &pDest) const { + LogDebug("Geant4e") << "G4e - Destination CMS plane position:" << posPlane << "cm\n" + << "G4e - (Ro, eta, phi): (" << posPlane.perp() << " cm, " << posPlane.eta() + << ", " << posPlane.phi().degrees() << " deg)\n" + << "G4e - Destination G4 plane position: " << surfPos << " mm, Ro = " << surfPos.perp() + << " mm"; + LogDebug("Geant4e") << "G4e - Destination CMS plane normal : " << normalPlane << "\n" + << "G4e - Destination G4 plane normal : " << normalPlane; + LogDebug("Geant4e") << "G4e - Distance from plane position to plane: " << pDest.localZ(posPlane) << " cm"; +} + +template +void Geant4ePropagator::debugReportTrackState(std::string const ¤tContext, + GlobalPoint const &cmsInitPos, + CLHEP::Hep3Vector const &g4InitPos, + GlobalVector const &cmsInitMom, + CLHEP::Hep3Vector const &g4InitMom, + const SurfaceType &pDest) const { + LogDebug("Geant4e") << "G4e - Current Context: " << currentContext; + LogDebug("Geant4e") << "G4e - CMS point position:" << cmsInitPos << "cm\n" + << "G4e - (Ro, eta, phi): (" << cmsInitPos.perp() << " cm, " << cmsInitPos.eta() + << ", " << cmsInitPos.phi().degrees() << " deg)\n" + << "G4e - G4 point position: " << g4InitPos << " mm, Ro = " << g4InitPos.perp() << " mm"; + LogDebug("Geant4e") << "G4e - CMS momentum :" << cmsInitMom << "GeV\n" + << " pt: " << cmsInitMom.perp() << "G4e - G4 momentum : " << g4InitMom << " MeV"; +} + + +//------------------------------------------------------------------------ +Eigen::Matrix Geant4ePropagator::PropagateErrorMSC( const G4Track* aTrack, double pforced) const +{ + G4ThreeVector vpPre = aTrack->GetMomentum()/GeV; +// G4double pPre = vpPre.mag(); +// G4double pBeta = pPre*pPre / (aTrack->GetTotalEnergy()/GeV); + G4double mass = aTrack->GetDynamicParticle()->GetMass() / GeV; + G4double pPre = pforced > 0. ? pforced : aTrack->GetMomentum().mag() / GeV; + G4double Etot = sqrt(pPre*pPre + mass*mass); + G4double beta = pPre/Etot; + G4double pBeta = pPre*beta; + G4double stepLengthCm = aTrack->GetStep()->GetStepLength()/cm; + + G4Material* mate = aTrack->GetVolume()->GetLogicalVolume()->GetMaterial(); + G4double effZ, effA; + CalculateEffectiveZandA( mate, effZ, effA ); + + +#ifdef G4EVERBOSE + if( iverbose >= 4 ) G4cout << "material " << mate->GetName() + //<< " " << mate->GetZ() << " " << mate->GetA() + << " effZ:" << effZ << " effA:" << effA + << " dens(g/mole):" << mate->GetDensity()/g*mole << " Radlen/cm:" << mate->GetRadlen()/cm << " nuclLen/cm" << mate->GetNuclearInterLength()/cm << G4endl; +#endif + + G4double RI = stepLengthCm / (mate->GetRadlen()/cm); +#ifdef G4EVERBOSE + if( iverbose >= 4 ) G4cout << std::setprecision(6) << std::setw(6) << "G4EP:MSC: RI=X/X0 " << RI << " stepLengthCm " << stepLengthCm << " radlen/cm " << (mate->GetRadlen()/cm) << " RI*1.e10:" << RI*1.e10 << G4endl; +#endif + G4double charge = aTrack->GetDynamicParticle()->GetCharge(); + G4double DDold = 1.8496E-4*RI*(charge/pBeta * charge/pBeta ); + G4double X0 = mate->GetRadlen()/cm; + G4double Xs = X0*(effZ + 1.)*std::log(287./std::sqrt(effZ))/std::log(159.*std::pow(effZ, -1./3.))/effZ; + +// std::cout << "name = " << mate->GetName() << " X0 = " << X0 << std::endl; - Matrix dErrorDxLast = Matrix::Zero(); +// msmodel->SetCurrentCouple(aTrack->GetMaterialCutsCouple()); +// const double lambdaeff = msmodel->GetTransportMeanFreePath(aTrack->GetParticleDefinition(), pPre) / cm; - bool continuePropagation = true; - while (continuePropagation) { - iterations++; - LogDebug("Geant4e") << std::endl << "step count " << iterations << " step length " << finalPathLength; +// const double lnb = -2.*lambdaeff/stepLengthCm; +// const double b = std::exp(lnb); +// const double expzr = b - b*lnb - 1.; +// const double expzr2 = 2. - b*lnb*lnb + 2.*b*lnb - 2.*b; +// +// const double varzr = expzr2 - expzr; - const GlobalPoint pos = TrackPropagation::hepPoint3DToGlobalPoint(g4eTrajState.GetPosition()); - const GlobalVector mom = TrackPropagation::hep3VectorToGlobalVector(g4eTrajState.GetMomentum() / GeV); -// constexpr double mass = 0.1056583745; - - const FreeTrajectoryState statepre(pos, mom, ftsStart.charge(), &ftsStart.parameters().magneticField()); - - //set the error matrix to null to disentangle MS and ionization contributions - g4eTrajState.SetError(errnull); - const int ierr = theG4eManager->PropagateOneStep(&g4eTrajState, mode); +// const double varz = 1. - b*lnb*lnb - b*b - b*b*lnb*lnb + b*b*lnb; +// const double DDlam = 0.5*stepLengthCm*stepLengthCm/lambdaeff/lambdaeff*varzr; - if (ierr != 0) { - // propagation failed, return invalid track state - return retDefault(); + + +// const double Nsnorm = 1.587e7/beta/beta*std::pow(effZ, -2./3.)/std::log(159.*std::pow(effZ, -1./3.))/Xs; +// const double Nsnorminv = 1./Nsnorm; + +// const double teff = stepLengthCm/lambdaeff; + +// const double Dwentzel = 2.0/stepLengthCm/lambdaeff; +// const double DDwentzel = Dwentzel*Dwentzel; + +// const double lambdaeffscaled = lambdaeff/pBeta/pBeta*charge*charge; +// std::cout << "lambdaeff = " << lambdaeff << std::endl; + + + if (false) { + msmodel->DefineMaterial(aTrack->GetMaterialCutsCouple()); + + + double pathlenghtmp = aTrack->GetStep()->GetStepLength(); + msmodel->ComputeTruePathLengthLimit(*aTrack, pathlenghtmp); + // + const double truesteplength = msmodel->ComputeTrueStepLength(aTrack->GetStep()->GetStepLength())/cm; + // std::cout << "stepLengthCm = " << stepLengthCm << " truesteplength = " << truesteplength << std::endl; + // + const G4ThreeVector testdir(1., 0., 0.); + + const int nsample = 1000*1000; + double sumw = 0.; + double sumdphi2 = 0.; + for (unsigned int isample = 0; isample < nsample; ++isample) { + const G4ThreeVector scattered = msmodel->SampleScatteringTest(testdir, 0.); + const double dphi = std::atan2(scattered.y(), scattered.x()); + + sumdphi2 += dphi*dphi; + sumw += 1.; } + const double phivar = sumdphi2/sumw; - const double thisPathLength = TrackPropagation::g4doubleToCmsDouble(g4eTrajState.GetG4Track()->GetStepLength()); + // const double DDlam = msmodel->ProjectedVariance(); + const double DDlam = phivar; + const double DDlamscaled = DDlam/stepLengthCm; + const double DDlamscaled2 = DDlam/stepLengthCm*pBeta*pBeta; - const double ePre = g4eTrajState.GetG4Track()->GetStep()->GetPreStepPoint()->GetTotalEnergy() / GeV; - const double ePost = g4eTrajState.GetG4Track()->GetStep()->GetPostStepPoint()->GetTotalEnergy() / GeV; - const double dEdx = (ePost - ePre)/thisPathLength; - const double mass = g4eTrajState.GetG4Track()->GetDynamicParticle()->GetMass() / GeV; + // std::cout << "lambdaeff = " << lambdaeff << " stepLengthCm = " << stepLengthCm << " lnb = " << lnb << " b = " << b << " expzr = " << expzr << " expzr2 = " << expzr2 << " varzr = " << varzr << " DDlam = " << DDlam << std::endl; - if (std::abs(thisPathLength) > 0.) { - dEdxlast = dEdx; - masslast = mass; + } + + + if (false) { + double invX0alt = 0.; + double X0alt2 = 0.; + double invXsalt = 0.; + double sumw = 0.; + const G4double* fracVec = mate->GetFractionVector(); + for(unsigned int ii = 0; ii < mate->GetNumberOfElements(); ++ii) + { + const double iZ = mate->GetElement(ii)->GetZ(); + const double iA = mate->GetElement(ii)->GetA() / g * mole; + // const double iDensity = fracVec[ii]*mate->GetDensity() / mg * mole; + const double iX0 = 716.4*iA/iZ/(iZ+1.)/std::log(287./std::sqrt(iZ)); + const double iXs = iX0*(iZ + 1.)*std::log(287./std::sqrt(iZ))/std::log(159.*std::pow(iZ, -1./3.))/iZ; + + invX0alt += fracVec[ii]/iX0; + invXsalt += fracVec[ii]/iXs; + + sumw += fracVec[ii]; } -// std::cout << "thisPathLength = " << thisPathLength << std::endl; -// const Matrix transportJac = transportJacobian(statepre, thisPathLength, dEdx, mass); - const Matrix transportJac = transportJacobianBz(statepre, thisPathLength, dEdx, mass); -// const Matrix transportJac = transportJacobianBzAdvanced(statepre, thisPathLength, dEdx, mass); - -// const Matrix transportJacAlt = transportJacobianBz(statepre, thisPathLength, dEdx, mass); + const double density = mate->GetDensity() / mg * mole; -// std::cout << "transportJac" << std::endl; -// std::cout << transportJac << std::endl; -// std::cout << "transportJacAlt" << std::endl; -// std::cout << transportJacAlt << std::endl; + const double X0alt = 1./invX0alt/density; + const double Xsalt = 1./invXsalt/density; +// std::cout << "name = " << mate->GetName() << " nelems = " << mate->GetNumberOfElements() << " effZ = " << effZ << " sumw = " << sumw << " X0alt/X0 = " << X0alt/X0 << " Xsalt/Xs = " << Xsalt/Xs << " Xs/X0 = " << Xs/X0 << " X0 = " << X0 << " stepLengthCm = " << stepLengthCm << " DDlamscaled = " << DDlamscaled << " DDlamscaled2 = " << DDlamscaled2 << std::endl; + } + + + G4double DD = 2.25e-4*stepLengthCm*(charge/pBeta * charge/pBeta )/Xs; + + // if (X0 > 25e3) { + // // std::cout << "low density material: " << mate->GetName() << " scaling down\n"; + // DD *= 1e-4; + // } + + if (false) { + //TODO check charge^2 dependence? -// bool isnan = false; -// for (unsigned int i=0; i()*g4errorEnd*transportJac.leftCols<5>().transpose()).eval(); + const double varT = 0.015*0.015*RI*charge*charge/pBeta/pBeta; + const double nbar = stepLengthCm*2.215e4*std::pow(effZ, 4./3.)/beta/beta/effA; -// const G4ErrorMatrix &transferMatrix = g4eTrajState.GetTransfMat(); + const double n = std::pow(effZ, 0.1)*std::log(nbar); - // transport contribution to error -// g4errorEnd = g4errorEnd.similarity(transferMatrix).T(); -// dQ = dQ.similarity(transferMatrix).T(); + const double var0 = 1.827e-1 + 3.803e-2*n + 5.783e-4*n*n; + const double a = 2.822e-1 + 9.828e-2*n - 1.355e-2*n*n + 1.330e-3*n*n*n - 4.590e-5*n*n*n*n; - // MS and ionization contributions to error + const double Q = 41e3*std::pow(effZ, -2./3.); + const double b = Q/std::sqrt(nbar*(std::log(Q) - 0.5)); + // const double b = Q*a; - G4ErrorTrajErr errMSI = g4eTrajState.GetError(); + const double epsilon = std::max(0., (1. - var0)/(a*a*(std::log(b/a) - 0.5) - var0)); - // recompute ionization contribution (without truncation for the moment) - errMSI(0+1, 0+1) = computeErrorIoni(g4eTrajState.GetG4Track()); + // const double alpha = 0.996; + const double alpha = 0.9995; + // const double alpha = 1.0; + // const double alpha = 0.95; - Matrix errMSIout = Matrix::Zero(); + // const double alphap = (alpha - (1.-epsilon))/epsilon; + const double alphap = alpha; + // const double alphap = std::clamp( (alpha - (1.-epsilon))/epsilon, 0., 1.); -// std::cout << "errMSI" << std::endl; -// std::cout << errMSI << std::endl; + const double k = b*b/(a*a + b*b); - for (unsigned int i = 0; i < 5; i++) { - for (unsigned int j = 0; j < 5; j++) { - double w = 1.; - if (i==0) { - w *= ftsStart.charge(); - } - if (j==0) { - w *= ftsStart.charge(); - } - errMSIout(i, j) += w*errMSI(i+1, j+1); - } - } - - + const double varalpha = (1. - epsilon)*var0 - 0.5*epsilon*a*a/k*(k*alphap + std::log(1. - k*alphap)); - g4errorEnd += errMSIout; + const double varone = (1. - epsilon)*var0 - 0.5*epsilon*a*a/k*(k + std::log(1. - k)); + + const double DDalpha = DD*varalpha; + const double DDone = DD*varone; - if (std::abs(thisPathLength) > 0.) { - dErrorDxLast = errMSIout/thisPathLength; - } + // std::cout << "mat = " << mate->GetName() << " stepLengthCm = " << stepLengthCm << " DD = " << DD << " epsilon = " << epsilon << " alphap = " << alphap << " var0 = " << var0 << " varalpha = " << varalpha << " varone = " << varone << " DDalpha/DD = " << DDalpha/DD << " DDone/DD = " << DDone/DD << std::endl; - // transport + nominal energy loss contribution to jacobian -// jac = transferMatrix*jac; - jac = (transportJac.leftCols<5>()*jac).eval(); + // DD = DDold; + // DD = DDalpha; + DD = varT*varalpha; - //TODO assess relevance of approximations (does the order matter? position/momentum before or after step?) - //b-field and material contributions to jacobian - jac.rightCols<2>() += transportJac.rightCols<2>(); - + } + // G4double DDX0 = 1.44e-4*RI*(charge/pBeta * charge/pBeta ); + // DD = DDX0; + + // DD *= 0.715; + + // DD *= 0.8; + + // DD *= 0.72*0.95; + + if (false) { + + //TODO more accurate coeffs from fine structure constant and electron mass? + const double tmin = 2.66e-6*std::pow(effZ, 1./3.)/pPre; + const double tmax = 0.14*std::pow(effA, -1./3.)/pPre; - //TODO assess relevance of approximations (does the order matter? position/momentum before or after step?) -// // local b-field contribution to jacobian -// const GlobalPoint pos = TrackPropagation::hepPoint3DToGlobalPoint(g4eTrajState.GetPosition()); -// const GlobalVector mom = TrackPropagation::hep3VectorToGlobalVector(g4eTrajState.GetMomentum() / GeV); -// const GlobalVector momPre = TrackPropagation::hep3VectorToGlobalVector(g4eTrajState.GetG4Track()->GetStep()->GetPreStepPoint()->GetMomentum() / GeV); -// const double B = theField->inTesla(pos).mag(); -// const double pinv = 1./mom.mag(); -// -// for (unsigned int i = 1; i < 5; ++i) { -// jac(i+1, 5+1) += transferMatrix(i+1, 0+1)*pinv/B; -// } + const double k = 2.*tmin*tmin*(1. + tmin*tmin/tmax/tmax); + const double nbar = 1.587e7*stepLengthCm*std::pow(effZ, -2./3.)/std::log(159.*std::pow(effZ, -1./3.))/Xs/beta/beta; - // local material contribution to jacobian -// const double ePre = g4eTrajState.GetG4Track()->GetStep()->GetPreStepPoint()->GetTotalEnergy() / GeV; -// const double ePost = g4eTrajState.GetG4Track()->GetStep()->GetPostStepPoint()->GetTotalEnergy() / GeV; -// const double dE = ePost - ePre; - -// std::cout << "ePost = " << ePost << " p = " << mom.mag() << std::endl; + // const double alpha = 0.996; + // const double alpha = 0.9999; + // const double alpha = 1. - 1e-7; + const double alpha = 1.0; + const double talpha = std::sqrt(2.*alpha/(k - 2.*tmin*tmin*alpha))*tmin*tmin; -// jac(0+1, 6+1) += -ePost*std::pow(pinv, 3)*dE; + // const double tbar = 0.5*k*(std::atan(talpha/tmin)/tmin - talpha/(tmin*tmin + talpha*talpha)); + const double tsqbar = k*(0.5*tmin*tmin/(talpha*talpha + tmin*tmin) - std::log(tmin) + 0.5*std::log(talpha*talpha + tmin*tmin) - 0.5); + // const double sigtsq = tsqbar - tbar*tbar; -// const double pPre = momPre.mag(); -// const double betaPre = pPre/ePre; -// const double mass = g4eTrajState.GetG4Track()->GetDynamicParticle()->GetMass() / GeV; + const double DDalpha = 0.5*nbar*tsqbar; + // const double DDalpha = 0.5*nbar*sigtsq; -// G4ErrorTrajErr errMS = errMSI; -// errMS(0+1, 0+1) = 0.; -// std::cout << "ppre = " << pPre << " betaPre = " << betaPre << " mass = " << mass << " ePre = " << ePre << std::endl; -// dQ += 2.*pPre*(betaPre*betaPre + 2.*mass*mass/ePre/ePre)*errMS; -// dQ += 2.*pPre*(betaPre*betaPre + 2.*mass*mass/ePre/ePre)*errMS; + // const double tbarone = 4.18e-6*std::pow(effZ, 1./3.)/pPre; + // const double tsqbarone = 2.84e-11*std::pow(effZ, 2./3.)*std::log(159.*std::pow(effZ, -1./3.))/pPre/pPre; + // + // const double tbaronealt = 0.5*M_PI*tmin; + // const double tsqbaronealt = 2.*tmin*tmin*(std::log(tmax/tmin) - 0.5); + // + // // DD = 0.5*sigtsq; + // + // std::cout << "tmin = " << tmin << " tmax = " << tmax << " talpha = " << talpha << std::endl; + // + // std::cout << "tbar = " << tbar << " tsqbar = " << tsqbar << " tbarone = " << tbarone << " tsqbarone = " << tsqbarone << " tbaronealt = " << tbaronealt << " tsqbaronealt = " << tsqbaronealt << std::endl; + // + // std::cout << "DD = " << DD << " DDalpha = " << DDalpha << std::endl; + // DD = DDalpha; + -// std::cout << "dE = " << dE << std::endl; + // if (std::fabs(effZ - 4.0) < 0.1) { + // zero MS for beryllium!? + // DD = 0.; + // } -// std::cout << "jac" << std::endl; -// std::cout << jac << std::endl; + // const double rhopost = aTrack->GetStep()->GetPostStepPoint()->GetPosition().rho()/cm; + // if (rhopost < 4.5) { + // std::cout << "material = " << mate->GetName() << " rhopost = " << rhopost << " zeroing material\n"; + // DD = 0.; + // } - LogDebug("Geant4e") << "step Length was " << thisPathLength << " cm, current global position: " - << TrackPropagation::hepPoint3DToGlobalPoint(g4eTrajState.GetPosition()) << std::endl; + // G4double DD = X0 < 25e3 ? 2.25e-4*stepLengthCm*(charge/pBeta * charge/pBeta )/Xs : 0.; - finalPathLength += thisPathLength; + // // G4double DD = 2.25e-4*RI*(charge/pBeta * charge/pBeta ); - // if (std::fabs(finalPathLength) > 10000.0f) - if (std::fabs(finalPathLength) > 200.0f) { - LogDebug("Geant4e") << "ERROR: Quitting propagation: path length mega large" << std::endl; - theG4eManager->GetPropagator()->InvokePostUserTrackingAction(g4eTrajState.GetG4Track()); - continuePropagation = false; - LogDebug("Geant4e") << "WARNING: Quitting propagation: max path length " - "exceeded, returning invalid state" - << std::endl; + // std::cout << "name = " << mate->GetName() << " X0 = " << X0 << " lambdaeff = " << lambdaeff << " Nsnorminv = " << Nsnorminv << " lambdaeff/Nsnorminv = " << lambdaeff/Nsnorminv << " lambdaeff/Xs = " << lambdaeff/Xs << " p = " << pPre << " beta = " << beta << std::endl; - // reached maximum path length, bail out - return retDefault(); - } + // const double DDlam = stepLengthCm/lambdaeff/lambdaeff; - if (theG4eManager->GetPropagator()->CheckIfLastStep(g4eTrajState.GetG4Track())) { - theG4eManager->GetPropagator()->InvokePostUserTrackingAction(g4eTrajState.GetG4Track()); - continuePropagation = false; - } - } + // std::cout << "name = " << mate->GetName() << " X0 = " << X0 << " Xs = " << Xs << " DD = " << DD << " DDold = " << DDold << " DDlam = " << DDlam << " DDlam/DD = " << DDlam/DD << " DDlam/DDold = " << DDlam/DDold << " stepLengthCm = " << stepLengthCm << " pPre = " << pPre << " DDlamscaled = " << DDlamscaled << " DDlamscaled2 = " << DDlamscaled2 << std::endl; - // CMSSW Tracking convention, backward propagations have negative path length - if (propagationDirection() == oppositeToMomentum) - finalPathLength = -finalPathLength; + // std::cout << "name = " << mate->GetName() << " X0 = " << X0 << " Xs = " << Xs << " lambdaeff = " << lambdaeff << " lambdaeff/X0 = " << lambdaeff/X0 << " lambdaeff/Xs = " << lambdaeff/Xs << " DD = " << DD << " phivar = " << phivar << " phivar/DD = " << phivar/DD << " phivar/DDold = " << phivar/DDold << " stepLengthCm = " << stepLengthCm << " pPre = " << pPre << std::endl; -// std::cout << "finalPathLength = " << finalPathLength << std::endl; + + // G4double DDcore = 0.0136*0.0136*RI*(charge/pBeta * charge/pBeta )*std::pow(1. + 0.038*std::log(RI), 2); + // DD = DDcore; + // // G4double DDcorealt = 0.0136*0.0136*RI*(charge/pBeta * charge/pBeta )*std::pow(1. + 0.038*std::log(RI/beta/beta), 2); + // + // const double dp = stepLengthCm/Xs/beta/beta; + // const double lndp = std::log(dp); + // const double var1 = 0.8510 + 0.03314*lndp - 0.001825*lndp*lndp; + // const double DDcorealt = 2.25e-4*dp*var1/pPre/pPre; + + // std::cout << "name = " << mate->GetName() << " mate->GetDensity() = " << mate->GetDensity() << " X0 = " << X0 << " Xs = " << Xs << " stepLengthCm = " << stepLengthCm << " var1 = " << var1 << " DDcorealt/DDcore = " << DDcorealt/DDcore << std::endl; - // store the correct location for the hit on the RECO surface - LogDebug("Geant4e") << "Position on the RECO surface" << g4eTrajState.GetPosition() << std::endl; - finalRecoPos = g4eTrajState.GetPosition(); + } + +// DD = DDold; + // DD = DDcore; +// std::cout << "DD/DDcore = " << DD/DDcore << std::endl; +// std::cout << "DDold = " << DDold << " DD = " << DD << std::endl; +#ifdef G4EVERBOSE + if( iverbose >= 3 ) G4cout << "G4EP:MSC: D*1E6= " << DD*1.E6 <<" pBeta " << pBeta << G4endl; +#endif + G4double S1 = DD*stepLengthCm*stepLengthCm/3.; + G4double S2 = DD; + G4double S3 = DD*stepLengthCm/2.; - theG4eManager->EventTermination(); + G4double CLA = std::sqrt( vpPre.x() * vpPre.x() + vpPre.y() * vpPre.y() )/pPre; +#ifdef G4EVERBOSE + if( iverbose >= 2 ) G4cout << std::setw(6) << "G4EP:MSC: RI " << RI << " S1 " << S1 << " S2 " << S2 << " S3 " << S3 << " CLA " << CLA << G4endl; +#endif + Eigen::Matrix res = Eigen::Matrix::Zero(); + res(1, 1) = S2; + res(1, 4) = -S3; + res(2, 2) = S2/CLA/CLA; + res(2, 3) = S3/CLA; + res(3, 3) = S1; + res(4, 4) = S1; + + res(4, 1) = res(1, 4); + res(3, 2) = res(2, 3); - LogDebug("Geant4e") << "Final position of the Track :" << g4eTrajState.GetPosition() << std::endl; +#ifdef G4EVERBOSE + if( iverbose >= 2 ) G4cout << "G4EP:MSC: error matrix propagated msc " << fError << G4endl; +#endif - ////////////////////////////// - // Retrieve the state in the end from Geant4e, convert them to CMS vectors - // and points, and build global trajectory parameters. - // CMS uses cm and GeV while Geant4 uses mm and MeV - // - const HepGeom::Vector3D momEnd = g4eTrajState.GetMomentum(); + return res; +} - // use the hit on the the RECO plane as the final position to be d'accor with - // the RecHit measurements - const GlobalPoint posEndGV = TrackPropagation::hepPoint3DToGlobalPoint(finalRecoPos); - GlobalVector momEndGV = TrackPropagation::hep3VectorToGlobalVector(momEnd) / GeV; - debugReportTrackState("final", posEndGV, finalRecoPos, momEndGV, momEnd, pDest); +//------------------------------------------------------------------------ +std::pair Geant4ePropagator::computeLandau(const G4Track* aTrack) const +{ - if (mode == G4ErrorMode_PropBackwards) { - GlobalTrajectoryParameters endParm( - posEndGV, momEndGV, ftsStart.parameters().charge(), &ftsStart.parameters().magneticField()); + G4double stepLengthCm = aTrack->GetStep()->GetStepLength() / cm; - // flip the momentum direction because it has been flipped before running - // G4's backwards prop - momEndGV = -momEndGV; + if (stepLengthCm <= 0.) { + return std::make_pair(0., 0.); } - -// FreeTrajectoryState stateiter(posEndGV, momEndGV, ftsStart.charge(), &ftsStart.parameters().magneticField()); -// -// // additional iterations with analytic propagation to get closer to surface -// for (unsigned int iiter = 0; iiter < 10; ++iiter) { -// auto const localpos = pDest.toLocal(stateiter.parameters().position()); -// -// -// std::cout << "iiter = " << iiter << " localz = " << localpos.z() << std::endl; -// if (std::abs(localpos.z()) < 1e-9) { -// break; -// } -// -// auto const localmom = pDest.toLocal(stateiter.parameters().momentum()); -// -// const double pathdir = std::copysign(1.0, -localpos.z()*localmom.z()); -// -// const double pathlength = pathdir*std::sqrt(std::pow(localpos.z()*localmom.x()/localmom.z(), 2) + std::pow(localpos.z()*localmom.y()/localmom.z(), 2) + std::pow(localpos.z(), 2)); -// -// std::cout << "pathlength = " << pathlength << std::endl; -// -// const Matrix transportRes = transportResult(stateiter, pathlength, dEdxlast, masslast); -// const Matrix transportJac = transportJacobianBz(stateiter, pathlength, dEdxlast, masslast); -// -// std::cout << "transportRes" << std::endl; -// std::cout << transportRes << std::endl; -// -// GlobalPoint posupd(transportRes[0], transportRes[1], transportRes[2]); -// posupd += stateiter.parameters().position(); -// const GlobalVector momupd(transportRes[3], transportRes[4], transportRes[5]); -// -// std::cout << "posdiff" << std::endl; -// std::cout << posupd - stateiter.parameters().position() << std::endl; -// std::cout << "momdiff" << std::endl; -// std::cout << momupd - stateiter.parameters().momentum() << std::endl; -// // -// std::cout << "pospre" << std::endl; -// std::cout << stateiter.parameters().position() << std::endl; -// std::cout << "mompre" << std::endl; -// std::cout << stateiter.parameters().momentum() << std::endl; -// -// stateiter = FreeTrajectoryState(posupd, momupd, ftsStart.charge(), &ftsStart.parameters().magneticField()); -// -// std::cout << "pospost" << std::endl; -// std::cout << stateiter.parameters().position() << std::endl; -// std::cout << "mompost" << std::endl; -// std::cout << stateiter.parameters().momentum() << std::endl; -// -// -// -// g4errorEnd = (transportJac.leftCols<5>()*g4errorEnd*transportJac.leftCols<5>().transpose()).eval(); -// g4errorEnd += pathlength*dErrorDxLast; -// -// jac = (transportJac.leftCols<5>()*jac).eval(); -// jac.rightCols<2>() += transportJac.rightCols<2>(); -// -// } - -// posEndGV = stateiter.parameters().position(); -// momEndGV = stateiter.parameters().momentum(); - - - // Get the error covariance matrix from Geant4e. It comes in curvilinear - // coordinates so use the appropiate CMS class -// G4ErrorTrajErr g4errorEnd = g4eTrajState.GetError(); -// AlgebraicMatrix55 endErrorMatrix; -// Map>(endErrorMatrix.Array()) = g4errorEnd; - - AlgebraicSymMatrix55 endErrorMatrixSym; - for (unsigned int i = 0; i < 5; i++) { - for (unsigned int j = 0; j < 5; j++) { - endErrorMatrixSym(i, j) = g4errorEnd(i, j); - } - } - - CurvilinearTrajectoryError curvError(endErrorMatrixSym); - -// CurvilinearTrajectoryError curvError( -// TrackPropagation::g4ErrorTrajErrToAlgebraicSymMatrix55(g4errorEnd, ftsStart.charge())); + // * Calculate xi factor (KeV). + G4Material* mate = aTrack->GetVolume()->GetLogicalVolume()->GetMaterial(); + G4double effZ, effA; + CalculateEffectiveZandA(mate, effZ, effA); +// G4double Etot = aTrack->GetTotalEnergy() / GeV; +// G4double beta = aTrack->GetMomentum().mag() / GeV / Etot; + G4double mass = aTrack->GetDynamicParticle()->GetMass() / GeV; + G4double pPre = aTrack->GetMomentum().mag() / GeV; + G4double Etot = sqrt(pPre*pPre + mass*mass); + G4double beta = pPre/Etot; + G4double gamma = Etot / mass; - LogDebug("Geant4e") << "G4e - Error matrix after propagation: " << std::endl << g4errorEnd; + // * Calculate xi factor (keV). + G4double XIkev = 153.5 * effZ * stepLengthCm * (mate->GetDensity() / mg * mole) / + (effA * beta * beta); - LogDebug("Geant4e") << "CMS - Error matrix after propagation: " << std::endl << curvError.matrix(); + // * Maximum energy transfer to atomic electron (KeV). + G4double eta = beta * gamma; + G4double etasq = eta * eta; + G4double eMass = 0.51099906 / GeV; + G4double massRatio = eMass / mass; + G4double F1 = 2 * eMass * etasq; + G4double F2 = 1. + 2. * massRatio * gamma + massRatio * massRatio; + G4double Emax = 1.E+6 * F1 / F2; // now in keV -// GlobalTrajectoryParameters tParsDest(posEndGV, momEndGV, ftsStart.charge(), theField); - -// auto const localf = pDest.toLocal(posEndGV); -// auto const locald = pDest.toLocal(posEndGV); - -// std::cout << "localf = " << localf << std::endl; -// std::cout << "locald = " << locald << std::endl; -// std::cout << "diff = " << localf-locald << std::endl; - - auto const localpos = pDest.toLocal(posEndGV); - auto const localmom = pDest.toLocal(momEndGV); - - std::cout << "final: local z = " << localpos.z() << " local pz = " << localmom.z() << std::endl; - - Point3DBase posEndDouble(finalRecoPos.x()*0.1, finalRecoPos.y()*0.1, finalRecoPos.z()*0.1); - auto const localposdouble = pDest.toLocal(posEndDouble); - std::cout << "local z double = " << localposdouble.z() << std::endl; - - const LocalPoint localposcor(localpos.x() - localpos.z()*localmom.x()/localmom.z(), localpos.y() - localpos.z()*localmom.y()/localmom.z(), 0.); - - const LocalTrajectoryParameters localparms(localposcor, localmom, ftsStart.charge()); - - const GlobalPoint posEndcor = pDest.toGlobal(localposcor); - - GlobalTrajectoryParameters tParsDest(posEndcor, momEndGV, ftsStart.charge(), theField); - + G4double Emaxmev = Emax*1e-3; + G4double stepLengthmm = stepLengthCm*10; + const double ekinmev = aTrack->GetStep()->GetPreStepPoint()->GetKineticEnergy(); - SurfaceSideDefinition::SurfaceSide side; + const double xi = XIkev*1e-3; - side = propagationDirection() == alongMomentum ? SurfaceSideDefinition::afterSurface - : SurfaceSideDefinition::beforeSurface; + const double rho = mate->GetDensity() / mg * mole; + const double hbarwp = std::sqrt(rho*effZ/effA)*28.816e-6; + const double I = mate->GetIonisation()->GetMeanExcitationEnergy(); + const double delta = 2.*std::log(hbarwp/I) + 2.*std::log(beta*gamma) - 1.; + const double m = mass*1e3; - AlgebraicMatrix57 jacfinal; - Map>(jacfinal.Array()) = jac; - - - // convert from 1/p to q/p -// for (unsigned int i = 0; i < 5; ++i) { -// for (unsigned int j = 0; j < 7; ++j) { -// jacfinal(i, j) = jac(i+1, j+1); -// if (i==0) { -// jacfinal(i, j) *= ftsStart.charge(); -// } -// if (j==0) { -// jacfinal(i, j) *= ftsStart.charge(); -// } -// } -// } - - AlgebraicMatrix55 dQfinal; -// for (unsigned int i = 0; i < 5; ++i) { -// for (unsigned int j = 0; j < 5; ++j) { -// dQfinal(i, j) = ftsStart.charge()*dQ(i+1, j+1); -// } -// } - -// JacobianCurvilinearToLocal curv2Loc(pDest, localparms, tParsDest, *theField); -// const AlgebraicMatrix55 curv2Loc = curv2localJacobianAlt(tParsDest, pDest); -// const AlgebraicMatrix55 curv2Loceloss = curv2localJacobianAlteloss(tParsDest, pDest, dEdxlast, masslast); -// auto const localerr = ROOT::Math::Similarity(curv2Loc, curvError.matrix()); -// auto const localerreloss = ROOT::Math::Similarity(curv2Loceloss, curvError.matrix()); -// -// std::cout << "curv2Loc" << std::endl; -// std::cout << curv2Loc << std::endl; -// std::cout << "curv2Loceloss" << std::endl; -// std::cout << curv2Loceloss << std::endl; -// std::cout << "localerr" << std::endl; -// std::cout << localerr << std::endl; -// std::cout << "localerreloss" << std::endl; -// std::cout << localerreloss << std::endl; - - const AlgebraicMatrix55 curv2Loc = curv2localJacobianAlteloss(tParsDest, pDest, dEdxlast, masslast); - auto const localerr = ROOT::Math::Similarity(curv2Loc, curvError.matrix()); + // std::cout << "I = " << I << std::endl; -// return std::tuple(TrajectoryStateOnSurface(tParsDest, curvError, pDest, side), jacfinal, dQfinal); - - return std::tuple(TrajectoryStateOnSurface(localparms, localerr, pDest, theField, side), jacfinal, dQfinal, dEdxlast); -} + if (false) { + const double deltap = xi*(std::log(2.*m*beta*beta*gamma*gamma/I) + std::log(xi/I) + 0.2 - beta*beta - delta)*1e-3; + const double fwhm = 4.*xi*1e-3; - std::tuple, Eigen::Matrix, Eigen::Matrix, double, Eigen::Matrix> Geant4ePropagator::propagateGenericWithJacobianAltD(const Eigen::Matrix &ftsStart, - const GloballyPositioned &pDest, double dBz, double dxi, double pforced) const { - - using namespace Eigen; + //MPV of landau distribution for mu = 0, c = pi/2 + constexpr double k = -0.22278; - - const G4Field *field = G4TransportationManager::GetTransportationManager()->GetFieldManager()->GetDetectorField(); - //FIXME check thread safety of this - sim::Field *cmsField = const_cast(static_cast(field)); - - cmsField->SetOffset(0., 0., dBz); - cmsField->SetMaterialOffset(dxi); - - auto retDefault = [cmsField]() { - cmsField->SetOffset(0., 0., 0.); - cmsField->SetMaterialOffset(0.); -// return std::tuple(); - return std::tuple, Matrix, Matrix, double, Matrix>(false, Matrix::Zero(), Matrix::Zero(), Matrix::Zero(), 0., Matrix::Zero()); - }; - + const double c = 0.125*M_PI*fwhm; + // const double c = 0.5*fwhm; + const double ascale = 0.25*fwhm; + const double mshift = deltap - ascale*k; + const double mu = mshift - ascale*std::log(ascale); - -// std::cout << "start propagation" << std::endl; - - /////////////////////////////// - // Construct the target surface - // - //* Set the target surface + // std::cout << "deltap = " << deltap << " c = " << c << std::endl; - //TODO fix precision of this - ErrorTargetPair g4eTarget_center = transformToG4SurfaceTargetD(pDest, false); - CLHEP::Hep3Vector g4InitPos(ftsStart[0]*cm, ftsStart[1]*cm, ftsStart[2]*cm); - CLHEP::Hep3Vector g4InitMom(ftsStart[3]*GeV, ftsStart[4]*GeV, ftsStart[5]*GeV); - - // * Get the starting point and direction and convert them to - // CLHEP::Hep3Vector - // for G4. CMS uses cm and GeV while Geant4 uses mm and MeV - GlobalPoint cmsInitPos(ftsStart[0], ftsStart[1], ftsStart[2]); - GlobalVector cmsInitMom(ftsStart[3], ftsStart[4], ftsStart[5]); - bool flipped = false; - if (propagationDirection() == oppositeToMomentum) { - // flip the momentum vector as Geant4 will not do this - // on it's own in a backward propagation - cmsInitMom = -cmsInitMom; - g4InitMom = -g4InitMom; - flipped = true; + return std::make_pair(mu, c); + } - - const double charge = ftsStart[6]; - // Set the mode of propagation according to the propagation direction - G4ErrorMode mode = G4ErrorMode_PropForwards; - if (!configurePropagation(mode, pDest, cmsInitPos, cmsInitMom)) - return retDefault(); + const double ePremev = aTrack->GetStep()->GetPreStepPoint()->GetTotalEnergy(); + const double ePostmev = aTrack->GetStep()->GetPostStepPoint()->GetTotalEnergy(); + const double elossmev = ePremev - ePostmev; - // re-check propagation direction chosen in case of AnyDirection - if (mode == G4ErrorMode_PropBackwards && !flipped) { - cmsInitMom = -cmsInitMom; - g4InitMom = -g4InitMom; +// const unsigned int nsamples = 100; + const unsigned int nsamples = 1; + + std::vector elossv; + elossv.reserve(nsamples); + for (unsigned int isample = 0; isample < nsamples; ++isample) { + double eloss = 1e-3*fluct->SampleFluctuations2(mate, aTrack->GetDynamicParticle(), Emaxmev, stepLengthmm, ekinmev, elossmev); + + elossv.push_back(eloss); } - - -// double posarr[4] = { ftsStart[0]*cm, ftsStart[1]*cm, ftsStart[2]*cm, 0. }; -// double fieldval[10]; -// field->GetFieldValue(posarr, fieldval); -// -// std::cout << "cmsInitPos:" << std::endl; -// std::cout << cmsInitPos << std::endl; -// std::cout << "fieldval " << fieldval[0] << " " << fieldval[1] << " " << fieldval[2] << std::endl; - -// CLHEP::Hep3Vector g4InitPos = TrackPropagation::globalPointToHep3Vector(cmsInitPos); -// CLHEP::Hep3Vector g4InitMom = TrackPropagation::globalVectorToHep3Vector(cmsInitMom * GeV); - + std::sort(elossv.begin(), elossv.end()); + unsigned int imode = 0.24672310*nsamples; + unsigned int imed = nsamples/2; -// debugReportTrackState("intitial", cmsInitPos, g4InitPos, cmsInitMom, g4InitMom, pDest); + const double mode = elossv[imode]; + const double med = elossv[imed]; - // Set the mode of propagation according to the propagation direction - // G4ErrorMode mode = G4ErrorMode_PropForwards; + constexpr double k = -0.22278; + constexpr double km = 1.35578; - // if (!configurePropagation(mode, pDest, cmsInitPos, cmsInitMom)) - // return TsosPP(TrajectoryStateOnSurface(), 0.0f); + const double deltap = mode; + const double c = std::max(0., 0.5*M_PI*(med-mode)/(km-k)); - /////////////////////////////// - // Set the error and trajectories, and finally propagate - // - G4ErrorTrajErr g4error(5, 0); -// if (ftsStart.hasError()) { -// CurvilinearTrajectoryError initErr; -// initErr = ftsStart.curvilinearError(); -// g4error = TrackPropagation::algebraicSymMatrix55ToG4ErrorTrajErr(initErr, ftsStart.charge()); -// LogDebug("Geant4e") << "CMS - Error matrix: " << std::endl << initErr.matrix(); -// } else { -// LogDebug("Geant4e") << "No error matrix available" << std::endl; -// return retDefault(); -// } + // const double deltap = xi*(std::log(2.*m*beta*beta*gamma*gamma/I) + std::log(xi/I) + 0.2 - beta*beta - delta)*1e-3; + // const double fwhm = 4.*xi*1e-3; + // const double c = 0.125*M_PI*fwhm; - LogDebug("Geant4e") << "G4e - Error matrix: " << std::endl << g4error; - // in CMSSW, the state errors are deflated when performing the backward - // propagation - if (mode == G4ErrorMode_PropForwards) { - G4ErrorPropagatorData::GetErrorPropagatorData()->SetStage(G4ErrorStage_Inflation); - } else if (mode == G4ErrorMode_PropBackwards) { - G4ErrorPropagatorData::GetErrorPropagatorData()->SetStage(G4ErrorStage_Deflation); - } - G4ErrorFreeTrajState g4eTrajState(generateParticleName(charge), g4InitPos, g4InitMom, g4error); - LogDebug("Geant4e") << "G4e - Traj. State: " << (g4eTrajState); + const double ascale = 2.*c/M_PI; + const double mshift = deltap - ascale*k; - ////////////////////////////// - // Propagate - int iterations = 0; - double finalPathLength = 0; + const double mu = c > 0. ? mshift - ascale*std::log(ascale) : deltap; - HepGeom::Point3D finalRecoPos; + // std::cout << "deltap = " << deltap << " deltapalt = " << deltapalt << " c = " << c << " calt = " << calt << std::endl; - G4ErrorPropagatorData::GetErrorPropagatorData()->SetMode(mode); + // std::cout << "deltap = " << deltap << " c = " << c << std::endl; - theG4eData->SetTarget(g4eTarget_center.second.get()); - LogDebug("Geant4e") << "Running Propagation to the RECO surface" << std::endl; - theG4eManager->InitTrackPropagation(); - - // initial jacobian is the identity matrix for the state components, - // and 0 for b-field and material variations -// G4ErrorMatrix jac(5, 7); -// for (unsigned int i = 0; i < 5; ++i) { -// for (unsigned int j = 0; j < 7; ++j) { -// jac(i+1, j+1) = i == j ? 1. : 0.; -// } -// } - - Matrix jac; - jac.leftCols<5>() = Matrix::Identity(); - jac.rightCols<2>() = Matrix::Zero(); - - const G4ErrorTrajErr errnull(5, 0); - - -// const G4ErrorTrajErr g4errorEnd = g4eTrajState.GetError(); -// const AlgebraicMatrix55 errstart = ftsStart.curvilinearError().matrix(); - Matrix g4errorEnd = Matrix::Zero(); - -// Matrix ftsEnd = ftsStart; - - -// G4ErrorTrajErr dQ(5, 0); - Matrix dQ = Matrix::Zero(); - - double dEdxlast = 0.; -// double masslast = 0.; - - Matrix dErrorDxLast = Matrix::Zero(); - - bool continuePropagation = true; - while (continuePropagation) { - iterations++; - LogDebug("Geant4e") << std::endl << "step count " << iterations << " step length " << finalPathLength; - -// const GlobalPoint pos = TrackPropagation::hepPoint3DToGlobalPoint(g4eTrajState.GetPosition()); -// const GlobalVector mom = TrackPropagation::hep3VectorToGlobalVector(g4eTrajState.GetMomentum() / GeV); -// constexpr double mass = 0.1056583745; - -// const FreeTrajectoryState statepre(pos, mom, ftsStart.charge(), &ftsStart.parameters().magneticField()); - - Matrix statepre; - statepre[0] = g4eTrajState.GetPosition().x()/cm; - statepre[1] = g4eTrajState.GetPosition().y()/cm; - statepre[2] = g4eTrajState.GetPosition().z()/cm; - statepre[3] = g4eTrajState.GetMomentum().x()/GeV; - statepre[4] = g4eTrajState.GetMomentum().y()/GeV; - statepre[5] = g4eTrajState.GetMomentum().z()/GeV; - statepre[6] = charge; - - //set the error matrix to null to disentangle MS and ionization contributions - g4eTrajState.SetError(errnull); - const int ierr = theG4eManager->PropagateOneStep(&g4eTrajState, mode); - - if (ierr != 0) { - // propagation failed, return invalid track state - return retDefault(); - } - - const double thisPathLength = TrackPropagation::g4doubleToCmsDouble(g4eTrajState.GetG4Track()->GetStepLength()); - - const double pPre = g4eTrajState.GetMomentum().mag()/GeV; - const double ePre = g4eTrajState.GetG4Track()->GetStep()->GetPreStepPoint()->GetTotalEnergy() / GeV; - const double ePost = g4eTrajState.GetG4Track()->GetStep()->GetPostStepPoint()->GetTotalEnergy() / GeV; - const double dEdx = (ePost - ePre)/thisPathLength; - const double mass = g4eTrajState.GetG4Track()->GetDynamicParticle()->GetMass() / GeV; - - if (std::abs(thisPathLength) > 0.) { - dEdxlast = dEdx; -// masslast = mass; - } - -// std::cout << "thisPathLength = " << thisPathLength << std::endl; -// const Matrix transportJac = transportJacobian(statepre, thisPathLength, dEdx, mass); - const Matrix transportJac = transportJacobianBzD(statepre, thisPathLength, dEdx, mass, dBz); - -// const Matrix transportJacAlt = transportJacobianD(statepre, thisPathLength, dEdx, mass); - -// const double dh = 1e-4; - -// const Matrix dest = transportResultD(statepre, thisPathLength, dEdx, mass, 0.); -// const Matrix destalt = transportResultD(statepre, thisPathLength, dEdx, mass, dh); - -// const Matrix ddest = (destalt-dest)/dh; -// -// Matrix statepost; -// statepost[0] = g4eTrajState.GetPosition().x()/cm; -// statepost[1] = g4eTrajState.GetPosition().y()/cm; -// statepost[2] = g4eTrajState.GetPosition().z()/cm; -// statepost[3] = g4eTrajState.GetMomentum().x()/GeV; -// statepost[4] = g4eTrajState.GetMomentum().y()/GeV; -// statepost[5] = g4eTrajState.GetMomentum().z()/GeV; -// statepost[6] = charge; -// -// const Matrix Wpost = statepost.segment<3>(3).normalized(); -// const Matrix khat(0., 0., 1.); -// -// const Matrix Upost = khat.cross(Wpost).normalized(); -// // const Matrix Upost = Upostpre.normalized(); -// -// const double ddxt = ddest.head<3>().dot(Upost); - -// std::cout << - - - -// const Matrix transportJac = transportJacobianBzAdvanced(statepre, thisPathLength, dEdx, mass); - -// const Matrix transportJacAlt = transportJacobianBz(statepre, thisPathLength, dEdx, mass); - -// std::cout << "transportJac" << std::endl; -// std::cout << transportJac << std::endl; -// // std::cout << "transportJacAlt" << std::endl; -// // std::cout << transportJacAlt << std::endl; -// std::cout << "ddest numerical" << std::endl; -// std::cout << ddest.transpose() << std::endl; -// std::cout << "ddxt jac = " << transportJac(3, 5) << " ddxt numerical = " << ddxt << std::endl; - -// bool isnan = false; -// for (unsigned int i=0; i()*g4errorEnd*transportJac.leftCols<5>().transpose()).eval(); - - dQ = (transportJac.leftCols<5>()*dQ*transportJac.leftCols<5>().transpose()).eval(); - -// const G4ErrorMatrix &transferMatrix = g4eTrajState.GetTransfMat(); - - // transport contribution to error -// g4errorEnd = g4errorEnd.similarity(transferMatrix).T(); -// dQ = dQ.similarity(transferMatrix).T(); - - // MS and ionization contributions to error - -// G4ErrorTrajErr errMSI = g4eTrajState.GetError(); -// -// // recompute ionization contribution (without truncation for the moment) -// errMSI(0+1, 0+1) = computeErrorIoni(g4eTrajState.GetG4Track()); -// -// Matrix errMSIout = Matrix::Zero(); -// -// // std::cout << "errMSI" << std::endl; -// // std::cout << errMSI << std::endl; -// -// for (unsigned int i = 0; i < 5; i++) { -// for (unsigned int j = 0; j < 5; j++) { -// double w = 1.; -// if (i==0) { -// w *= charge; -// } -// if (j==0) { -// w *= charge; -// } -// errMSIout(i, j) += w*errMSI(i+1, j+1); -// } -// } - - Matrix errMSIout = PropagateErrorMSC(g4eTrajState.GetG4Track(), pforced); - errMSIout(0, 0) = computeErrorIoni(g4eTrajState.GetG4Track(), pforced); - - - - g4errorEnd += errMSIout; - - - - - if (std::abs(thisPathLength) > 0.) { - dErrorDxLast = errMSIout/thisPathLength; - } - - // transport + nominal energy loss contribution to jacobian -// jac = transferMatrix*jac; - jac = (transportJac.leftCols<5>()*jac).eval(); - - //TODO assess relevance of approximations (does the order matter? position/momentum before or after step?) - //b-field and material contributions to jacobian - jac.rightCols<2>() += transportJac.rightCols<2>(); - - -// const double betaPre = pPre/ePre; -// G4ErrorTrajErr errMS = errMSI; -// errMS(0+1, 0+1) = 0.; -// // std::cout << "ppre = " << pPre << " betaPre = " << betaPre << " mass = " << mass << " ePre = " << ePre << std::endl; -// // G4ErrorTrajErr dQpre = 2.*pPre*(betaPre*betaPre + 2.*mass*mass/ePre/ePre)*errMS; -// G4ErrorTrajErr dQpre = (2.*pPre + 4.*mass*mass/pPre)*betaPre*betaPre*errMS; -// -// for (unsigned int i = 0; i < 5; i++) { -// for (unsigned int j = 0; j < 5; j++) { -// double w = charge; -// if (i==0) { -// w *= charge; -// } -// if (j==0) { -// w *= charge; -// } -// w *= jac(0,0); -// dQ(i, j) += w*dQpre(i+1, j+1); -// } -// } - -// Matrix errMS = errMSIout; -// errMS(0,0) = 0.; - -// Matrix errIoni = Matrix::Zero(); -// errIoni(0,0) = errMSIout(0,0); -// -// const double q = charge; -// const double qop = charge/pPre; -// const double eMass = 510.99906e-6; -// -// const double x0 = std::pow(q, 2); -// const double x1 = std::pow(mass, 2)*std::pow(qop, 2); -// const double x2 = 1./(qop*(x0 + x1)); -// const Matrix dQms = 2.*errMS*x2*(x0 + 2.*x1); -// const Matrix dQion = errIoni*x2*(4.0*x0 + 10.0*x1); - - - -// std::cout << "dQionNorm" << std::endl; -// std::cout << dQion/errIoni(0,0) << std::endl; - - -// const double dQmsNorm = 2*x0*(x1 + 2*x4)/(x1 + x4); -// std::cout << "dQmsNorm = " << dQmsNorm << std::endl; - -// std::cout << "errMSIout" << std::endl; -// std::cout << errMSIout << std::endl; -// -// std::cout << "dQms" << std::endl; -// std::cout << dQms << std::endl; -// -// std::cout << "dQion" << std::endl; -// std::cout << dQion << std::endl; - - - - - - -// dQ += 0.*jac(0,0)*dQion; -// dQ += jac(0,0)*dQms; - - - Matrix errMS = errMSIout; - errMS(0,0) = 0.; - - dQ += errMS; - - - - - - LogDebug("Geant4e") << "step Length was " << thisPathLength << " cm, current global position: " - << TrackPropagation::hepPoint3DToGlobalPoint(g4eTrajState.GetPosition()) << std::endl; - - finalPathLength += thisPathLength; - - // if (std::fabs(finalPathLength) > 10000.0f) - if (std::fabs(finalPathLength) > 200.0f) { - LogDebug("Geant4e") << "ERROR: Quitting propagation: path length mega large" << std::endl; - theG4eManager->GetPropagator()->InvokePostUserTrackingAction(g4eTrajState.GetG4Track()); - continuePropagation = false; - LogDebug("Geant4e") << "WARNING: Quitting propagation: max path length " - "exceeded, returning invalid state" - << std::endl; - - // reached maximum path length, bail out - return retDefault(); - } - - if (theG4eManager->GetPropagator()->CheckIfLastStep(g4eTrajState.GetG4Track())) { - theG4eManager->GetPropagator()->InvokePostUserTrackingAction(g4eTrajState.GetG4Track()); - continuePropagation = false; - } - } - - // CMSSW Tracking convention, backward propagations have negative path length - if (propagationDirection() == oppositeToMomentum) - finalPathLength = -finalPathLength; - -// std::cout << "finalPathLength = " << finalPathLength << std::endl; - - // store the correct location for the hit on the RECO surface - LogDebug("Geant4e") << "Position on the RECO surface" << g4eTrajState.GetPosition() << std::endl; - finalRecoPos = g4eTrajState.GetPosition(); - - theG4eManager->EventTermination(); - - LogDebug("Geant4e") << "Final position of the Track :" << g4eTrajState.GetPosition() << std::endl; - - ////////////////////////////// - // Retrieve the state in the end from Geant4e, convert them to CMS vectors - // and points, and build global trajectory parameters. - // CMS uses cm and GeV while Geant4 uses mm and MeV - // - - Matrix ftsEnd; - ftsEnd[0] = g4eTrajState.GetPosition().x()/cm; - ftsEnd[1] = g4eTrajState.GetPosition().y()/cm; - ftsEnd[2] = g4eTrajState.GetPosition().z()/cm; - ftsEnd[3] = g4eTrajState.GetMomentum().x()/GeV; - ftsEnd[4] = g4eTrajState.GetMomentum().y()/GeV; - ftsEnd[5] = g4eTrajState.GetMomentum().z()/GeV; - ftsEnd[6] = charge; - - cmsField->SetOffset(0., 0., 0.); - cmsField->SetMaterialOffset(0.); - - return std::tuple, Matrix, Matrix, double, Matrix>(true, ftsEnd, g4errorEnd, jac, dEdxlast, dQ); - -// return std::tuple(TrajectoryStateOnSurface(localparms, localerr, pDest, theField, side), jacfinal, dQfinal, dEdxlast); -} - -// -//////////////////////////////////////////////////////////////////////////// -// - -/** The methods propagateWithPath() are identical to the corresponding - * methods propagate() in what concerns the resulting - * TrajectoryStateOnSurface, but they provide in addition the - * exact path length along the trajectory. - */ - -std::pair Geant4ePropagator::propagateWithPath(const FreeTrajectoryState &ftsStart, - const Plane &pDest) const { - // Finally build the pair<...> that needs to be returned where the second - // parameter is the exact path length. Currently calculated with a stepping - // action that adds up the length of every step - return propagateGeneric(ftsStart, pDest); -} - -std::pair Geant4ePropagator::propagateWithPath(const FreeTrajectoryState &ftsStart, - const Cylinder &cDest) const { - // Finally build the pair<...> that needs to be returned where the second - // parameter is the exact path length. - return propagateGeneric(ftsStart, cDest); -} - -std::pair Geant4ePropagator::propagateWithPath( - const TrajectoryStateOnSurface &tsosStart, const Plane &pDest) const { - // Finally build the pair<...> that needs to be returned where the second - // parameter is the exact path length. - const FreeTrajectoryState ftsStart = *tsosStart.freeState(); - return propagateGeneric(ftsStart, pDest); -} - -std::pair Geant4ePropagator::propagateWithPath( - const TrajectoryStateOnSurface &tsosStart, const Cylinder &cDest) const { - const FreeTrajectoryState ftsStart = *tsosStart.freeState(); - // Finally build the pair<...> that needs to be returned where the second - // parameter is the exact path length. - return propagateGeneric(ftsStart, cDest); -} - -void Geant4ePropagator::debugReportPlaneSetup(GlobalPoint const &posPlane, - HepGeom::Point3D const &surfPos, - GlobalVector const &normalPlane, - HepGeom::Normal3D const &surfNorm, - const Plane &pDest) const { - LogDebug("Geant4e") << "G4e - Destination CMS plane position:" << posPlane << "cm\n" - << "G4e - (Ro, eta, phi): (" << posPlane.perp() << " cm, " << posPlane.eta() - << ", " << posPlane.phi().degrees() << " deg)\n" - << "G4e - Destination G4 plane position: " << surfPos << " mm, Ro = " << surfPos.perp() - << " mm"; - LogDebug("Geant4e") << "G4e - Destination CMS plane normal : " << normalPlane << "\n" - << "G4e - Destination G4 plane normal : " << normalPlane; - LogDebug("Geant4e") << "G4e - Distance from plane position to plane: " << pDest.localZ(posPlane) << " cm"; -} - -template -void Geant4ePropagator::debugReportTrackState(std::string const ¤tContext, - GlobalPoint const &cmsInitPos, - CLHEP::Hep3Vector const &g4InitPos, - GlobalVector const &cmsInitMom, - CLHEP::Hep3Vector const &g4InitMom, - const SurfaceType &pDest) const { - LogDebug("Geant4e") << "G4e - Current Context: " << currentContext; - LogDebug("Geant4e") << "G4e - CMS point position:" << cmsInitPos << "cm\n" - << "G4e - (Ro, eta, phi): (" << cmsInitPos.perp() << " cm, " << cmsInitPos.eta() - << ", " << cmsInitPos.phi().degrees() << " deg)\n" - << "G4e - G4 point position: " << g4InitPos << " mm, Ro = " << g4InitPos.perp() << " mm"; - LogDebug("Geant4e") << "G4e - CMS momentum :" << cmsInitMom << "GeV\n" - << " pt: " << cmsInitMom.perp() << "G4e - G4 momentum : " << g4InitMom << " MeV"; -} - - -//------------------------------------------------------------------------ -Eigen::Matrix Geant4ePropagator::PropagateErrorMSC( const G4Track* aTrack, double pforced) const -{ - G4ThreeVector vpPre = aTrack->GetMomentum()/GeV; -// G4double pPre = vpPre.mag(); -// G4double pBeta = pPre*pPre / (aTrack->GetTotalEnergy()/GeV); - G4double mass = aTrack->GetDynamicParticle()->GetMass() / GeV; - G4double pPre = pforced > 0. ? pforced : aTrack->GetMomentum().mag() / GeV; - G4double Etot = sqrt(pPre*pPre + mass*mass); - G4double beta = pPre/Etot; - G4double pBeta = pPre*beta; - G4double stepLengthCm = aTrack->GetStep()->GetStepLength()/cm; - - G4Material* mate = aTrack->GetVolume()->GetLogicalVolume()->GetMaterial(); - G4double effZ, effA; - CalculateEffectiveZandA( mate, effZ, effA ); - -#ifdef G4EVERBOSE - if( iverbose >= 4 ) G4cout << "material " << mate->GetName() - //<< " " << mate->GetZ() << " " << mate->GetA() - << " effZ:" << effZ << " effA:" << effA - << " dens(g/mole):" << mate->GetDensity()/g*mole << " Radlen/cm:" << mate->GetRadlen()/cm << " nuclLen/cm" << mate->GetNuclearInterLength()/cm << G4endl; -#endif - - G4double RI = stepLengthCm / (mate->GetRadlen()/cm); -#ifdef G4EVERBOSE - if( iverbose >= 4 ) G4cout << std::setprecision(6) << std::setw(6) << "G4EP:MSC: RI=X/X0 " << RI << " stepLengthCm " << stepLengthCm << " radlen/cm " << (mate->GetRadlen()/cm) << " RI*1.e10:" << RI*1.e10 << G4endl; -#endif - G4double charge = aTrack->GetDynamicParticle()->GetCharge(); - G4double DD = 1.8496E-4*RI*(charge/pBeta * charge/pBeta ); -#ifdef G4EVERBOSE - if( iverbose >= 3 ) G4cout << "G4EP:MSC: D*1E6= " << DD*1.E6 <<" pBeta " << pBeta << G4endl; -#endif - G4double S1 = DD*stepLengthCm*stepLengthCm/3.; - G4double S2 = DD; - G4double S3 = DD*stepLengthCm/2.; - - G4double CLA = std::sqrt( vpPre.x() * vpPre.x() + vpPre.y() * vpPre.y() )/pPre; -#ifdef G4EVERBOSE - if( iverbose >= 2 ) G4cout << std::setw(6) << "G4EP:MSC: RI " << RI << " S1 " << S1 << " S2 " << S2 << " S3 " << S3 << " CLA " << CLA << G4endl; -#endif - Eigen::Matrix res = Eigen::Matrix::Zero(); - res(1, 1) = S2; - res(1, 4) = -S3; - res(2, 2) = S2/CLA/CLA; - res(2, 3) = S3/CLA; - res(3, 3) = S1; - res(4, 4) = S1; - - res(4, 1) = res(1, 4); - res(3, 2) = res(2, 3); - -#ifdef G4EVERBOSE - if( iverbose >= 2 ) G4cout << "G4EP:MSC: error matrix propagated msc " << fError << G4endl; -#endif - - return res; -} - -//------------------------------------------------------------------------ -double Geant4ePropagator::computeErrorIoni(const G4Track* aTrack, double pforced) const -{ - G4double stepLengthCm = aTrack->GetStep()->GetStepLength() / cm; -#ifdef G4EVERBOSE - G4double DEDX2; - if(stepLengthCm < 1.E-7) - { - DEDX2 = 0.; - } -#endif - // * Calculate xi factor (KeV). - G4Material* mate = aTrack->GetVolume()->GetLogicalVolume()->GetMaterial(); - G4double effZ, effA; - CalculateEffectiveZandA(mate, effZ, effA); - -// G4double Etot = aTrack->GetTotalEnergy() / GeV; -// G4double beta = aTrack->GetMomentum().mag() / GeV / Etot; - - G4double mass = aTrack->GetDynamicParticle()->GetMass() / GeV; - G4double pPre = pforced > 0. ? pforced : aTrack->GetMomentum().mag() / GeV; - G4double Etot = sqrt(pPre*pPre + mass*mass); - G4double beta = pPre/Etot; - G4double gamma = Etot / mass; - - // * Calculate xi factor (keV). - G4double XI = 153.5 * effZ * stepLengthCm * (mate->GetDensity() / mg * mole) / - (effA * beta * beta); - - -#ifdef G4EVERBOSE - if(iverbose >= 2) - { - G4cout << "G4EP:IONI: XI/keV " << XI << " beta " << beta << " gamma " - << gamma << G4endl; - G4cout << " density " << (mate->GetDensity() / mg * mole) << " effA " - << effA << " step " << stepLengthCm << G4endl; - } -#endif - // * Maximum energy transfer to atomic electron (KeV). - G4double eta = beta * gamma; - G4double etasq = eta * eta; - G4double eMass = 0.51099906 / GeV; - G4double massRatio = eMass / mass; - G4double F1 = 2 * eMass * etasq; - G4double F2 = 1. + 2. * massRatio * gamma + massRatio * massRatio; - G4double Emax = 1.E+6 * F1 / F2; // now in keV - -// std::cout << "eMass = " << eMass << " mass = " << mass << std::endl; - // * *** and now sigma**2 in GeV - G4double dedxSq = - XI * Emax * (1. - (beta * beta / 2.)) * 1.E-12; // now in GeV^2 - /*The above formula for var(1/p) good for dens scatterers. However, for MIPS - passing through a gas it leads to overestimation. Further more for incident - electrons the Emax is almost equal to incident energy. This leads to - k=Xi/Emax as small as e-6 and gradually the cov matrix explodes. - http://www2.pv.infn.it/~rotondi/kalman_1.pdf - Since I do not have enough info at the moment to implement Landau & - sub-Landau models for k=Xi/Emax <0.01 I'll saturate k at this value for now - */ - - -// const double ePre = aTrack->GetStep()->GetPreStepPoint()->GetTotalEnergy() / GeV; -// const double ePost = aTrack->GetStep()->GetPostStepPoint()->GetTotalEnergy() / GeV; -// const double dEdxactual = -(ePost - ePre)/stepLengthCm; -// -// // const double poti = 16.e-9 * 10.75; // = 16 eV * Z**0.9, for Si Z=14 -// // const double eplasma = 28.816e-9 * sqrt(2.33 * 0.498); // 28.816 eV * sqrt(rho*(Z/A)) for Si -// const double poti = 16.e-9 * pow(effZ, 0.9); // = 16 eV * Z**0.9, for Si Z=14 -// const double density = mate->GetDensity() / mg * mole; -// const double eplasma = 28.816e-9 * sqrt(density*effZ/effA); // 28.816 eV * sqrt(rho*(Z/A)) for Si -// // const double delta0 = 2 * log(eplasma / poti) - 1.; -// const double delta0 = 2 * log(eplasma / poti) - 1. + 2.*log(beta*gamma); -// const double beta2 = beta*beta; -// // const double dEdx = XI*1e-6 * (log(2. * eMass * Emax*1e-6 / (poti * poti)) - 2. * (beta2)-delta0)/stepLengthCm; -// const double dEdx = XI*1e-6 * (log(2. * eMass * Emax*1e-6*beta2*gamma*gamma / (poti * poti)) - 2. * (beta2)-delta0)/stepLengthCm; -// -// std::cout << "material = " << mate->GetName() <<" ePre = " << ePre << " dEdx simple = " << dEdx << " dEdx actual = " << dEdxactual << " actual/simple = " << dEdxactual/dEdx << std::endl; - -// dedxSq *= 0.; - - // Implementation based on PANDA Report PV/01-07 Section 7 - // TODO understand implications for thick scatterer split into small steps - -// G4double dedxSq; - -// const double kappa = XI/Emax; -// std::cout << "steplength = " << stepLengthCm << " xi = " << XI*1e-6 << " xi/dx = " << XI*1e-6/stepLengthCm << " emax = " << Emax*1e-6 << " kappa = " << kappa << " effZ = " << effZ << std::endl; - -// if (kappa > 0.005) { -// //vavilov distribution (or gaussian limit which is equivalent for the variance) -// -// dedxSq = XI * Emax * (1. - (beta * beta / 2.)) * 1.E-12; // now in GeV^2 -// } -// else { -// -// -// const double I = 16.*pow(effZ,0.9); -// const double f2 = effZ <= 2. ? 0. : 2./effZ; -// const double f1 = 1. - f2; -// const double e2 = 10.*effZ*effZ; -// const double e1 = pow(I/pow(e2,f2),1./f1); -// const double r = 0.4; -// const double emaxev = Emax*1e6; -// const double massev = mass*1e9; -// -// const double ePre = aTrack->GetStep()->GetPreStepPoint()->GetTotalEnergy() / GeV; -// const double ePost = aTrack->GetStep()->GetPostStepPoint()->GetTotalEnergy() / GeV; -// const double C = (ePre-ePost)/stepLengthCm*1e9; -// -// const double sigma1 = C*f1*(log(2.*massev*beta*beta*gamma*gamma/e1) - beta*beta)/e1/(log(2.*massev*beta*beta*gamma*gamma/I) - beta*beta)*(1.-r); -// -// const double sigma2 = C*f1*(log(2.*massev*beta*beta*gamma*gamma/e2) - beta*beta)/e2/(log(2.*massev*beta*beta*gamma*gamma/I) - beta*beta)*(1.-r); -// -// const double sigma3 = C*emaxev/I/(emaxev+I)/log((emaxev+I)/I)*r; -// -// const double Nc = (sigma1 + sigma2 + sigma3)*stepLengthCm; -// -// // std::cout << "Nc = " << Nc << std::endl; -// -// if (Nc > 50.) { -// // if (false) { -// //landau -// dedxSq = 15.76*15.76*XI*XI*1e-12; //corresponds to 0.996 quantile -// -// const double dedxsqvavilov = XI * Emax * (1. - (beta * beta / 2.)) * 1.E-12; -// -// std::cout << "dedxsq: vavilov = " << dedxsqvavilov << " landau = " << dedxSq << std::endl; -// } -// else { -// //sub-landau -// const double alpha = 0.996; -// // const double alpha = 1.; -// const double ealpha = I/(1. - alpha*emaxev/(emaxev + I)); -// const double e3 = I*(emaxev +I)*log(ealpha/I)/emaxev; -// const double e3sq = I*(emaxev + I)*(ealpha - I)/emaxev; -// const double sigmae3sq = e3sq - e3*e3; -// -// dedxSq = sigma1*stepLengthCm*e1*e1 + sigma2*stepLengthCm*e2*e2 + sigma3*stepLengthCm*e3*e3 + sigma3*stepLengthCm*sigmae3sq*(sigma3*stepLengthCm + 1.); -// dedxSq *= 1e-18; -// -// const double dedxsqlandau = 15.76*15.76*XI*XI*1e-12; -// const double dedxsqvavilov = XI * Emax * (1. - (beta * beta / 2.)) * 1.E-12; -// // -// std::cout << "dedxsq: vavilov = " << dedxsqvavilov << " landau = " << dedxsqlandau << " sublandau = " << dedxSq << std::endl;; -// -// } -// -// -// } - - -// dedxSq = 1.7*1.7*XI*XI*1e-12; - -// if (false) { -// if(XI / Emax < 0.01) -// dedxSq *= -// XI / Emax * 100; // Quench for low Elos, see above: newVar=odVar *k/0.01 -// } - -// std::cout << "Geant4ePropagator xi = " << XI << " emax = " << Emax << " dedxSq = " << dedxSq << std::endl; - -#ifdef G4EVERBOSE - if(iverbose >= 2) - G4cout << "G4EP:IONI: DEDX^2(GeV^2) " << dedxSq << " emass/GeV: " << eMass - << " Emax/keV: " << Emax << " k=Xi/Emax=" << XI / Emax << G4endl; - -#endif - - Etot = aTrack->GetTotalEnergy() / GeV; - - G4double pPre6 = - (aTrack->GetStep()->GetPreStepPoint()->GetMomentum() / GeV).mag(); - pPre6 = std::pow(pPre6, 6); - // Apply it to error - const double res = Etot * Etot * dedxSq / pPre6; -#ifdef G4EVERBOSE - if(iverbose >= 2) - G4cout << "G4:IONI Etot/GeV: " << Etot << " err_dedx^2/GeV^2: " << dedxSq - << " p^6: " << pPre6 << G4endl; - if(iverbose >= 2) - G4cout << "G4EP:IONI: error2_from_ionisation " - << (Etot * Etot * dedxSq) / pPre6 << G4endl; -#endif - - return res; -} - -//------------------------------------------------------------------------ -void Geant4ePropagator::CalculateEffectiveZandA(const G4Material* mate, - G4double& effZ, - G4double& effA) const -{ - effZ = 0.; - effA = 0.; - G4int ii, nelem = mate->GetNumberOfElements(); - const G4double* fracVec = mate->GetFractionVector(); - for(ii = 0; ii < nelem; ii++) - { - effZ += mate->GetElement(ii)->GetZ() * fracVec[ii]; - effA += mate->GetElement(ii)->GetA() * fracVec[ii] / g * mole; - } -} - -AlgebraicMatrix55 Geant4ePropagator::curv2localJacobianAlt(const GlobalTrajectoryParameters &globalSource, const Surface &surface) const { - - const GlobalVector &bfield = globalSource.magneticFieldInInverseGeV(); - - CurvilinearTrajectoryParameters curvparms(globalSource.position(), globalSource.momentum(), globalSource.charge()); - - const double qop0 = curvparms.Qbp(); - const double lam0 = curvparms.lambda(); - const double phi0 = curvparms.phi(); - const double xt0 = curvparms.xT(); - const double yt0 = curvparms.yT(); - - const double p0 = globalSource.momentum().mag(); - const GlobalVector W0 = globalSource.momentum()/p0; - const double W0x = W0.x(); - const double W0y = W0.y(); - const double W0z = W0.z(); - - const LocalVector lx(1.,0.,0.); - const LocalVector ly(0.,1.,0.); - const LocalVector lz(0.,0.,1.); - const LocalPoint l0(0., 0.); - - const GlobalVector I1 = surface.toGlobal(lz); - const GlobalVector J1 = surface.toGlobal(lx); - const GlobalVector K1 = surface.toGlobal(ly); - const GlobalPoint r1 = surface.toGlobal(l0); - - const double Ix1 = I1.x(); - const double Iy1 = I1.y(); - const double Iz1 = I1.z(); - - const double Jx1 = J1.x(); - const double Jy1 = J1.y(); - const double Jz1 = J1.z(); - - const double Kx1 = K1.x(); - const double Ky1 = K1.y(); - const double Kz1 = K1.z(); - - const double rx1 = r1.x(); - const double ry1 = r1.y(); - const double rz1 = r1.z(); - - const double B = bfield.mag(); - const GlobalVector H = bfield/B; - const double hx = H.x(); - const double hy = H.y(); - const double hz = H.z(); - - const double x0 = std::sin(lam0); - const double x1 = Iz1*x0; - const double x2 = std::cos(lam0); - const double x3 = std::cos(phi0); - const double x4 = Ix1*x3; - const double x5 = std::sin(phi0); - const double x6 = Iy1*x5; - const double x7 = x1 + x2*x4 + x2*x6; - const double x8 = std::pow(x7, -2); - const double x9 = Iz1*Jx1; - const double x10 = Iz1*Jy1; - const double x11 = lam0 + phi0; - const double x12 = -phi0; - const double x13 = lam0 + x12; - const double x14 = std::pow(Ix1*std::cos(x11) + Ix1*std::cos(x13) + Iy1*std::sin(x11) - Iy1*std::sin(x13) + 2*x1, -2); - const double x15 = 2*Ix1; - const double x16 = Jy1*x15; - const double x17 = 2*Iy1; - const double x18 = Jx1*x17; - const double x19 = 2*lam0; - const double x20 = std::cos(x19); - const double x21 = phi0 + x19; - const double x22 = std::cos(x21); - const double x23 = std::sin(x21); - const double x24 = Ix1*Jz1; - const double x25 = Iy1*Jz1; - const double x26 = x12 + x19; - const double x27 = std::cos(x26); - const double x28 = std::sin(x26); - const double x29 = Ix1*W0y - Iy1*W0x; - const double x30 = hz*x2; - const double x31 = hx*x0 - x3*x30; - const double x32 = hy*x0 - x30*x5; - const double x33 = x2*(hx*x5 - hy*x3); - const double x34 = x2*x3; - const double x35 = x2*x5; - const double x36 = Jx1*x34 + Jy1*x35 + Jz1*x0; - const double x37 = -Ix1*x32 + Iy1*x31 - Iz1*x33; - const double x38 = std::pow(W0x, 2) + std::pow(W0y, 2); - const double x39 = std::pow(x38, -1.0/2.0); - const double x40 = B*qop0*x39/std::pow(x7, 3); - const double x41 = x40*(-x36*x37 + x7*(-Jx1*x32 + Jy1*x31 - Jz1*x33)); - const double x42 = Ix1*W0x*W0z + Iy1*W0y*W0z - Iz1*x38; - const double x43 = Iz1*Kx1; - const double x44 = Iz1*Ky1; - const double x45 = Ky1*x15; - const double x46 = Kx1*x17; - const double x47 = Ix1*Kz1; - const double x48 = Iy1*Kz1; - const double x49 = Kx1*x34 + Ky1*x35 + Kz1*x0; - const double x50 = x40*(-x37*x49 + x7*(-Kx1*x32 + Ky1*x31 - Kz1*x33)); - const double x51 = x39/x7; - const double x52 = x38*x7; - const double x53 = W0z*x7; - const double dqopdqop0 = 1; - const double dqopdlam0 = 0; - const double dqopdphi0 = 0; - const double dqopdxt0 = 0; - const double dqopdyt0 = 0; - const double ddxdzdqop0 = 0; - const double ddxdzdlam0 = x8*(Jz1*x4 + Jz1*x6 - x10*x5 - x3*x9); - const double ddxdzdphi0 = x14*(x10*x23 + x10*x28 + x16*x20 + x16 - x18*x20 - x18 - x22*x24 + x22*x9 - x23*x25 + x24*x27 - x25*x28 - x27*x9); - const double ddxdzdxt0 = x29*x41; - const double ddxdzdyt0 = x41*x42; - const double ddydzdqop0 = 0; - const double ddydzdlam0 = x8*(Kz1*x4 + Kz1*x6 - x3*x43 - x44*x5); - const double ddydzdphi0 = x14*(x20*x45 - x20*x46 + x22*x43 - x22*x47 + x23*x44 - x23*x48 - x27*x43 + x27*x47 + x28*x44 - x28*x48 + x45 - x46); - const double ddydzdxt0 = x29*x50; - const double ddydzdyt0 = x42*x50; - const double dxdqop0 = 0; - const double dxdlam0 = 0; - const double dxdphi0 = 0; - const double dxdxt0 = x51*(x29*x36 + x7*(-Jx1*W0y + Jy1*W0x)); - const double dxdyt0 = x51*(Jz1*x52 + x36*x42 - x53*(Jx1*W0x + Jy1*W0y)); - const double dydqop0 = 0; - const double dydlam0 = 0; - const double dydphi0 = 0; - const double dydxt0 = x51*(x29*x49 + x7*(-Kx1*W0y + Ky1*W0x)); - const double dydyt0 = x51*(Kz1*x52 + x42*x49 - x53*(Kx1*W0x + Ky1*W0y)); - AlgebraicMatrix55 res; - res(0,0) = dqopdqop0; - res(0,1) = dqopdlam0; - res(0,2) = dqopdphi0; - res(0,3) = dqopdxt0; - res(0,4) = dqopdyt0; - res(1,0) = ddxdzdqop0; - res(1,1) = ddxdzdlam0; - res(1,2) = ddxdzdphi0; - res(1,3) = ddxdzdxt0; - res(1,4) = ddxdzdyt0; - res(2,0) = ddydzdqop0; - res(2,1) = ddydzdlam0; - res(2,2) = ddydzdphi0; - res(2,3) = ddydzdxt0; - res(2,4) = ddydzdyt0; - res(3,0) = dxdqop0; - res(3,1) = dxdlam0; - res(3,2) = dxdphi0; - res(3,3) = dxdxt0; - res(3,4) = dxdyt0; - res(4,0) = dydqop0; - res(4,1) = dydlam0; - res(4,2) = dydphi0; - res(4,3) = dydxt0; - res(4,4) = dydyt0; - - - return res; - -} + return std::make_pair(mu, c); -AlgebraicMatrix55 Geant4ePropagator::curv2localJacobianAlteloss(const GlobalTrajectoryParameters &globalSource, const Surface &surface, double dEdx, double mass) const { - - const GlobalVector &bfield = globalSource.magneticFieldInInverseGeV(); - - CurvilinearTrajectoryParameters curvparms(globalSource.position(), globalSource.momentum(), globalSource.charge()); - - const double qop0 = curvparms.Qbp(); - const double lam0 = curvparms.lambda(); - const double phi0 = curvparms.phi(); - const double xt0 = curvparms.xT(); - const double yt0 = curvparms.yT(); - - const double q = globalSource.charge(); - - const double p0 = globalSource.momentum().mag(); - const GlobalVector W0 = globalSource.momentum()/p0; - const double W0x = W0.x(); - const double W0y = W0.y(); - const double W0z = W0.z(); - - const LocalVector lx(1.,0.,0.); - const LocalVector ly(0.,1.,0.); - const LocalVector lz(0.,0.,1.); - const LocalPoint l0(0., 0.); - - const GlobalVector I1 = surface.toGlobal(lz); - const GlobalVector J1 = surface.toGlobal(lx); - const GlobalVector K1 = surface.toGlobal(ly); - const GlobalPoint r1 = surface.toGlobal(l0); - - const double Ix1 = I1.x(); - const double Iy1 = I1.y(); - const double Iz1 = I1.z(); - - const double Jx1 = J1.x(); - const double Jy1 = J1.y(); - const double Jz1 = J1.z(); - - const double Kx1 = K1.x(); - const double Ky1 = K1.y(); - const double Kz1 = K1.z(); - - const double rx1 = r1.x(); - const double ry1 = r1.y(); - const double rz1 = r1.z(); - - const double B = bfield.mag(); - const GlobalVector H = bfield/B; - const double hx = H.x(); - const double hy = H.y(); - const double hz = H.z(); - - const double x0 = std::pow(q, 2); - const double x1 = std::pow(x0, -3.0/2.0); - const double x2 = Iy1*W0x; - const double x3 = Ix1*W0y; - const double x4 = std::pow(qop0, 2); - const double x5 = std::pow(W0x, 2) + std::pow(W0y, 2); - const double x6 = std::pow(x5, -1.0/2.0); - const double x7 = std::sin(lam0); - const double x8 = Iz1*x7; - const double x9 = std::cos(lam0); - const double x10 = std::cos(phi0); - const double x11 = Ix1*x10; - const double x12 = std::sin(phi0); - const double x13 = Iy1*x12; - const double x14 = x11*x9 + x13*x9 + x8; - const double x15 = x6/x14; - const double x16 = dEdx*q*x1*x15*x4*std::sqrt(std::pow(mass, 2)*x4 + x0); - const double x17 = Ix1*W0x*W0z + Iy1*W0y*W0z - Iz1*x5; - const double x18 = std::pow(x14, -2); - const double x19 = Iz1*Jx1; - const double x20 = Iz1*Jy1; - const double x21 = lam0 + phi0; - const double x22 = -phi0; - const double x23 = lam0 + x22; - const double x24 = std::pow(Ix1*std::cos(x21) + Ix1*std::cos(x23) + Iy1*std::sin(x21) - Iy1*std::sin(x23) + 2*x8, -2); - const double x25 = 2*Ix1; - const double x26 = Jy1*x25; - const double x27 = 2*Iy1; - const double x28 = Jx1*x27; - const double x29 = 2*lam0; - const double x30 = std::cos(x29); - const double x31 = phi0 + x29; - const double x32 = std::cos(x31); - const double x33 = std::sin(x31); - const double x34 = Ix1*Jz1; - const double x35 = Iy1*Jz1; - const double x36 = x22 + x29; - const double x37 = std::cos(x36); - const double x38 = std::sin(x36); - const double x39 = -x2 + x3; - const double x40 = hz*x9; - const double x41 = hx*x7 - x10*x40; - const double x42 = hy*x7 - x12*x40; - const double x43 = x9*(hx*x12 - hy*x10); - const double x44 = x10*x9; - const double x45 = x12*x9; - const double x46 = Jx1*x44 + Jy1*x45 + Jz1*x7; - const double x47 = -Ix1*x42 + Iy1*x41 - Iz1*x43; - const double x48 = B*qop0*x6/std::pow(x14, 3); - const double x49 = x48*(x14*(-Jx1*x42 + Jy1*x41 - Jz1*x43) - x46*x47); - const double x50 = Iz1*Kx1; - const double x51 = Iz1*Ky1; - const double x52 = Ky1*x25; - const double x53 = Kx1*x27; - const double x54 = Ix1*Kz1; - const double x55 = Iy1*Kz1; - const double x56 = Kx1*x44 + Ky1*x45 + Kz1*x7; - const double x57 = x48*(x14*(-Kx1*x42 + Ky1*x41 - Kz1*x43) - x47*x56); - const double x58 = x14*x5; - const double x59 = W0z*x14; - const double dqopdqop0 = std::pow(q, 3)*x1*std::fabs(qop0)/qop0; - const double dqopdlam0 = 0; - const double dqopdphi0 = 0; - const double dqopdxt0 = x16*(x2 - x3); - const double dqopdyt0 = -x16*x17; - const double ddxdzdqop0 = 0; - const double ddxdzdlam0 = x18*(Jz1*x11 + Jz1*x13 - x10*x19 - x12*x20); - const double ddxdzdphi0 = x24*(x19*x32 - x19*x37 + x20*x33 + x20*x38 + x26*x30 + x26 - x28*x30 - x28 - x32*x34 - x33*x35 + x34*x37 - x35*x38); - const double ddxdzdxt0 = x39*x49; - const double ddxdzdyt0 = x17*x49; - const double ddydzdqop0 = 0; - const double ddydzdlam0 = x18*(Kz1*x11 + Kz1*x13 - x10*x50 - x12*x51); - const double ddydzdphi0 = x24*(x30*x52 - x30*x53 + x32*x50 - x32*x54 + x33*x51 - x33*x55 - x37*x50 + x37*x54 + x38*x51 - x38*x55 + x52 - x53); - const double ddydzdxt0 = x39*x57; - const double ddydzdyt0 = x17*x57; - const double dxdqop0 = 0; - const double dxdlam0 = 0; - const double dxdphi0 = 0; - const double dxdxt0 = x15*(x14*(-Jx1*W0y + Jy1*W0x) + x39*x46); - const double dxdyt0 = x15*(Jz1*x58 + x17*x46 - x59*(Jx1*W0x + Jy1*W0y)); - const double dydqop0 = 0; - const double dydlam0 = 0; - const double dydphi0 = 0; - const double dydxt0 = x15*(x14*(-Kx1*W0y + Ky1*W0x) + x39*x56); - const double dydyt0 = x15*(Kz1*x58 + x17*x56 - x59*(Kx1*W0x + Ky1*W0y)); - AlgebraicMatrix55 res; - res(0,0) = dqopdqop0; - res(0,1) = dqopdlam0; - res(0,2) = dqopdphi0; - res(0,3) = dqopdxt0; - res(0,4) = dqopdyt0; - res(1,0) = ddxdzdqop0; - res(1,1) = ddxdzdlam0; - res(1,2) = ddxdzdphi0; - res(1,3) = ddxdzdxt0; - res(1,4) = ddxdzdyt0; - res(2,0) = ddydzdqop0; - res(2,1) = ddydzdlam0; - res(2,2) = ddydzdphi0; - res(2,3) = ddydzdxt0; - res(2,4) = ddydzdyt0; - res(3,0) = dxdqop0; - res(3,1) = dxdlam0; - res(3,2) = dxdphi0; - res(3,3) = dxdxt0; - res(3,4) = dxdyt0; - res(4,0) = dydqop0; - res(4,1) = dydlam0; - res(4,2) = dydphi0; - res(4,3) = dydxt0; - res(4,4) = dydyt0; - - return res; - } -Eigen::Matrix Geant4ePropagator::transportJacobian(const FreeTrajectoryState &start, double s, double dEdx, double mass) const { - - if (s==0.) { - Eigen::Matrix res; - res.leftCols<5>() = Eigen::Matrix::Identity(); - res.rightCols<2>() = Eigen::Matrix::Zero(); - return res; +//------------------------------------------------------------------------ +double Geant4ePropagator::computeErrorIoni(const G4Track* aTrack, double pforced) const +{ + G4double stepLengthCm = aTrack->GetStep()->GetStepLength() / cm; +#ifdef G4EVERBOSE + G4double DEDX2; + if(stepLengthCm < 1.E-7) + { + DEDX2 = 0.; } - - const GlobalTrajectoryParameters &globalSource = start.parameters(); - const GlobalVector& bfield = start.parameters().magneticFieldInInverseGeV(); - - const double M0x = globalSource.position().x(); - const double M0y = globalSource.position().y(); - const double M0z = globalSource.position().z(); - - const double p0 = globalSource.momentum().mag(); - const GlobalVector W0 = globalSource.momentum()/p0; - const double W0x = W0.x(); - const double W0y = W0.y(); - const double W0z = W0.z(); - - const double B = bfield.mag(); - const GlobalVector H = bfield/B; - const double hx = H.x(); - const double hy = H.y(); - const double hz = H.z(); - - const double qop0 = globalSource.signedInverseMomentum(); - const double q = start.charge(); - - - const double x0 = std::pow(q, 2); - const double x1 = x0/std::pow(qop0, 3); - const double x2 = std::pow(qop0, -2); - const double x3 = x0*x2; - const double x4 = std::sqrt(std::pow(mass, 2) + x3); - const double x5 = std::pow(dEdx, 2); - const double x6 = std::pow(s, 2)*x5; - const double x7 = dEdx*x4; - const double x8 = s*x7; - const double x9 = q/std::pow(x3 + x6 + 2*x8, 3.0/2.0); - const double x10 = B*s; - const double x11 = qop0*x10; - const double x12 = std::cos(x11); - const double x13 = std::pow(W0z, 2); - const double x14 = std::pow(W0x, 2); - const double x15 = std::pow(W0y, 2); - const double x16 = x14 + x15; - const double x17 = 1.0/x16; - const double x18 = std::pow(x13*x17 + 1, -1.0/2.0); - const double x19 = x12*x18; - const double x20 = std::sqrt(x16); - const double x21 = 1.0/x20; - const double x22 = W0z*x21; - const double x23 = x19*x22; - const double x24 = W0y*hx; - const double x25 = x21*x24; - const double x26 = W0x*hy; - const double x27 = x21*x26; - const double x28 = x25 - x27; - const double x29 = std::sin(x11); - const double x30 = x18*x29; - const double x31 = x28*x30; - const double x32 = x12 - 1; - const double x33 = hx*x18; - const double x34 = W0x*x21; - const double x35 = hy*x18; - const double x36 = W0y*x21; - const double x37 = hz*x18; - const double x38 = x22*x37 + x33*x34 + x35*x36; - const double x39 = hz*x38; - const double x40 = x23 - x31 - x32*x39; - const double x41 = x22*x30; - const double x42 = x18*x32; - const double x43 = x28*x42; - const double x44 = B*qop0; - const double x45 = W0z*x34; - const double x46 = W0z*x36; - const double x47 = -M0x*x45 - M0y*x46 + M0z*(x14*x21 + x15*x21); - const double x48 = x20*x47; - const double x49 = x11 - x29; - const double x50 = x39*x49 + x41 + x43 + x44*x48; - const double x51 = 1.0/B; - const double x52 = x2*x51; - const double x53 = x50*x52; - const double x54 = x10*x12; - const double x55 = x10 - x54; - const double x56 = 1.0/qop0; - const double x57 = x51*x56; - const double x58 = x57*(B*x48 + x10*x23 - x10*x31 + x39*x55); - const double x59 = x19*x36; - const double x60 = x34*x37; - const double x61 = x22*x33 - x60; - const double x62 = x29*x61; - const double x63 = hy*x38; - const double x64 = x32*x63; - const double x65 = x59 + x62 - x64; - const double x66 = -M0x*x36 + M0y*x34; - const double x67 = W0z*x47; - const double x68 = W0x*x66 - W0y*x67; - const double x69 = B*x68; - const double x70 = x30*x36; - const double x71 = qop0*x69 + x20*(-x32*x61 + x49*x63 + x70); - const double x72 = x21*x52; - const double x73 = x71*x72; - const double x74 = x20*(x10*x59 + x10*x62 + x55*x63) + x69; - const double x75 = x21*x57; - const double x76 = x74*x75; - const double x77 = x19*x34; - const double x78 = x22*x35; - const double x79 = x36*x37; - const double x80 = x78 - x79; - const double x81 = x29*x80; - const double x82 = hx*x38; - const double x83 = -x32*x82 + x77 - x81; - const double x84 = W0x*x67 + W0y*x66; - const double x85 = B*x84; - const double x86 = x30*x34; - const double x87 = -qop0*x85 + x20*(x32*x80 + x49*x82 + x86); - const double x88 = x72*x87; - const double x89 = x20*(x10*x77 - x10*x81 + x55*x82) - x85; - const double x90 = x75*x89; - const double x91 = -x40*(-x53 + x58) - x65*(-x73 + x76) - x83*(-x88 + x90); - const double x92 = x9*(-s*x5 - x7); - const double x93 = W0z*x17; - const double x94 = W0x*x93; - const double x95 = W0y*x93; - const double x96 = -x33*x94 - x35*x95 + x37; - const double x97 = x49*x96; - const double x98 = hz*x97 - x22*x43 + x30; - const double x99 = x40*x57; - const double x100 = x37*x95; - const double x101 = hx*x97 - x30*x94 + x32*(x100 + x35); - const double x102 = x57*x83; - const double x103 = 1 - x12; - const double x104 = x33 + x37*x94; - const double x105 = hy*x97 + x103*x104 - x30*x95; - const double x106 = x57*x65; - const double x107 = -x101*x102 - x105*x106 - x98*x99; - const double x108 = W0x*hx; - const double x109 = x108*x21; - const double x110 = W0y*hy; - const double x111 = x110*x21; - const double x112 = -x33*x36 + x34*x35; - const double x113 = x112*x49; - const double x114 = hz*x113 + x42*(x109 + x111); - const double x115 = hy*x113 + x103*x79 + x86; - const double x116 = hx*x113 - x32*x60 - x70; - const double x117 = -x102*x116 - x106*x115 - x114*x99; - const double x118 = -x34*x65 + x36*x83; - const double x119 = -x20*x40 + x45*x83 + x46*x65; - const double x120 = x56/std::pow(B, 2); - const double x121 = x120*x50; - const double x122 = qop0*s; - const double x123 = x12*x122; - const double x124 = x122 - x123; - const double x125 = x57*(qop0*x48 + x122*x23 - x122*x31 + x124*x39); - const double x126 = x120*x21; - const double x127 = x126*x71; - const double x128 = qop0*x68 + x20*(x122*x59 + x122*x62 + x124*x63); - const double x129 = x128*x75; - const double x130 = x126*x87; - const double x131 = -qop0*x84 + x20*(x122*x77 - x122*x81 + x124*x82); - const double x132 = x131*x75; - const double x133 = -x40*(-x121 + x125) - x65*(-x127 + x129) - x83*(-x130 + x132); - const double x134 = std::pow(x65, 2) + std::pow(x83, 2); - const double x135 = 1.0/x134; - const double x136 = 1.0/(x135*std::pow(x40, 2) + 1); - const double x137 = -x25 + x27; - const double x138 = x137*x19; - const double x139 = x29*x39; - const double x140 = std::pow(x134, -1.0/2.0); - const double x141 = x10*x70; - const double x142 = x54*x61; - const double x143 = x10*x29; - const double x144 = x143*x63; - const double x145 = (1.0/2.0)*x65; - const double x146 = x10*x86; - const double x147 = -x78 + x79; - const double x148 = x147*x54; - const double x149 = x143*x82; - const double x150 = (1.0/2.0)*x83; - const double x151 = x40/std::pow(x134, 3.0/2.0); - const double x152 = x44*x70; - const double x153 = x12*x44; - const double x154 = x153*x61; - const double x155 = x29*x44; - const double x156 = x155*x63; - const double x157 = x44*x86; - const double x158 = x147*x153; - const double x159 = x155*x82; - const double x160 = x136*(x140*(x138*x44 + x139*x44 - x41*x44) + x151*(-x145*(-2*x152 + 2*x154 + 2*x156) - x150*(-2*x157 + 2*x158 + 2*x159))); - const double x161 = hz*x32; - const double x162 = x19*x95; - const double x163 = x104*x29; - const double x164 = x32*x96; - const double x165 = hy*x164; - const double x166 = x19*x94; - const double x167 = x29*(-x100 - x35); - const double x168 = hx*x164; - const double x169 = W0y*hz; - const double x170 = x21*x30; - const double x171 = x169*x170; - const double x172 = x112*x32; - const double x173 = hy*x172; - const double x174 = W0x*hz; - const double x175 = x170*x174; - const double x176 = hx*x172; - const double x177 = x122*x70; - const double x178 = x123*x61; - const double x179 = x122*x29; - const double x180 = x179*x63; - const double x181 = x122*x86; - const double x182 = x123*x147; - const double x183 = x179*x82; - const double x184 = x135*x83; - const double x185 = -x59; - const double x186 = x135*(x185 - x62 + x64); - const double x187 = x184*(-x152 + x154 + x156) + x186*(-x157 + x158 + x159); - const double x188 = W0z*hz + x108 + x110; - const double x189 = x188*x32; - const double x190 = W0y*x12 - hy*x189 + x29*(W0z*hx - x174); - const double x191 = x13 + x16; - const double x192 = 1.0/x191; - const double x193 = W0x*x12 - hx*x189 + x29*(-W0z*hy + x169); - const double x194 = x192*std::pow(x193, 2); - const double x195 = std::pow(x190, 2)*x192; - const double x196 = std::pow(x194 + x195, -1.0/2.0); - const double x197 = x196/std::sqrt(x17*x191); - const double x198 = x17*x197; - const double x199 = x190*x198; - const double x200 = x199*x87; - const double x201 = x193*x198; - const double x202 = x201*x71; - const double x203 = x201*x57; - const double x204 = x199*x57; - const double x205 = -x153 + x44; - const double x206 = x205*x63 + x44*x59 + x44*x62; - const double x207 = x193*x197; - const double x208 = x207*x75; - const double x209 = x205*x82 + x44*x77 - x44*x81; - const double x210 = x190*x197; - const double x211 = x210*x75; - const double x212 = x206*x208 - x209*x211; - const double x213 = x192*x196*(W0z*x12 - x161*x188 + x29*(-x24 + x26)); - const double x214 = x193*x213; - const double x215 = x190*x213; - const double x216 = x194*x196 + x195*x196; - const double x217 = x215*x57; - const double x218 = x214*x57; - const double x219 = x216*x57; - const double x220 = -x206*x217 - x209*x218 + x219*(x205*x39 + x23*x44 - x31*x44); - const double dqopdqop0 = x9*(dEdx*s*x1/x4 + x1) + x91*x92; - const double dqopdlam0 = x107*x92; - const double dqopdphi0 = x117*x92; - const double dqopdxt0 = x118*x92; - const double dqopdyt0 = x119*x92; - const double dqopdB = x133*x92; - const double dqopdxi = x9*(-x6 - x8); - const double dlamdqop0 = x136*(x140*(x10*x138 + x10*x139 - x10*x41) + x151*(-x145*(-2*x141 + 2*x142 + 2*x144) - x150*(-2*x146 + 2*x148 + 2*x149))) + x160*x91; - const double dlamdlam0 = x107*x160 + x136*(x140*(-x137*x41 - x161*x96 + x19) + x151*(-x145*(-2*x162 + 2*x163 - 2*x165) - x150*(-2*x166 + 2*x167 - 2*x168))); - const double dlamdphi0 = x117*x160 + x136*(x140*(-x112*x161 + x30*(-x109 - x111)) + x151*(-x145*(2*x171 - 2*x173 + 2*x77) - x150*(2*x175 - 2*x176 - 2*x59))); - const double dlamdxt0 = x118*x160; - const double dlamdyt0 = x119*x160; - const double dlamdB = x133*x160 + x136*(x140*(x122*x138 + x122*x139 - x122*x41) + x151*(-x145*(-2*x177 + 2*x178 + 2*x180) - x150*(-2*x181 + 2*x182 + 2*x183))); - const double dlamdxi = 0; - const double dphidqop0 = x184*(-x141 + x142 + x144) + x186*(-x146 + x148 + x149) + x187*x91; - const double dphidlam0 = x107*x187 + x184*(-x162 + x163 - x165) + x186*(-x166 + x167 - x168); - const double dphidphi0 = x117*x187 + x184*(x171 - x173 + x77) + x186*(x175 - x176 + x185); - const double dphidxt0 = x118*x187; - const double dphidyt0 = x119*x187; - const double dphidB = x133*x187 + x184*(-x177 + x178 + x180) + x186*(-x181 + x182 + x183); - const double dphidxi = 0; - const double dxtdqop0 = x200*x52 - x202*x52 + x203*x74 - x204*x89 + x212*x91; - const double dxtdlam0 = -x101*x211 + x105*x208 + x107*x212; - const double dxtdphi0 = x115*x208 - x116*x211 + x117*x212; - const double dxtdxt0 = W0x*x201 + W0y*x199 + x118*x212; - const double dxtdyt0 = x119*x212 - x207*x95 + x210*x94; - const double dxtdB = x120*x200 - x120*x202 + x128*x203 - x131*x204 + x133*x212; - const double dxtdxi = 0; - const double dytdqop0 = x214*x88 - x214*x90 + x215*x73 - x215*x76 - x216*x53 + x216*x58 + x220*x91; - const double dytdlam0 = -x101*x218 - x105*x217 + x107*x220 + x219*x98; - const double dytdphi0 = x114*x219 - x115*x217 - x116*x218 + x117*x220; - const double dytdxt0 = x118*x220 + x214*x36 - x215*x34; - const double dytdyt0 = x119*x220 + x20*x216 + x214*x45 + x215*x46; - const double dytdB = -x121*x216 + x125*x216 + x127*x215 - x129*x215 + x130*x214 - x132*x214 + x133*x220; - const double dytdxi = 0; - Eigen::Matrix res; - res(0,0) = dqopdqop0; - res(0,1) = dqopdlam0; - res(0,2) = dqopdphi0; - res(0,3) = dqopdxt0; - res(0,4) = dqopdyt0; - res(0,5) = dqopdB; - res(0,6) = dqopdxi; - res(1,0) = dlamdqop0; - res(1,1) = dlamdlam0; - res(1,2) = dlamdphi0; - res(1,3) = dlamdxt0; - res(1,4) = dlamdyt0; - res(1,5) = dlamdB; - res(1,6) = dlamdxi; - res(2,0) = dphidqop0; - res(2,1) = dphidlam0; - res(2,2) = dphidphi0; - res(2,3) = dphidxt0; - res(2,4) = dphidyt0; - res(2,5) = dphidB; - res(2,6) = dphidxi; - res(3,0) = dxtdqop0; - res(3,1) = dxtdlam0; - res(3,2) = dxtdphi0; - res(3,3) = dxtdxt0; - res(3,4) = dxtdyt0; - res(3,5) = dxtdB; - res(3,6) = dxtdxi; - res(4,0) = dytdqop0; - res(4,1) = dytdlam0; - res(4,2) = dytdphi0; - res(4,3) = dytdxt0; - res(4,4) = dytdyt0; - res(4,5) = dytdB; - res(4,6) = dytdxi; - - res.col(5) *= 2.99792458e-3; - - return res; - - -} - +#endif + // * Calculate xi factor (KeV). + G4Material* mate = aTrack->GetVolume()->GetLogicalVolume()->GetMaterial(); + G4double effZ, effA; + CalculateEffectiveZandA(mate, effZ, effA); -Eigen::Matrix Geant4ePropagator::transportResult(const FreeTrajectoryState &start, double s, double dEdx, double mass) const { - - const GlobalTrajectoryParameters &globalSource = start.parameters(); - - if (s==0.) { - Eigen::Matrix res; - res[0] = globalSource.position().x(); - res[1] = globalSource.position().y(); - res[2] = globalSource.position().z(); - res[3] = globalSource.momentum().x(); - res[4] = globalSource.momentum().y(); - res[5] = globalSource.momentum().z(); - return res; - } +// G4double Etot = aTrack->GetTotalEnergy() / GeV; +// G4double beta = aTrack->GetMomentum().mag() / GeV / Etot; - const GlobalVector& bfield = start.parameters().magneticFieldInInverseGeV(); + G4double mass = aTrack->GetDynamicParticle()->GetMass() / GeV; + G4double pPre = pforced > 0. ? pforced : aTrack->GetMomentum().mag() / GeV; + G4double Etot = sqrt(pPre*pPre + mass*mass); + G4double beta = pPre/Etot; + G4double gamma = Etot / mass; - const double M0x = globalSource.position().x(); - const double M0y = globalSource.position().y(); - const double M0z = globalSource.position().z(); - - const double p0 = globalSource.momentum().mag(); - const GlobalVector W0 = globalSource.momentum()/p0; - const double W0x = W0.x(); - const double W0y = W0.y(); - const double W0z = W0.z(); - - const double Bx = bfield.x(); - const double By = bfield.y(); - const double Bz = bfield.z(); - - - const double qop0 = globalSource.signedInverseMomentum(); - const double q = start.charge(); - - const double xc0 = std::pow(Bx, 2) + std::pow(By, 2) + std::pow(Bz, 2); - const double xc1 = qop0*s*std::sqrt(xc0); - const double xc2 = std::sin(xc1); - const double xc3 = std::pow(xc0, 5.0/2.0)*xc2; - const double xc4 = By*W0z - Bz*W0y; - const double xc5 = std::cos(xc1); - const double xc6 = std::pow(xc0, 2)*(xc5 - 1); - const double xc7 = Bx*W0x + By*W0y + Bz*W0z; - const double xc8 = Bx*xc7; - const double xc9 = std::pow(xc0, 3.0/2.0)*(xc1 - xc2); - const double xc10 = std::pow(W0x, 2) + std::pow(W0y, 2); - const double xc11 = std::pow(W0z, 2) + xc10; - const double xc12 = xc11/xc10; - const double xc13 = std::pow(xc0, 3); - const double xc14 = 1.0/xc13; - const double xc15 = xc11*xc14/(qop0*std::pow(xc10, 3.0/2.0)*std::pow(xc12, 3.0/2.0)); - const double xc16 = Bx*W0z - Bz*W0x; - const double xc17 = xc7*xc9; - const double xc18 = Bx*W0y - By*W0x; - const double xc19 = xc13*xc5; - const double xc20 = std::fabs(qop0); - const double xc21 = std::pow(q, 2); - const double xc22 = std::pow(qop0, 2); - const double xc23 = xc14*std::sqrt(std::pow(dEdx, 2)*std::pow(s, 2)*xc20*xc22 + 2*dEdx*s*xc22*std::sqrt(std::pow(mass, 2)*xc22 + xc21) + xc20*xc21)/(std::sqrt(xc10)*std::sqrt(xc12)*std::pow(xc20, 3.0/2.0)); - const double xc24 = xc6*xc7; - const double x = xc15*(W0x*xc3 + xc4*xc6 + xc8*xc9); - const double y = xc15*(By*xc17 + W0y*xc3 - xc16*xc6); - const double z = xc15*(Bz*xc17 + W0z*xc3 + xc18*xc6); - const double px = xc23*(W0x*xc19 - xc3*xc4 - xc6*xc8); - const double py = xc23*(-By*xc24 + W0y*xc19 + xc16*xc3); - const double pz = xc23*(-Bz*xc24 + W0z*xc19 - xc18*xc3); - Eigen::Matrix rescart; - rescart[0] = x; - rescart[1] = y; - rescart[2] = z; - rescart[3] = px; - rescart[4] = py; - rescart[5] = pz; - - - - return rescart; -} + // * Calculate xi factor (keV). + G4double XI = 153.5 * effZ * stepLengthCm * (mate->GetDensity() / mg * mole) / + (effA * beta * beta); + -Eigen::Matrix Geant4ePropagator::transportResultD(const Eigen::Matrix &start, double s, double dEdx, double mass, double bzoffset) const { - -// const GlobalTrajectoryParameters &globalSource = start.parameters(); - - if (s==0.) { - return start.head<6>(); +#ifdef G4EVERBOSE + if(iverbose >= 2) + { + G4cout << "G4EP:IONI: XI/keV " << XI << " beta " << beta << " gamma " + << gamma << G4endl; + G4cout << " density " << (mate->GetDensity() / mg * mole) << " effA " + << effA << " step " << stepLengthCm << G4endl; } - - const GlobalPoint pos(start[0], start[1], start[2]); - const GlobalVector &bfield = theField->inInverseGeV(pos); - - const double M0x = start[0]; - const double M0y = start[1]; - const double M0z = start[2]; - - const Eigen::Matrix W0 = start.segment<3>(3).normalized(); - const double W0x = W0[0]; - const double W0y = W0[1]; - const double W0z = W0[2]; - - const double Bx = bfield.x(); - const double By = bfield.y(); - const double Bz = bfield.z() + 2.99792458e-3*bzoffset; - - const double q = start[6]; - const double qop0 = q/start.segment<3>(3).norm(); - - const double xc0 = std::pow(Bx, 2) + std::pow(By, 2) + std::pow(Bz, 2); - const double xc1 = qop0*s*std::sqrt(xc0); - const double xc2 = std::sin(xc1); - const double xc3 = std::pow(xc0, 5.0/2.0)*xc2; - const double xc4 = By*W0z - Bz*W0y; - const double xc5 = std::cos(xc1); - const double xc6 = std::pow(xc0, 2)*(xc5 - 1); - const double xc7 = Bx*W0x + By*W0y + Bz*W0z; - const double xc8 = Bx*xc7; - const double xc9 = std::pow(xc0, 3.0/2.0)*(xc1 - xc2); - const double xc10 = std::pow(W0x, 2) + std::pow(W0y, 2); - const double xc11 = std::pow(W0z, 2) + xc10; - const double xc12 = xc11/xc10; - const double xc13 = std::pow(xc0, 3); - const double xc14 = 1.0/xc13; - const double xc15 = xc11*xc14/(qop0*std::pow(xc10, 3.0/2.0)*std::pow(xc12, 3.0/2.0)); - const double xc16 = Bx*W0z - Bz*W0x; - const double xc17 = xc7*xc9; - const double xc18 = Bx*W0y - By*W0x; - const double xc19 = xc13*xc5; - const double xc20 = std::fabs(qop0); - const double xc21 = std::pow(q, 2); - const double xc22 = std::pow(qop0, 2); - const double xc23 = xc14*std::sqrt(std::pow(dEdx, 2)*std::pow(s, 2)*xc20*xc22 + 2*dEdx*s*xc22*std::sqrt(std::pow(mass, 2)*xc22 + xc21) + xc20*xc21)/(std::sqrt(xc10)*std::sqrt(xc12)*std::pow(xc20, 3.0/2.0)); - const double xc24 = xc6*xc7; - const double x = xc15*(W0x*xc3 + xc4*xc6 + xc8*xc9); - const double y = xc15*(By*xc17 + W0y*xc3 - xc16*xc6); - const double z = xc15*(Bz*xc17 + W0z*xc3 + xc18*xc6); - const double px = xc23*(W0x*xc19 - xc3*xc4 - xc6*xc8); - const double py = xc23*(-By*xc24 + W0y*xc19 + xc16*xc3); - const double pz = xc23*(-Bz*xc24 + W0z*xc19 - xc18*xc3); - Eigen::Matrix rescart; - rescart[0] = x; - rescart[1] = y; - rescart[2] = z; - rescart[3] = px; - rescart[4] = py; - rescart[5] = pz; - - - - return rescart; -} +#endif + // * Maximum energy transfer to atomic electron (KeV). + G4double eta = beta * gamma; + G4double etasq = eta * eta; + G4double eMass = 0.51099906 / GeV; + G4double massRatio = eMass / mass; + G4double F1 = 2 * eMass * etasq; + G4double F2 = 1. + 2. * massRatio * gamma + massRatio * massRatio; + G4double Emax = 1.E+6 * F1 / F2; // now in keV -Eigen::Matrix Geant4ePropagator::transportJacobianBz(const FreeTrajectoryState &start, double s, double dEdx, double mass) const { - - if (s==0.) { - Eigen::Matrix res; - res.leftCols<5>() = Eigen::Matrix::Identity(); - res.rightCols<2>() = Eigen::Matrix::Zero(); - return res; - } +// std::cout << "eMass = " << eMass << " mass = " << mass << std::endl; + // * *** and now sigma**2 in GeV +// G4double dedxSq = +// XI * Emax * (1. - (beta * beta / 2.)) * 1.E-12; // now in GeV^2 + /*The above formula for var(1/p) good for dens scatterers. However, for MIPS + passing through a gas it leads to overestimation. Further more for incident + electrons the Emax is almost equal to incident energy. This leads to + k=Xi/Emax as small as e-6 and gradually the cov matrix explodes. + http://www2.pv.infn.it/~rotondi/kalman_1.pdf + Since I do not have enough info at the moment to implement Landau & + sub-Landau models for k=Xi/Emax <0.01 I'll saturate k at this value for now + */ - const GlobalTrajectoryParameters &globalSource = start.parameters(); - const GlobalVector& bfield = start.parameters().magneticFieldInInverseGeV(); - const double M0x = globalSource.position().x(); - const double M0y = globalSource.position().y(); - const double M0z = globalSource.position().z(); - - const double p0 = globalSource.momentum().mag(); - const GlobalVector W0 = globalSource.momentum()/p0; - const double W0x = W0.x(); - const double W0y = W0.y(); - const double W0z = W0.z(); - - const double Bx = bfield.x(); - const double By = bfield.y(); - const double Bz = bfield.z(); +// const double ePre = aTrack->GetStep()->GetPreStepPoint()->GetTotalEnergy() / GeV; +// const double ePost = aTrack->GetStep()->GetPostStepPoint()->GetTotalEnergy() / GeV; +// const double dEdxactual = -(ePost - ePre)/stepLengthCm; +// +// // const double poti = 16.e-9 * 10.75; // = 16 eV * Z**0.9, for Si Z=14 +// // const double eplasma = 28.816e-9 * sqrt(2.33 * 0.498); // 28.816 eV * sqrt(rho*(Z/A)) for Si +// const double poti = 16.e-9 * pow(effZ, 0.9); // = 16 eV * Z**0.9, for Si Z=14 +// const double density = mate->GetDensity() / mg * mole; +// const double eplasma = 28.816e-9 * sqrt(density*effZ/effA); // 28.816 eV * sqrt(rho*(Z/A)) for Si +// // const double delta0 = 2 * log(eplasma / poti) - 1.; +// const double delta0 = 2 * log(eplasma / poti) - 1. + 2.*log(beta*gamma); +// const double beta2 = beta*beta; +// // const double dEdx = XI*1e-6 * (log(2. * eMass * Emax*1e-6 / (poti * poti)) - 2. * (beta2)-delta0)/stepLengthCm; +// const double dEdx = XI*1e-6 * (log(2. * eMass * Emax*1e-6*beta2*gamma*gamma / (poti * poti)) - 2. * (beta2)-delta0)/stepLengthCm; +// +// std::cout << "material = " << mate->GetName() <<" ePre = " << ePre << " dEdx simple = " << dEdx << " dEdx actual = " << dEdxactual << " actual/simple = " << dEdxactual/dEdx << std::endl; +// dedxSq *= 0.; - const double qop0 = globalSource.signedInverseMomentum(); - const double q = start.charge(); - - const double x0 = std::pow(q, 2); - const double x1 = x0/std::pow(qop0, 3); - const double x2 = std::pow(qop0, -2); - const double x3 = x0*x2; - const double x4 = std::sqrt(std::pow(mass, 2) + x3); - const double x5 = std::pow(dEdx, 2); - const double x6 = std::pow(s, 2)*x5; - const double x7 = dEdx*x4; - const double x8 = s*x7; - const double x9 = q/std::pow(x3 + x6 + 2*x8, 3.0/2.0); - const double x10 = std::pow(W0z, 2); - const double x11 = std::pow(W0x, 2); - const double x12 = std::pow(W0y, 2); - const double x13 = x11 + x12; - const double x14 = 1.0/x13; - const double x15 = std::pow(x10*x14 + 1, -1.0/2.0); - const double x16 = std::pow(Bz, 2); - const double x17 = std::pow(Bx, 2) + std::pow(By, 2) + x16; - const double x18 = std::sqrt(x17); - const double x19 = s*x18; - const double x20 = qop0*x19; - const double x21 = std::sin(x20); - const double x22 = std::pow(x17, 5.0/2.0)*x21; - const double x23 = x15*x22; - const double x24 = std::sqrt(x13); - const double x25 = 1.0/x24; - const double x26 = W0z*x25; - const double x27 = Bx*W0y; - const double x28 = By*W0x; - const double x29 = x25*x27 - x25*x28; - const double x30 = std::pow(x17, 2); - const double x31 = std::cos(x20); - const double x32 = x31 - 1; - const double x33 = x30*x32; - const double x34 = x15*x33; - const double x35 = x29*x34; - const double x36 = std::pow(x17, 3); - const double x37 = W0x*x25; - const double x38 = W0z*x37; - const double x39 = W0y*x25; - const double x40 = W0z*x39; - const double x41 = -M0x*x38 - M0y*x40 + M0z*(x11*x25 + x12*x25); - const double x42 = x24*x41; - const double x43 = x36*x42; - const double x44 = x20 - x21; - const double x45 = std::pow(x17, 3.0/2.0); - const double x46 = Bx*x15; - const double x47 = By*x15; - const double x48 = Bz*x15; - const double x49 = x26*x48; - const double x50 = x37*x46 + x39*x47 + x49; - const double x51 = x45*x50; - const double x52 = x44*x51; - const double x53 = Bz*x52 + qop0*x43 + x23*x26 + x35; - const double x54 = 1.0/x36; - const double x55 = x2*x54; - const double x56 = x53*x55; - const double x57 = x31*x36; - const double x58 = x15*x57; - const double x59 = x26*x58; - const double x60 = x23*x29; - const double x61 = -x19*x31 + x19; - const double x62 = Bz*x51; - const double x63 = 1.0/qop0; - const double x64 = x54*x63; - const double x65 = x64*(s*x59 - s*x60 + x43 + x61*x62); - const double x66 = x33*x50; - const double x67 = -Bz*x66 + x59 - x60; - const double x68 = x54*x67; - const double x69 = W0y*x23; - const double x70 = x26*x46; - const double x71 = x37*x48; - const double x72 = x70 - x71; - const double x73 = x24*x33; - const double x74 = -M0x*x39 + M0y*x37; - const double x75 = W0z*x41; - const double x76 = W0x*x74 - W0y*x75; - const double x77 = x36*x76; - const double x78 = By*x24; - const double x79 = qop0*x77 + x52*x78 + x69 - x72*x73; - const double x80 = x25*x55; - const double x81 = s*x58; - const double x82 = x22*x72; - const double x83 = s*x24; - const double x84 = x51*x61; - const double x85 = W0y*x81 + x77 + x78*x84 + x82*x83; - const double x86 = x25*x64; - const double x87 = x39*x58; - const double x88 = -By*x66 + x82 + x87; - const double x89 = x54*x88; - const double x90 = W0x*x23; - const double x91 = x26*x47; - const double x92 = x39*x48; - const double x93 = x91 - x92; - const double x94 = W0x*x75 + W0y*x74; - const double x95 = x36*x94; - const double x96 = Bx*x24; - const double x97 = -qop0*x95 + x52*x96 + x73*x93 + x90; - const double x98 = x22*x93; - const double x99 = W0x*x81 - x83*x98 + x84*x96 - x95; - const double x100 = x37*x58; - const double x101 = -Bx*x66 + x100 - x98; - const double x102 = x101*x54; - const double x103 = -x102*(-x80*x97 + x86*x99) - x68*(-x56 + x65) - x89*(-x79*x80 + x85*x86); - const double x104 = x9*(-s*x5 - x7); - const double x105 = W0z*x14; - const double x106 = W0x*x105; - const double x107 = W0y*x105; - const double x108 = -x106*x46 - x107*x47 + x48; - const double x109 = Bz*x108; - const double x110 = x44*x45; - const double x111 = x109*x110 + x23 - x26*x35; - const double x112 = std::pow(x17, -6); - const double x113 = x112*x63; - const double x114 = x113*x67; - const double x115 = x107*x48 + x47; - const double x116 = x108*x110; - const double x117 = x115*x73 + x116*x96 - x23*x38; - const double x118 = x113*x25; - const double x119 = x101*x118; - const double x120 = x106*x48 + x46; - const double x121 = x116*x78 - x120*x73 - x23*x40; - const double x122 = x118*x88; - const double x123 = -x111*x114 - x117*x119 - x121*x122; - const double x124 = Bx*W0x; - const double x125 = By*W0y; - const double x126 = x124*x25 + x125*x25; - const double x127 = x37*x47 - x39*x46; - const double x128 = Bz*x127; - const double x129 = x110*x128 + x126*x34; - const double x130 = x33*x48; - const double x131 = x110*x127; - const double x132 = -W0y*x130 + x131*x78 + x90; - const double x133 = -W0x*x130 + x131*x96 - x69; - const double x134 = -x114*x129 - x119*x133 - x122*x132; - const double x135 = x102*x39 - x37*x89; - const double x136 = x102*x38 - x24*x68 + x40*x89; - const double x137 = 6*Bz; - const double x138 = x137/std::pow(x17, 4); - const double x139 = x138*x63; - const double x140 = x139*x53; - const double x141 = x21*x45; - const double x142 = 5*x141; - const double x143 = x30*x31; - const double x144 = qop0*s; - const double x145 = x143*x144; - const double x146 = x17*x32; - const double x147 = 4*x146; - const double x148 = x29*x48; - const double x149 = x144*x148; - const double x150 = Bz*qop0; - const double x151 = 6*x30; - const double x152 = x150*x151; - const double x153 = x16*x50; - const double x154 = 3*x18*x44; - const double x155 = s*x150; - const double x156 = x155/x18; - const double x157 = -x156*x31 + x156; - const double x158 = x64*(x110*x49 - x141*x149 + x142*x49 + x145*x49 + x147*x148 + x152*x42 + x153*x154 + x157*x62 + x52); - const double x159 = x139*x25; - const double x160 = x142*x48; - const double x161 = x145*x48; - const double x162 = W0z*x110; - const double x163 = Bz*x72; - const double x164 = x147*x24; - const double x165 = x155*x72; - const double x166 = x141*x24; - const double x167 = Bz*x50; - const double x168 = x154*x167; - const double x169 = x157*x51; - const double x170 = W0x*x34 + W0y*x160 + W0y*x161 + x152*x76 + x162*x47 - x163*x164 + x165*x166 + x168*x78 + x169*x78; - const double x171 = Bz*x93; - const double x172 = x155*x93; - const double x173 = W0x*x160 + W0x*x161 - W0y*x34 - x152*x94 + x162*x46 + x164*x171 - x166*x172 + x168*x96 + x169*x96; - const double x174 = -x102*(-x159*x97 + x173*x86) - x68*(-x140 + x158) - x89*(-x159*x79 + x170*x86); - const double x175 = std::pow(x88, 2); - const double x176 = std::pow(x101, 2); - const double x177 = x112*x175 + x112*x176; - const double x178 = 1.0/x177; - const double x179 = x112*x178; - const double x180 = 1.0/(x179*std::pow(x67, 2) + 1); - const double x181 = s*x21; - const double x182 = x15*std::pow(x17, 7.0/2.0); - const double x183 = x182*x26; - const double x184 = x22*x50; - const double x185 = s*x184; - const double x186 = std::pow(x177, -1.0/2.0); - const double x187 = x186*x54; - const double x188 = x181*x182; - const double x189 = x188*x39; - const double x190 = s*x57; - const double x191 = x190*x72; - const double x192 = By*x185; - const double x193 = (1.0/2.0)*x112; - const double x194 = x193*x88; - const double x195 = x188*x37; - const double x196 = x190*x93; - const double x197 = Bx*x185; - const double x198 = x101*x193; - const double x199 = x68/std::pow(x177, 3.0/2.0); - const double x200 = qop0*x21; - const double x201 = qop0*x58; - const double x202 = x182*x200; - const double x203 = x202*x39; - const double x204 = qop0*x57; - const double x205 = x204*x72; - const double x206 = qop0*x184; - const double x207 = By*x206; - const double x208 = x202*x37; - const double x209 = x204*x93; - const double x210 = Bx*x206; - const double x211 = x180*(x187*(x150*x184 - x183*x200 - x201*x29) + x199*(-x194*(-2*x203 + 2*x205 + 2*x207) - x198*(-2*x208 - 2*x209 + 2*x210))); - const double x212 = x107*x58; - const double x213 = x120*x22; - const double x214 = x108*x33; - const double x215 = By*x214; - const double x216 = x106*x58; - const double x217 = x115*x22; - const double x218 = Bx*x214; - const double x219 = x22*x92; - const double x220 = 2*x219; - const double x221 = x127*x33; - const double x222 = By*x221; - const double x223 = x22*x71; - const double x224 = 2*x223; - const double x225 = Bx*x221; - const double x226 = x151*x31; - const double x227 = qop0*x181; - const double x228 = x137/std::pow(x17, 7); - const double x229 = x23*x37; - const double x230 = 12*x143; - const double x231 = x33*x91; - const double x232 = 10*x141; - const double x233 = x143*x165; - const double x234 = By*x167; - const double x235 = 8*x146; - const double x236 = x227*x62; - const double x237 = By*x236; - const double x238 = x23*x39; - const double x239 = x33*x70; - const double x240 = x143*x172; - const double x241 = Bx*x167; - const double x242 = Bx*x236; - const double x243 = x101*x179; - const double x244 = x179*x88; - const double x245 = x243*(-x203 + x205 + x207) - x244*(-x208 - x209 + x210); - const double x246 = x33*(Bz*W0z + x124 + x125); - const double x247 = -By*x246 + W0y*x57 + x22*(Bx*W0z - Bz*W0x); - const double x248 = x10 + x13; - const double x249 = 1.0/x248; - const double x250 = x112*x249; - const double x251 = std::pow(x247, 2)*x250; - const double x252 = -Bx*x246 + W0x*x57 - x22*(By*W0z - Bz*W0y); - const double x253 = x250*std::pow(x252, 2); - const double x254 = std::pow(x251 + x253, -1.0/2.0); - const double x255 = x247*x254; - const double x256 = x2*x97; - const double x257 = std::pow(x14*x248, -1.0/2.0); - const double x258 = x14*x257; - const double x259 = x112*x258; - const double x260 = x252*x254; - const double x261 = x2*x79; - const double x262 = x113*x258; - const double x263 = x260*x262; - const double x264 = x255*x262; - const double x265 = qop0*x24; - const double x266 = qop0*x18; - const double x267 = -x266*x31 + x266; - const double x268 = x267*x51; - const double x269 = W0y*x201 + x265*x82 + x268*x78; - const double x270 = W0x*x201 - x265*x98 + x268*x96; - const double x271 = x263*x269 - x264*x270; - const double x272 = x258*x54; - const double x273 = x257*x54; - const double x274 = x228*x258; - const double x275 = x255*x63; - const double x276 = x260*x63; - const double x277 = -Bz*x246 + W0z*x57 - x22*(x27 - x28); - const double x278 = x249*x25*x277/std::pow(x17, 9); - const double x279 = x255*x278; - const double x280 = x279*x63; - const double x281 = x276*x278; - const double x282 = x251*x254 + x253*x254; - const double x283 = x282*x64; - const double x284 = -x269*x280 - x270*x281 + x283*(qop0*x59 - qop0*x60 + x267*x62); - const double x285 = x250*x277; - const double x286 = x255*x285; - const double x287 = x260*x285; - const double x288 = x137*x249*x25*x277/std::pow(x17, 10); - const double dqopdqop0 = x103*x104 + x9*(dEdx*s*x1/x4 + x1); - const double dqopdlam0 = x104*x123; - const double dqopdphi0 = x104*x134; - const double dqopdxt0 = x104*x135; - const double dqopdyt0 = x104*x136; - const double dqopdBz = x104*x174; - const double dqopdxi = x9*(-x6 - x8); - const double dlamdqop0 = x103*x211 + x180*(x187*(Bz*x185 - x181*x183 - x29*x81) + x199*(-x194*(-2*x189 + 2*x191 + 2*x192) - x198*(-2*x195 - 2*x196 + 2*x197))); - const double dlamdlam0 = x123*x211 + x180*(x187*(-x109*x33 + x26*x60 + x58) + x199*(-x194*(-2*x212 + 2*x213 - 2*x215) - x198*(-2*x216 - 2*x217 - 2*x218))); - const double dlamdphi0 = x134*x211 + x180*(x187*(-x126*x23 - x128*x33) + x199*(-x194*(2*x100 + x220 - 2*x222) - x198*(x224 - 2*x225 - 2*x87))); - const double dlamdxt0 = x135*x211; - const double dlamdyt0 = x136*x211; - const double dlamdBz = x174*x211 + x180*(-x138*x186*x67 + x187*(-x142*x148 - x143*x149 - x144*x22*x49 - x147*x153 + x16*x227*x51 + x226*x49 - x33*x49 - x66) + x199*(x175*x228 + x176*x228 - x194*(-x144*x220 + x163*x232 - 2*x229 + x230*x92 - 2*x231 + 2*x233 - x234*x235 + 2*x237) - x198*(-x144*x224 - x171*x232 + x230*x71 - x235*x241 + 2*x238 - 2*x239 - 2*x240 + 2*x242))); - const double dlamdxi = 0; - const double dphidqop0 = x103*x245 + x243*(-x189 + x191 + x192) - x244*(-x195 - x196 + x197); - const double dphidlam0 = x123*x245 + x243*(-x212 + x213 - x215) - x244*(-x216 - x217 - x218); - const double dphidphi0 = x134*x245 + x243*(x100 + x219 - x222) - x244*(x223 - x225 - x87); - const double dphidxt0 = x135*x245; - const double dphidyt0 = x136*x245; - const double dphidBz = x102*x178*(-x138*x88 + x54*(x142*x163 - x144*x219 - x147*x234 + x226*x92 - x229 - x231 + x233 + x237)) + x174*x245 - x178*x89*(-x101*x138 + x54*(-x142*x171 - x144*x223 - x147*x241 + x226*x71 + x238 - x239 - x240 + x242)); - const double dphidxi = 0; - const double dxtdqop0 = x103*x271 + x255*x256*x259 - x259*x260*x261 + x263*x85 - x264*x99; - const double dxtdlam0 = -x117*x264 + x121*x263 + x123*x271; - const double dxtdphi0 = x132*x263 - x133*x264 + x134*x271; - const double dxtdxt0 = W0x*x260*x272 + W0y*x255*x272 + x135*x271; - const double dxtdyt0 = x106*x255*x273 - x107*x260*x273 + x136*x271; - const double dxtdBz = x170*x263 - x173*x264 + x174*x271 + x274*x275*x97 - x274*x276*x79; - const double dxtdxi = 0; - const double dytdqop0 = x103*x284 + x256*x260*x278 + x261*x279 - x280*x85 - x281*x99 - x282*x56 + x282*x65; - const double dytdlam0 = x111*x283 - x117*x281 - x121*x280 + x123*x284; - const double dytdphi0 = x129*x283 - x132*x280 - x133*x281 + x134*x284; - const double dytdxt0 = x135*x284 - x286*x37 + x287*x39; - const double dytdyt0 = x136*x284 + x24*x282 + x286*x40 + x287*x38; - const double dytdBz = -x140*x282 + x158*x282 - x170*x280 - x173*x281 + x174*x284 + x275*x288*x79 + x276*x288*x97; - const double dytdxi = 0; - Eigen::Matrix res; - res(0,0) = dqopdqop0; - res(0,1) = dqopdlam0; - res(0,2) = dqopdphi0; - res(0,3) = dqopdxt0; - res(0,4) = dqopdyt0; - res(0,5) = dqopdBz; - res(0,6) = dqopdxi; - res(1,0) = dlamdqop0; - res(1,1) = dlamdlam0; - res(1,2) = dlamdphi0; - res(1,3) = dlamdxt0; - res(1,4) = dlamdyt0; - res(1,5) = dlamdBz; - res(1,6) = dlamdxi; - res(2,0) = dphidqop0; - res(2,1) = dphidlam0; - res(2,2) = dphidphi0; - res(2,3) = dphidxt0; - res(2,4) = dphidyt0; - res(2,5) = dphidBz; - res(2,6) = dphidxi; - res(3,0) = dxtdqop0; - res(3,1) = dxtdlam0; - res(3,2) = dxtdphi0; - res(3,3) = dxtdxt0; - res(3,4) = dxtdyt0; - res(3,5) = dxtdBz; - res(3,6) = dxtdxi; - res(4,0) = dytdqop0; - res(4,1) = dytdlam0; - res(4,2) = dytdphi0; - res(4,3) = dytdxt0; - res(4,4) = dytdyt0; - res(4,5) = dytdBz; - res(4,6) = dytdxi; + // Implementation based on PANDA Report PV/01-07 Section 7 + // TODO understand implications for thick scatterer split into small steps - res.col(5) *= 2.99792458e-3; +#if 0 + G4double dedxSq; - return res; + const double kappa = XI/Emax; +// std::cout << "steplength = " << stepLengthCm << " xi = " << XI*1e-6 << " xi/dx = " << XI*1e-6/stepLengthCm << " emax = " << Emax*1e-6 << " kappa = " << kappa << " effZ = " << effZ << std::endl; + + if (kappa > 0.005) { + //vavilov distribution (or gaussian limit which is equivalent for the variance) + + dedxSq = XI * Emax * (1. - (beta * beta / 2.)) * 1.E-12; // now in GeV^2 + + // std::cout << "vavilov: dedxSq = " << dedxSq << std::endl; + } + else { + + + const double I = 16.*pow(effZ,0.9); + const double f2 = effZ <= 2. ? 0. : 2./effZ; + const double f1 = 1. - f2; + const double e2 = 10.*effZ*effZ; + const double e1 = pow(I/pow(e2,f2),1./f1); + const double r = 0.4; +// const double emaxev = Emax*1e6; + const double emaxev = Emax*1e3; // keV -> eV + const double massev = mass*1e9; + + const double ePre = aTrack->GetStep()->GetPreStepPoint()->GetTotalEnergy() / GeV; + const double ePost = aTrack->GetStep()->GetPostStepPoint()->GetTotalEnergy() / GeV; + const double C = (ePre-ePost)/stepLengthCm*1e9; + + const double sigma1 = C*f1*(log(2.*massev*beta*beta*gamma*gamma/e1) - beta*beta)/e1/(log(2.*massev*beta*beta*gamma*gamma/I) - beta*beta)*(1.-r); + + const double sigma2 = C*f1*(log(2.*massev*beta*beta*gamma*gamma/e2) - beta*beta)/e2/(log(2.*massev*beta*beta*gamma*gamma/I) - beta*beta)*(1.-r); + + const double sigma3 = C*emaxev/I/(emaxev+I)/log((emaxev+I)/I)*r; + + const double Nc = (sigma1 + sigma2 + sigma3)*stepLengthCm; + +// std::cout << "Nc = " << Nc << std::endl; + + // if (Nc > 50.) { + if (false) { + //landau + // constexpr double sigalpha = 15.76; //corresponds to 0.996 quantile +// constexpr double sigalpha = 22.33; //corresponds to 0.998 quantile + constexpr double sigalpha = 31.59; //corresponds to 0.999 quantile + + dedxSq = sigalpha*sigalpha*XI*XI*1e-12; + + const double dedxsqvavilov = XI * Emax * (1. - (beta * beta / 2.)) * 1.E-12; + + // std::cout << "dedxsq: vavilov = " << dedxsqvavilov << " landau = " << dedxSq << std::endl; +// std::cout << "landau\n"; + } + else { + //sub-landau + // const double alpha = 0.996; +// const double alpha = 0.998; + // const double alpha = 0.999; + const double alpha = 0.9999; + // const double alpha = 1.; + const double ealpha = I/(1. - alpha*emaxev/(emaxev + I)); + const double e3 = I*(emaxev +I)*log(ealpha/I)/emaxev; + const double e3sq = I*(emaxev + I)*(ealpha - I)/emaxev; + const double sigmae3sq = e3sq - e3*e3; + + dedxSq = sigma1*stepLengthCm*e1*e1 + sigma2*stepLengthCm*e2*e2 + sigma3*stepLengthCm*e3*e3 + sigma3*stepLengthCm*sigmae3sq*(sigma3*stepLengthCm + 1.); + dedxSq *= 1e-18; + + const double dedxsqlandau = 15.76*15.76*XI*XI*1e-12; + const double dedxsqvavilov = XI * Emax * (1. - (beta * beta / 2.)) * 1.E-12; +// + // std::cout << "dedxsq: vavilov = " << dedxsqvavilov << " landau = " << dedxsqlandau << " sublandau = " << dedxSq << " Emax = " << Emax << " ealpha = " << ealpha << std::endl;; +// std::cout << "sublandau\n"; + + } + + + } +#endif + + G4double Emaxmev = Emax*1e-3; + G4double stepLengthmm = stepLengthCm*10; + const double ePre = aTrack->GetStep()->GetPreStepPoint()->GetKineticEnergy() / GeV; + const double ePost = aTrack->GetStep()->GetPostStepPoint()->GetKineticEnergy() / GeV; + G4double eav = (ePre-ePost)*1e3; +// G4double epremev = ePre*1e3; + G4double ekinmev = 0.5*(ePre + ePost)*1e3; + +// std::cout << "eav = " << eav << std::endl; + G4double dedxsqurban = 1e-6*fluct->SampleFluctuations(mate, aTrack->GetDynamicParticle(), Emaxmev, stepLengthmm, ekinmev); +// G4double dispurban = fluct->Dispersion(mate, aTrack->GetDynamicParticle(), Emaxmev, stepLengthmm); +// G4double dispurbansq = 1e-6*dispurban*dispurban; -} + // const unsigned int nsamples = 100; + // + // std::vector elossv; + // elossv.reserve(nsamples); + // for (unsigned int isample = 0; isample < nsamples; ++isample) { + // double eloss = 1e-3*fluct->SampleFluctuations2(mate, aTrack->GetDynamicParticle(), Emaxmev, stepLengthmm, ekinmev); + // + // elossv.push_back(eloss); + // } + // + // std::sort(elossv.begin(), elossv.end()); + // + // unsigned int imode = 0.24672310*nsamples; + // unsigned int imed = nsamples/2; + // + // const double mode = elossv[imode]; + // const double med = elossv[imed]; + // + // constexpr double k = -0.22278; + // constexpr double km = 1.35578; + // + // const double csampled = 0.5*M_PI*(med-mode)/(km-k); + + // std::cout << "mode = " << mode << " med = " << med << " csampled = " << csampled << std::endl; -Eigen::Matrix Geant4ePropagator::transportJacobianBzD(const Eigen::Matrix &start, double s, double dEdx, double mass, double dBz) const { - if (s==0.) { - Eigen::Matrix res; - res.leftCols<5>() = Eigen::Matrix::Identity(); - res.rightCols<2>() = Eigen::Matrix::Zero(); - return res; - } + +// std::cout << "dedxSq = " << dedxSq << " dedxsqurban = " << dedxsqurban << std::endl; - const GlobalPoint pos(start[0], start[1], start[2]); - const GlobalVector &bfield = theField->inInverseGeV(pos); + G4double dedxsqvavilov = + XI * Emax * (1. - (beta * beta / 2.)) * 1.E-12; // now in GeV^2 + - const double Bx = bfield.x(); - const double By = bfield.y(); - const double Bz = bfield.z() + 2.99792458e-3*dBz; + G4double dedxsqvavilovtruncated = + XI * std::min(Emax, 1e6) * (1. - (beta * beta / 2.)) * 1.E-12; // now in GeV^2 + + const double dedxsqfixed = XI*1e-6; + + // constexpr double sigalpha = 15.76; //corresponds to 0.996 quantile +// constexpr double sigalpha = 22.33; //corresponds to 0.998 quantile + constexpr double sigalpha = 31.59; //corresponds to 0.999 quantile - const double M0x = start[0]; - const double M0y = start[1]; - const double M0z = start[2]; + const double dedxSqlandau = sigalpha*sigalpha*XI*XI*1e-12; - const Eigen::Matrix W0 = start.segment<3>(3).normalized(); - const double W0x = W0[0]; - const double W0y = W0[1]; - const double W0z = W0[2]; + // std::cout << "material = " << mate->GetName() << " stepLengthCm = " << stepLengthCm << " urban/vavilov = " << dedxsqurban/dedxsqvavilov << std::endl; - const double q = start[6]; - const double qop0 = q/start.segment<3>(3).norm(); - - const double x0 = std::pow(q, 2); - const double x1 = x0/std::pow(qop0, 3); - const double x2 = std::pow(qop0, -2); - const double x3 = x0*x2; - const double x4 = std::sqrt(std::pow(mass, 2) + x3); - const double x5 = std::pow(dEdx, 2); - const double x6 = std::pow(s, 2)*x5; - const double x7 = dEdx*x4; - const double x8 = s*x7; - const double x9 = q/std::pow(x3 + x6 + 2*x8, 3.0/2.0); - const double x10 = std::pow(W0z, 2); - const double x11 = std::pow(W0x, 2); - const double x12 = std::pow(W0y, 2); - const double x13 = x11 + x12; - const double x14 = 1.0/x13; - const double x15 = std::pow(x10*x14 + 1, -1.0/2.0); - const double x16 = std::pow(Bz, 2); - const double x17 = std::pow(Bx, 2) + std::pow(By, 2) + x16; - const double x18 = std::sqrt(x17); - const double x19 = s*x18; - const double x20 = qop0*x19; - const double x21 = std::sin(x20); - const double x22 = std::pow(x17, 5.0/2.0)*x21; - const double x23 = x15*x22; - const double x24 = std::sqrt(x13); - const double x25 = 1.0/x24; - const double x26 = W0z*x25; - const double x27 = Bx*W0y; - const double x28 = By*W0x; - const double x29 = x25*x27 - x25*x28; - const double x30 = std::pow(x17, 2); - const double x31 = std::cos(x20); - const double x32 = x31 - 1; - const double x33 = x30*x32; - const double x34 = x15*x33; - const double x35 = x29*x34; - const double x36 = std::pow(x17, 3); - const double x37 = W0x*x25; - const double x38 = W0z*x37; - const double x39 = W0y*x25; - const double x40 = W0z*x39; - const double x41 = -M0x*x38 - M0y*x40 + M0z*(x11*x25 + x12*x25); - const double x42 = x24*x41; - const double x43 = x36*x42; - const double x44 = x20 - x21; - const double x45 = std::pow(x17, 3.0/2.0); - const double x46 = Bx*x15; - const double x47 = By*x15; - const double x48 = Bz*x15; - const double x49 = x26*x48; - const double x50 = x37*x46 + x39*x47 + x49; - const double x51 = x45*x50; - const double x52 = x44*x51; - const double x53 = Bz*x52 + qop0*x43 + x23*x26 + x35; - const double x54 = 1.0/x36; - const double x55 = x2*x54; - const double x56 = x53*x55; - const double x57 = x31*x36; - const double x58 = x15*x57; - const double x59 = x26*x58; - const double x60 = x23*x29; - const double x61 = -x19*x31 + x19; - const double x62 = Bz*x51; - const double x63 = 1.0/qop0; - const double x64 = x54*x63; - const double x65 = x64*(s*x59 - s*x60 + x43 + x61*x62); - const double x66 = x33*x50; - const double x67 = -Bz*x66 + x59 - x60; - const double x68 = x54*x67; - const double x69 = W0y*x23; - const double x70 = x26*x46; - const double x71 = x37*x48; - const double x72 = x70 - x71; - const double x73 = x24*x33; - const double x74 = -M0x*x39 + M0y*x37; - const double x75 = W0z*x41; - const double x76 = W0x*x74 - W0y*x75; - const double x77 = x36*x76; - const double x78 = By*x24; - const double x79 = qop0*x77 + x52*x78 + x69 - x72*x73; - const double x80 = x25*x55; - const double x81 = s*x58; - const double x82 = x22*x72; - const double x83 = s*x24; - const double x84 = x51*x61; - const double x85 = W0y*x81 + x77 + x78*x84 + x82*x83; - const double x86 = x25*x64; - const double x87 = x39*x58; - const double x88 = -By*x66 + x82 + x87; - const double x89 = x54*x88; - const double x90 = W0x*x23; - const double x91 = x26*x47; - const double x92 = x39*x48; - const double x93 = x91 - x92; - const double x94 = W0x*x75 + W0y*x74; - const double x95 = x36*x94; - const double x96 = Bx*x24; - const double x97 = -qop0*x95 + x52*x96 + x73*x93 + x90; - const double x98 = x22*x93; - const double x99 = W0x*x81 - x83*x98 + x84*x96 - x95; - const double x100 = x37*x58; - const double x101 = -Bx*x66 + x100 - x98; - const double x102 = x101*x54; - const double x103 = -x102*(-x80*x97 + x86*x99) - x68*(-x56 + x65) - x89*(-x79*x80 + x85*x86); - const double x104 = x9*(-s*x5 - x7); - const double x105 = W0z*x14; - const double x106 = W0x*x105; - const double x107 = W0y*x105; - const double x108 = -x106*x46 - x107*x47 + x48; - const double x109 = Bz*x108; - const double x110 = x44*x45; - const double x111 = x109*x110 + x23 - x26*x35; - const double x112 = std::pow(x17, -6); - const double x113 = x112*x63; - const double x114 = x113*x67; - const double x115 = x107*x48 + x47; - const double x116 = x108*x110; - const double x117 = x115*x73 + x116*x96 - x23*x38; - const double x118 = x113*x25; - const double x119 = x101*x118; - const double x120 = x106*x48 + x46; - const double x121 = x116*x78 - x120*x73 - x23*x40; - const double x122 = x118*x88; - const double x123 = -x111*x114 - x117*x119 - x121*x122; - const double x124 = Bx*W0x; - const double x125 = By*W0y; - const double x126 = x124*x25 + x125*x25; - const double x127 = x37*x47 - x39*x46; - const double x128 = Bz*x127; - const double x129 = x110*x128 + x126*x34; - const double x130 = x33*x48; - const double x131 = x110*x127; - const double x132 = -W0y*x130 + x131*x78 + x90; - const double x133 = -W0x*x130 + x131*x96 - x69; - const double x134 = -x114*x129 - x119*x133 - x122*x132; - const double x135 = x102*x39 - x37*x89; - const double x136 = x102*x38 - x24*x68 + x40*x89; - const double x137 = 6*Bz; - const double x138 = x137/std::pow(x17, 4); - const double x139 = x138*x63; - const double x140 = x139*x53; - const double x141 = x21*x45; - const double x142 = 5*x141; - const double x143 = x30*x31; - const double x144 = qop0*s; - const double x145 = x143*x144; - const double x146 = x17*x32; - const double x147 = 4*x146; - const double x148 = x29*x48; - const double x149 = x144*x148; - const double x150 = Bz*qop0; - const double x151 = 6*x30; - const double x152 = x150*x151; - const double x153 = x16*x50; - const double x154 = 3*x18*x44; - const double x155 = s*x150; - const double x156 = x155/x18; - const double x157 = -x156*x31 + x156; - const double x158 = x64*(x110*x49 - x141*x149 + x142*x49 + x145*x49 + x147*x148 + x152*x42 + x153*x154 + x157*x62 + x52); - const double x159 = x139*x25; - const double x160 = x142*x48; - const double x161 = x145*x48; - const double x162 = W0z*x110; - const double x163 = Bz*x72; - const double x164 = x147*x24; - const double x165 = x155*x72; - const double x166 = x141*x24; - const double x167 = Bz*x50; - const double x168 = x154*x167; - const double x169 = x157*x51; - const double x170 = W0x*x34 + W0y*x160 + W0y*x161 + x152*x76 + x162*x47 - x163*x164 + x165*x166 + x168*x78 + x169*x78; - const double x171 = Bz*x93; - const double x172 = x155*x93; - const double x173 = W0x*x160 + W0x*x161 - W0y*x34 - x152*x94 + x162*x46 + x164*x171 - x166*x172 + x168*x96 + x169*x96; - const double x174 = -x102*(-x159*x97 + x173*x86) - x68*(-x140 + x158) - x89*(-x159*x79 + x170*x86); - const double x175 = std::pow(x88, 2); - const double x176 = std::pow(x101, 2); - const double x177 = x112*x175 + x112*x176; - const double x178 = 1.0/x177; - const double x179 = x112*x178; - const double x180 = 1.0/(x179*std::pow(x67, 2) + 1); - const double x181 = s*x21; - const double x182 = x15*std::pow(x17, 7.0/2.0); - const double x183 = x182*x26; - const double x184 = x22*x50; - const double x185 = s*x184; - const double x186 = std::pow(x177, -1.0/2.0); - const double x187 = x186*x54; - const double x188 = x181*x182; - const double x189 = x188*x39; - const double x190 = s*x57; - const double x191 = x190*x72; - const double x192 = By*x185; - const double x193 = (1.0/2.0)*x112; - const double x194 = x193*x88; - const double x195 = x188*x37; - const double x196 = x190*x93; - const double x197 = Bx*x185; - const double x198 = x101*x193; - const double x199 = x68/std::pow(x177, 3.0/2.0); - const double x200 = qop0*x21; - const double x201 = qop0*x58; - const double x202 = x182*x200; - const double x203 = x202*x39; - const double x204 = qop0*x57; - const double x205 = x204*x72; - const double x206 = qop0*x184; - const double x207 = By*x206; - const double x208 = x202*x37; - const double x209 = x204*x93; - const double x210 = Bx*x206; - const double x211 = x180*(x187*(x150*x184 - x183*x200 - x201*x29) + x199*(-x194*(-2*x203 + 2*x205 + 2*x207) - x198*(-2*x208 - 2*x209 + 2*x210))); - const double x212 = x107*x58; - const double x213 = x120*x22; - const double x214 = x108*x33; - const double x215 = By*x214; - const double x216 = x106*x58; - const double x217 = x115*x22; - const double x218 = Bx*x214; - const double x219 = x22*x92; - const double x220 = 2*x219; - const double x221 = x127*x33; - const double x222 = By*x221; - const double x223 = x22*x71; - const double x224 = 2*x223; - const double x225 = Bx*x221; - const double x226 = x151*x31; - const double x227 = qop0*x181; - const double x228 = x137/std::pow(x17, 7); - const double x229 = x23*x37; - const double x230 = 12*x143; - const double x231 = x33*x91; - const double x232 = 10*x141; - const double x233 = x143*x165; - const double x234 = By*x167; - const double x235 = 8*x146; - const double x236 = x227*x62; - const double x237 = By*x236; - const double x238 = x23*x39; - const double x239 = x33*x70; - const double x240 = x143*x172; - const double x241 = Bx*x167; - const double x242 = Bx*x236; - const double x243 = x101*x179; - const double x244 = x179*x88; - const double x245 = x243*(-x203 + x205 + x207) - x244*(-x208 - x209 + x210); - const double x246 = x33*(Bz*W0z + x124 + x125); - const double x247 = -By*x246 + W0y*x57 + x22*(Bx*W0z - Bz*W0x); - const double x248 = x10 + x13; - const double x249 = 1.0/x248; - const double x250 = x112*x249; - const double x251 = std::pow(x247, 2)*x250; - const double x252 = -Bx*x246 + W0x*x57 - x22*(By*W0z - Bz*W0y); - const double x253 = x250*std::pow(x252, 2); - const double x254 = std::pow(x251 + x253, -1.0/2.0); - const double x255 = x247*x254; - const double x256 = x2*x97; - const double x257 = std::pow(x14*x248, -1.0/2.0); - const double x258 = x14*x257; - const double x259 = x112*x258; - const double x260 = x252*x254; - const double x261 = x2*x79; - const double x262 = x113*x258; - const double x263 = x260*x262; - const double x264 = x255*x262; - const double x265 = qop0*x24; - const double x266 = qop0*x18; - const double x267 = -x266*x31 + x266; - const double x268 = x267*x51; - const double x269 = W0y*x201 + x265*x82 + x268*x78; - const double x270 = W0x*x201 - x265*x98 + x268*x96; - const double x271 = x263*x269 - x264*x270; - const double x272 = x258*x54; - const double x273 = x257*x54; - const double x274 = x228*x258; - const double x275 = x255*x63; - const double x276 = x260*x63; - const double x277 = -Bz*x246 + W0z*x57 - x22*(x27 - x28); - const double x278 = x249*x25*x277/std::pow(x17, 9); - const double x279 = x255*x278; - const double x280 = x279*x63; - const double x281 = x276*x278; - const double x282 = x251*x254 + x253*x254; - const double x283 = x282*x64; - const double x284 = -x269*x280 - x270*x281 + x283*(qop0*x59 - qop0*x60 + x267*x62); - const double x285 = x250*x277; - const double x286 = x255*x285; - const double x287 = x260*x285; - const double x288 = x137*x249*x25*x277/std::pow(x17, 10); - const double dqopdqop0 = x103*x104 + x9*(dEdx*s*x1/x4 + x1); - const double dqopdlam0 = x104*x123; - const double dqopdphi0 = x104*x134; - const double dqopdxt0 = x104*x135; - const double dqopdyt0 = x104*x136; - const double dqopdBz = x104*x174; - const double dqopdxi = x9*(-x6 - x8); - const double dlamdqop0 = x103*x211 + x180*(x187*(Bz*x185 - x181*x183 - x29*x81) + x199*(-x194*(-2*x189 + 2*x191 + 2*x192) - x198*(-2*x195 - 2*x196 + 2*x197))); - const double dlamdlam0 = x123*x211 + x180*(x187*(-x109*x33 + x26*x60 + x58) + x199*(-x194*(-2*x212 + 2*x213 - 2*x215) - x198*(-2*x216 - 2*x217 - 2*x218))); - const double dlamdphi0 = x134*x211 + x180*(x187*(-x126*x23 - x128*x33) + x199*(-x194*(2*x100 + x220 - 2*x222) - x198*(x224 - 2*x225 - 2*x87))); - const double dlamdxt0 = x135*x211; - const double dlamdyt0 = x136*x211; - const double dlamdBz = x174*x211 + x180*(-x138*x186*x67 + x187*(-x142*x148 - x143*x149 - x144*x22*x49 - x147*x153 + x16*x227*x51 + x226*x49 - x33*x49 - x66) + x199*(x175*x228 + x176*x228 - x194*(-x144*x220 + x163*x232 - 2*x229 + x230*x92 - 2*x231 + 2*x233 - x234*x235 + 2*x237) - x198*(-x144*x224 - x171*x232 + x230*x71 - x235*x241 + 2*x238 - 2*x239 - 2*x240 + 2*x242))); - const double dlamdxi = 0; - const double dphidqop0 = x103*x245 + x243*(-x189 + x191 + x192) - x244*(-x195 - x196 + x197); - const double dphidlam0 = x123*x245 + x243*(-x212 + x213 - x215) - x244*(-x216 - x217 - x218); - const double dphidphi0 = x134*x245 + x243*(x100 + x219 - x222) - x244*(x223 - x225 - x87); - const double dphidxt0 = x135*x245; - const double dphidyt0 = x136*x245; - const double dphidBz = x102*x178*(-x138*x88 + x54*(x142*x163 - x144*x219 - x147*x234 + x226*x92 - x229 - x231 + x233 + x237)) + x174*x245 - x178*x89*(-x101*x138 + x54*(-x142*x171 - x144*x223 - x147*x241 + x226*x71 + x238 - x239 - x240 + x242)); - const double dphidxi = 0; - const double dxtdqop0 = x103*x271 + x255*x256*x259 - x259*x260*x261 + x263*x85 - x264*x99; - const double dxtdlam0 = -x117*x264 + x121*x263 + x123*x271; - const double dxtdphi0 = x132*x263 - x133*x264 + x134*x271; - const double dxtdxt0 = W0x*x260*x272 + W0y*x255*x272 + x135*x271; - const double dxtdyt0 = x106*x255*x273 - x107*x260*x273 + x136*x271; - const double dxtdBz = x170*x263 - x173*x264 + x174*x271 + x274*x275*x97 - x274*x276*x79; - const double dxtdxi = 0; - const double dytdqop0 = x103*x284 + x256*x260*x278 + x261*x279 - x280*x85 - x281*x99 - x282*x56 + x282*x65; - const double dytdlam0 = x111*x283 - x117*x281 - x121*x280 + x123*x284; - const double dytdphi0 = x129*x283 - x132*x280 - x133*x281 + x134*x284; - const double dytdxt0 = x135*x284 - x286*x37 + x287*x39; - const double dytdyt0 = x136*x284 + x24*x282 + x286*x40 + x287*x38; - const double dytdBz = -x140*x282 + x158*x282 - x170*x280 - x173*x281 + x174*x284 + x275*x288*x79 + x276*x288*x97; - const double dytdxi = 0; - Eigen::Matrix res; - res(0,0) = dqopdqop0; - res(0,1) = dqopdlam0; - res(0,2) = dqopdphi0; - res(0,3) = dqopdxt0; - res(0,4) = dqopdyt0; - res(0,5) = dqopdBz; - res(0,6) = dqopdxi; - res(1,0) = dlamdqop0; - res(1,1) = dlamdlam0; - res(1,2) = dlamdphi0; - res(1,3) = dlamdxt0; - res(1,4) = dlamdyt0; - res(1,5) = dlamdBz; - res(1,6) = dlamdxi; - res(2,0) = dphidqop0; - res(2,1) = dphidlam0; - res(2,2) = dphidphi0; - res(2,3) = dphidxt0; - res(2,4) = dphidyt0; - res(2,5) = dphidBz; - res(2,6) = dphidxi; - res(3,0) = dxtdqop0; - res(3,1) = dxtdlam0; - res(3,2) = dxtdphi0; - res(3,3) = dxtdxt0; - res(3,4) = dxtdyt0; - res(3,5) = dxtdBz; - res(3,6) = dxtdxi; - res(4,0) = dytdqop0; - res(4,1) = dytdlam0; - res(4,2) = dytdphi0; - res(4,3) = dytdxt0; - res(4,4) = dytdyt0; - res(4,5) = dytdBz; - res(4,6) = dytdxi; + G4double dedxSq = dedxsqurban; +// G4double dedxSq = 0.01*dedxsqvavilov; + // G4double dedxSq = dedxsqvavilov; + // G4double dedxSq = dedxsqvavilovtruncated; + // G4double dedxSq = dedxsqfixed; +// G4double dedxSq = dedxSqlandau; - res.col(5) *= 2.99792458e-3; +// std::cout << "dedxsqurban = " << dedxsqurban << " dedxsqvavilov = " << dedxsqvavilov << " dedxsqvavilovtruncated = " << dedxsqvavilovtruncated << std::endl;; + + +// dedxSq = 1.7*1.7*XI*XI*1e-12; +// dedxSq = 15.76*15.76*XI*XI*1e-12; //corresponds to 0.996 quantile + +// if (true) { +// if(XI / Emax < 0.01) +// dedxSq *= +// XI / Emax * 100; // Quench for low Elos, see above: newVar=odVar *k/0.01 +// } +// std::cout << "Geant4ePropagator xi = " << XI << " emax = " << Emax << " dedxSq = " << dedxSq << std::endl; + +#ifdef G4EVERBOSE + if(iverbose >= 2) + G4cout << "G4EP:IONI: DEDX^2(GeV^2) " << dedxSq << " emass/GeV: " << eMass + << " Emax/keV: " << Emax << " k=Xi/Emax=" << XI / Emax << G4endl; + +#endif + + Etot = aTrack->GetTotalEnergy() / GeV; + + G4double pPre6 = + (aTrack->GetStep()->GetPreStepPoint()->GetMomentum() / GeV).mag(); + pPre6 = std::pow(pPre6, 6); + // Apply it to error + const double res = Etot * Etot * dedxSq / pPre6; +#ifdef G4EVERBOSE + if(iverbose >= 2) + G4cout << "G4:IONI Etot/GeV: " << Etot << " err_dedx^2/GeV^2: " << dedxSq + << " p^6: " << pPre6 << G4endl; + if(iverbose >= 2) + G4cout << "G4EP:IONI: error2_from_ionisation " + << (Etot * Etot * dedxSq) / pPre6 << G4endl; +#endif + return res; +} - +//------------------------------------------------------------------------ +void Geant4ePropagator::CalculateEffectiveZandA(const G4Material* mate, + G4double& effZ, + G4double& effA) +{ + effZ = 0.; + effA = 0.; + G4int ii, nelem = mate->GetNumberOfElements(); + const G4double* fracVec = mate->GetFractionVector(); + for(ii = 0; ii < nelem; ii++) + { + effZ += mate->GetElement(ii)->GetZ() * fracVec[ii]; + effA += mate->GetElement(ii)->GetA() * fracVec[ii] / g * mole; + } } -Eigen::Matrix Geant4ePropagator::transportJacobianD(const Eigen::Matrix &start, double s, double dEdx, double mass) const { +Eigen::Matrix Geant4ePropagator::transportJacobianBzD(const Eigen::Matrix &start, double s, double dEdx, double mass, double dBz) const { if (s==0.) { Eigen::Matrix res; @@ -3675,12 +1899,9 @@ Eigen::Matrix Geant4ePropagator::transportJacobianD(const Eigen::M const GlobalPoint pos(start[0], start[1], start[2]); const GlobalVector &bfield = theField->inInverseGeV(pos); - const Eigen::Matrix Bv(bfield.x(), bfield.y(), bfield.z()); - const double B = Bv.norm(); - const Eigen::Matrix Hv = Bv.normalized(); - const double hx = Hv[0]; - const double hy = Hv[1]; - const double hz = Hv[2]; + const double Bx = bfield.x(); + const double By = bfield.y(); + const double Bz = bfield.z() + 2.99792458e-3*dBz; const double M0x = start[0]; const double M0y = start[1]; @@ -3693,780 +1914,323 @@ Eigen::Matrix Geant4ePropagator::transportJacobianD(const Eigen::M const double q = start[6]; const double qop0 = q/start.segment<3>(3).norm(); - - const double x0 = std::pow(q, 2); - const double x1 = x0/std::pow(qop0, 3); - const double x2 = std::pow(qop0, -2); - const double x3 = x0*x2; - const double x4 = std::sqrt(std::pow(mass, 2) + x3); - const double x5 = std::pow(dEdx, 2); - const double x6 = std::pow(s, 2)*x5; - const double x7 = dEdx*x4; - const double x8 = s*x7; - const double x9 = q/std::pow(x3 + x6 + 2*x8, 3.0/2.0); - const double x10 = B*s; - const double x11 = qop0*x10; - const double x12 = std::cos(x11); - const double x13 = std::pow(W0z, 2); - const double x14 = std::pow(W0x, 2); - const double x15 = std::pow(W0y, 2); - const double x16 = x14 + x15; - const double x17 = 1.0/x16; - const double x18 = std::pow(x13*x17 + 1, -1.0/2.0); - const double x19 = x12*x18; - const double x20 = std::sqrt(x16); - const double x21 = 1.0/x20; - const double x22 = W0z*x21; - const double x23 = x19*x22; - const double x24 = W0y*hx; - const double x25 = x21*x24; - const double x26 = W0x*hy; - const double x27 = x21*x26; - const double x28 = x25 - x27; - const double x29 = std::sin(x11); - const double x30 = x18*x29; - const double x31 = x28*x30; - const double x32 = x12 - 1; - const double x33 = hx*x18; - const double x34 = W0x*x21; - const double x35 = hy*x18; - const double x36 = W0y*x21; - const double x37 = hz*x18; - const double x38 = x22*x37 + x33*x34 + x35*x36; - const double x39 = hz*x38; - const double x40 = x23 - x31 - x32*x39; - const double x41 = x22*x30; - const double x42 = x18*x32; - const double x43 = x28*x42; - const double x44 = B*qop0; - const double x45 = W0z*x34; - const double x46 = W0z*x36; - const double x47 = -M0x*x45 - M0y*x46 + M0z*(x14*x21 + x15*x21); - const double x48 = x20*x47; - const double x49 = x11 - x29; - const double x50 = x39*x49 + x41 + x43 + x44*x48; - const double x51 = 1.0/B; - const double x52 = x2*x51; - const double x53 = x50*x52; - const double x54 = x10*x12; - const double x55 = x10 - x54; - const double x56 = 1.0/qop0; - const double x57 = x51*x56; - const double x58 = x57*(B*x48 + x10*x23 - x10*x31 + x39*x55); - const double x59 = x19*x36; - const double x60 = x34*x37; - const double x61 = x22*x33 - x60; - const double x62 = x29*x61; - const double x63 = hy*x38; - const double x64 = x32*x63; - const double x65 = x59 + x62 - x64; - const double x66 = -M0x*x36 + M0y*x34; - const double x67 = W0z*x47; - const double x68 = W0x*x66 - W0y*x67; - const double x69 = B*x68; - const double x70 = x30*x36; - const double x71 = qop0*x69 + x20*(-x32*x61 + x49*x63 + x70); - const double x72 = x21*x52; - const double x73 = x71*x72; - const double x74 = x20*(x10*x59 + x10*x62 + x55*x63) + x69; - const double x75 = x21*x57; - const double x76 = x74*x75; - const double x77 = x19*x34; - const double x78 = x22*x35; - const double x79 = x36*x37; - const double x80 = x78 - x79; - const double x81 = x29*x80; - const double x82 = hx*x38; - const double x83 = -x32*x82 + x77 - x81; - const double x84 = W0x*x67 + W0y*x66; - const double x85 = B*x84; - const double x86 = x30*x34; - const double x87 = -qop0*x85 + x20*(x32*x80 + x49*x82 + x86); - const double x88 = x72*x87; - const double x89 = x20*(x10*x77 - x10*x81 + x55*x82) - x85; - const double x90 = x75*x89; - const double x91 = -x40*(-x53 + x58) - x65*(-x73 + x76) - x83*(-x88 + x90); - const double x92 = x9*(-s*x5 - x7); - const double x93 = W0z*x17; - const double x94 = W0x*x93; - const double x95 = W0y*x93; - const double x96 = -x33*x94 - x35*x95 + x37; - const double x97 = x49*x96; - const double x98 = hz*x97 - x22*x43 + x30; - const double x99 = x40*x57; - const double x100 = x37*x95; - const double x101 = hx*x97 - x30*x94 + x32*(x100 + x35); - const double x102 = x57*x83; - const double x103 = 1 - x12; - const double x104 = x33 + x37*x94; - const double x105 = hy*x97 + x103*x104 - x30*x95; - const double x106 = x57*x65; - const double x107 = -x101*x102 - x105*x106 - x98*x99; - const double x108 = W0x*hx; - const double x109 = x108*x21; - const double x110 = W0y*hy; - const double x111 = x110*x21; - const double x112 = -x33*x36 + x34*x35; - const double x113 = x112*x49; - const double x114 = hz*x113 + x42*(x109 + x111); - const double x115 = hy*x113 + x103*x79 + x86; - const double x116 = hx*x113 - x32*x60 - x70; - const double x117 = -x102*x116 - x106*x115 - x114*x99; - const double x118 = -x34*x65 + x36*x83; - const double x119 = -x20*x40 + x45*x83 + x46*x65; - const double x120 = x56/std::pow(B, 2); - const double x121 = x120*x50; - const double x122 = qop0*s; - const double x123 = x12*x122; - const double x124 = x122 - x123; - const double x125 = x57*(qop0*x48 + x122*x23 - x122*x31 + x124*x39); - const double x126 = x120*x21; - const double x127 = x126*x71; - const double x128 = qop0*x68 + x20*(x122*x59 + x122*x62 + x124*x63); - const double x129 = x128*x75; - const double x130 = x126*x87; - const double x131 = -qop0*x84 + x20*(x122*x77 - x122*x81 + x124*x82); - const double x132 = x131*x75; - const double x133 = -x40*(-x121 + x125) - x65*(-x127 + x129) - x83*(-x130 + x132); - const double x134 = std::pow(x65, 2) + std::pow(x83, 2); - const double x135 = 1.0/x134; - const double x136 = 1.0/(x135*std::pow(x40, 2) + 1); - const double x137 = -x25 + x27; - const double x138 = x137*x19; - const double x139 = x29*x39; - const double x140 = std::pow(x134, -1.0/2.0); - const double x141 = x10*x70; - const double x142 = x54*x61; - const double x143 = x10*x29; - const double x144 = x143*x63; - const double x145 = (1.0/2.0)*x65; - const double x146 = x10*x86; - const double x147 = -x78 + x79; - const double x148 = x147*x54; - const double x149 = x143*x82; - const double x150 = (1.0/2.0)*x83; - const double x151 = x40/std::pow(x134, 3.0/2.0); - const double x152 = x44*x70; - const double x153 = x12*x44; - const double x154 = x153*x61; - const double x155 = x29*x44; - const double x156 = x155*x63; - const double x157 = x44*x86; - const double x158 = x147*x153; - const double x159 = x155*x82; - const double x160 = x136*(x140*(x138*x44 + x139*x44 - x41*x44) + x151*(-x145*(-2*x152 + 2*x154 + 2*x156) - x150*(-2*x157 + 2*x158 + 2*x159))); - const double x161 = hz*x32; - const double x162 = x19*x95; - const double x163 = x104*x29; - const double x164 = x32*x96; - const double x165 = hy*x164; - const double x166 = x19*x94; - const double x167 = x29*(-x100 - x35); - const double x168 = hx*x164; - const double x169 = W0y*hz; - const double x170 = x21*x30; - const double x171 = x169*x170; - const double x172 = x112*x32; - const double x173 = hy*x172; - const double x174 = W0x*hz; - const double x175 = x170*x174; - const double x176 = hx*x172; - const double x177 = x122*x70; - const double x178 = x123*x61; - const double x179 = x122*x29; - const double x180 = x179*x63; - const double x181 = x122*x86; - const double x182 = x123*x147; - const double x183 = x179*x82; - const double x184 = x135*x83; - const double x185 = -x59; - const double x186 = x135*(x185 - x62 + x64); - const double x187 = x184*(-x152 + x154 + x156) + x186*(-x157 + x158 + x159); - const double x188 = W0z*hz + x108 + x110; - const double x189 = x188*x32; - const double x190 = W0y*x12 - hy*x189 + x29*(W0z*hx - x174); - const double x191 = x13 + x16; - const double x192 = 1.0/x191; - const double x193 = W0x*x12 - hx*x189 + x29*(-W0z*hy + x169); - const double x194 = x192*std::pow(x193, 2); - const double x195 = std::pow(x190, 2)*x192; - const double x196 = std::pow(x194 + x195, -1.0/2.0); - const double x197 = x196/std::sqrt(x17*x191); - const double x198 = x17*x197; - const double x199 = x190*x198; - const double x200 = x199*x87; - const double x201 = x193*x198; - const double x202 = x201*x71; - const double x203 = x201*x57; - const double x204 = x199*x57; - const double x205 = -x153 + x44; - const double x206 = x205*x63 + x44*x59 + x44*x62; - const double x207 = x193*x197; - const double x208 = x207*x75; - const double x209 = x205*x82 + x44*x77 - x44*x81; - const double x210 = x190*x197; - const double x211 = x210*x75; - const double x212 = x206*x208 - x209*x211; - const double x213 = x192*x196*(W0z*x12 - x161*x188 + x29*(-x24 + x26)); - const double x214 = x193*x213; - const double x215 = x190*x213; - const double x216 = x194*x196 + x195*x196; - const double x217 = x215*x57; - const double x218 = x214*x57; - const double x219 = x216*x57; - const double x220 = -x206*x217 - x209*x218 + x219*(x205*x39 + x23*x44 - x31*x44); - const double dqopdqop0 = x9*(dEdx*s*x1/x4 + x1) + x91*x92; - const double dqopdlam0 = x107*x92; - const double dqopdphi0 = x117*x92; - const double dqopdxt0 = x118*x92; - const double dqopdyt0 = x119*x92; - const double dqopdB = x133*x92; - const double dqopdxi = x9*(-x6 - x8); - const double dlamdqop0 = x136*(x140*(x10*x138 + x10*x139 - x10*x41) + x151*(-x145*(-2*x141 + 2*x142 + 2*x144) - x150*(-2*x146 + 2*x148 + 2*x149))) + x160*x91; - const double dlamdlam0 = x107*x160 + x136*(x140*(-x137*x41 - x161*x96 + x19) + x151*(-x145*(-2*x162 + 2*x163 - 2*x165) - x150*(-2*x166 + 2*x167 - 2*x168))); - const double dlamdphi0 = x117*x160 + x136*(x140*(-x112*x161 + x30*(-x109 - x111)) + x151*(-x145*(2*x171 - 2*x173 + 2*x77) - x150*(2*x175 - 2*x176 - 2*x59))); - const double dlamdxt0 = x118*x160; - const double dlamdyt0 = x119*x160; - const double dlamdB = x133*x160 + x136*(x140*(x122*x138 + x122*x139 - x122*x41) + x151*(-x145*(-2*x177 + 2*x178 + 2*x180) - x150*(-2*x181 + 2*x182 + 2*x183))); - const double dlamdxi = 0; - const double dphidqop0 = x184*(-x141 + x142 + x144) + x186*(-x146 + x148 + x149) + x187*x91; - const double dphidlam0 = x107*x187 + x184*(-x162 + x163 - x165) + x186*(-x166 + x167 - x168); - const double dphidphi0 = x117*x187 + x184*(x171 - x173 + x77) + x186*(x175 - x176 + x185); - const double dphidxt0 = x118*x187; - const double dphidyt0 = x119*x187; - const double dphidB = x133*x187 + x184*(-x177 + x178 + x180) + x186*(-x181 + x182 + x183); - const double dphidxi = 0; - const double dxtdqop0 = x200*x52 - x202*x52 + x203*x74 - x204*x89 + x212*x91; - const double dxtdlam0 = -x101*x211 + x105*x208 + x107*x212; - const double dxtdphi0 = x115*x208 - x116*x211 + x117*x212; - const double dxtdxt0 = W0x*x201 + W0y*x199 + x118*x212; - const double dxtdyt0 = x119*x212 - x207*x95 + x210*x94; - const double dxtdB = x120*x200 - x120*x202 + x128*x203 - x131*x204 + x133*x212; - const double dxtdxi = 0; - const double dytdqop0 = x214*x88 - x214*x90 + x215*x73 - x215*x76 - x216*x53 + x216*x58 + x220*x91; - const double dytdlam0 = -x101*x218 - x105*x217 + x107*x220 + x219*x98; - const double dytdphi0 = x114*x219 - x115*x217 - x116*x218 + x117*x220; - const double dytdxt0 = x118*x220 + x214*x36 - x215*x34; - const double dytdyt0 = x119*x220 + x20*x216 + x214*x45 + x215*x46; - const double dytdB = -x121*x216 + x125*x216 + x127*x215 - x129*x215 + x130*x214 - x132*x214 + x133*x220; - const double dytdxi = 0; - Eigen::Matrix res; - res(0,0) = dqopdqop0; - res(0,1) = dqopdlam0; - res(0,2) = dqopdphi0; - res(0,3) = dqopdxt0; - res(0,4) = dqopdyt0; - res(0,5) = dqopdB; - res(0,6) = dqopdxi; - res(1,0) = dlamdqop0; - res(1,1) = dlamdlam0; - res(1,2) = dlamdphi0; - res(1,3) = dlamdxt0; - res(1,4) = dlamdyt0; - res(1,5) = dlamdB; - res(1,6) = dlamdxi; - res(2,0) = dphidqop0; - res(2,1) = dphidlam0; - res(2,2) = dphidphi0; - res(2,3) = dphidxt0; - res(2,4) = dphidyt0; - res(2,5) = dphidB; - res(2,6) = dphidxi; - res(3,0) = dxtdqop0; - res(3,1) = dxtdlam0; - res(3,2) = dxtdphi0; - res(3,3) = dxtdxt0; - res(3,4) = dxtdyt0; - res(3,5) = dxtdB; - res(3,6) = dxtdxi; - res(4,0) = dytdqop0; - res(4,1) = dytdlam0; - res(4,2) = dytdphi0; - res(4,3) = dytdxt0; - res(4,4) = dytdyt0; - res(4,5) = dytdB; - res(4,6) = dytdxi; - - res.col(5) *= 2.99792458e-3; - - return res; - - -} - -Eigen::Matrix Geant4ePropagator::transportJacobianBzAdvanced(const FreeTrajectoryState &start, double s, double dEdx, double mass) const { - - using namespace Eigen; - - if (s==0.) { - Eigen::Matrix res; - res.leftCols<5>() = Eigen::Matrix::Identity(); - res.rightCols<2>() = Eigen::Matrix::Zero(); - return res; - } - - const GlobalTrajectoryParameters &globalSource = start.parameters(); - const GlobalVector& bfield = start.parameters().magneticFieldInInverseGeV(); - - const double M0x = globalSource.position().x(); - const double M0y = globalSource.position().y(); - const double M0z = globalSource.position().z(); - - const double p0 = globalSource.momentum().mag(); - const GlobalVector W0 = globalSource.momentum()/p0; - const double W0x = W0.x(); - const double W0y = W0.y(); - const double W0z = W0.z(); - - const double Bx = bfield.x(); - const double By = bfield.y(); - const double Bz = bfield.z(); - - //compute gradient of magnetic field perpendicular to track - - const Matrix curv2cartjac = curv2cartJacobianAlt(start); - - const double dx = 10e-4; - - Matrix dxt0 = Matrix::Zero(); - dxt0[3] = dx; - - Matrix dyt0 = Matrix::Zero(); - dyt0[4] = dx; - - const Vector3d dxt0cart = (curv2cartjac*dxt0).head<3>(); - const Vector3d dyt0cart = (curv2cartjac*dyt0).head<3>(); - - const GlobalPoint xt0pos(M0x + dxt0cart[0], M0y + dxt0cart[1], M0z + dxt0cart[2]); - const GlobalPoint yt0pos(M0x + dyt0cart[0], M0y + dyt0cart[1], M0z + dyt0cart[2]); - - - - const GlobalVector Bxt0 = start.parameters().magneticField().inInverseGeV(xt0pos); - const GlobalVector Byt0 = start.parameters().magneticField().inInverseGeV(yt0pos); - - const GlobalVector dBdxt0 = (bfield - Bxt0)/dx; - const GlobalVector dBdyt0 = (bfield - Byt0)/dx; - - std::cout << "dBdxt0" << std::endl; - std::cout << dBdxt0 << std::endl; - std::cout << "dBdyt0" << std::endl; - std::cout << dBdyt0 << std::endl; - - const GlobalPoint spos(M0x + W0x*dx, M0y + W0y*dx, M0z + W0z*dx); - const GlobalVector Bs = start.parameters().magneticField().inInverseGeV(spos); - const GlobalVector dBds = (bfield - Bs)/dx; - std::cout << "dBds" << std::endl; - std::cout << dBds << std::endl; - - const double dBdxt0x = dBdxt0.x(); - const double dBdxt0y = dBdxt0.y(); - const double dBdxt0z = dBdxt0.z(); - - const double dBdyt0x = dBdyt0.x(); - const double dBdyt0y = dBdyt0.y(); - const double dBdyt0z = dBdyt0.z(); - - const double qop0 = globalSource.signedInverseMomentum(); - const double q = start.charge(); - - const double x0 = std::pow(q, 2); - const double x1 = x0/std::pow(qop0, 3); - const double x2 = std::pow(qop0, 2); - const double x3 = 1.0/x2; - const double x4 = x0*x3; - const double x5 = std::sqrt(std::pow(mass, 2) + x4); - const double x6 = std::pow(dEdx, 2); - const double x7 = std::pow(s, 2)*x6; - const double x8 = dEdx*x5; - const double x9 = s*x8; - const double x10 = q/std::pow(x4 + x7 + 2*x9, 3.0/2.0); - const double x11 = std::pow(W0x, 2); - const double x12 = std::pow(W0y, 2); - const double x13 = x11 + x12; - const double x14 = std::pow(x13, -1.0/2.0); - const double x15 = W0x*x14; - const double x16 = std::pow(Bz, 2); - const double x17 = std::pow(Bx, 2) + std::pow(By, 2) + x16; - const double x18 = std::sqrt(x17); - const double x19 = qop0*x18; - const double x20 = s*x19; - const double x21 = std::cos(x20); - const double x22 = std::pow(W0z, 2); - const double x23 = 1.0/x13; - const double x24 = std::pow(x22*x23 + 1, -1.0/2.0); - const double x25 = x21*x24; - const double x26 = x15*x25; - const double x27 = std::sin(x20); - const double x28 = By*W0z; - const double x29 = 1.0/x18; - const double x30 = x24*x29; - const double x31 = x14*x30; - const double x32 = x28*x31; - const double x33 = W0y*x14; - const double x34 = Bz*x30; - const double x35 = x33*x34; - const double x36 = x32 - x35; - const double x37 = x27*x36; - const double x38 = x19*x21; - const double x39 = -x19 + x38; - const double x40 = 1.0/qop0; - const double x41 = 1.0/x17; - const double x42 = Bz*W0z; - const double x43 = Bx*x30; - const double x44 = x15*x43; - const double x45 = By*x30; - const double x46 = x33*x45; - const double x47 = x44 + x46; - const double x48 = x31*x42 + x47; + const double x0 = std::pow(mass, 2); + const double x1 = std::pow(qop0, -2); + const double x2 = std::sqrt(std::pow(q, 2)*x1 + x0); + const double x3 = dEdx*s; + const double x4 = x2 + x3; + const double x5 = std::pow(-x0 + std::pow(x4, 2), -3.0/2.0); + const double x6 = std::pow(W0z, 2); + const double x7 = std::pow(W0x, 2); + const double x8 = std::pow(W0y, 2); + const double x9 = x7 + x8; + const double x10 = 1.0/x9; + const double x11 = std::pow(x10*x6 + 1, -1.0/2.0); + const double x12 = std::pow(Bz, 2); + const double x13 = std::pow(Bx, 2) + std::pow(By, 2) + x12; + const double x14 = std::pow(x13, 5.0/2.0); + const double x15 = std::sqrt(x13); + const double x16 = s*x15; + const double x17 = qop0*x16; + const double x18 = std::sin(x17); + const double x19 = x14*x18; + const double x20 = x11*x19; + const double x21 = std::sqrt(x9); + const double x22 = 1.0/x21; + const double x23 = W0z*x22; + const double x24 = Bx*W0y; + const double x25 = By*W0x; + const double x26 = x22*x24 - x22*x25; + const double x27 = std::pow(x13, 2); + const double x28 = std::cos(x17); + const double x29 = x28 - 1; + const double x30 = x27*x29; + const double x31 = x11*x30; + const double x32 = x26*x31; + const double x33 = std::pow(x13, 3); + const double x34 = M0x*W0x; + const double x35 = M0y*W0y; + const double x36 = M0z*W0z + x34 + x35; + const double x37 = M0z*(x22*x7 + x22*x8) - x23*x34 - x23*x35; + const double x38 = W0z*x36 + x21*x37; + const double x39 = x33*x38; + const double x40 = x17 - x18; + const double x41 = std::pow(x13, 3.0/2.0); + const double x42 = Bx*x11; + const double x43 = W0x*x22; + const double x44 = By*x11; + const double x45 = W0y*x22; + const double x46 = Bz*x11; + const double x47 = x23*x46; + const double x48 = x42*x43 + x44*x45 + x47; const double x49 = x41*x48; const double x50 = x40*x49; - const double x51 = x39*x50; - const double x52 = -Bx*x51 + x26 - x37; - const double x53 = s*x40; - const double x54 = x27*x30; - const double x55 = x3*x54; - const double x56 = 1 - x21; - const double x57 = x29*x56; - const double x58 = x3*x57; - const double x59 = -x20 + x27; - const double x60 = x3*x49; - const double x61 = x59*x60; - const double x62 = s*x18; - const double x63 = x21*x62; - const double x64 = x50*(-x62 + x63); - const double x65 = Bx*x61 - Bx*x64 - x15*x55 + x26*x53 + x36*x58 - x37*x53; - const double x66 = x25*x33; - const double x67 = x15*x34; - const double x68 = Bx*W0z; - const double x69 = x31*x68; - const double x70 = x67 - x69; - const double x71 = x27*x70; - const double x72 = By*x51; - const double x73 = x66 - x71 - x72; - const double x74 = By*x61 - By*x64 - x33*x55 + x53*x66 - x53*x71 + x58*x70; - const double x75 = W0z*x14; - const double x76 = x25*x75; - const double x77 = x33*x43; - const double x78 = x15*x45; - const double x79 = x77 - x78; - const double x80 = x27*x79; - const double x81 = -Bz*x51 + x76 - x80; - const double x82 = Bz*x59; - const double x83 = -Bz*x64 + x53*x76 - x53*x80 - x55*x75 + x58*x79 + x60*x82; - const double x84 = -x52*x65 - x73*x74 - x81*x83; - const double x85 = x10*(-s*x6 - x8); - const double x86 = W0x*x23; - const double x87 = x40*x54; - const double x88 = W0z*x87; - const double x89 = Bz*W0y; - const double x90 = W0z*x23*x30; - const double x91 = x89*x90; - const double x92 = x40*x57; - const double x93 = Bx*W0x; - const double x94 = W0y*x23; - const double x95 = x28*x30; - const double x96 = x40*x41; - const double x97 = x96*(x34 - x90*x93 - x94*x95); - const double x98 = x59*x97; - const double x99 = -Bx*x98 - x86*x88 - x92*(x45 + x91); - const double x100 = x86*x95; - const double x101 = x30*x68*x94; - const double x102 = -x82*x97 + x87 - x92*(x100 - x101); - const double x103 = x30*x42*x86; - const double x104 = -By*x98 - x88*x94 - x92*(-x103 - x43); - const double x105 = -x102*x81 - x104*x73 - x52*x99; - const double x106 = -x77 + x78; - const double x107 = x106*x96; - const double x108 = -x107*x82 - x47*x92; - const double x109 = Bz*x24*x56*x96; - const double x110 = x107*x59; - const double x111 = -By*x110 + x109*x33 + x15*x87; - const double x112 = -Bx*x110 + x109*x15 - x33*x87; - const double x113 = -x108*x81 - x111*x73 - x112*x52; - const double x114 = Bx*dBdxt0x; - const double x115 = By*dBdxt0y; - const double x116 = Bz*dBdxt0z; - const double x117 = x114 + x115 + x116; - const double x118 = s*x117; - const double x119 = x118*x41; - const double x120 = std::pow(x17, 3.0/2.0); - const double x121 = 1.0/x120; - const double x122 = x121*(-x114 - x115 - x116); - const double x123 = x122*x24; - const double x124 = x27*x40; - const double x125 = x124*x75; - const double x126 = x40*x56; - const double x127 = x122*x126; - const double x128 = x50*x59; - const double x129 = std::pow(x17, 2); - const double x130 = 1.0/x129; - const double x131 = x130*x40*x48; - const double x132 = x131*(-2*x114 - 2*x115 - 2*x116); - const double x133 = qop0*x29; - const double x134 = x117*x133; - const double x135 = s*x134; - const double x136 = x135*x21; - const double x137 = x50*(-x135 + x136); - const double x138 = x30*x33; - const double x139 = dBdxt0x*x138; - const double x140 = x15*x30; - const double x141 = dBdxt0y*x140; - const double x142 = Bx*x33; - const double x143 = x123*x142; - const double x144 = x123*x15; - const double x145 = By*x144; - const double x146 = x30*x75; - const double x147 = x123*x33; - const double x148 = x123*x14; - const double x149 = x96*(Bx*x144 + By*x147 + dBdxt0x*x140 + dBdxt0y*x138 + dBdxt0z*x146 + x148*x42); - const double x150 = -Bz*x137 - dBdxt0z*x128 + x119*x76 - x119*x80 + x123*x125 - x127*x79 - x132*x82 - x149*x82 - x92*(x139 - x141 + x143 - x145); - const double x151 = x132*x59; - const double x152 = dBdxt0z*x140; - const double x153 = dBdxt0x*x146; - const double x154 = Bz*x144; - const double x155 = x148*x68; - const double x156 = x149*x59; - const double x157 = -By*x137 - By*x151 - By*x156 - dBdxt0y*x128 + x119*x66 - x119*x71 + x124*x147 - x127*x70 + x15 - x92*(x152 - x153 + x154 - x155); - const double x158 = dBdxt0y*x146; - const double x159 = dBdxt0z*x138; - const double x160 = x148*x28; - const double x161 = Bz*x147; - const double x162 = -Bx*x137 - Bx*x151 - Bx*x156 - dBdxt0x*x128 + x119*x26 - x119*x37 + x124*x144 - x127*x36 - x33 - x92*(x158 - x159 + x160 - x161); - const double x163 = -x150*x81 - x157*x73 - x162*x52; - const double x164 = Bx*dBdyt0x; - const double x165 = By*dBdyt0y; - const double x166 = Bz*dBdyt0z; - const double x167 = x164 + x165 + x166; - const double x168 = s*x41; - const double x169 = x167*x168; - const double x170 = -x164 - x165 - x166; - const double x171 = x121*x24; - const double x172 = x170*x171; - const double x173 = x15*x172; - const double x174 = x121*x126; - const double x175 = x170*x174; - const double x176 = x131*(-2*x164 - 2*x165 - 2*x166); - const double x177 = x176*x59; - const double x178 = x133*x167; - const double x179 = s*x178; - const double x180 = x179*x21; - const double x181 = x50*(-x179 + x180); - const double x182 = dBdyt0y*x146; - const double x183 = dBdyt0z*x138; - const double x184 = x14*x172; - const double x185 = x184*x28; - const double x186 = x172*x33; - const double x187 = Bz*x186; - const double x188 = x96*(Bx*x173 + By*x186 + dBdyt0x*x140 + dBdyt0y*x138 + dBdyt0z*x146 + x184*x42); - const double x189 = x188*x59; - const double x190 = -Bx*x177 - Bx*x181 - Bx*x189 - W0z*x15 - dBdyt0x*x128 + x124*x173 + x169*x26 - x169*x37 - x175*x36 - x92*(x182 - x183 + x185 - x187); - const double x191 = dBdyt0z*x140; - const double x192 = dBdyt0x*x146; - const double x193 = Bz*x173; - const double x194 = x184*x68; - const double x195 = -By*x177 - By*x181 - By*x189 - W0z*x33 - dBdyt0y*x128 + x124*x186 + x169*x66 - x169*x71 - x175*x70 - x92*(x191 - x192 + x193 - x194); - const double x196 = dBdyt0x*x138; - const double x197 = dBdyt0y*x140; - const double x198 = x142*x172; - const double x199 = By*x173; - const double x200 = -Bz*x181 - dBdyt0z*x128 + x11*x14 + x12*x14 + x125*x172 + x169*x76 - x169*x80 - x175*x79 - x176*x82 - x188*x82 - x92*(x196 - x197 + x198 - x199); - const double x201 = -x190*x52 - x195*x73 - x200*x81; - const double x202 = Bz*x168; - const double x203 = Bz*x171; - const double x204 = x203*x33; - const double x205 = Bz*x174; - const double x206 = x14*x42; - const double x207 = x171*x206; - const double x208 = Bx*x207; - const double x209 = x16*x171; - const double x210 = x15*x209; - const double x211 = 2*x131; - const double x212 = x211*x82; - const double x213 = Bz*x133; - const double x214 = s*x213; - const double x215 = x21*x214; - const double x216 = x50*(-x214 + x215); - const double x217 = x15*x203; - const double x218 = x96*(-Bx*x217 - By*x204 + x146 - x209*x75); - const double x219 = x218*x59; - const double x220 = By*x212 - By*x216 - By*x219 - x124*x204 + x202*x66 - x202*x71 + x205*x70 - x92*(x140 + x208 - x210); - const double x221 = x209*x33; - const double x222 = x14*x203*x28; - const double x223 = Bx*x212 - Bx*x216 - Bx*x219 - x124*x217 + x202*x26 - x202*x37 + x205*x36 - x92*(-x138 + x221 - x222); - const double x224 = By*x217; - const double x225 = x142*x203; - const double x226 = x16*x211; - const double x227 = -Bz*x216 - x124*x207 - x128 + x168*x206*x25 - x202*x80 + x205*x79 - x218*x82 + x226*x59 - x92*(x224 - x225); - const double x228 = -x220*x73 - x223*x52 - x227*x81; - const double x229 = std::pow(x52, 2) + std::pow(x73, 2); - const double x230 = 1.0/x229; - const double x231 = 1.0/(x230*std::pow(x81, 2) + 1); - const double x232 = x24*x27; - const double x233 = x232*x75; - const double x234 = Bz*x39; - const double x235 = qop0*x27; - const double x236 = s*x235; - const double x237 = -x17*x236 + x18*x21 - x18; - const double x238 = Bz*x50; - const double x239 = std::pow(x229, -1.0/2.0); - const double x240 = x232*x62; - const double x241 = x15*x240; - const double x242 = -x32 + x35; - const double x243 = x242*x63; - const double x244 = x39*x60; - const double x245 = Bx*x244; - const double x246 = x237*x50; - const double x247 = Bx*x246; - const double x248 = (1.0/2.0)*x52; - const double x249 = x240*x33; - const double x250 = -x67 + x69; - const double x251 = x250*x63; - const double x252 = By*x244; - const double x253 = By*x246; - const double x254 = (1.0/2.0)*x73; - const double x255 = x81/std::pow(x229, 3.0/2.0); - const double x256 = x235*x48; - const double x257 = x19*x232; - const double x258 = x15*x257; - const double x259 = x242*x38; - const double x260 = Bx*x256; - const double x261 = x257*x33; - const double x262 = x250*x38; - const double x263 = By*x256; - const double x264 = x231*(x239*(Bz*x256 + x106*x38 - x19*x233) + x255*(-x248*(-2*x258 + 2*x259 + 2*x260) - x254*(-2*x261 + 2*x262 + 2*x263))); - const double x265 = W0z*x25; - const double x266 = x265*x94; - const double x267 = x27*(x103 + x43); - const double x268 = x39*x97; - const double x269 = By*x268; - const double x270 = x265*x86; - const double x271 = x27*(-x45 - x91); - const double x272 = Bx*x268; - const double x273 = x27*x67; - const double x274 = x107*x39; - const double x275 = Bx*x274; - const double x276 = x27*x35; - const double x277 = By*x274; - const double x278 = qop0*x54; - const double x279 = x278*x75; - const double x280 = x2*x27; - const double x281 = -x118*x280 + x134*x21 - x134; - const double x282 = x118*x278; - const double x283 = x15*x282; - const double x284 = x136*x242; - const double x285 = dBdxt0x*x51; - const double x286 = x132*x39; - const double x287 = Bx*x286; - const double x288 = x27*(-x158 + x159 - x160 + x161); - const double x289 = x281*x50; - const double x290 = Bx*x289; - const double x291 = x149*x39; - const double x292 = Bx*x291; - const double x293 = x282*x33; - const double x294 = x136*x250; - const double x295 = dBdxt0y*x51; - const double x296 = By*x286; - const double x297 = x27*(-x152 + x153 - x154 + x155); - const double x298 = By*x289; - const double x299 = By*x291; - const double x300 = s*x167; - const double x301 = s*x280; - const double x302 = -x167*x301 + x178*x21 - x178; - const double x303 = x278*x300; - const double x304 = x15*x303; - const double x305 = x180*x242; - const double x306 = dBdyt0x*x51; - const double x307 = x176*x39; - const double x308 = Bx*x307; - const double x309 = x27*(-x182 + x183 - x185 + x187); - const double x310 = x302*x50; - const double x311 = Bx*x310; - const double x312 = x188*x39; - const double x313 = Bx*x312; - const double x314 = x303*x33; - const double x315 = x180*x250; - const double x316 = dBdyt0y*x51; - const double x317 = By*x307; - const double x318 = x27*(-x191 + x192 - x193 + x194); - const double x319 = By*x310; - const double x320 = By*x312; - const double x321 = -Bz*x301 + x21*x213 - x213; - const double x322 = x236*x67; - const double x323 = x215*x242; - const double x324 = x27*(x138 - x221 + x222); - const double x325 = Bx*x234; - const double x326 = 4*x131; - const double x327 = x321*x50; - const double x328 = Bx*x327; - const double x329 = x218*x39; - const double x330 = Bx*x329; - const double x331 = x236*x35; - const double x332 = x215*x250; - const double x333 = x27*(-x140 - x208 + x210); - const double x334 = By*x234; - const double x335 = By*x327; - const double x336 = By*x329; - const double x337 = -x66; - const double x338 = x230*(x337 + x71 + x72); - const double x339 = x230*x52; - const double x340 = x338*(-x258 + x259 + x260) + x339*(-x261 + x262 + x263); - const double x341 = x129*x21; - const double x342 = x120*x27; - const double x343 = x17*(x21 - 1)*(By*W0y + x42 + x93); - const double x344 = -By*x343 + W0y*x341 + x342*(-Bz*W0x + x68); - const double x345 = x13 + x22; - const double x346 = 1/(std::pow(x17, 4)*x345); - const double x347 = std::pow(x344, 2)*x346; - const double x348 = -Bx*x343 + W0x*x341 - x342*(x28 - x89); - const double x349 = x346*std::pow(x348, 2); - const double x350 = std::pow(x347 + x349, -1.0/2.0); - const double x351 = x348*x350; - const double x352 = x130*x345/(std::pow(x13, 3.0/2.0)*std::pow(x23*x345, 3.0/2.0)); - const double x353 = x351*x352; - const double x354 = x344*x350; - const double x355 = x352*x354; - const double x356 = x353*x73 - x355*x52; - const double x357 = x346*(-Bz*x343 + W0z*x341 - x342*(Bx*W0y - By*W0x)); - const double x358 = x354*x357; - const double x359 = x351*x357; - const double x360 = x347*x350 + x349*x350; - const double x361 = -x358*x73 - x359*x52 + x360*x81; - const double dqopdqop0 = x10*(dEdx*s*x1/x5 + x1) + x84*x85; - const double dqopdlam0 = x105*x85; - const double dqopdphi0 = x113*x85; - const double dqopdxt0 = x163*x85; - const double dqopdyt0 = x201*x85; - const double dqopdBz = x228*x85; - const double dqopdxi = x10*(-x7 - x9); - const double dlamdqop0 = x231*(x239*(x106*x63 - x233*x62 + x234*x60 - x237*x238) + x255*(-x248*(-2*x241 + 2*x243 + 2*x245 - 2*x247) - x254*(-2*x249 + 2*x251 + 2*x252 - 2*x253))) + x264*x84; - const double dlamdlam0 = x105*x264 + x231*(x239*(-x234*x97 + x25 + x27*(-x100 + x101)) + x255*(-x248*(-2*x270 + 2*x271 - 2*x272) - x254*(-2*x266 + 2*x267 - 2*x269))); - const double dlamdphi0 = x113*x264 + x231*(x239*(-x107*x234 + x27*(-x44 - x46)) + x255*(-x248*(2*x273 - 2*x275 - 2*x66) - x254*(2*x26 + 2*x276 - 2*x277))); - const double dlamdxt0 = x163*x264 + x231*(x239*(-dBdxt0z*x51 + x106*x136 - x118*x279 - x132*x234 - x149*x234 - x238*x281 + x27*(-x139 + x141 - x143 + x145)) + x255*(-x248*(-2*x283 + 2*x284 - 2*x285 - 2*x287 + 2*x288 - 2*x290 - 2*x292) - x254*(-2*x293 + 2*x294 - 2*x295 - 2*x296 + 2*x297 - 2*x298 - 2*x299))); - const double dlamdyt0 = x201*x264 + x231*(x239*(-dBdyt0z*x51 + x106*x180 - x176*x234 - x188*x234 - x238*x302 + x27*(-x196 + x197 - x198 + x199) - x279*x300) + x255*(-x248*(-2*x304 + 2*x305 - 2*x306 - 2*x308 + 2*x309 - 2*x311 - 2*x313) - x254*(-2*x314 + 2*x315 - 2*x316 - 2*x317 + 2*x318 - 2*x319 - 2*x320))); - const double dlamdBz = x228*x264 + x231*(x239*(-s*x206*x278 + x106*x215 - x218*x234 + x226*x39 - x238*x321 + x27*(-x224 + x225) - x51) + x255*(-x248*(-2*x322 + 2*x323 + 2*x324 + x325*x326 - 2*x328 - 2*x330) - x254*(x326*x334 - 2*x331 + 2*x332 + 2*x333 - 2*x335 - 2*x336))); + const double x51 = Bz*x50 + qop0*x39 + x20*x23 + x32; + const double x52 = 1.0/x33; + const double x53 = x1*x52; + const double x54 = x51*x53; + const double x55 = x28*x33; + const double x56 = x11*x55; + const double x57 = x23*x56; + const double x58 = x20*x26; + const double x59 = s*x15 - x16*x28; + const double x60 = Bz*x49; + const double x61 = 1.0/qop0; + const double x62 = x52*x61; + const double x63 = x62*(s*x57 - s*x58 + x39 + x59*x60); + const double x64 = x30*x48; + const double x65 = -Bz*x64 + W0z*x11*x22*x28*x33 - x58; + const double x66 = x52*x65; + const double x67 = x21*x36; + const double x68 = x33*x67; + const double x69 = W0y*x68; + const double x70 = W0y*x20; + const double x71 = x23*x42; + const double x72 = x43*x46; + const double x73 = x71 - x72; + const double x74 = x21*x30; + const double x75 = -M0x*x45 + M0y*W0x*x22; + const double x76 = W0z*x37; + const double x77 = W0x*x75 - W0y*x76; + const double x78 = x33*x77; + const double x79 = By*x21; + const double x80 = qop0*x69 + qop0*x78 + x50*x79 + x70 - x73*x74; + const double x81 = x22*x53; + const double x82 = s*x56; + const double x83 = x19*x73; + const double x84 = s*x21; + const double x85 = x49*x59; + const double x86 = W0y*x82 + x69 + x78 + x79*x85 + x83*x84; + const double x87 = x22*x62; + const double x88 = x45*x56; + const double x89 = -By*x64 + x83 + x88; + const double x90 = x52*x89; + const double x91 = W0x*x68; + const double x92 = W0x*x20; + const double x93 = x23*x44; + const double x94 = x45*x46; + const double x95 = x93 - x94; + const double x96 = W0x*x76 + W0y*x75; + const double x97 = x33*x96; + const double x98 = Bx*x21; + const double x99 = qop0*x91 - qop0*x97 + x50*x98 + x74*x95 + x92; + const double x100 = x19*x95; + const double x101 = W0x*x82 - x100*x84 + x85*x98 + x91 - x97; + const double x102 = -Bx*x64 + W0x*x11*x22*x28*x33 - x100; + const double x103 = x102*x52; + const double x104 = -x103*(x101*x87 - x81*x99) - x66*(-x54 + x63) - x90*(-x80*x81 + x86*x87); + const double x105 = q*x4*x5; + const double x106 = dEdx*x105; + const double x107 = W0z*x10; + const double x108 = W0x*x107; + const double x109 = W0y*x107; + const double x110 = Bz*x11 - x108*x42 - x109*x44; + const double x111 = Bz*x110; + const double x112 = x40*x41; + const double x113 = x111*x112 + x20 - x23*x32; + const double x114 = std::pow(x13, -6); + const double x115 = x114*x61; + const double x116 = x115*x65; + const double x117 = W0z*x43; + const double x118 = x109*x46 + x44; + const double x119 = x110*x112*x98 - x117*x20 + x118*x74; + const double x120 = x115*x22; + const double x121 = x102*x120; + const double x122 = W0z*x45; + const double x123 = x108*x46 + x42; + const double x124 = By*x110*x21*x40*x41 - x122*x20 - x123*x74; + const double x125 = x120*x89; + const double x126 = -x113*x116 - x119*x121 - x124*x125; + const double x127 = Bx*W0x; + const double x128 = By*W0y; + const double x129 = x127*x22 + x128*x22; + const double x130 = By*W0x*x11*x22 - x42*x45; + const double x131 = Bz*x130; + const double x132 = x112*x131 + x129*x31; + const double x133 = x30*x46; + const double x134 = -W0y*x133 + x112*x130*x79 + x92; + const double x135 = Bx*x130*x21*x40*x41 - W0x*x133 - x70; + const double x136 = -x116*x132 - x121*x135 - x125*x134; + const double x137 = W0y*x102*x22*x52 - x43*x90; + const double x138 = x103*x117 + x122*x90 - x21*x66; + const double x139 = 6*Bz; + const double x140 = x139/std::pow(x13, 4); + const double x141 = x140*x61; + const double x142 = x141*x51; + const double x143 = x18*x41; + const double x144 = 5*x143; + const double x145 = x27*x28; + const double x146 = qop0*s; + const double x147 = x145*x146; + const double x148 = x13*x29; + const double x149 = 4*x148; + const double x150 = x26*x46; + const double x151 = x146*x150; + const double x152 = Bz*qop0; + const double x153 = 6*x27; + const double x154 = x152*x153; + const double x155 = x12*x48; + const double x156 = 3*x15*x40; + const double x157 = 1.0/x15; + const double x158 = s*x152; + const double x159 = Bz*qop0*s*x157 - x157*x158*x28; + const double x160 = x112*x47 - x143*x151 + x144*x47 + x147*x47 + x149*x150 + x154*x38 + x155*x156 + x159*x60 + x50; + const double x161 = x141*x22; + const double x162 = Bz*W0y; + const double x163 = qop0*x153*x67; + const double x164 = x144*x46; + const double x165 = x147*x46; + const double x166 = W0z*x112; + const double x167 = Bz*x73; + const double x168 = x149*x21; + const double x169 = x158*x73; + const double x170 = x143*x21; + const double x171 = Bz*x48; + const double x172 = x156*x171; + const double x173 = x159*x49; + const double x174 = W0x*x31 + W0y*x164 + W0y*x165 + x154*x77 + x162*x163 + x166*x44 - x167*x168 + x169*x170 + x172*x79 + x173*x79; + const double x175 = Bz*W0x; + const double x176 = Bz*x95; + const double x177 = x158*x95; + const double x178 = W0x*x164 + W0x*x165 - W0y*x31 - x154*x96 + x163*x175 + x166*x42 + x168*x176 - x170*x177 + x172*x98 + x173*x98; + const double x179 = -x103*(-x161*x99 + x178*x22*x52*x61) - x66*(-x142 + x160*x52*x61) - x90*(-x161*x80 + x174*x22*x52*x61); + const double x180 = std::pow(x89, 2); + const double x181 = std::pow(x102, 2); + const double x182 = x114*x180 + x114*x181; + const double x183 = 1.0/x182; + const double x184 = x114*x183; + const double x185 = 1.0/(x184*std::pow(x65, 2) + 1); + const double x186 = s*x18; + const double x187 = x11*std::pow(x13, 7.0/2.0); + const double x188 = x187*x23; + const double x189 = std::pow(x182, -1.0/2.0); + const double x190 = x189*x52; + const double x191 = x186*x187; + const double x192 = x191*x45; + const double x193 = s*x55; + const double x194 = x193*x73; + const double x195 = x19*x48; + const double x196 = By*s*x195; + const double x197 = (1.0/2.0)*x114; + const double x198 = x197*x89; + const double x199 = x191*x43; + const double x200 = x193*x95; + const double x201 = x102*x197; + const double x202 = x66/std::pow(x182, 3.0/2.0); + const double x203 = qop0*x18; + const double x204 = qop0*x56; + const double x205 = x187*x203; + const double x206 = x205*x45; + const double x207 = qop0*x55; + const double x208 = x207*x73; + const double x209 = By*qop0*x195; + const double x210 = x205*x43; + const double x211 = x207*x95; + const double x212 = x185*(x190*(Bz*qop0*x14*x18*x48 - x188*x203 - x204*x26) + x202*(-x198*(-2*x206 + 2*x208 + 2*x209) - x201*(2*Bx*qop0*x14*x18*x48 - 2*x210 - 2*x211))); + const double x213 = x109*x56; + const double x214 = x110*x30; + const double x215 = By*x214; + const double x216 = x108*x56; + const double x217 = x118*x19; + const double x218 = Bx*x214; + const double x219 = x43*x56; + const double x220 = x19*x94; + const double x221 = 2*x220; + const double x222 = x130*x30; + const double x223 = By*x222; + const double x224 = Bx*x222; + const double x225 = x139/std::pow(x13, 7); + const double x226 = x20*x43; + const double x227 = x30*x93; + const double x228 = 10*x143; + const double x229 = x145*x169; + const double x230 = By*x171; + const double x231 = 8*x148; + const double x232 = By*qop0*x186*x60; + const double x233 = x30*x71; + const double x234 = x19*x72; + const double x235 = x145*x177; + const double x236 = Bx*x171; + const double x237 = x102*x184; + const double x238 = x184*x89; + const double x239 = x237*(-x206 + x208 + x209) - x238*(Bx*qop0*x14*x18*x48 - x210 - x211); + const double x240 = x30*(Bz*W0z + x127 + x128); + const double x241 = -By*x240 + W0y*x55 + x19*(Bx*W0z - x175); + const double x242 = x6 + x9; + const double x243 = 1.0/x242; + const double x244 = x114*x243; + const double x245 = std::pow(x241, 2)*x244; + const double x246 = -Bx*x240 + W0x*x28*x33 - x19*(By*W0z - x162); + const double x247 = x244*std::pow(x246, 2); + const double x248 = std::pow(x245 + x247, -1.0/2.0); + const double x249 = x241*x248; + const double x250 = x1*x99; + const double x251 = std::pow(x10*x242, -1.0/2.0); + const double x252 = x10*x251; + const double x253 = x114*x252; + const double x254 = x246*x248; + const double x255 = x1*x80; + const double x256 = x115*x252; + const double x257 = x254*x256; + const double x258 = x249*x256; + const double x259 = qop0*x21; + const double x260 = -qop0*x15*x28 + qop0*x15; + const double x261 = x260*x49; + const double x262 = W0y*x204 + x259*x83 + x261*x79; + const double x263 = W0x*x204 - x100*x259 + x261*x98; + const double x264 = x257*x262 - x258*x263; + const double x265 = x252*x52; + const double x266 = x251*x52; + const double x267 = x225*x252; + const double x268 = x249*x61; + const double x269 = x254*x61; + const double x270 = -Bz*x240 + W0z*x28*x33 - x19*(x24 - x25); + const double x271 = x22*x243*x270/std::pow(x13, 9); + const double x272 = x249*x271; + const double x273 = x272*x61; + const double x274 = x269*x271; + const double x275 = x245*x248 + x247*x248; + const double x276 = -x262*x273 - x263*x274 + x275*x52*x61*(qop0*x57 - qop0*x58 + x260*x60); + const double x277 = x275*x62; + const double x278 = x244*x270; + const double x279 = x249*x278; + const double x280 = x254*x278; + const double x281 = x139*x22*x243*x270/std::pow(x13, 10); + const double dqopdqop0 = std::pow(q, 3)*x4*x5/(std::pow(qop0, 3)*x2) - x104*x106; + const double dqopdlam0 = -x106*x126; + const double dqopdphi0 = -x106*x136; + const double dqopdxt0 = -x106*x137; + const double dqopdyt0 = -x106*x138; + const double dqopdBz = -x106*x179; + const double dqopdxi = -x105*x3; + const double dlamdqop0 = x104*x212 + x185*(x190*(Bz*s*x14*x18*x48 - x186*x188 - x26*x82) + x202*(-x198*(-2*x192 + 2*x194 + 2*x196) - x201*(2*Bx*s*x14*x18*x48 - 2*x199 - 2*x200))); + const double dlamdlam0 = x126*x212 + x185*(x190*(-x111*x30 + x23*x58 + x56) + x202*(-x198*(2*x123*x14*x18 - 2*x213 - 2*x215) - x201*(-2*x216 - 2*x217 - 2*x218))); + const double dlamdphi0 = x136*x212 + x185*(x190*(-x129*x20 - x131*x30) + x202*(-x198*(2*x219 + x221 - 2*x223) - x201*(2*Bz*W0x*x11*x14*x18*x22 - 2*x224 - 2*x88))); + const double dlamdxt0 = x137*x212; + const double dlamdyt0 = x138*x212; + const double dlamdBz = x179*x212 + x185*(-x140*x189*x65 + x190*(6*Bz*W0z*x11*x22*x27*x28 + qop0*s*x12*x18*x41*x48 - x144*x150 - x145*x151 - x146*x19*x47 - x149*x155 - x30*x47 - x64) + x202*(x180*x225 + x181*x225 - x198*(12*x145*x94 - x146*x221 + x167*x228 - 2*x226 - 2*x227 + 2*x229 - x230*x231 + 2*x232) - x201*(2*Bx*Bz*qop0*s*x18*x41*x48 + 12*Bz*W0x*x11*x22*x27*x28 + 2*W0y*x11*x14*x18*x22 - 2*x146*x234 - x176*x228 - x231*x236 - 2*x233 - 2*x235))); const double dlamdxi = 0; - const double dphidqop0 = x338*(-x241 + x243 + x245 - x247) + x339*(-x249 + x251 + x252 - x253) + x340*x84; - const double dphidlam0 = x105*x340 + x338*(-x270 + x271 - x272) + x339*(-x266 + x267 - x269); - const double dphidphi0 = x113*x340 + x338*(x273 - x275 + x337) + x339*(x26 + x276 - x277); - const double dphidxt0 = x163*x340 + x338*(-x283 + x284 - x285 - x287 + x288 - x290 - x292) + x339*(-x293 + x294 - x295 - x296 + x297 - x298 - x299); - const double dphidyt0 = x201*x340 + x338*(-x304 + x305 - x306 - x308 + x309 - x311 - x313) + x339*(-x314 + x315 - x316 - x317 + x318 - x319 - x320); - const double dphidBz = x228*x340 + x338*(x211*x325 - x322 + x323 + x324 - x328 - x330) + x339*(x211*x334 - x331 + x332 + x333 - x335 - x336); + const double dphidqop0 = x104*x239 + x237*(-x192 + x194 + x196) - x238*(Bx*s*x14*x18*x48 - x199 - x200); + const double dphidlam0 = x126*x239 + x237*(x123*x14*x18 - x213 - x215) - x238*(-x216 - x217 - x218); + const double dphidphi0 = x136*x239 + x237*(x219 + x220 - x223) - x238*(Bz*W0x*x11*x14*x18*x22 - x224 - x88); + const double dphidxt0 = x137*x239; + const double dphidyt0 = x138*x239; + const double dphidBz = x103*x183*(-x140*x89 + x52*(x144*x167 - x146*x220 - x149*x230 + x153*x28*x94 - x226 - x227 + x229 + x232)) + x179*x239 - x183*x90*(-x102*x140 + x52*(Bx*Bz*qop0*s*x18*x41*x48 + 6*Bz*W0x*x11*x22*x27*x28 + W0y*x11*x14*x18*x22 - x144*x176 - x146*x234 - x149*x236 - x233 - x235)); const double dphidxi = 0; - const double dxtdqop0 = x353*x74 - x355*x65 + x356*x84; - const double dxtdlam0 = x104*x353 + x105*x356 - x355*x99; - const double dxtdphi0 = x111*x353 - x112*x355 + x113*x356; - const double dxtdxt0 = x157*x353 - x162*x355 + x163*x356; - const double dxtdyt0 = -x190*x355 + x195*x353 + x201*x356; - const double dxtdBz = x220*x353 - x223*x355 + x228*x356; + const double dxtdqop0 = -x101*x258 + x104*x264 + x249*x250*x253 - x253*x254*x255 + x257*x86; + const double dxtdlam0 = -x119*x258 + x124*x257 + x126*x264; + const double dxtdphi0 = x134*x257 - x135*x258 + x136*x264; + const double dxtdxt0 = W0x*x254*x265 + W0y*x249*x265 + x137*x264; + const double dxtdyt0 = x108*x249*x266 - x109*x254*x266 + x138*x264; + const double dxtdBz = x174*x257 - x178*x258 + x179*x264 + x267*x268*x99 - x267*x269*x80; const double dxtdxi = 0; - const double dytdqop0 = -x358*x74 - x359*x65 + x360*x83 + x361*x84; - const double dytdlam0 = x102*x360 - x104*x358 + x105*x361 - x359*x99; - const double dytdphi0 = x108*x360 - x111*x358 - x112*x359 + x113*x361; - const double dytdxt0 = x150*x360 - x157*x358 - x162*x359 + x163*x361; - const double dytdyt0 = -x190*x359 - x195*x358 + x200*x360 + x201*x361; - const double dytdBz = -x220*x358 - x223*x359 + x227*x360 + x228*x361; + const double dytdqop0 = -x101*x274 + x104*x276 + x250*x254*x271 + x255*x272 - x273*x86 - x275*x54 + x275*x63; + const double dytdlam0 = x113*x277 - x119*x274 - x124*x273 + x126*x276; + const double dytdphi0 = x132*x277 - x134*x273 - x135*x274 + x136*x276; + const double dytdxt0 = x137*x276 - x279*x43 + x280*x45; + const double dytdyt0 = x117*x280 + x122*x279 + x138*x276 + x21*x275; + const double dytdBz = -x142*x275 + x160*x275*x62 - x174*x273 - x178*x274 + x179*x276 + x268*x281*x80 + x269*x281*x99; const double dytdxi = 0; Eigen::Matrix res; res(0,0) = dqopdqop0; @@ -4505,108 +2269,9 @@ Eigen::Matrix Geant4ePropagator::transportJacobianBzAdvanced(const res(4,5) = dytdBz; res(4,6) = dytdxi; - res.col(5) *= 2.99792458e-3; return res; } - - -Eigen::Matrix Geant4ePropagator::curv2cartJacobianAlt(const FreeTrajectoryState &state) const { - - const GlobalTrajectoryParameters &globalSource = state.parameters(); - - CurvilinearTrajectoryParameters curvparms(globalSource.position(), globalSource.momentum(), globalSource.charge()); - - const double qop0 = curvparms.Qbp(); - const double lam0 = curvparms.lambda(); - const double phi0 = curvparms.phi(); - const double xt0 = curvparms.xT(); - const double yt0 = curvparms.yT(); - - const double p0 = globalSource.momentum().mag(); - const GlobalVector W0 = globalSource.momentum()/p0; - const double W0x = W0.x(); - const double W0y = W0.y(); - const double W0z = W0.z(); - - const double x0 = std::sqrt(std::pow(W0x, 2) + std::pow(W0y, 2)); - const double x1 = 1.0/x0; - const double x2 = W0y*x1; - const double x3 = W0x*x1; - const double x4 = 1.0/qop0; - const double x5 = std::cos(phi0); - const double x6 = 1.0/std::fabs(qop0); - const double x7 = x6*std::cos(lam0); - const double x8 = x5*x7; - const double x9 = x6*std::sin(lam0); - const double x10 = std::sin(phi0); - const double x11 = x10*x7; - const double dxdqop0 = 0; - const double dxdlam0 = 0; - const double dxdphi0 = 0; - const double dxdxt0 = -x2; - const double dxdyt0 = -W0z*x3; - const double dydqop0 = 0; - const double dydlam0 = 0; - const double dydphi0 = 0; - const double dydxt0 = x3; - const double dydyt0 = -W0z*x2; - const double dzdqop0 = 0; - const double dzdlam0 = 0; - const double dzdphi0 = 0; - const double dzdxt0 = 0; - const double dzdyt0 = x0; - const double dpxdqop0 = -x4*x8; - const double dpxdlam0 = -x5*x9; - const double dpxdphi0 = -x11; - const double dpxdxt0 = 0; - const double dpxdyt0 = 0; - const double dpydqop0 = -x11*x4; - const double dpydlam0 = -x10*x9; - const double dpydphi0 = x8; - const double dpydxt0 = 0; - const double dpydyt0 = 0; - const double dpzdqop0 = -x4*x9; - const double dpzdlam0 = x7; - const double dpzdphi0 = 0; - const double dpzdxt0 = 0; - const double dpzdyt0 = 0; - Eigen::Matrix res; - res(0,0) = dxdqop0; - res(0,1) = dxdlam0; - res(0,2) = dxdphi0; - res(0,3) = dxdxt0; - res(0,4) = dxdyt0; - res(1,0) = dydqop0; - res(1,1) = dydlam0; - res(1,2) = dydphi0; - res(1,3) = dydxt0; - res(1,4) = dydyt0; - res(2,0) = dzdqop0; - res(2,1) = dzdlam0; - res(2,2) = dzdphi0; - res(2,3) = dzdxt0; - res(2,4) = dzdyt0; - res(3,0) = dpxdqop0; - res(3,1) = dpxdlam0; - res(3,2) = dpxdphi0; - res(3,3) = dpxdxt0; - res(3,4) = dpxdyt0; - res(4,0) = dpydqop0; - res(4,1) = dpydlam0; - res(4,2) = dpydphi0; - res(4,3) = dpydxt0; - res(4,4) = dpydyt0; - res(5,0) = dpzdqop0; - res(5,1) = dpzdlam0; - res(5,2) = dpzdphi0; - res(5,3) = dpzdxt0; - res(5,4) = dpzdyt0; - - return res; - - -}