TrackFitters/src/KFTrajectoryFitter.cc

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     // if UNLIKELY(hit.det() == nullptr) continue;
0072
0073     if UNLIKELY ((!hit.isValid()) && hit.surface() == nullptr) {
0074       LogDebug("TrackFitters") << " Error: invalid hit with no GeomDet attached .... skipping";
0075       continue;
0076     }
0077     // if (hit.det() && hit.geographicalId()<1000U) LogDebug("TrackFitters")<< "Problem 0 det id for " << typeid(hit).name() << ' ' <<  hit.det()->geographicalId() ;
0078     // if (hit.isValid() && hit.geographicalId()<1000U) LogDebug("TrackFitters")<< "Problem 0 det id for " << typeid(hit).name() << ' ' <<  hit.det()->geographicalId();
0079
0080     if (hitcounter != 1)  //no propagation needed for the first hit
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       //update
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         //check for valid hits with no det (refitter with constraints)
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           //There is a no-fail policy here. So, it's time to give up
0148           //Keep the traj with invalid TSOS so that it's clear what happened
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 {  // invalid hit
0171       dump(hit, hitcounter, "TrackFitters");
0172       //no update
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 }