SiTrackerMultiRecHitUpdator.cc

Go to the documentation of this file.

#include "DataFormats/TrackerRecHit2D/interface/SiTrackerMultiRecHit.h"
#include "DataFormats/TrackingRecHit/interface/KfComponentsHolder.h"
#include "DataFormats/Math/interface/invertPosDefMatrix.h"
#include "DataFormats/Math/interface/ProjectMatrix.h"
#include "DataFormats/SiStripDetId/interface/StripSubdetector.h"

#include "RecoTracker/SiTrackerMRHTools/interface/GenericProjectedRecHit2D.h"
#include "RecoTracker/SiTrackerMRHTools/interface/SiTrackerMultiRecHitUpdator.h"
#include "RecoTracker/TransientTrackingRecHit/interface/TkTransientTrackingRecHitBuilder.h"

#include "TrackingTools/TrajectoryState/interface/TrajectoryStateOnSurface.h"
#include "TrackingTools/TransientTrackingRecHit/interface/TrackingRecHitProjector.h"
#include "Geometry/CommonDetUnit/interface/GeomDetType.h"

#include "FWCore/MessageLogger/interface/MessageLogger.h"

SiTrackerMultiRecHitUpdator::SiTrackerMultiRecHitUpdator(const TransientTrackingRecHitBuilder* builder,
                                                         const TrackingRecHitPropagator* hitpropagator,
                                                         const float Chi2Cut1D,
                                                         const float Chi2Cut2D,
                                                         const std::vector<double>& anAnnealingProgram,
                                                         bool debug)
    : theBuilder(builder),
      theHitPropagator(hitpropagator),
      theChi2Cut1D(Chi2Cut1D),
      theChi2Cut2D(Chi2Cut2D),
      theAnnealingProgram(anAnnealingProgram),
      debug_(debug) {
  theHitCloner = static_cast<TkTransientTrackingRecHitBuilder const*>(builder)->cloner();
}

TransientTrackingRecHit::RecHitPointer SiTrackerMultiRecHitUpdator::buildMultiRecHit(
    const std::vector<const TrackingRecHit*>& rhv,
    const TrajectoryStateOnSurface& tsos,
    MeasurementDetWithData& measDet,
    float annealing) const {
  LogTrace("SiTrackerMultiRecHitUpdator")
      << "Calling SiTrackerMultiRecHitUpdator::buildMultiRecHit with AnnealingFactor: " << annealing;

  TransientTrackingRecHit::ConstRecHitContainer tcomponents;
  for (std::vector<const TrackingRecHit*>::const_iterator iter = rhv.begin(); iter != rhv.end(); iter++) {
    TransientTrackingRecHit::RecHitPointer transient = theBuilder->build(*iter);
    if (transient->isValid())
      tcomponents.push_back(transient);
  }
  return update(tcomponents, tsos, measDet, annealing);
}

TransientTrackingRecHit::RecHitPointer SiTrackerMultiRecHitUpdator::update(
    TransientTrackingRecHit::ConstRecHitPointer original,
    const TrajectoryStateOnSurface& tsos,
    MeasurementDetWithData& measDet,
    double annealing) const {
  LogTrace("SiTrackerMultiRecHitUpdator")
      << "Calling SiTrackerMultiRecHitUpdator::update with AnnealingFactor: " << annealing;

  if (!tsos.isValid()) {
    //return original->clone();
    throw cms::Exception("SiTrackerMultiRecHitUpdator") << "!!! MultiRecHitUpdator::update(..): tsos NOT valid!!! ";
  }

  //check if to clone is the right thing
  if (original->isValid()) {
    if (original->transientHits().empty()) {
      return theHitCloner.makeShared(original, tsos);
    }
  } else {
    return theHitCloner.makeShared(original, tsos);
  }

  TransientTrackingRecHit::ConstRecHitContainer tcomponents = original->transientHits();
  return update(tcomponents, tsos, measDet, annealing);
}

/*------------------------------------------------------------------------------------------------------------------------*/
TransientTrackingRecHit::RecHitPointer SiTrackerMultiRecHitUpdator::update(
    TransientTrackingRecHit::ConstRecHitContainer& tcomponents,
    const TrajectoryStateOnSurface& tsos,
    MeasurementDetWithData& measDet,
    double annealing) const {
  if (tcomponents.empty()) {
    LogTrace("SiTrackerMultiRecHitUpdator") << "Empty components vector passed to SiTrackerMultiRecHitUpdator::update, "
                                               "returning an InvalidTransientRecHit ";
    return std::make_shared<InvalidTrackingRecHit>(measDet.mdet().geomDet(), TrackingRecHit::missing);
  }

  if (!tsos.isValid()) {
    LogTrace("SiTrackerMultiRecHitUpdator")
        << "SiTrackerMultiRecHitUpdator::update: tsos NOT valid!!!, returning an InvalidTransientRecHit";
    return std::make_shared<InvalidTrackingRecHit>(measDet.mdet().geomDet(), TrackingRecHit::missing);
  }

  std::vector<TransientTrackingRecHit::RecHitPointer> updatedcomponents;
  const GeomDet* geomdet = nullptr;

  //running on all over the MRH components
  for (TransientTrackingRecHit::ConstRecHitContainer::const_iterator iter = tcomponents.begin();
       iter != tcomponents.end();
       iter++) {
    //the first rechit must belong to the same surface of TSOS
    if (iter == tcomponents.begin()) {
      if (&((*iter)->det()->surface()) != &(tsos.surface())) {
        throw cms::Exception("SiTrackerMultiRecHitUpdator")
            << "the Trajectory state and the first rechit "
               "passed to the SiTrackerMultiRecHitUpdator lay on different surfaces!: state lays on surface "
            << tsos.surface().position() << " hit with detid " << (*iter)->det()->geographicalId().rawId()
            << " lays on surface " << (*iter)->det()->surface().position();
      }

      geomdet = (*iter)->det();
    }

    //if the rechit does not belong to the surface of the tsos
    //GenericProjectedRecHit2D is used to prepagate
    if (&((*iter)->det()->surface()) != &(tsos.surface())) {
      TransientTrackingRecHit::RecHitPointer cloned =
          theHitPropagator->project<GenericProjectedRecHit2D>(*iter, *geomdet, tsos, theBuilder);
      //if it is used a sensor by sensor grouping this should not appear
      if (cloned->isValid())
        updatedcomponents.push_back(cloned);

    } else {
      TransientTrackingRecHit::RecHitPointer cloned = theHitCloner.makeShared(*iter, tsos);
      if (cloned->isValid()) {
        updatedcomponents.push_back(cloned);
      }
    }
  }

  std::vector<std::pair<const TrackingRecHit*, float> > mymap;
  std::vector<std::pair<const TrackingRecHit*, float> > normmap;

  double a_sum = 0, c_sum = 0;

  for (std::vector<TransientTrackingRecHit::RecHitPointer>::iterator ihit = updatedcomponents.begin();
       ihit != updatedcomponents.end();
       ihit++) {
    double a_i = ComputeWeight(tsos, *(*ihit), false, annealing);  //exp(-0.5*Chi2)
    LogTrace("SiTrackerMultiRecHitUpdator") << "\t\t a_i:" << a_i;
    //double c_i = ComputeWeight(tsos, *(*ihit), true, annealing);  //exp(-0.5*theChi2Cut/annealing)/(2.*M_PI*sqrt(det));
    //LogTrace("SiTrackerMultiRecHitUpdator")<< "\t\t c_i:" << c_i ;
    mymap.push_back(std::pair<const TrackingRecHit*, float>((*ihit)->hit(), a_i));

    a_sum += a_i;
    //with the new definition, the cut weight is computed only once
    if (ihit == updatedcomponents.begin())
      c_sum = ComputeWeight(tsos, *(*ihit), true, annealing);  //exp(-0.5*theChi2Cut/annealing)
  }
  double total_sum = a_sum + c_sum;
  LogTrace("SiTrackerMultiRecHitUpdator") << "\t\t c_sum:" << c_sum;
  LogTrace("SiTrackerMultiRecHitUpdator") << "\t\t total sum:" << total_sum;

  unsigned int counter = 0;
  bool invalid = true;
  for (std::vector<TransientTrackingRecHit::RecHitPointer>::iterator ihit = updatedcomponents.begin();
       ihit != updatedcomponents.end();
       ihit++) {
    double p = ((mymap[counter].second) / total_sum > 1.e-12 ? (mymap[counter].second) / total_sum : 1.e-12);
    //ORCA: float p = ((mymap[counter].second)/total_sum > 0.01 ? (mymap[counter].second)/total_sum : 1.e-6);

#ifdef EDM_ML_DEBUG
    auto const& tmp = *mymap[counter].first;
    LogTrace("SiTrackerMultiRecHitUpdator")
        << "  Component hit type " << typeid(tmp).name() << " and dim:" << tmp.dimension() << " position (PRECISE!!!)"
        << tmp.localPosition() << " error " << tmp.localPositionError() << " with weight " << p;
#endif

    if (p > 10e-6) {
      invalid = false;
      normmap.push_back(std::pair<const TrackingRecHit*, float>(mymap[counter].first, p));
    }

    counter++;
  }

  if (!invalid) {
    SiTrackerMultiRecHitUpdator::LocalParameters param = calcParameters(tsos, normmap);
    SiTrackerMultiRecHit updated(param.first, param.second, *normmap.front().first->det(), normmap, annealing);
    LogTrace("SiTrackerMultiRecHitUpdator") << " Updated Hit position " << updated.localPosition() << " updated error "
                                            << updated.localPositionError() << std::endl;

    return std::make_shared<SiTrackerMultiRecHit>(
        param.first, param.second, *normmap.front().first->det(), normmap, annealing);

  } else {
    LogTrace("SiTrackerMultiRecHitUpdator")
        << " No hits with weight (> 10e-6) have been found for this MRH." << std::endl;
    return std::make_shared<InvalidTrackingRecHit>(measDet.mdet().geomDet(), TrackingRecHit::missing);
  }
}

//---------------------------------------------------------------------------------------------------------------
double SiTrackerMultiRecHitUpdator::ComputeWeight(const TrajectoryStateOnSurface& tsos,
                                                  const TransientTrackingRecHit& aRecHit,
                                                  bool CutWeight,
                                                  double annealing) const {
  switch (aRecHit.dimension()) {
    case 1:
      return ComputeWeight<1>(tsos, aRecHit, CutWeight, annealing);
    case 2:
      return ComputeWeight<2>(tsos, aRecHit, CutWeight, annealing);
    case 3:
      return ComputeWeight<3>(tsos, aRecHit, CutWeight, annealing);
    case 4:
      return ComputeWeight<4>(tsos, aRecHit, CutWeight, annealing);
    case 5:
      return ComputeWeight<5>(tsos, aRecHit, CutWeight, annealing);
  }
  throw cms::Exception("Rec hit of invalid dimension (not 1,2,3,4,5)")
      << "The value was " << aRecHit.dimension() << ", type is " << typeid(aRecHit).name() << "\n";
}

//---------------------------------------------------------------------------------------------------------------
template <unsigned int N>
double SiTrackerMultiRecHitUpdator::ComputeWeight(const TrajectoryStateOnSurface& tsos,
                                                  const TransientTrackingRecHit& aRecHit,
                                                  bool CutWeight,
                                                  double annealing) const {
  //  typedef typename AlgebraicROOTObject<N,5>::Matrix MatN5;
  //  typedef typename AlgebraicROOTObject<5,N>::Matrix Mat5N;
  //  typedef typename AlgebraicROOTObject<N,N>::SymMatrix SMatNN;
  //  typedef typename AlgebraicROOTObject<N>::Vector VecN;

  // define variables that will be used to setup the KfComponentsHolder
  ProjectMatrix<double, 5, N> pf;
  typename AlgebraicROOTObject<N>::Vector r, rMeas;
  typename AlgebraicROOTObject<N, N>::SymMatrix R, RMeas, W;
  AlgebraicVector5 x = tsos.localParameters().vector();
  const AlgebraicSymMatrix55& C = (tsos.localError().matrix());

  // setup the holder with the correct dimensions and get the values
  KfComponentsHolder holder;
  holder.template setup<N>(&r, &R, &pf, &rMeas, &RMeas, x, C);
  aRecHit.getKfComponents(holder);

  typename AlgebraicROOTObject<N>::Vector diff = r - rMeas;

  if (!CutWeight) {
    LogTrace("SiTrackerMultiRecHitUpdator") << "\t\t r:" << r;
    LogTrace("SiTrackerMultiRecHitUpdator") << "\t\t tsospos:" << rMeas;
    LogTrace("SiTrackerMultiRecHitUpdator") << "\t\t diff:" << diff;
    LogTrace("SiTrackerMultiRecHitUpdator") << "\t\t R:" << R;
    LogTrace("SiTrackerMultiRecHitUpdator") << "\t\t RMeas:" << RMeas;
  }

  R += RMeas;  //assume that TSOS is predicted || comb one
  if (!CutWeight)
    LogTrace("SiTrackerMultiRecHitUpdator") << "\t\t R+RMeas:" << R;

  //computing chi2 with the smoothTsos
  // SMatNN R_smooth = R - RMeas;
  // if(!CutWeight)  LogTrace("SiTrackerMultiRecHitUpdator")<< "\t\t R-=Rmeas:" << R_smooth ;
  // bool ierr2_bis = invertPosDefMatrix(R_smooth);
  // double Chi2_smooth = ROOT::Math::Similarity(diff, R_smooth);

  //computing chi2 with the smoothTsos
  //SMatNN R_pred   = R + RMeas;
  //if(!CutWeight)  LogTrace("SiTrackerMultiRecHitUpdator")<< "\t\t R+=Rmeas:" << R_pred   ;
  //bool ierr2_bis = invertPosDefMatrix(R_pred  );
  //double Chi2_pred   = ROOT::Math::Similarity(diff, R_pred  );

  //Det2 method will preserve the content of the Matrix
  //and return true when the calculation is successfull
  double det;
  bool ierr = R.Det2(det);

  bool ierr2 = invertPosDefMatrix(R);  //ierr will be set to true when inversion is successfull
  double Chi2 = ROOT::Math::Similarity(diff, R);

  if (!ierr || !ierr2) {
    LogTrace("SiTrackerMultiRecHitUpdator") << "SiTrackerMultiRecHitUpdator::ComputeWeight: W not valid!" << std::endl;
    LogTrace("SiTrackerMultiRecHitUpdator") << "V: " << R << " AnnealingFactor: " << annealing << std::endl;
  }

  if (!CutWeight) {
    LogTrace("SiTrackerMultiRecHitUpdator") << "\t\t det:" << det;
    LogTrace("SiTrackerMultiRecHitUpdator") << "\t\t Chi2:" << Chi2;
    LogTrace("SiTrackerMultiRecHitUpdator") << "\t\t Chi2/ann:" << Chi2 / annealing;
  }

  double temp_weight = 0.0;
  if (CutWeight && N == 1) {
    LogTrace("SiTrackerMultiRecHitUpdator") << "\t\t Chi2Cut1D:" << theChi2Cut1D;
    temp_weight = exp(-0.5 * theChi2Cut1D / annealing);
  } else if (CutWeight && N == 2) {
    LogTrace("SiTrackerMultiRecHitUpdator") << "\t\t Chi2Cut2D:" << theChi2Cut2D;
    temp_weight = exp(-0.5 * theChi2Cut2D / annealing);
  }

  if (!CutWeight) {
    temp_weight = exp(-0.5 * Chi2 / annealing);
  }

  return temp_weight;
}

//-----------------------------------------------------------------------------------------------------------
SiTrackerMultiRecHitUpdator::LocalParameters SiTrackerMultiRecHitUpdator::calcParameters(
    const TrajectoryStateOnSurface& tsos, std::vector<std::pair<const TrackingRecHit*, float> >& aHitMap) const {
  //supposing all the hits inside of a MRH have the same dimension
  LogTrace("SiTrackerMultiRecHitUpdator")
      << "SiTrackerMultiRecHitUpdator::LocalParameters: dim first recHit: " << aHitMap[0].first->dimension()
      << std::endl;
  switch (aHitMap[0].first->dimension()) {
    case 1:
      return calcParameters<1>(tsos, aHitMap);
    case 2:
      return calcParameters<2>(tsos, aHitMap);
  }
  throw cms::Exception("Rec hit of invalid dimension for computing MRH (not 1,2)")
      << "The value was " << aHitMap[0].first->dimension() << ", type is " << typeid(aHitMap[0].first).name() << "\n";
}

//-----------------------------------------------------------------------------------------------------------
template <unsigned int N>
SiTrackerMultiRecHitUpdator::LocalParameters SiTrackerMultiRecHitUpdator::calcParameters(
    const TrajectoryStateOnSurface& tsos, std::vector<std::pair<const TrackingRecHit*, float> >& aHitMap) const {
  typedef typename AlgebraicROOTObject<N, N>::SymMatrix SMatNN;
  typedef typename AlgebraicROOTObject<N>::Vector VecN;

  VecN m_sum;
  SMatNN W_sum;
  LocalPoint position;
  LocalError error;

  //for TID and TEC the correlation is really high -> need to be scorrelated and then correlated again
  float s = 0.1;

  for (std::vector<std::pair<const TrackingRecHit*, float> >::const_iterator ihit = aHitMap.begin();
       ihit != aHitMap.end();
       ihit++) {
    // define variables that will be used to setup the KfComponentsHolder
    ProjectMatrix<double, 5, N> pf;
    typename AlgebraicROOTObject<N>::Vector r, rMeas;
    typename AlgebraicROOTObject<N, N>::SymMatrix V, VMeas, Wtemp;
    AlgebraicVector5 x = tsos.localParameters().vector();
    const AlgebraicSymMatrix55& C = (tsos.localError().matrix());

    // setup the holder with the correct dimensions and get the values
    KfComponentsHolder holder;
    holder.template setup<N>(&r, &V, &pf, &rMeas, &VMeas, x, C);
    (ihit->first)->getKfComponents(holder);

    LogTrace("SiTrackerMultiRecHitUpdator") << "\t position: " << r;
    LogTrace("SiTrackerMultiRecHitUpdator") << "\t error: " << V;

    //scorrelation  in TID and TEC
    if (N == 2 && TIDorTEChit(ihit->first)) {
      V(0, 1) = V(1, 0) = V(0, 1) * s;
      //      V(1,0) = V(1,0)*s;
      LogTrace("SiTrackerMultiRecHitUpdator") << "\t error scorr: " << V;
    }

    bool ierr = invertPosDefMatrix(V);
    if (!ierr) {
      edm::LogError("SiTrackerMultiRecHitUpdator") << "SiTrackerMultiRecHitUpdator::calcParameters: V not valid!";
    }
    LogTrace("SiTrackerMultiRecHitUpdator") << "\t inverse error: " << V;

    //compute m_sum and W_sum
    m_sum += (ihit->second * V * r);
    W_sum += (ihit->second * V);
  }

  bool ierr_sum = invertPosDefMatrix(W_sum);
  if (!ierr_sum) {
    edm::LogError("SiTrackerMultiRecHitUpdator") << "SiTrackerMultiRecHitUpdator::calcParameters: W_sum not valid!";
  }

  LogTrace("SiTrackerMultiRecHitUpdator") << "\t inverse total error: " << W_sum;
  typename AlgebraicROOTObject<N>::Vector parameters = W_sum * m_sum;
  if (N == 1) {
    position = LocalPoint(parameters(0), 0.f);
    error = LocalError(W_sum(0, 0), 0.f, std::numeric_limits<float>::max());
  } else if (N == 2) {
    position = LocalPoint(parameters(0), parameters(1));
    //ri-correlation  in TID and TEC
    if (TIDorTEChit(aHitMap.at(0).first))
      error = LocalError(W_sum(0, 0), W_sum(0, 1) / s, W_sum(1, 1));
    else
      error = LocalError(W_sum(0, 0), W_sum(0, 1), W_sum(1, 1));
  }

  return std::make_pair(position, error);
}

bool SiTrackerMultiRecHitUpdator::TIDorTEChit(const TrackingRecHit* const& hit) const {
  DetId hitId = hit->geographicalId();

  if (hitId.det() == DetId::Tracker &&
      (hitId.subdetId() == StripSubdetector::TEC || hitId.subdetId() == StripSubdetector::TID)) {
    return true;
  }

  return false;
}