TrackFitters/src/KFTrajectorySmoother.cc

0001 #include "TrackingTools/TrackFitters/interface/KFTrajectorySmoother.h"
0002 #include "TrackingTools/TrackFitters/interface/DebugHelpers.h"
0003 #include "TrackingTools/TransientTrackingRecHit/interface/TransientTrackingRecHit.h"
0004 #include "TrackingTools/TrackFitters/interface/TrajectoryStateWithArbitraryError.h"
0005 #include "TrackingTools/TrackFitters/interface/TrajectoryStateCombiner.h"
0006 #include "FWCore/MessageLogger/interface/MessageLogger.h"
0007
0008 #include "DataFormats/TrackerRecHit2D/interface/TkCloner.h"
0009 #include "DataFormats/TrackerRecHit2D/interface/BaseTrackerRecHit.h"
0010 #include "FWCore/Utilities/interface/Likely.h"
0011
0012 KFTrajectorySmoother::~KFTrajectorySmoother() {
0013   delete theAlongPropagator;
0014   delete theOppositePropagator;
0015   delete theUpdator;
0016   delete theEstimator;
0017 }
0018
0019 Trajectory KFTrajectorySmoother::trajectory(const Trajectory& aTraj) const {
0020   if (aTraj.empty())
0021     return Trajectory();
0022
0023   const Propagator* usePropagator = theAlongPropagator;
0024   if (aTraj.direction() == alongMomentum) {
0025     usePropagator = theOppositePropagator;
0026   }
0027
0028   const std::vector<TM>& avtm = aTraj.measurements();
0029
0030 #ifdef EDM_ML_DEBUG
0031   LogDebug("TrackFitters") << "KFTrajectorySmoother::trajectories starting with " << avtm.size() << " HITS\n";
0032   for (unsigned int j = 0; j < avtm.size(); j++) {
0033     if (avtm[j].recHit()->det())
0034       LogTrace("TrackFitters") << "hit #:" << j + 1 << " rawId=" << avtm[j].recHit()->det()->geographicalId().rawId()
0035                                << " validity=" << avtm[j].recHit()->isValid();
0036     else
0037       LogTrace("TrackFitters") << "hit #:" << j + 1 << " Hit with no Det information";
0038   }
0039 #endif  // EDM_ML_DEBUG
0040
0041   TrajectoryStateCombiner combiner;
0042   bool retry = false;
0043   auto start = avtm.rbegin();
0044
0045   do {
0046     auto hitSize = avtm.rend() - start;
0047     if UNLIKELY (hitSize < minHits_) {
0048       LogDebug("TrackFitters") << " killing trajectory"
0049                                << "\n";
0050       return Trajectory();
0051     }
0052     Trajectory ret(aTraj.seed(), usePropagator->propagationDirection());
0053     Trajectory& myTraj = ret;
0054     myTraj.reserve(hitSize);
0055     retry = false;
0056
0057     TSOS predTsos = (*start).forwardPredictedState();
0058     predTsos.rescaleError(theErrorRescaling);
0059     TSOS currTsos;
0060
0061     auto hitCounter = hitSize;
0062     for (std::vector<TM>::const_reverse_iterator itm = start; itm != (avtm.rend()); ++itm, --hitCounter) {
0063       TransientTrackingRecHit::ConstRecHitPointer hit = itm->recHit();
0064
0065       //check surface just for safety: should never be ==0 because they are skipped in the fitter
0066       // if UNLIKELY(hit->det() == nullptr) continue;
0067       if UNLIKELY (hit->surface() == nullptr) {
0068         LogDebug("TrackFitters") << " Error: invalid hit with no GeomDet attached .... skipping";
0069         continue;
0070       }
0071
0072       if (itm != start)  //no propagation needed for first smoothed (==last fitted) hit
0073         predTsos = usePropagator->propagate(currTsos, *(hit->surface()));
0074
0075       if UNLIKELY (!predTsos.isValid()) {
0076         LogDebug("TrackFitters") << "KFTrajectorySmoother: predicted tsos not valid!";
0077         LogDebug("TrackFitters") << " retry with last hit removed"
0078                                  << "\n";
0079         LogDebug("TrackFitters")
0080             // std::cout
0081             << "tsos not valid " << currTsos.globalMomentum().perp() << ' ' << hitSize << ' ' << hitCounter << ' '
0082             << int(hit->geographicalId()) << ' ' << hit->surface()->position().perp() << ' ' << hit->surface()->eta()
0083             << ' ' << hit->surface()->phi() << std::endl;
0084         start++;
0085         retry = true;
0086         break;
0087       }
0088
0089       if (hit->isValid()) {
0090         TSOS combTsos, smooTsos;
0091
0092         //3 different possibilities to calculate smoothed state:
0093         //1: update combined predictions with hit
0094         //2: combine fwd-prediction with bwd-filter
0095         //3: combine bwd-prediction with fwd-filter
0096
0097         //combTsos is the predicted state with N-1 hits information. this means:
0098         //forward predicted state for first smoothed (last fitted) hit
0099         //backward predicted state for last smoothed (first fitted) hit
0100         //combination of forward and backward predictions for other hits
0101         if (itm == start)
0102           combTsos = itm->forwardPredictedState();
0103         else if (hitCounter == 1)
0104           combTsos = predTsos;
0105         else
0106           combTsos = combiner(predTsos, itm->forwardPredictedState());
0107
0108         if UNLIKELY (!combTsos.isValid()) {
0109           LogDebug("TrackFitters") << "KFTrajectorySmoother: combined tsos not valid!\n"
0110                                    << "pred Tsos pos: " << predTsos.globalPosition() << "\n"
0111                                    << "pred Tsos mom: " << predTsos.globalMomentum() << "\n"
0112                                    << "TrackingRecHit: " << hit->surface()->toGlobal(hit->localPosition()) << "\n";
0113           start++;
0114           retry = true;
0115           break;
0116         }
0117
0118         assert((hit->geographicalId() != 0U) || (!hit->canImproveWithTrack()));
0119         assert(hit->surface() != nullptr);
0120         assert((!(hit)->canImproveWithTrack()) || (nullptr != theHitCloner));
0121         assert((!(hit)->canImproveWithTrack()) || (nullptr != dynamic_cast<BaseTrackerRecHit const*>(hit.get())));
0122         auto preciseHit = theHitCloner->makeShared(hit, combTsos);
0123         assert(preciseHit->isValid());
0124         assert((preciseHit->geographicalId() != 0U) || (!preciseHit->canImproveWithTrack()));
0125         assert(preciseHit->surface() != nullptr);
0126
0127         dump(*hit, hitCounter, "TrackFitters");
0128
0129         if UNLIKELY (!preciseHit->isValid()) {
0130           LogTrace("TrackFitters") << "THE Precise HIT IS NOT VALID: using currTsos = predTsos"
0131                                    << "\n";
0132           currTsos = predTsos;
0133           myTraj.push(TM(predTsos, hit, 0, theGeometry->idToLayer(hit->geographicalId())));
0134         } else {
0135           LogTrace("TrackFitters") << "THE Precise HIT IS VALID: updating currTsos"
0136                                    << "\n";
0137
0138           //update backward predicted tsos with the hit
0139           currTsos = updator()->update(predTsos, *preciseHit);
0140           if UNLIKELY (!currTsos.isValid()) {
0141             currTsos = predTsos;
0142             edm::LogWarning("KFSmoother_UpdateFailed")
0143                 << "Failed updating state with hit. Rolling back to non-updated state.\n"
0144                 << "State: " << predTsos << "Hit local pos:  " << hit->localPosition() << "\n"
0145                 << "Hit local err:  " << hit->localPositionError() << "\n"
0146                 << "Hit global pos: " << hit->globalPosition() << "\n"
0147                 << "Hit global err: " << hit->globalPositionError().matrix() << "\n";
0148           }
0149
0150           //smooTsos updates the N-1 hits prediction with the hit
0151           if (itm == start)
0152             smooTsos = itm->updatedState();
0153           else if (hitCounter == 1)
0154             smooTsos = currTsos;
0155           else
0156             smooTsos = combiner(itm->forwardPredictedState(), currTsos);
0157
0158           if UNLIKELY (!smooTsos.isValid()) {
0159             LogDebug("TrackFitters") << "KFTrajectorySmoother: smoothed tsos not valid!";
0160             start++;
0161             retry = true;
0162             break;
0163           }
0164
0165           double estimate;
0166           if (itm != start)
0167             estimate = estimator()->estimate(combTsos, *preciseHit).second;  //correct?
0168           else
0169             estimate = itm->estimate();
0170
0171           LogTrace("TrackFitters") << "predTsos !"
0172                                    << "\n"
0173                                    << predTsos << " with local position " << predTsos.localPosition() << "\n\n"
0174                                    << "currTsos !"
0175                                    << "\n"
0176                                    << currTsos << "\n"
0177                                    << " with local position " << currTsos.localPosition() << "\n\n"
0178                                    << "smooTsos !"
0179                                    << "\n"
0180                                    << smooTsos << " with local position " << smooTsos.localPosition() << "\n\n"
0181                                    << "smoothing estimate (with combTSOS)=" << estimate << "\n"
0182                                    << "filtering estimate=" << itm->estimate() << "\n";
0183
0184           //check for valid hits with no det (refitter with constraints)
0185           if (preciseHit->det())
0186             myTraj.push(TM(itm->forwardPredictedState(),
0187                            predTsos,
0188                            smooTsos,
0189                            preciseHit,
0190                            estimate,
0191                            theGeometry->idToLayer(preciseHit->geographicalId())),
0192                         estimator()->estimate(predTsos, *preciseHit).second);
0193           else
0194             myTraj.push(TM(itm->forwardPredictedState(), predTsos, smooTsos, preciseHit, estimate),
0195                         estimator()->estimate(predTsos, *preciseHit).second);
0196           //itm->estimate());
0197         }
0198       } else {
0199         LogDebug("TrackFitters") << "----------------- HIT #" << hitCounter << " (INVALID)-----------------------";
0200
0201         //no update
0202         currTsos = predTsos;
0203         TSOS combTsos;
0204         if (itm == start)
0205           combTsos = itm->forwardPredictedState();
0206         else if (hitCounter == 1)
0207           combTsos = predTsos;
0208         else
0209           combTsos = combiner(predTsos, itm->forwardPredictedState());
0210
0211         if UNLIKELY (!combTsos.isValid()) {
0212           LogDebug("TrackFitters") << "KFTrajectorySmoother: combined tsos not valid!";
0213           return Trajectory();
0214         }
0215         assert((hit->det() == nullptr) || hit->geographicalId() != 0U);
0216         if (hit->det())
0217           myTraj.push(TM(
0218               itm->forwardPredictedState(), predTsos, combTsos, hit, 0, theGeometry->idToLayer(hit->geographicalId())));
0219         else
0220           myTraj.push(TM(itm->forwardPredictedState(), predTsos, combTsos, hit, 0));
0221       }
0222     }  // for loop
0223
0224     if (!retry)
0225       return ret;
0226   } while (true);
0227
0228   return Trajectory();
0229 }