File indexing completed on 2022-05-07 00:41:30
0001
0002
0003
0004
0005 #include "L1Trigger/TrackFindingTMTT/interface/KFbase.h"
0006 #include "L1Trigger/TrackFindingTMTT/interface/Utility.h"
0007 #include "L1Trigger/TrackFindingTMTT/interface/TP.h"
0008 #include "L1Trigger/TrackFindingTMTT/interface/KalmanState.h"
0009 #include "L1Trigger/TrackFindingTMTT/interface/StubKiller.h"
0010 #include "L1Trigger/TrackFindingTMTT/interface/PrintL1trk.h"
0011 #include "FWCore/ServiceRegistry/interface/Service.h"
0012 #include "CommonTools/UtilAlgos/interface/TFileService.h"
0013 #include "DataFormats/Math/interface/deltaPhi.h"
0014 #include "TMatrixD.h"
0015
0016 #include <algorithm>
0017 #include <functional>
0018 #include <fstream>
0019 #include <iomanip>
0020 #include <atomic>
0021 #include <sstream>
0022
0023 using namespace std;
0024
0025 namespace tmtt {
0026
0027
0028
0029 KFbase::KFbase(const Settings *settings, const uint nHelixPar, const string &fitterName, const uint nMeas)
0030 : TrackFitGeneric(settings, fitterName) {
0031 nHelixPar_ = nHelixPar;
0032 nMeas_ = nMeas;
0033 numEtaRegions_ = settings->numEtaRegions();
0034 }
0035
0036
0037
0038 L1fittedTrack KFbase::fit(const L1track3D &l1track3D) {
0039 iPhiSec_ = l1track3D.iPhiSec();
0040 iEtaReg_ = l1track3D.iEtaReg();
0041 resetStates();
0042 numUpdateCalls_ = 0;
0043
0044 vector<Stub *> stubs = l1track3D.stubs();
0045
0046 auto orderByLayer = [](const Stub *a, const Stub *b) { return bool(a->layerId() < b->layerId()); };
0047 sort(stubs.begin(), stubs.end(), orderByLayer);
0048
0049
0050 const TP *tpa(nullptr);
0051 if (l1track3D.matchedTP()) {
0052 tpa = l1track3D.matchedTP();
0053 }
0054 tpa_ = tpa;
0055
0056
0057 if (settings_->kalmanDebugLevel() >= 1) {
0058 PrintL1trk() << "===============================================================================";
0059 std::stringstream text;
0060 text << std::fixed << std::setprecision(4);
0061 text << "Input track cand: [phiSec,etaReg]=[" << l1track3D.iPhiSec() << "," << l1track3D.iEtaReg() << "]";
0062 text << " HT(m,c)=(" << l1track3D.cellLocationHT().first << "," << l1track3D.cellLocationHT().second
0063 << ") q/pt=" << l1track3D.qOverPt() << " tanL=" << l1track3D.tanLambda() << " z0=" << l1track3D.z0()
0064 << " phi0=" << l1track3D.phi0() << " nStubs=" << l1track3D.numStubs() << " d0=" << l1track3D.d0();
0065 PrintL1trk() << text.str();
0066 if (not settings_->hybrid())
0067 printTP(tpa);
0068 if (settings_->kalmanDebugLevel() >= 2) {
0069 printStubLayers(stubs, l1track3D.iEtaReg());
0070 printStubs(stubs);
0071 }
0072 }
0073
0074
0075 const KalmanState *cand = doKF(l1track3D, stubs, tpa);
0076
0077
0078 if (cand != nullptr) {
0079
0080 TVectorD trackPars = trackParams(cand);
0081 double d0 = (nHelixPar_ == 5) ? trackPars[D0] : 0.;
0082
0083 L1fittedTrack fitTrk(settings_,
0084 &l1track3D,
0085 cand->stubs(),
0086 cand->hitPattern(),
0087 trackPars[QOVERPT],
0088 d0,
0089 trackPars[PHI0],
0090 trackPars[Z0],
0091 trackPars[T],
0092 cand->chi2rphi(),
0093 cand->chi2rz(),
0094 nHelixPar_);
0095
0096
0097 fitTrk.setInfoKF(cand->nSkippedLayers(), numUpdateCalls_);
0098
0099
0100
0101 if (settings_->kalmanAddBeamConstr()) {
0102 if (nHelixPar_ == 5) {
0103 double chi2rphi_bcon = 0.;
0104 TVectorD trackPars_bcon = trackParams_BeamConstr(cand, chi2rphi_bcon);
0105
0106
0107 vector<double> kfLayerVsChiSqCut = settings_->kfLayerVsChiSq5();
0108 double chi2scaled = chi2rphi_bcon / settings_->kalmanChi2RphiScale() + fitTrk.chi2rz();
0109 bool accepted = true;
0110 if (chi2scaled > kfLayerVsChiSqCut[cand->nStubLayers()])
0111 accepted = false;
0112
0113 fitTrk.setBeamConstr(trackPars_bcon[QOVERPT], trackPars_bcon[PHI0], chi2rphi_bcon, accepted);
0114 }
0115 }
0116
0117
0118 if (!settings_->hybrid()) {
0119
0120
0121 if (settings_->enableDigitize())
0122 fitTrk.digitizeTrack("KF4ParamsComb");
0123
0124 if (!fitTrk.consistentSector()) {
0125 if (settings_->kalmanDebugLevel() >= 1)
0126 PrintL1trk() << "Track rejected by sector consistency test";
0127 L1fittedTrack rejectedTrk;
0128 return rejectedTrk;
0129 }
0130 }
0131
0132 return fitTrk;
0133
0134 } else {
0135
0136 if (settings_->kalmanDebugLevel() >= 1) {
0137 bool goodTrack = (tpa && tpa->useForAlgEff());
0138 if (goodTrack) {
0139 int tpin = tpa->index();
0140 PrintL1trk() << "TRACK LOST: eta=" << l1track3D.iEtaReg() << " pt=" << l1track3D.pt() << " tp=" << tpin;
0141
0142 for (auto stub : stubs) {
0143 int kalmanLay =
0144 this->kalmanLayer(l1track3D.iEtaReg(), stub->layerIdReduced(), stub->barrel(), stub->r(), stub->z());
0145 std::stringstream text;
0146 text << std::fixed << std::setprecision(4);
0147 text << " Stub: lay_red=" << stub->layerIdReduced() << " KFlay=" << kalmanLay << " r=" << stub->r()
0148 << " z=" << stub->z() << " assoc TPs =";
0149 for (const TP *tp_i : stub->assocTPs())
0150 text << " " << tp_i->index();
0151 PrintL1trk() << text.str();
0152 if (stub->assocTPs().empty())
0153 PrintL1trk() << " none";
0154 }
0155 PrintL1trk() << "=====================";
0156 }
0157 }
0158
0159
0160 if (settings_->kalmanDebugLevel() >= 3) {
0161 if (tpa && tpa->useForAlgEff()) {
0162 PrintL1trk() << "TP for eff. missed addr. index : " << tpa << " " << tpa->index();
0163 printStubs(stubs);
0164 }
0165 }
0166
0167 L1fittedTrack rejectedTrk;
0168 return rejectedTrk;
0169 }
0170 }
0171
0172
0173
0174 const KalmanState *KFbase::doKF(const L1track3D &l1track3D, const vector<Stub *> &stubs, const TP *tpa) {
0175 const KalmanState *finished_state = nullptr;
0176
0177 map<unsigned int, const KalmanState *, std::greater<unsigned int>>
0178 best_state_by_nstubs;
0179
0180
0181 TVectorD x0 = seedX(l1track3D);
0182 TMatrixD pxx0 = seedC(l1track3D);
0183 TMatrixD K(nHelixPar_, 2);
0184 TMatrixD dcov(2, 2);
0185
0186 const KalmanState *state0 = mkState(l1track3D, 0, -1, nullptr, x0, pxx0, K, dcov, nullptr, 0, 0);
0187
0188
0189 vector<const KalmanState *> new_states;
0190 vector<const KalmanState *> prev_states;
0191 prev_states.push_back(state0);
0192
0193
0194 bool remove2PSCut = settings_->kalmanRemove2PScut();
0195 set<unsigned> kfDeadLayers = kalmanDeadLayers(remove2PSCut);
0196
0197
0198 int etaReg = l1track3D.iEtaReg();
0199 map<int, vector<Stub *>> layerStubs;
0200
0201 for (auto stub : stubs) {
0202
0203 int kalmanLay = this->kalmanLayer(etaReg, stub->layerIdReduced(), stub->barrel(), stub->r(), stub->z());
0204
0205 constexpr unsigned int invalidKFlayer = 7;
0206 if (kalmanLay != invalidKFlayer) {
0207 if (layerStubs[kalmanLay].size() < settings_->kalmanMaxStubsPerLayer()) {
0208 layerStubs[kalmanLay].push_back(stub);
0209 } else {
0210
0211 layerStubs[kalmanLay].back() = stub;
0212 }
0213 }
0214 }
0215
0216
0217 constexpr unsigned int nTypicalLayers = 6;
0218
0219 const unsigned int maxIterations = std::max(nTypicalLayers, settings_->kalmanMaxNumStubs());
0220 for (unsigned iteration = 0; iteration < maxIterations; iteration++) {
0221 int combinations_per_iteration = 0;
0222
0223 bool easy = (l1track3D.numStubs() < settings_->kalmanMaxStubsEasy());
0224 unsigned int kalmanMaxSkipLayers =
0225 easy ? settings_->kalmanMaxSkipLayersEasy() : settings_->kalmanMaxSkipLayersHard();
0226
0227
0228 vector<const KalmanState *>::const_iterator i_state = prev_states.begin();
0229 for (; i_state != prev_states.end(); i_state++) {
0230 const KalmanState *the_state = *i_state;
0231
0232 unsigned int layer = the_state->nextLayer();
0233 unsigned nSkipped = the_state->nSkippedLayers();
0234
0235
0236
0237
0238
0239 unsigned nSkippedDeadLayers = 0;
0240 unsigned nSkippedAmbiguousLayers = 0;
0241 while (kfDeadLayers.find(layer) != kfDeadLayers.end() && layerStubs[layer].empty()) {
0242 layer += 1;
0243 ++nSkippedDeadLayers;
0244 }
0245 while (this->kalmanAmbiguousLayer(etaReg, layer) && layerStubs[layer].empty()) {
0246 layer += 1;
0247 ++nSkippedAmbiguousLayers;
0248 }
0249
0250
0251 vector<const KalmanState *> next_states;
0252 vector<const KalmanState *> next_states_skipped;
0253
0254
0255
0256 vector<Stub *> thislay_stubs = layerStubs[layer];
0257
0258
0259
0260 vector<Stub *> nextlay_stubs;
0261
0262
0263
0264 unsigned nSkippedDeadLayers_nextStubs = 0;
0265 unsigned nSkippedAmbiguousLayers_nextStubs = 0;
0266 if (nSkipped < kalmanMaxSkipLayers) {
0267 if (kfDeadLayers.find(layer + 1) != kfDeadLayers.end() && layerStubs[layer + 1].empty()) {
0268 nextlay_stubs = layerStubs[layer + 2];
0269 nSkippedDeadLayers_nextStubs++;
0270 } else if (this->kalmanAmbiguousLayer(etaReg, layer) && layerStubs[layer + 1].empty()) {
0271 nextlay_stubs = layerStubs[layer + 2];
0272 nSkippedAmbiguousLayers_nextStubs++;
0273 } else {
0274 nextlay_stubs = layerStubs[layer + 1];
0275 }
0276 }
0277
0278
0279
0280 if (settings_->kalmanDebugLevel() >= 2 && best_state_by_nstubs.empty() && thislay_stubs.empty() &&
0281 nextlay_stubs.empty())
0282 PrintL1trk() << "State is lost by start of iteration " << iteration
0283 << " : #thislay_stubs=" << thislay_stubs.size() << " #nextlay_stubs=" << nextlay_stubs.size()
0284 << " layer=" << layer << " eta=" << l1track3D.iEtaReg();
0285
0286
0287 nSkipped += nSkippedDeadLayers;
0288 nSkipped += nSkippedAmbiguousLayers;
0289
0290
0291
0292
0293 if (iteration == 1 && !remove2PSCut) {
0294 vector<Stub *> temp_thislaystubs;
0295 vector<Stub *> temp_nextlaystubs;
0296 for (auto stub : thislay_stubs) {
0297 if (stub->psModule())
0298 temp_thislaystubs.push_back(stub);
0299 }
0300 for (auto stub : nextlay_stubs) {
0301 if (stub->psModule())
0302 temp_nextlaystubs.push_back(stub);
0303 }
0304 thislay_stubs = temp_thislaystubs;
0305 nextlay_stubs = temp_nextlaystubs;
0306 }
0307
0308 combinations_per_iteration += thislay_stubs.size() + nextlay_stubs.size();
0309
0310
0311 for (unsigned i = 0; i < thislay_stubs.size(); i++) {
0312 Stub *stub = thislay_stubs[i];
0313
0314
0315 const KalmanState *new_state = kalmanUpdate(nSkipped, layer, stub, the_state, tpa);
0316
0317
0318 if (isGoodState(*new_state))
0319 next_states.push_back(new_state);
0320 }
0321
0322
0323 for (unsigned i = 0; i < nextlay_stubs.size(); i++) {
0324 Stub *stub = nextlay_stubs[i];
0325
0326 const KalmanState *new_state =
0327 kalmanUpdate(nSkipped + 1 + nSkippedDeadLayers_nextStubs + nSkippedAmbiguousLayers_nextStubs,
0328 layer + 1 + nSkippedDeadLayers_nextStubs + nSkippedAmbiguousLayers_nextStubs,
0329 stub,
0330 the_state,
0331 tpa);
0332
0333 if (isGoodState(*new_state))
0334 next_states_skipped.push_back(new_state);
0335 }
0336
0337
0338 auto orderByChi2 = [](const KalmanState *a, const KalmanState *b) {
0339 return bool(a->chi2scaled() < b->chi2scaled());
0340 };
0341 sort(next_states.begin(), next_states.end(), orderByChi2);
0342 sort(next_states_skipped.begin(), next_states_skipped.end(), orderByChi2);
0343
0344 new_states.insert(new_states.end(), next_states.begin(), next_states.end());
0345 new_states.insert(new_states.end(), next_states_skipped.begin(), next_states_skipped.end());
0346
0347
0348
0349
0350
0351
0352
0353
0354
0355
0356
0357
0358
0359 }
0360
0361
0362
0363
0364 auto orderByMinSkipChi2 = [](const KalmanState *a, const KalmanState *b) {
0365 return bool((a->chi2scaled()) * (a->nSkippedLayers() + 1) < (b->chi2scaled()) * (b->nSkippedLayers() + 1));
0366 };
0367 sort(new_states.begin(), new_states.end(), orderByMinSkipChi2);
0368
0369 unsigned int nStubs = iteration + 1;
0370
0371 if (nStubs >= settings_->kalmanMinNumStubs() && not new_states.empty())
0372 best_state_by_nstubs[nStubs] = new_states[0];
0373
0374 if (nStubs == settings_->kalmanMaxNumStubs()) {
0375
0376 prev_states.clear();
0377 new_states.clear();
0378
0379 } else {
0380
0381 prev_states = new_states;
0382 new_states.clear();
0383 }
0384 }
0385
0386 if (not best_state_by_nstubs.empty()) {
0387
0388 finished_state = best_state_by_nstubs.begin()->second;
0389 if (settings_->kalmanDebugLevel() >= 1) {
0390 std::stringstream text;
0391 text << std::fixed << std::setprecision(4);
0392 text << "Track found! final state selection: nLay=" << finished_state->nStubLayers()
0393 << " hitPattern=" << std::hex << finished_state->hitPattern() << std::dec
0394 << " phiSec=" << l1track3D.iPhiSec() << " etaReg=" << l1track3D.iEtaReg() << " HT(m,c)=("
0395 << l1track3D.cellLocationHT().first << "," << l1track3D.cellLocationHT().second << ")";
0396 TVectorD y = trackParams(finished_state);
0397 text << " q/pt=" << y[QOVERPT] << " tanL=" << y[T] << " z0=" << y[Z0] << " phi0=" << y[PHI0];
0398 if (nHelixPar_ == 5)
0399 text << " d0=" << y[D0];
0400 text << " chosen from states:";
0401 for (const auto &p : best_state_by_nstubs)
0402 text << " " << p.second->chi2() << "/" << p.second->nStubLayers();
0403 PrintL1trk() << text.str();
0404 }
0405 } else {
0406 if (settings_->kalmanDebugLevel() >= 1) {
0407 PrintL1trk() << "Track lost";
0408 }
0409 }
0410
0411 return finished_state;
0412 }
0413
0414
0415
0416 const KalmanState *KFbase::kalmanUpdate(
0417 unsigned nSkipped, unsigned int layer, Stub *stub, const KalmanState *state, const TP *tpa) {
0418 if (settings_->kalmanDebugLevel() >= 4) {
0419 PrintL1trk() << "---------------";
0420 PrintL1trk() << "kalmanUpdate";
0421 PrintL1trk() << "---------------";
0422 printStub(stub);
0423 }
0424
0425 numUpdateCalls_++;
0426
0427
0428 TVectorD vecX = state->vectorX();
0429 TMatrixD matC = state->matrixC();
0430 if (state->barrel() && !stub->barrel()) {
0431 if (settings_->kalmanDebugLevel() >= 4) {
0432 PrintL1trk() << "STATE BARREL TO ENDCAP BEFORE ";
0433 PrintL1trk() << "state : " << vecX[0] << " " << vecX[1] << " " << vecX[2] << " " << vecX[3];
0434 PrintL1trk() << "cov(x): ";
0435 matC.Print();
0436 }
0437 if (settings_->kalmanDebugLevel() >= 4) {
0438 PrintL1trk() << "STATE BARREL TO ENDCAP AFTER ";
0439 PrintL1trk() << "state : " << vecX[0] << " " << vecX[1] << " " << vecX[2] << " " << vecX[3];
0440 PrintL1trk() << "cov(x): ";
0441 matC.Print();
0442 }
0443 }
0444
0445 TMatrixD matF = matrixF(stub, state);
0446 TMatrixD matFtrans(TMatrixD::kTransposed, matF);
0447 if (settings_->kalmanDebugLevel() >= 4) {
0448 PrintL1trk() << "matF";
0449 matF.Print();
0450 }
0451
0452
0453 TVectorD vecXref = matF * vecX;
0454 if (settings_->kalmanDebugLevel() >= 4) {
0455 PrintL1trk() << "vecFref = [";
0456 for (unsigned i = 0; i < nHelixPar_; i++)
0457 PrintL1trk() << vecXref[i] << ", ";
0458 PrintL1trk() << "]";
0459 }
0460
0461
0462 TVectorD delta = residual(stub, vecXref, state->candidate().qOverPt());
0463 if (settings_->kalmanDebugLevel() >= 4) {
0464 PrintL1trk() << "delta = " << delta[0] << ", " << delta[1];
0465 }
0466
0467
0468 TMatrixD matH = matrixH(stub);
0469 if (settings_->kalmanDebugLevel() >= 4) {
0470 PrintL1trk() << "matH";
0471 matH.Print();
0472 }
0473
0474 if (settings_->kalmanDebugLevel() >= 4) {
0475 PrintL1trk() << "previous state covariance";
0476 matC.Print();
0477 }
0478
0479 TMatrixD matScat(nHelixPar_, nHelixPar_);
0480
0481
0482 TMatrixD matCref = matF * matC * matFtrans + matScat;
0483 if (settings_->kalmanDebugLevel() >= 4) {
0484 PrintL1trk() << "matCref";
0485 matCref.Print();
0486 }
0487
0488 TMatrixD matV = matrixV(stub, state);
0489 if (settings_->kalmanDebugLevel() >= 4) {
0490 PrintL1trk() << "matV";
0491 matV.Print();
0492 }
0493
0494 TMatrixD matRinv = matrixRinv(matH, matCref, matV);
0495 if (settings_->kalmanDebugLevel() >= 4) {
0496 PrintL1trk() << "matRinv";
0497 matRinv.Print();
0498 }
0499
0500
0501 TMatrixD matK = getKalmanGainMatrix(matH, matCref, matRinv);
0502 if (settings_->kalmanDebugLevel() >= 4) {
0503 PrintL1trk() << "matK";
0504 matK.Print();
0505 }
0506
0507
0508 TVectorD new_vecX(nHelixPar_);
0509 TMatrixD new_matC(nHelixPar_, nHelixPar_);
0510 adjustState(matK, matCref, vecXref, matH, delta, new_vecX, new_matC);
0511
0512
0513 double new_chi2rphi = 0, new_chi2rz = 0;
0514 this->adjustChi2(state, matRinv, delta, new_chi2rphi, new_chi2rz);
0515
0516 if (settings_->kalmanDebugLevel() >= 4) {
0517 if (nHelixPar_ == 4)
0518 PrintL1trk() << "adjusted x = " << new_vecX[0] << ", " << new_vecX[1] << ", " << new_vecX[2] << ", "
0519 << new_vecX[3];
0520 else if (nHelixPar_ == 5)
0521 PrintL1trk() << "adjusted x = " << new_vecX[0] << ", " << new_vecX[1] << ", " << new_vecX[2] << ", "
0522 << new_vecX[3] << ", " << new_vecX[4];
0523 PrintL1trk() << "adjusted C ";
0524 new_matC.Print();
0525 PrintL1trk() << "adjust chi2rphi=" << new_chi2rphi << " chi2rz=" << new_chi2rz;
0526 }
0527
0528 const KalmanState *new_state = mkState(
0529 state->candidate(), nSkipped, layer, state, new_vecX, new_matC, matK, matV, stub, new_chi2rphi, new_chi2rz);
0530
0531 return new_state;
0532 }
0533
0534
0535
0536 const KalmanState *KFbase::mkState(const L1track3D &candidate,
0537 unsigned nSkipped,
0538 unsigned layer,
0539 const KalmanState *last_state,
0540 const TVectorD &vecX,
0541 const TMatrixD &matC,
0542 const TMatrixD &matK,
0543 const TMatrixD &matV,
0544 Stub *stub,
0545 double chi2rphi,
0546 double chi2rz) {
0547 auto new_state = std::make_unique<const KalmanState>(
0548 settings_, candidate, nSkipped, layer, last_state, vecX, matC, matK, matV, stub, chi2rphi, chi2rz);
0549
0550 const KalmanState *p_new_state = new_state.get();
0551 listAllStates_.push_back(std::move(new_state));
0552 return p_new_state;
0553 }
0554
0555
0556
0557 TMatrixD KFbase::matrixHCHt(const TMatrixD &matH, const TMatrixD &matC) const {
0558 TMatrixD matHtrans(TMatrixD::kTransposed, matH);
0559 return matH * matC * matHtrans;
0560 }
0561
0562
0563
0564 TMatrixD KFbase::matrixRinv(const TMatrixD &matH, const TMatrixD &matCref, const TMatrixD &matV) const {
0565 TMatrixD matHCHt = matrixHCHt(matH, matCref);
0566 TMatrixD matR = matV + matHCHt;
0567 TMatrixD matRinv(2, 2);
0568 if (matR.Determinant() > 0) {
0569 matRinv = TMatrixD(TMatrixD::kInverted, matR);
0570 } else {
0571
0572 const TMatrixD unitMatrix(TMatrixD::kUnit, TMatrixD(nHelixPar_, nHelixPar_));
0573 const double big = 9.9e9;
0574 matRinv = big * unitMatrix;
0575 }
0576 if (settings_->kalmanDebugLevel() >= 4) {
0577 PrintL1trk() << "matHCHt";
0578 matHCHt.Print();
0579 PrintL1trk() << "matR";
0580 matR.Print();
0581 }
0582 return matRinv;
0583 }
0584
0585
0586
0587 TMatrixD KFbase::getKalmanGainMatrix(const TMatrixD &matH, const TMatrixD &matCref, const TMatrixD &matRinv) const {
0588 TMatrixD matHtrans(TMatrixD::kTransposed, matH);
0589 TMatrixD matCrefht = matCref * matHtrans;
0590 TMatrixD matK = matCrefht * matRinv;
0591 return matK;
0592 }
0593
0594
0595
0596 TVectorD KFbase::residual(const Stub *stub, const TVectorD &vecX, double candQoverPt) const {
0597 TVectorD vd = vectorM(stub);
0598 TMatrixD h = matrixH(stub);
0599 TVectorD hx = h * vecX;
0600 TVectorD delta = vd - hx;
0601
0602
0603
0604 if (not settings_->kalmanHOfw()) {
0605 TVectorD correction(2);
0606
0607 float inv2R = (settings_->invPtToInvR()) * 0.5 * candQoverPt;
0608 float tanL = vecX[T];
0609 float z0 = vecX[Z0];
0610
0611 float deltaS = 0.;
0612 if (settings_->kalmanHOhelixExp()) {
0613
0614 double corr = stub->r() * inv2R;
0615
0616
0617 correction[0] += (1. / 6.) * pow(corr, 3);
0618
0619 deltaS = (1. / 6.) * (stub->r()) * pow(corr, 2);
0620 correction[1] -= deltaS * tanL;
0621 }
0622
0623 if ((not stub->barrel()) && not(stub->psModule())) {
0624
0625 float rShift = (stub->z() - z0) / tanL - stub->r();
0626
0627 if (settings_->kalmanHOhelixExp())
0628 rShift -= deltaS;
0629
0630 if (settings_->kalmanHOprojZcorr() == 1) {
0631
0632 correction[0] += inv2R * rShift;
0633 }
0634
0635 if (settings_->kalmanHOalpha() == 1) {
0636
0637 correction[0] += stub->alpha() * rShift;
0638 }
0639 }
0640
0641
0642 delta += correction;
0643 }
0644
0645 delta[0] = reco::deltaPhi(delta[0], 0.);
0646
0647 return delta;
0648 }
0649
0650
0651
0652 void KFbase::adjustState(const TMatrixD &matK,
0653 const TMatrixD &matCref,
0654 const TVectorD &vecXref,
0655 const TMatrixD &matH,
0656 const TVectorD &delta,
0657 TVectorD &new_vecX,
0658 TMatrixD &new_matC) const {
0659 new_vecX = vecXref + matK * delta;
0660 const TMatrixD unitMatrix(TMatrixD::kUnit, TMatrixD(nHelixPar_, nHelixPar_));
0661 TMatrixD tmp = unitMatrix - matK * matH;
0662 new_matC = tmp * matCref;
0663 }
0664
0665
0666
0667 void KFbase::adjustChi2(const KalmanState *state,
0668 const TMatrixD &matRinv,
0669 const TVectorD &delta,
0670 double &chi2rphi,
0671 double &chi2rz) const {
0672
0673 double delChi2rphi = delta[PHI] * delta[PHI] * matRinv[PHI][PHI] + 2 * delta[PHI] * delta[Z] * matRinv[PHI][Z];
0674 double delChi2rz = delta[Z] * delta[Z] * matRinv[Z][Z];
0675
0676 if (settings_->kalmanDebugLevel() >= 4) {
0677 PrintL1trk() << "delta(chi2rphi)=" << delChi2rphi << " delta(chi2rz)= " << delChi2rz;
0678 }
0679 chi2rphi = state->chi2rphi() + delChi2rphi;
0680 chi2rz = state->chi2rz() + delChi2rz;
0681 return;
0682 }
0683
0684
0685
0686 void KFbase::resetStates() { listAllStates_.clear(); }
0687
0688
0689
0690 unsigned int KFbase::kalmanLayer(
0691 unsigned int iEtaReg, unsigned int layerIDreduced, bool barrel, float r, float z) const {
0692
0693
0694
0695
0696
0697
0698
0699
0700 const unsigned int nEta = 16;
0701 const unsigned int nGPlayID = 7;
0702
0703 if (nEta != numEtaRegions_)
0704 throw cms::Exception("LogicError")
0705 << "ERROR KFbase::getKalmanLayer hardwired value of nEta differs from NumEtaRegions cfg param";
0706
0707
0708
0709 constexpr unsigned layerMap[nEta / 2][nGPlayID + 1] = {
0710 {7, 0, 1, 5, 4, 3, 7, 2},
0711 {7, 0, 1, 5, 4, 3, 7, 2},
0712 {7, 0, 1, 5, 4, 3, 7, 2},
0713 {7, 0, 1, 5, 4, 3, 7, 2},
0714 {7, 0, 1, 5, 4, 3, 7, 2},
0715 {7, 0, 1, 3, 4, 2, 6, 2},
0716 {7, 0, 1, 1, 2, 3, 4, 5},
0717 {7, 0, 7, 1, 2, 3, 4, 5},
0718 };
0719
0720 unsigned int kfEtaReg;
0721 if (iEtaReg < numEtaRegions_ / 2) {
0722 kfEtaReg = numEtaRegions_ / 2 - 1 - iEtaReg;
0723 } else {
0724 kfEtaReg = iEtaReg - numEtaRegions_ / 2;
0725 }
0726
0727 unsigned int kalmanLay = layerMap[kfEtaReg][layerIDreduced];
0728
0729
0730 if (settings_->kfUseMaybeLayers()) {
0731 switch (kfEtaReg) {
0732 case 5:
0733 if (layerIDreduced == 6) {
0734 kalmanLay = 5;
0735 }
0736 break;
0737 case 6:
0738 if (layerIDreduced > 2) {
0739 kalmanLay++;
0740 }
0741 break;
0742 default:
0743 break;
0744 }
0745 }
0746
0747
0748
0749 if (not barrel) {
0750 switch (kfEtaReg) {
0751 case 4:
0752 if (layerIDreduced == 3) {
0753 kalmanLay = 4;
0754 } else if (layerIDreduced == 4) {
0755 kalmanLay = 5;
0756 } else if (layerIDreduced == 5) {
0757 kalmanLay = 6;
0758 }
0759 break;
0760
0761 case 5:
0762 if (layerIDreduced == 5) {
0763 kalmanLay = 5;
0764 } else if (layerIDreduced == 7) {
0765 kalmanLay = 6;
0766 }
0767 break;
0768 default:
0769 break;
0770 }
0771 }
0772
0773
0774
0775
0776
0777
0778
0779
0780
0781
0782
0783
0784
0785
0786
0787
0788
0789
0790
0791
0792
0793
0794
0795
0796
0797
0798
0799
0800
0801
0802
0803
0804
0805 return kalmanLay;
0806 }
0807
0808
0809
0810
0811 bool KFbase::kalmanAmbiguousLayer(unsigned int iEtaReg, unsigned int kfLayer) {
0812
0813
0814
0815 const unsigned int nEta = 16;
0816 const unsigned int nKFlayer = 7;
0817 constexpr bool ambiguityMap[nEta / 2][nKFlayer] = {
0818 {false, false, false, false, false, false, false},
0819 {false, false, false, false, false, false, false},
0820 {false, false, false, false, false, false, false},
0821 {false, false, false, false, false, false, false},
0822 {false, false, false, false, false, false, false},
0823 {false, false, true, false, false, false, false},
0824 {true, true, false, false, false, false, false},
0825 {true, false, false, false, false, false, false},
0826 };
0827
0828 unsigned int kfEtaReg;
0829 if (iEtaReg < numEtaRegions_ / 2) {
0830 kfEtaReg = numEtaRegions_ / 2 - 1 - iEtaReg;
0831 } else {
0832 kfEtaReg = iEtaReg - numEtaRegions_ / 2;
0833 }
0834
0835 bool ambiguous = false;
0836 if (settings_->kfUseMaybeLayers() and kfLayer < nKFlayer)
0837 ambiguous = ambiguityMap[kfEtaReg][kfLayer];
0838
0839 return ambiguous;
0840 }
0841
0842
0843
0844 set<unsigned> KFbase::kalmanDeadLayers(bool &remove2PSCut) const {
0845
0846
0847
0848 const StubKiller::KillOptions killScenario = static_cast<StubKiller::KillOptions>(settings_->killScenario());
0849
0850 const bool killRecover = settings_->killRecover();
0851
0852 set<pair<unsigned, bool>> deadGPlayers;
0853
0854
0855 if (killRecover) {
0856 if (killScenario == StubKiller::KillOptions::layer5) {
0857 if (iEtaReg_ >= 3 && iEtaReg_ <= 7 && iPhiSec_ >= 1 && iPhiSec_ <= 5) {
0858 deadGPlayers.insert(pair<unsigned, bool>(4, true));
0859 }
0860 } else if (killScenario == StubKiller::KillOptions::layer1) {
0861 if (iEtaReg_ <= 7 && iPhiSec_ >= 1 && iPhiSec_ <= 5) {
0862 deadGPlayers.insert(pair<unsigned, bool>(1, true));
0863 }
0864 remove2PSCut = true;
0865 } else if (killScenario == StubKiller::KillOptions::layer1layer2) {
0866 if (iEtaReg_ <= 7 && iPhiSec_ >= 1 && iPhiSec_ <= 5) {
0867 deadGPlayers.insert(pair<unsigned, bool>(1, true));
0868 }
0869 if (iEtaReg_ >= 1 && iEtaReg_ <= 7 && iPhiSec_ >= 1 && iPhiSec_ <= 5) {
0870 deadGPlayers.insert(pair<unsigned, bool>(2, true));
0871 }
0872 remove2PSCut = true;
0873 } else if (killScenario == StubKiller::KillOptions::layer1disk1) {
0874 if (iEtaReg_ <= 7 && iPhiSec_ >= 1 && iPhiSec_ <= 5) {
0875 deadGPlayers.insert(pair<unsigned, bool>(1, true));
0876 }
0877 if (iEtaReg_ <= 3 && iPhiSec_ >= 1 && iPhiSec_ <= 5) {
0878 deadGPlayers.insert(pair<unsigned, bool>(3, false));
0879 }
0880 remove2PSCut = true;
0881 }
0882 }
0883
0884 set<unsigned> kfDeadLayers;
0885 for (const auto &p : deadGPlayers) {
0886 unsigned int layer = p.first;
0887 bool barrel = p.second;
0888 float r = 0.;
0889 float z = 999.;
0890 unsigned int kalmanLay = this->kalmanLayer(iEtaReg_, layer, barrel, r, z);
0891 kfDeadLayers.insert(kalmanLay);
0892 }
0893
0894 return kfDeadLayers;
0895 }
0896
0897
0898
0899 float KFbase::approxB(float z, float r) const {
0900 return settings_->bApprox_gradient() * std::abs(z) / r + settings_->bApprox_intercept();
0901 }
0902
0903
0904
0905 void KFbase::printTP(const TP *tp) const {
0906 TVectorD tpParams(5);
0907 bool useForAlgEff(false);
0908 if (tp) {
0909 useForAlgEff = tp->useForAlgEff();
0910 tpParams[QOVERPT] = tp->qOverPt();
0911 tpParams[PHI0] = tp->phi0();
0912 tpParams[Z0] = tp->z0();
0913 tpParams[T] = tp->tanLambda();
0914 tpParams[D0] = tp->d0();
0915 }
0916 std::stringstream text;
0917 text << std::fixed << std::setprecision(4);
0918 if (tp) {
0919 text << " TP index = " << tp->index() << " useForAlgEff = " << useForAlgEff << " ";
0920 const string helixNames[5] = {"qOverPt", "phi0", "z0", "tanL", "d0"};
0921 for (int i = 0; i < tpParams.GetNrows(); i++) {
0922 text << helixNames[i] << ":" << tpParams[i] << ", ";
0923 }
0924 text << " inv2R = " << tp->qOverPt() * settings_->invPtToInvR() * 0.5;
0925 } else {
0926 text << " Fake";
0927 }
0928 PrintL1trk() << text.str();
0929 }
0930
0931
0932
0933 void KFbase::printStubLayers(const vector<Stub *> &stubs, unsigned int iEtaReg) const {
0934 std::stringstream text;
0935 text << std::fixed << std::setprecision(4);
0936 if (stubs.empty())
0937 text << "stub layers = []\n";
0938 else {
0939 text << "stub layers = [ ";
0940 for (unsigned i = 0; i < stubs.size(); i++) {
0941 text << stubs[i]->layerId();
0942 if (i != stubs.size() - 1)
0943 text << ", ";
0944 }
0945 text << " ] ";
0946 text << "KF stub layers = [ ";
0947 for (unsigned j = 0; j < stubs.size(); j++) {
0948 unsigned int kalmanLay =
0949 this->kalmanLayer(iEtaReg, stubs[j]->layerIdReduced(), stubs[j]->barrel(), stubs[j]->r(), stubs[j]->z());
0950 text << kalmanLay;
0951 if (j != stubs.size() - 1)
0952 text << ", ";
0953 }
0954 text << " ]\n";
0955 }
0956 PrintL1trk() << text.str();
0957 }
0958
0959
0960
0961 void KFbase::printStub(const Stub *stub) const {
0962 std::stringstream text;
0963 text << std::fixed << std::setprecision(4);
0964 text << "stub ";
0965 text << "index=" << stub->index() << " ";
0966 text << "layerId=" << stub->layerId() << " ";
0967 text << "r=" << stub->r() << " ";
0968 text << "phi=" << stub->phi() << " ";
0969 text << "z=" << stub->z() << " ";
0970 text << "sigmaX=" << stub->sigmaPerp() << " ";
0971 text << "sigmaZ=" << stub->sigmaPar() << " ";
0972 text << "TPids=";
0973 std::set<const TP *> tps = stub->assocTPs();
0974 for (auto tp : tps)
0975 text << tp->index() << ",";
0976 PrintL1trk() << text.str();
0977 }
0978
0979
0980
0981 void KFbase::printStubs(const vector<Stub *> &stubs) const {
0982 for (auto &stub : stubs) {
0983 printStub(stub);
0984 }
0985 }
0986
0987 }