/data/refman/pasoursint/CMSSW_5_3_9_patch3/src/TrackingTools/TrackFitters/src/KFFittingSmoother.cc

Go to the documentation of this file.

#include "TrackingTools/TrackFitters/interface/KFFittingSmoother.h"
// #include "TrackingTools/MaterialEffects/interface/PropagatorWithMaterial.h"
#include "TrackingTools/KalmanUpdators/interface/KFUpdator.h"
#include "TrackingTools/TrackFitters/interface/KFTrajectoryFitter.h"
#include "TrackingTools/TrackFitters/interface/KFTrajectorySmoother.h"
#include "TrackingTools/GeomPropagators/interface/AnalyticalPropagator.h"
#include "FWCore/MessageLogger/interface/MessageLogger.h"
#include "TrackingTools/TransientTrackingRecHit/interface/InvalidTransientRecHit.h"

// ggiurgiu@fnal.gov: Add headers needed to cut on pixel hit probability
#include "DataFormats/TrackerRecHit2D/interface/SiPixelRecHit.h"
#include "DataFormats/SiPixelDetId/interface/PXBDetId.h"
#include "DataFormats/SiPixelDetId/interface/PXFDetId.h"
#include <cmath>

using namespace std;

KFFittingSmoother::~KFFittingSmoother() 
{
  delete theSmoother;
  delete theFitter;
}

vector<Trajectory> KFFittingSmoother::fit(const Trajectory& t) const 
{
  vector<Trajectory> smoothed;
  if ( t.isValid() ) 
    { 
      vector<Trajectory> fitted = fitter()->fit(t);
      smoothingStep(fitted, smoothed);
    }
  return smoothed;
}

bool KFFittingSmoother::checkForNans(const Trajectory & theTraj) const
{
  if (std::isnan(theTraj.chiSquared ())) return false;
  const std::vector<TrajectoryMeasurement> & vtm = theTraj.measurements();
  for (std::vector<TrajectoryMeasurement>::const_iterator tm=vtm.begin(); tm!=vtm.end();tm++) {
    if (std::isnan(tm->estimate())) return false;
    AlgebraicVector5 v = tm->updatedState ().localParameters ().vector();
    for (int i=0;i<5;++i) if (std::isnan(v[i])) return false;
    const AlgebraicSymMatrix55 & m = tm->updatedState ().curvilinearError ().matrix();
    for (int i=0;i<5;++i) 
      for (int j=0;j<(i+1);++j) if (std::isnan(m(i,j))) return false;
  }
  return true;
}

vector<Trajectory> KFFittingSmoother::fit(const TrajectorySeed& aSeed,
                                          const RecHitContainer& hits, 
                                          const TrajectoryStateOnSurface& firstPredTsos) const 
{
  LogDebug("TrackFitters") << "In KFFittingSmoother::fit";

  //if(hits.empty()) return vector<Trajectory>(); // gio: moved later to optimize return value
  
  bool hasoutliers;
  bool has_low_pixel_prob; // ggiurgiu@fnal.gov: Add flag for pixel hits with low template probability

  // ggiurgiu@fnal.gov: If log(Prob) < -15.0 or if Prob <= 0.0 then set log(Prob) = -15.0
  double log_pixel_probability_lower_limit = -15.0;

  RecHitContainer myHits = hits; 
  vector<Trajectory> smoothed;
  vector<Trajectory> tmp_first;

  do
    {
      if ( hits.empty() ) 
        { 
          smoothed.clear(); 
          break; 
        }
      
      //if no outliers the fit is done only once
      //for (unsigned int j=0;j<myHits.size();j++) { 
      //if (myHits[j]->det()) 
      //LogTrace("TrackFitters") << "hit #:" << j+1 << " rawId=" << myHits[j]->det()->geographicalId().rawId() 
      //<< " validity=" << myHits[j]->isValid();
      //else
      //LogTrace("TrackFitters") << "hit #:" << j+1 << " Hit with no Det information";
      //}
      
      hasoutliers        = false;
      has_low_pixel_prob = false; // ggiurgiu@fnal.gov
      
      double cut = theEstimateCut;
      
      double log_pixel_prob_cut = theLogPixelProbabilityCut;  // ggiurgiu@fnal.gov
      
      
      unsigned int outlierId = 0;
      const GeomDet* outlierDet = 0;
      
      unsigned int low_pixel_prob_Id = 0; // ggiurgiu@fnal.gov
      const GeomDet* low_pixel_prob_Det = 0; // ggiurgiu@fnal.gov
      
      //call the fitter
      vector<Trajectory> fitted = fitter()->fit(aSeed, myHits, firstPredTsos);
      //call the smoother
      smoothed.clear();
      smoothingStep(fitted, smoothed);
      
      //if (tmp_first.size()==0) tmp_first = smoothed; moved later
      
      if ( smoothed.empty() ) 
        {
          if ( rejectTracksFlag )
            {
              LogTrace("TrackFitters") << "smoothed.size()==0 => trajectory rejected";
              //return vector<Trajectory>(); // break is enough to get this
            } 
          else 
            {
              LogTrace("TrackFitters") << "smoothed.size()==0 => returning orignal trajectory" ;
              smoothed.swap(tmp_first); // if first attempt, tmp_first would be empty anyway
            }
          break;
        } 
      //else {
      //LogTrace("TrackFitters") << "dump hits after smoothing";
      //Trajectory::DataContainer meas = smoothed[0].measurements();
      //for (Trajectory::DataContainer::iterator it=meas.begin();it!=meas.end();++it) {
      //LogTrace("TrackFitters") << "hit #" << meas.end()-it-1 << " validity=" << it->recHit()->isValid() 
      //<< " det=" << it->recHit()->geographicalId().rawId();
      //}
      //}
      if (!checkForNans(smoothed[0])) {
        edm::LogError("TrackNaN")<<"Track has NaN";
        smoothed.clear();
        break;
      }
      
      if ( theEstimateCut > 0 || log_pixel_prob_cut > log_pixel_probability_lower_limit ) 
        {
          if ( smoothed[0].foundHits() < theMinNumberOfHits ) 
            {
              if ( rejectTracksFlag ) 
                {
                  LogTrace("TrackFitters") << "smoothed[0].foundHits()<theMinNumberOfHits => trajectory rejected";
                  smoothed.clear();
                  //return vector<Trajectory>(); // break is enough
                } 
              else 
                {
                  // it might be it's the first step
                  if ( !tmp_first.empty() ) 
                    { 
                      tmp_first.swap(smoothed); 
                    } 
                  
                  LogTrace("TrackFitters") 
                    << "smoothed[0].foundHits()<theMinNumberOfHits => returning orignal trajectory with chi2=" 
                    <<  smoothed[0].chiSquared() ;
                }
              break;
            }
          
          // Check if there are outliers or low probability pixel rec hits
          const std::vector<TrajectoryMeasurement> & vtm = smoothed[0].measurements();
          
          double log_pixel_hit_probability = -999999.9;
          
          for (std::vector<TrajectoryMeasurement>::const_iterator tm=vtm.begin(); tm!=vtm.end();tm++)
            {
              double estimate = tm->estimate();
              
              // --- here is the block of code about generic chi2-based Outlier Rejection ---
              if ( estimate > cut && theEstimateCut > 0 ) 
                {
                  hasoutliers = true;
                  cut = estimate;
                  outlierId  = tm->recHit()->geographicalId().rawId();
                  outlierDet = tm->recHit()->det();
                }
              // --- here the block of code about generic chi2-based Outlier Rejection ends ---


              // --- here is the block of code about PXL Outlier Rejection ---
              if(log_pixel_prob_cut > log_pixel_probability_lower_limit){ 
                // TO BE FIXED: the following code should really be moved into an external class or 
                // at least in a separate function. Current solution is ugly!
                // The KFFittingSmoother shouldn't handle the details of 
                // Outliers identification and rejection. It shoudl just fit tracks.

                // ggiurgiu@fnal.gov: Get pixel hit probability here 
                TransientTrackingRecHit::ConstRecHitPointer hit = tm->recHit();
                unsigned int testSubDetID = ( hit->geographicalId().subdetId() );
                
                if ( hit->isValid() && 
                     hit->geographicalId().det() == DetId::Tracker && 
                     ( testSubDetID == PixelSubdetector::PixelBarrel || 
                       testSubDetID == PixelSubdetector::PixelEndcap )
                     )
                  {
                    // get the enclosed persistent hit
                    const TrackingRecHit* persistentHit = hit->hit();
                    
                    // check if it's not null, and if it's a valid pixel hit
                    if ( !persistentHit == 0 && 
                         typeid(*persistentHit) == typeid(SiPixelRecHit) ) 
                      {
                        
                        // tell the C++ compiler that the hit is a pixel hit
                        const SiPixelRecHit* pixhit = dynamic_cast<const SiPixelRecHit*>( hit->hit() );
                        
                        double pixel_hit_probability = (float)pixhit->clusterProbability(0);
                        
                        if ( pixel_hit_probability < 0.0 )
                          LogDebug("From KFFittingSmoother.cc") 
                            << "Wraning : Negative pixel hit probability !!!! Will set the probability to 10^{-15} !!!" << endl;
                        
                        if ( pixel_hit_probability <= 0.0 || log10( pixel_hit_probability ) < log_pixel_probability_lower_limit )  
                          log_pixel_hit_probability = log_pixel_probability_lower_limit; 
                        else 
                          log_pixel_hit_probability = log10( pixel_hit_probability );
                        
                        int qbin = (int)pixhit->qBin();

                        if ( ( log_pixel_hit_probability <  log_pixel_prob_cut ) &&
                             ( qbin                      != 0                  ) )
                          {
                            has_low_pixel_prob = true;
                            log_pixel_prob_cut = log_pixel_hit_probability;
                            low_pixel_prob_Id  = tm->recHit()->geographicalId().rawId();
                            low_pixel_prob_Det = tm->recHit()->det();
                          }         
                        
                      } // if ( !persistentHit == 0 && ... )
                    
                  } // if ( hit->isValid() && ... )
              }       
              // --- here the block of code about PXL Outlier Rejection ends --- 
              

            } // for (std::vector<TrajectoryMeasurement>::const_iterator tm=vtm.begin(); tm!=vtm.end(); tm++)
          
      
          if ( hasoutliers || has_low_pixel_prob ) 
            { // Reject outliers and pixel hits with low probability 
              
              // Replace outlier hit or low probability pixel hit with invalid hit
              for ( unsigned int j=0; j<myHits.size(); ++j ) 
                { 
                  if ( hasoutliers && outlierId == myHits[j]->geographicalId().rawId() )
                    {
                      LogTrace("TrackFitters") << "Rejecting outlier hit  with estimate " << cut << " at position " 
                                               << j << " with rawId=" << myHits[j]->geographicalId().rawId();
                      LogTrace("TrackFitters") << "The fit will be repeated without the outlier";
                      myHits[j] = InvalidTransientRecHit::build(outlierDet, TrackingRecHit::missing);
                    }
                  else if ( has_low_pixel_prob && low_pixel_prob_Id == myHits[j]->geographicalId().rawId() )
                    {
                      LogTrace("TrackFitters") << "Rejecting low proability pixel hit with log_pixel_prob_cut = " 
                                               << log_pixel_prob_cut << " at position " 
                                               << j << " with rawId =" << myHits[j]->geographicalId().rawId();
                      LogTrace("TrackFitters") << "The fit will be repeated without the outlier";
                      myHits[j] = InvalidTransientRecHit::build(low_pixel_prob_Det, TrackingRecHit::missing);
                    }
                  
                } // for ( unsigned int j=0; j<myHits.size(); ++j)
              
              // Look if there are two consecutive invalid hits
              if ( breakTrajWith2ConsecutiveMissing ) 
                {
                  unsigned int firstinvalid = myHits.size()-1;
                  for ( unsigned int j=0; j<myHits.size()-1; ++j ) 
                    { 
                      if ( ((myHits[j  ]->type() == TrackingRecHit::missing) && (myHits[j  ]->geographicalId().rawId() != 0)) && 
                           ((myHits[j+1]->type() == TrackingRecHit::missing) && (myHits[j+1]->geographicalId().rawId() != 0)) ) 
                        {
                          firstinvalid = j;
                          LogTrace("TrackFitters") << "Found two consecutive missing hits. First invalid: " << firstinvalid;
                          break;
                        }
                    }
                  
                  //reject all the hits after the last valid before two consecutive invalid (missing) hits
                  //hits are sorted in the same order as in the track candidate FIXME??????
                  myHits.erase(myHits.begin()+firstinvalid,myHits.end());
                  
                }
              
            } // if ( hasoutliers || has_low_pixel_prob ) 
          
        } // if ( theEstimateCut > 0 ... )
  
      if ( ( hasoutliers ||        // otherwise there won't be a 'next' loop where tmp_first could be useful 
             has_low_pixel_prob ) &&  // ggiurgiu@fnal.gov
           !rejectTracksFlag &&  // othewrise we won't ever need tmp_first
           tmp_first.empty() ) 
        { // only at first step
          smoothed.swap(tmp_first);
        }   
      
    } // do
  while ( hasoutliers || has_low_pixel_prob ); // ggiurgiu@fnal.gov
  
  if ( !smoothed.empty() ) 
    {
      if ( noInvalidHitsBeginEnd ) 
        {
          // discard latest dummy measurements
          if ( !smoothed[0].empty() && 
               !smoothed[0].lastMeasurement().recHit()->isValid() ) 
            LogTrace("TrackFitters") << "Last measurement is invalid";
        
          while ( !smoothed[0].empty() && 
                  !smoothed[0].lastMeasurement().recHit()->isValid() ) 
            smoothed[0].pop();
          
          //remove the invalid hits at the begin of the trajectory
          if ( !smoothed[0].firstMeasurement().recHit()->isValid() ) 
            {
              LogTrace("TrackFitters") << "First measurement is invalid";
              Trajectory tmpTraj(smoothed[0].seed(),smoothed[0].direction());
              Trajectory::DataContainer meas = smoothed[0].measurements();
              
              Trajectory::DataContainer::iterator it;//first valid hit
              for ( it=meas.begin(); it!=meas.end(); ++it ) 
                {
                  if ( !it->recHit()->isValid() ) 
                    continue;
                  else break;
                }
              tmpTraj.push(*it,smoothed[0].chiSquared());//push the first valid measurement and set the same global chi2
             
              for (Trajectory::DataContainer::iterator itt=it+1; itt!=meas.end();++itt) 
                {
                  tmpTraj.push(*itt,0);//add all the other measurements
                }
              
              smoothed.clear();
              smoothed.push_back(tmpTraj);
           
            } //  if ( !smoothed[0].firstMeasurement().recHit()->isValid() ) 
        
        } // if ( noInvalidHitsBeginEnd ) 
      
      LogTrace("TrackFitters") << "end: returning smoothed trajectory with chi2=" 
                               << smoothed[0].chiSquared() ;
      
      //LogTrace("TrackFitters") << "dump hits before return";
      //Trajectory::DataContainer meas = smoothed[0].measurements();
      //for (Trajectory::DataContainer::iterator it=meas.begin();it!=meas.end();++it) {
      //LogTrace("TrackFitters") << "hit #" << meas.end()-it-1 << " validity=" << it->recHit()->isValid() 
      //<< " det=" << it->recHit()->geographicalId().rawId();
      //}
      
    }

  return smoothed;

}


void 
KFFittingSmoother::smoothingStep(vector<Trajectory>& fitted, vector<Trajectory> &smoothed) const
{
 
  for(vector<Trajectory>::iterator it = fitted.begin(); it != fitted.end();
      it++) {
    vector<Trajectory> mysmoothed = smoother()->trajectories(*it);
    smoothed.insert(smoothed.end(), mysmoothed.begin(), mysmoothed.end());
  }
  LogDebug("TrackFitters") << "In KFFittingSmoother::smoothingStep "<<smoothed.size();
}

vector<Trajectory> KFFittingSmoother::fit(const TrajectorySeed& aSeed,
                                          const RecHitContainer& hits) const{

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

  return vector<Trajectory>();
}