KFTrajectoryFitter.cc

Go to the documentation of this file.

#include "TrackingTools/TrackFitters/interface/KFTrajectoryFitter.h"
#include "TrackingTools/TrackFitters/interface/TrajectoryStateWithArbitraryError.h"
#include "TrackingTools/TrackFitters/interface/DebugHelpers.h"
#include "TrackingTools/TransientTrackingRecHit/interface/TransientTrackingRecHit.h"
#include "FWCore/Utilities/interface/Exception.h"
#include "FWCore/MessageLogger/interface/MessageLogger.h"
#include "FWCore/Utilities/interface/isFinite.h"
#include "DataFormats/TrackerRecHit2D/interface/BaseTrackerRecHit.h"
#include "DataFormats/TrackerRecHit2D/interface/TkCloner.h"

const DetLayerGeometry KFTrajectoryFitter::dummyGeometry;

Trajectory KFTrajectoryFitter::fitOne(const Trajectory& aTraj, fitType type) const {

  if(aTraj.empty()) return Trajectory();

  const TM& firstTM = aTraj.firstMeasurement();
  TSOS firstTsos = TrajectoryStateWithArbitraryError()(firstTM.updatedState());

  return fitOne(aTraj.seed(), aTraj.recHits(), firstTsos,type);
}

Trajectory KFTrajectoryFitter::fitOne(const TrajectorySeed&,
                      const RecHitContainer&, fitType) const{

  throw cms::Exception("TrackFitters",
               "KFTrajectoryFitter::fit(TrajectorySeed, <TransientTrackingRecHit>) not implemented");

  return Trajectory();
}

Trajectory KFTrajectoryFitter::fitOne(const TrajectorySeed& aSeed,
                      const RecHitContainer& hits,
                      const TSOS& firstPredTsos,fitType) const
{
  if(hits.empty()) return Trajectory();


  if unlikely(aSeed.direction() == anyDirection)
    throw cms::Exception("KFTrajectoryFitter","TrajectorySeed::direction() requested but not set");

  std::unique_ptr<Propagator> p_cloned = SetPropagationDirection(*thePropagator,
                                                                 aSeed.direction());

#ifdef EDM_ML_DEBUG
  LogDebug("TrackFitters")
    <<" +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n"
    <<" KFTrajectoryFitter::fit starting with " << hits.size() <<" HITS";

  for (unsigned int j=0;j<hits.size();j++) {
    if (hits[j]->det())
      LogTrace("TrackFitters") << "hit #:" << j+1 << " rawId=" << hits[j]->det()->geographicalId().rawId()
                   << " validity=" << hits[j]->isValid();
    else
      LogTrace("TrackFitters") << "hit #:" << j+1 << " Hit with no Det information";
  }
  LogTrace("TrackFitters") << " INITIAL STATE "<< firstPredTsos;
#endif

  Trajectory ret(aSeed, p_cloned->propagationDirection());
  Trajectory & myTraj = ret;
  myTraj.reserve(hits.size());

  TSOS predTsos(firstPredTsos);
  TSOS currTsos;

  int hitcounter = 0;
  for(auto ihit : hits) {
    ++hitcounter;

    const TransientTrackingRecHit & hit = (*ihit);

    // if unlikely(hit.det() == nullptr) continue;

    if unlikely( (!hit.isValid()) && hit.surface() == nullptr) {
       LogDebug("TrackFitters")<< " Error: invalid hit with no GeomDet attached .... skipping";
      continue;
    }
   // if (hit.det() && hit.geographicalId()<1000U) LogDebug("TrackFitters")<< "Problem 0 det id for " << typeid(hit).name() << ' ' <<  hit.det()->geographicalId() ;
   // if (hit.isValid() && hit.geographicalId()<1000U) LogDebug("TrackFitters")<< "Problem 0 det id for " << typeid(hit).name() << ' ' <<  hit.det()->geographicalId();

    if ( hitcounter != 1) //no propagation needed for the first hit
      predTsos = p_cloned->propagate( currTsos, *(hit.surface()) );


    if unlikely(!predTsos.isValid()) {
      LogDebug("TrackFitters")
    << "SOMETHING WRONG !" << "\n"
    << "KFTrajectoryFitter: predicted tsos not valid!\n"
    << "current TSOS: " << currTsos << "\n";

      if(hit.surface()) LogTrace("TrackFitters") << "next Surface: " << hit.surface()->position() << "\n";

      if( myTraj.foundHits() >= minHits_ ) {
    LogDebug("TrackFitters") << " breaking trajectory" << "\n";
    break;
      } else {
    LogDebug("TrackFitters") << " killing trajectory" << "\n";
    return Trajectory();
      }
    }


    if likely(hit.isValid()) {
        assert( (hit.geographicalId()!=0U) | !hit.canImproveWithTrack() ) ;
        assert(hit.surface()!=nullptr);
    //update
    LogTrace("TrackFitters") << "THE HIT IS VALID: updating hit with predTsos";
        assert( (!hit.canImproveWithTrack()) | (nullptr!=theHitCloner));
        assert( (!hit.canImproveWithTrack()) | (nullptr!=dynamic_cast<BaseTrackerRecHit const*>((ihit).get())));
    auto preciseHit = theHitCloner->makeShared(ihit,predTsos);
        dump(*preciseHit,hitcounter,"TrackFitters");
        assert(preciseHit->isValid());
        assert( (preciseHit->geographicalId()!=0U)  | (!preciseHit->canImproveWithTrack()) );
        assert(preciseHit->surface()!=nullptr);

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

      }else{
      LogTrace("TrackFitters") << "THE Precise HIT IS VALID: updating currTsos" << "\n";
      currTsos = updator()->update(predTsos, *preciseHit);
      //check for valid hits with no det (refitter with constraints)
      bool badState = (!currTsos.isValid())
          || (hit.geographicalId().det() == DetId::Tracker
              &&
              (std::abs(currTsos.localParameters().qbp())>100
               || std::abs(currTsos.localParameters().position().y()) > 1000
               || std::abs(currTsos.localParameters().position().x()) > 1000
               ) ) 
            || edm::isNotFinite(currTsos.localParameters().qbp()) 
            || !currTsos.localError().posDef();
      if unlikely(badState){
        if (!currTsos.isValid()) {
          edm::LogError("FailedUpdate") <<"updating with the hit failed. Not updating the trajectory with the hit";

            } 
        else if (edm::isNotFinite(currTsos.localParameters().qbp())) {
              edm::LogError("TrajectoryNaN")<<"Trajectory has NaN";

            }
        else if (!currTsos.localError().posDef()) {
              edm::LogError("TrajectoryNotPosDef")<<"Trajectory covariance is not positive-definite";

            }
        else{ 
              LogTrace("FailedUpdate")<<"updated state is valid but pretty bad, skipping. currTsos " <<currTsos<<"\n predTsos "<<predTsos;
            }
        myTraj.push(TM(predTsos, ihit,0,theGeometry->idToLayer((ihit)->geographicalId())  ));
        //There is a no-fail policy here. So, it's time to give up
        //Keep the traj with invalid TSOS so that it's clear what happened
        if( myTraj.foundHits() >= minHits_ ) {
          LogDebug("TrackFitters") << " breaking trajectory" << "\n";
          break;
        } else {
          LogDebug("TrackFitters") << " killing trajectory" << "\n";
          return Trajectory();
        }
      } else{
        if (preciseHit->det()){
          myTraj.push(TM(predTsos, currTsos, preciseHit,
                          estimator()->estimate(predTsos, *preciseHit).second,
                          theGeometry->idToLayer(preciseHit->geographicalId())  ));
            }
        else{
               myTraj.push(TM(predTsos, currTsos, preciseHit,
                estimator()->estimate(predTsos, *preciseHit).second));
            }
      }
    }
      } else {  // invalid hit
        dump(hit,hitcounter,"TrackFitters");
        //no update
        LogDebug("TrackFitters") << "THE HIT IS NOT VALID: using currTsos" << "\n";
        currTsos = predTsos;
        assert( ((ihit)->det()==nullptr) || (ihit)->geographicalId()!=0U);
        if ((ihit)->det()) myTraj.push(TM(predTsos, ihit,0,theGeometry->idToLayer((ihit)->geographicalId())  ));
        else   myTraj.push(TM(predTsos, ihit,0));
    }
    LogTrace("TrackFitters")
      << "predTsos !" << "\n"
      << predTsos 
      <<" with local position " << predTsos.localPosition()
      <<"currTsos !" << "\n"
      << currTsos
      <<" with local position " << currTsos.localPosition();
  }

  LogDebug("TrackFitters") << "Found 1 trajectory with " << myTraj.foundHits() << " valid hits\n";

  return ret;
}