File indexing completed on 2023-10-25 10:05:43
0001 #include "TrackingTools/TrackFitters/interface/KFTrajectoryFitter.h"
0002 #include "TrackingTools/TrackFitters/interface/TrajectoryStateWithArbitraryError.h"
0003 #include "TrackingTools/TrackFitters/interface/DebugHelpers.h"
0004 #include "TrackingTools/TransientTrackingRecHit/interface/TransientTrackingRecHit.h"
0005 #include "FWCore/Utilities/interface/Exception.h"
0006 #include "FWCore/MessageLogger/interface/MessageLogger.h"
0007 #include "FWCore/Utilities/interface/isFinite.h"
0008 #include "DataFormats/TrackerRecHit2D/interface/BaseTrackerRecHit.h"
0009 #include "DataFormats/TrackerRecHit2D/interface/TkCloner.h"
0010 #include "FWCore/Utilities/interface/Likely.h"
0011
0012 const DetLayerGeometry KFTrajectoryFitter::dummyGeometry;
0013
0014 Trajectory KFTrajectoryFitter::fitOne(const Trajectory& aTraj, fitType type) const {
0015 if (aTraj.empty())
0016 return Trajectory();
0017
0018 const TM& firstTM = aTraj.firstMeasurement();
0019 TSOS firstTsos = TrajectoryStateWithArbitraryError()(firstTM.updatedState());
0020
0021 return fitOne(aTraj.seed(), aTraj.recHits(), firstTsos, type);
0022 }
0023
0024 Trajectory KFTrajectoryFitter::fitOne(const TrajectorySeed&, const RecHitContainer&, fitType) const {
0025 throw cms::Exception("TrackFitters",
0026 "KFTrajectoryFitter::fit(TrajectorySeed, <TransientTrackingRecHit>) not implemented");
0027
0028 return Trajectory();
0029 }
0030
0031 Trajectory KFTrajectoryFitter::fitOne(const TrajectorySeed& aSeed,
0032 const RecHitContainer& hits,
0033 const TSOS& firstPredTsos,
0034 fitType) const {
0035 if (hits.empty())
0036 return Trajectory();
0037
0038 if UNLIKELY (aSeed.direction() == anyDirection)
0039 throw cms::Exception("KFTrajectoryFitter", "TrajectorySeed::direction() requested but not set");
0040
0041 std::unique_ptr<Propagator> p_cloned = SetPropagationDirection(*thePropagator, aSeed.direction());
0042
0043 #ifdef EDM_ML_DEBUG
0044 LogDebug("TrackFitters")
0045 << " +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n"
0046 << " KFTrajectoryFitter::fit starting with " << hits.size() << " HITS";
0047
0048 for (unsigned int j = 0; j < hits.size(); j++) {
0049 if (hits[j]->det())
0050 LogTrace("TrackFitters") << "hit #:" << j + 1 << " rawId=" << hits[j]->det()->geographicalId().rawId()
0051 << " validity=" << hits[j]->isValid();
0052 else
0053 LogTrace("TrackFitters") << "hit #:" << j + 1 << " Hit with no Det information";
0054 }
0055 LogTrace("TrackFitters") << " INITIAL STATE " << firstPredTsos;
0056 #endif
0057
0058 Trajectory ret(aSeed, p_cloned->propagationDirection());
0059 Trajectory& myTraj = ret;
0060 myTraj.reserve(hits.size());
0061
0062 TSOS predTsos(firstPredTsos);
0063 TSOS currTsos;
0064
0065 int hitcounter = 0;
0066 for (const auto& ihit : hits) {
0067 ++hitcounter;
0068
0069 const TransientTrackingRecHit& hit = (*ihit);
0070
0071
0072
0073 if UNLIKELY ((!hit.isValid()) && hit.surface() == nullptr) {
0074 LogDebug("TrackFitters") << " Error: invalid hit with no GeomDet attached .... skipping";
0075 continue;
0076 }
0077
0078
0079
0080 if (hitcounter != 1)
0081 predTsos = p_cloned->propagate(currTsos, *(hit.surface()));
0082
0083 if UNLIKELY (!predTsos.isValid()) {
0084 LogDebug("TrackFitters") << "SOMETHING WRONG !"
0085 << "\n"
0086 << "KFTrajectoryFitter: predicted tsos not valid!\n"
0087 << "current TSOS: " << currTsos << "\n";
0088
0089 if (hit.surface())
0090 LogTrace("TrackFitters") << "next Surface: " << hit.surface()->position() << "\n";
0091
0092 if (myTraj.foundHits() >= minHits_) {
0093 LogDebug("TrackFitters") << " breaking trajectory"
0094 << "\n";
0095 break;
0096 } else {
0097 LogDebug("TrackFitters") << " killing trajectory"
0098 << "\n";
0099 return Trajectory();
0100 }
0101 }
0102
0103 if LIKELY (hit.isValid()) {
0104 assert((hit.geographicalId() != 0U) || !hit.canImproveWithTrack());
0105 assert(hit.surface() != nullptr);
0106
0107 LogTrace("TrackFitters") << "THE HIT IS VALID: updating hit with predTsos";
0108 assert((!hit.canImproveWithTrack()) || (nullptr != theHitCloner));
0109 assert((!hit.canImproveWithTrack()) || (nullptr != dynamic_cast<BaseTrackerRecHit const*>((ihit).get())));
0110 auto preciseHit = theHitCloner->makeShared(ihit, predTsos);
0111 dump(*preciseHit, hitcounter, "TrackFitters");
0112 assert(preciseHit->isValid());
0113 assert((preciseHit->geographicalId() != 0U) || (!preciseHit->canImproveWithTrack()));
0114 assert(preciseHit->surface() != nullptr);
0115
0116 if UNLIKELY (!preciseHit->isValid()) {
0117 LogTrace("TrackFitters") << "THE Precise HIT IS NOT VALID: using currTsos = predTsos"
0118 << "\n";
0119 currTsos = predTsos;
0120 myTraj.push(TM(predTsos, ihit, 0, theGeometry->idToLayer((ihit)->geographicalId())));
0121 } else {
0122 LogTrace("TrackFitters") << "THE Precise HIT IS VALID: updating currTsos"
0123 << "\n";
0124 currTsos = updator()->update(predTsos, *preciseHit);
0125
0126 bool badState = (!currTsos.isValid()) ||
0127 (hit.geographicalId().det() == DetId::Tracker &&
0128 (std::abs(currTsos.localParameters().qbp()) > 100 ||
0129 std::abs(currTsos.localParameters().position().y()) > 1000 ||
0130 std::abs(currTsos.localParameters().position().x()) > 1000)) ||
0131 edm::isNotFinite(currTsos.localParameters().qbp()) || !currTsos.localError().posDef();
0132 if UNLIKELY (badState) {
0133 if (!currTsos.isValid()) {
0134 edm::LogError("FailedUpdate") << "updating with the hit failed. Not updating the trajectory with the hit";
0135
0136 } else if (edm::isNotFinite(currTsos.localParameters().qbp())) {
0137 edm::LogError("TrajectoryNaN") << "Trajectory has NaN";
0138
0139 } else if (!currTsos.localError().posDef()) {
0140 edm::LogError("TrajectoryNotPosDef") << "Trajectory covariance is not positive-definite";
0141
0142 } else {
0143 LogTrace("FailedUpdate") << "updated state is valid but pretty bad, skipping. currTsos " << currTsos
0144 << "\n predTsos " << predTsos;
0145 }
0146 myTraj.push(TM(predTsos, ihit, 0, theGeometry->idToLayer((ihit)->geographicalId())));
0147
0148
0149 if (myTraj.foundHits() >= minHits_) {
0150 LogDebug("TrackFitters") << " breaking trajectory"
0151 << "\n";
0152 break;
0153 } else {
0154 LogDebug("TrackFitters") << " killing trajectory"
0155 << "\n";
0156 return Trajectory();
0157 }
0158 } else {
0159 if (preciseHit->det()) {
0160 myTraj.push(TM(predTsos,
0161 currTsos,
0162 preciseHit,
0163 estimator()->estimate(predTsos, *preciseHit).second,
0164 theGeometry->idToLayer(preciseHit->geographicalId())));
0165 } else {
0166 myTraj.push(TM(predTsos, currTsos, preciseHit, estimator()->estimate(predTsos, *preciseHit).second));
0167 }
0168 }
0169 }
0170 } else {
0171 dump(hit, hitcounter, "TrackFitters");
0172
0173 LogDebug("TrackFitters") << "THE HIT IS NOT VALID: using currTsos"
0174 << "\n";
0175 currTsos = predTsos;
0176 assert(((ihit)->det() == nullptr) || (ihit)->geographicalId() != 0U);
0177 if ((ihit)->det())
0178 myTraj.push(TM(predTsos, ihit, 0, theGeometry->idToLayer((ihit)->geographicalId())));
0179 else
0180 myTraj.push(TM(predTsos, ihit, 0));
0181 }
0182 LogTrace("TrackFitters") << "predTsos !"
0183 << "\n"
0184 << predTsos << " with local position " << predTsos.localPosition() << "currTsos !"
0185 << "\n"
0186 << currTsos << " with local position " << currTsos.localPosition();
0187 }
0188
0189 LogDebug("TrackFitters") << "Found 1 trajectory with " << myTraj.foundHits() << " valid hits\n";
0190
0191 return ret;
0192 }