KFTrajectorySmoother.cc

Go to the documentation of this file.

#include "TrackingTools/TrackFitters/interface/KFTrajectorySmoother.h"
#include "TrackingTools/TrackFitters/interface/DebugHelpers.h"
#include "TrackingTools/TransientTrackingRecHit/interface/TransientTrackingRecHit.h"
#include "TrackingTools/TrackFitters/interface/TrajectoryStateWithArbitraryError.h"
#include "TrackingTools/TrackFitters/interface/TrajectoryStateCombiner.h"
#include "FWCore/MessageLogger/interface/MessageLogger.h"

#include "DataFormats/TrackerRecHit2D/interface/TkCloner.h"
#include "DataFormats/TrackerRecHit2D/interface/BaseTrackerRecHit.h"
#include "FWCore/Utilities/interface/Likely.h"

KFTrajectorySmoother::~KFTrajectorySmoother() {
  delete theAlongPropagator;
  delete theOppositePropagator;
  delete theUpdator;
  delete theEstimator;
}

Trajectory KFTrajectorySmoother::trajectory(const Trajectory& aTraj) const {
  if (aTraj.empty())
    return Trajectory();

  const Propagator* usePropagator = theAlongPropagator;
  if (aTraj.direction() == alongMomentum) {
    usePropagator = theOppositePropagator;
  }

  const std::vector<TM>& avtm = aTraj.measurements();

#ifdef EDM_ML_DEBUG
  LogDebug("TrackFitters") << "KFTrajectorySmoother::trajectories starting with " << avtm.size() << " HITS\n";
  for (unsigned int j = 0; j < avtm.size(); j++) {
    if (avtm[j].recHit()->det())
      LogTrace("TrackFitters") << "hit #:" << j + 1 << " rawId=" << avtm[j].recHit()->det()->geographicalId().rawId()
                               << " validity=" << avtm[j].recHit()->isValid();
    else
      LogTrace("TrackFitters") << "hit #:" << j + 1 << " Hit with no Det information";
  }
#endif  // EDM_ML_DEBUG

  TrajectoryStateCombiner combiner;
  bool retry = false;
  auto start = avtm.rbegin();

  do {
    auto hitSize = avtm.rend() - start;
    if UNLIKELY (hitSize < minHits_) {
      LogDebug("TrackFitters") << " killing trajectory"
                               << "\n";
      return Trajectory();
    }
    Trajectory ret(aTraj.seed(), usePropagator->propagationDirection());
    Trajectory& myTraj = ret;
    myTraj.reserve(hitSize);
    retry = false;

    TSOS predTsos = (*start).forwardPredictedState();
    predTsos.rescaleError(theErrorRescaling);
    TSOS currTsos;

    auto hitCounter = hitSize;
    for (std::vector<TM>::const_reverse_iterator itm = start; itm != (avtm.rend()); ++itm, --hitCounter) {
      TransientTrackingRecHit::ConstRecHitPointer hit = itm->recHit();

      //check surface just for safety: should never be ==0 because they are skipped in the fitter
      // if UNLIKELY(hit->det() == nullptr) continue;
      if UNLIKELY (hit->surface() == nullptr) {
        LogDebug("TrackFitters") << " Error: invalid hit with no GeomDet attached .... skipping";
        continue;
      }

      if (itm != start)  //no propagation needed for first smoothed (==last fitted) hit
        predTsos = usePropagator->propagate(currTsos, *(hit->surface()));

      if UNLIKELY (!predTsos.isValid()) {
        LogDebug("TrackFitters") << "KFTrajectorySmoother: predicted tsos not valid!";
        LogDebug("TrackFitters") << " retry with last hit removed"
                                 << "\n";
        LogDebug("TrackFitters")
            // std::cout
            << "tsos not valid " << currTsos.globalMomentum().perp() << ' ' << hitSize << ' ' << hitCounter << ' '
            << int(hit->geographicalId()) << ' ' << hit->surface()->position().perp() << ' ' << hit->surface()->eta()
            << ' ' << hit->surface()->phi() << std::endl;
        start++;
        retry = true;
        break;
      }

      if (hit->isValid()) {
        TSOS combTsos, smooTsos;

        //3 different possibilities to calculate smoothed state:
        //1: update combined predictions with hit
        //2: combine fwd-prediction with bwd-filter
        //3: combine bwd-prediction with fwd-filter

        //combTsos is the predicted state with N-1 hits information. this means:
        //forward predicted state for first smoothed (last fitted) hit
        //backward predicted state for last smoothed (first fitted) hit
        //combination of forward and backward predictions for other hits
        if (itm == start)
          combTsos = itm->forwardPredictedState();
        else if (hitCounter == 1)
          combTsos = predTsos;
        else
          combTsos = combiner(predTsos, itm->forwardPredictedState());

        if UNLIKELY (!combTsos.isValid()) {
          LogDebug("TrackFitters") << "KFTrajectorySmoother: combined tsos not valid!\n"
                                   << "pred Tsos pos: " << predTsos.globalPosition() << "\n"
                                   << "pred Tsos mom: " << predTsos.globalMomentum() << "\n"
                                   << "TrackingRecHit: " << hit->surface()->toGlobal(hit->localPosition()) << "\n";
          start++;
          retry = true;
          break;
        }

        assert((hit->geographicalId() != 0U) | (!hit->canImproveWithTrack()));
        assert(hit->surface() != nullptr);
        assert((!(hit)->canImproveWithTrack()) | (nullptr != theHitCloner));
        assert((!(hit)->canImproveWithTrack()) | (nullptr != dynamic_cast<BaseTrackerRecHit const*>(hit.get())));
        auto preciseHit = theHitCloner->makeShared(hit, combTsos);
        assert(preciseHit->isValid());
        assert((preciseHit->geographicalId() != 0U) | (!preciseHit->canImproveWithTrack()));
        assert(preciseHit->surface() != nullptr);

        dump(*hit, hitCounter, "TrackFitters");

        if UNLIKELY (!preciseHit->isValid()) {
          LogTrace("TrackFitters") << "THE Precise HIT IS NOT VALID: using currTsos = predTsos"
                                   << "\n";
          currTsos = predTsos;
          myTraj.push(TM(predTsos, hit, 0, theGeometry->idToLayer(hit->geographicalId())));
        } else {
          LogTrace("TrackFitters") << "THE Precise HIT IS VALID: updating currTsos"
                                   << "\n";

          //update backward predicted tsos with the hit
          currTsos = updator()->update(predTsos, *preciseHit);
          if UNLIKELY (!currTsos.isValid()) {
            currTsos = predTsos;
            edm::LogWarning("KFSmoother_UpdateFailed")
                << "Failed updating state with hit. Rolling back to non-updated state.\n"
                << "State: " << predTsos << "Hit local pos:  " << hit->localPosition() << "\n"
                << "Hit local err:  " << hit->localPositionError() << "\n"
                << "Hit global pos: " << hit->globalPosition() << "\n"
                << "Hit global err: " << hit->globalPositionError().matrix() << "\n";
          }

          //smooTsos updates the N-1 hits prediction with the hit
          if (itm == start)
            smooTsos = itm->updatedState();
          else if (hitCounter == 1)
            smooTsos = currTsos;
          else
            smooTsos = combiner(itm->forwardPredictedState(), currTsos);

          if UNLIKELY (!smooTsos.isValid()) {
            LogDebug("TrackFitters") << "KFTrajectorySmoother: smoothed tsos not valid!";
            start++;
            retry = true;
            break;
          }

          double estimate;
          if (itm != start)
            estimate = estimator()->estimate(combTsos, *preciseHit).second;  //correct?
          else
            estimate = itm->estimate();

          LogTrace("TrackFitters") << "predTsos !"
                                   << "\n"
                                   << predTsos << " with local position " << predTsos.localPosition() << "\n\n"
                                   << "currTsos !"
                                   << "\n"
                                   << currTsos << "\n"
                                   << " with local position " << currTsos.localPosition() << "\n\n"
                                   << "smooTsos !"
                                   << "\n"
                                   << smooTsos << " with local position " << smooTsos.localPosition() << "\n\n"
                                   << "smoothing estimate (with combTSOS)=" << estimate << "\n"
                                   << "filtering estimate=" << itm->estimate() << "\n";

          //check for valid hits with no det (refitter with constraints)
          if (preciseHit->det())
            myTraj.push(TM(itm->forwardPredictedState(),
                           predTsos,
                           smooTsos,
                           preciseHit,
                           estimate,
                           theGeometry->idToLayer(preciseHit->geographicalId())),
                        estimator()->estimate(predTsos, *preciseHit).second);
          else
            myTraj.push(TM(itm->forwardPredictedState(), predTsos, smooTsos, preciseHit, estimate),
                        estimator()->estimate(predTsos, *preciseHit).second);
          //itm->estimate());
        }
      } else {
        LogDebug("TrackFitters") << "----------------- HIT #" << hitCounter << " (INVALID)-----------------------";

        //no update
        currTsos = predTsos;
        TSOS combTsos;
        if (itm == start)
          combTsos = itm->forwardPredictedState();
        else if (hitCounter == 1)
          combTsos = predTsos;
        else
          combTsos = combiner(predTsos, itm->forwardPredictedState());

        if UNLIKELY (!combTsos.isValid()) {
          LogDebug("TrackFitters") << "KFTrajectorySmoother: combined tsos not valid!";
          return Trajectory();
        }
        assert((hit->det() == nullptr) || hit->geographicalId() != 0U);
        if (hit->det())
          myTraj.push(TM(
              itm->forwardPredictedState(), predTsos, combTsos, hit, 0, theGeometry->idToLayer(hit->geographicalId())));
        else
          myTraj.push(TM(itm->forwardPredictedState(), predTsos, combTsos, hit, 0));
      }
    }  // for loop

    if (!retry)
      return ret;
  } while (true);

  return Trajectory();
}