KHit.h

Go to the documentation of this file.

#ifndef KHIT_H
#define KHIT_H

#include "lardata/RecoObjects/KHitBase.h"
#include "cetlib_except/exception.h"

namespace trkf {

  template <int N>
  class KHit : public KHitBase
  {
  public:

    KHit();

    KHit(const std::shared_ptr<const Surface>& psurf);

    KHit(const std::shared_ptr<const Surface>& psurf,
         const typename KVector<N>::type& mvec,
         const typename KSymMatrix<N>::type& merr);

    virtual ~KHit();

    // Modifiers.

    void setMeasVector(const typename KVector<N>::type& mvec) {fMvec = mvec;}

    void setMeasError(const typename KSymMatrix<N>::type& merr) {fMerr = merr;}

    // Accessors.

    const typename KVector<N>::type& getMeasVector() const {return fMvec;}

    const typename KSymMatrix<N>::type& getMeasError() const {return fMerr;}

    const typename KVector<N>::type& getPredVector() const {return fPvec;}

    const typename KSymMatrix<N>::type& getPredError() const {return fPerr;}

    const typename KVector<N>::type& getResVector() const {return fRvec;}

    const typename KSymMatrix<N>::type& getResError() const {return fRerr;}

    const typename KSymMatrix<N>::type& getResInvError() const {return fRinv;}

    const typename KHMatrix<N>::type& getH() const {return fH;}

    double getChisq() const {return fChisq;}

    // Overrides.
    // Implementation of overrides is found at the bottom of this header.

    bool predict(const KETrack& tre, const Propagator* prop = 0, const KTrack* ref = 0) const;

    void update(KETrack& tre) const;

    // Pure virtual methods.

    virtual bool subpredict(const KETrack& tre,
                            typename KVector<N>::type& pvec,
                            typename KSymMatrix<N>::type& perr,
                            typename KHMatrix<N>::type& hmatrix) const = 0;

    virtual std::ostream& Print(std::ostream& out, bool doTitle = true) const;

  private:

    // Attributes.

    typename KVector<N>::type fMvec;             
    typename KSymMatrix<N>::type fMerr;          
    mutable typename KVector<N>::type fPvec;     
    mutable typename KSymMatrix<N>::type fPerr;  
    mutable typename KVector<N>::type fRvec;     
    mutable typename KSymMatrix<N>::type fRerr;  
    mutable typename KSymMatrix<N>::type fRinv;  
    mutable typename KHMatrix<N>::type fH;       
    mutable double fChisq;                       
  };

  // Method implementations.

  template <int N>
  KHit<N>::KHit() :
    fChisq(0.)
  {}

  template<int N>
  KHit<N>::KHit(const std::shared_ptr<const Surface>& psurf) :
    KHitBase(psurf),
    fChisq(0.)
  {}

  template<int N>
  KHit<N>::KHit(const std::shared_ptr<const Surface>& psurf,
                const typename KVector<N>::type& mvec,
                const typename KSymMatrix<N>::type& merr) :
    KHitBase(psurf),
    fMvec(mvec),
    fMerr(merr),
    fChisq(0.)
  {}

  template <int N>
  KHit<N>::~KHit()
  {}

  template <int N>
    bool KHit<N>::predict(const KETrack& tre, const Propagator* prop, const KTrack* ref) const
  {
    // Update the prediction surface to be the track surface.

    fPredSurf = tre.getSurface();
    fPredDist = 0.;

    // Default result.

    bool ok = false;

    // Update prediction vector, error matrox, and H-matrix.

    // First test whether the prediction surface matches the
    // measurement surface.

    if(getMeasSurface()->isEqual(*fPredSurf)) {

      // Prediction and measurement surfaces agree.
      //Just call subpredict method (don't do propagation).

      ok = subpredict(tre, fPvec, fPerr, fH);
    }
    else {

      // Track surface does not match the prediction surface, so an
      // internal propagation will be required.  Throw an exception if
      // no propagator was provided.

      if(prop == 0)
        throw cet::exception("KHit") << "Track surface does not match measurement surface and no propagator.\n";

      // First make a copy of the original KETrack.

      KETrack treprop(tre);

      // Also make a copy of the reference track (if specified).

      KTrack refprop;
      KTrack* prefprop = 0;
      if(ref != 0) {
        refprop = *ref;
        prefprop = &refprop;
      }
      

      // Make a no-noise, no-dE/dx propagation to the measurement
      // surface.  But do calculate the propagation matrix, which we
      // will use to update the H-matrix calculated in the derived
      // class.

      TrackMatrix prop_matrix;
      boost::optional<double> dist = prop->err_prop(treprop, getMeasSurface(),
                                                    Propagator::UNKNOWN, false, 
                                                    prefprop, &prop_matrix);
      ok = !!dist;
      if(ok) {

        // Update prediction distance.

        fPredDist = *dist;

        // Now we are ready to calculate the prediction on the
        // measurement surface.

        typename KHMatrix<N>::type hmatrix;
        ok = subpredict(treprop, fPvec, fPerr, hmatrix);
        if(ok) {

          // Use the propagation matrix to transform the H-matrix back
          // to the prediction surface.

          fH = prod(hmatrix, prop_matrix);
        }
      }
    }
    if(ok) {

      // Update residual

      fRvec = fMvec - fPvec;
      fRerr = fMerr + fPerr;
      fRinv = fRerr;
      ok = syminvert(fRinv);
      if(ok) {

        // Calculate incremental chisquare.

        // (workaround: if we use the copy constructor, gcc emits a spurious warning)
//      typename KVector<N>::type rtemp = prod(fRinv, fRvec);
        fChisq = inner_prod(fRvec, prod(fRinv, fRvec));
      }
    }

    // If a problem occured at any step, clear the prediction surface pointer.

    if(!ok) {
      fPredSurf.reset();
      fPredDist = 0.;
    }

    // Done.

    return ok;
  }

  template <int N>
  void KHit<N>::update(KETrack& tre) const
  {
    // Make sure that the track surface and the prediction surface are the same.
    // Throw an exception if they are not.

    if(!getPredSurface()->isEqual(*tre.getSurface()))
      throw cet::exception("KHit") << "Track surface not the same as prediction surface.\n";

    const TrackVector& tvec = tre.getVector();
    const TrackError& terr = tre.getError();
    TrackVector::size_type size = tvec.size();

    // Calculate gain matrix.

    typename KGMatrix<N>::type temp(size, N);
    typename KGMatrix<N>::type gain(size, N);
    temp = prod(trans(fH), fRinv);
    gain = prod(terr, temp);

    // Calculate updated track state.

    TrackVector newvec = tre.getVector() + prod(gain, fRvec);

    // Calculate updated error matrix.

    TrackMatrix fact = ublas::identity_matrix<TrackVector::value_type>(size);
    fact -= prod(gain, fH);
    TrackMatrix errtemp1 = prod(terr, trans(fact));
    TrackMatrix errtemp2 = prod(fact, errtemp1);
    TrackError errtemp2s = ublas::symmetric_adaptor<TrackMatrix>(errtemp2);
    typename KHMatrix<N>::type errtemp3 = prod(fMerr, trans(gain));
    TrackMatrix errtemp4 = prod(gain, errtemp3);
    TrackError errtemp4s = ublas::symmetric_adaptor<TrackMatrix>(errtemp4);
    TrackError newerr = errtemp2s + errtemp4s;

    // Update track.

    tre.setVector(newvec);
    tre.setError(newerr);
  }

  template <int N>
  std::ostream& KHit<N>::Print(std::ostream& out, bool doTitle) const
  {
    if(doTitle)
      out << "KHit<" << N << ">:\n";

    // Print base class.

    KHitBase::Print(out, false);

    // Print measurement vector.

    out << "  Measurement vector:\n"
        << "  [";
    for(unsigned int i = 0; i < fMvec.size(); ++i) {
      if(i != 0)
        out << ", ";
      out << fMvec(i);
    }
    out << "]\n";

    // Print diagonal measurement  errors.

    out << "  Diagonal measurement errors:\n"
        << "  [";
    for(unsigned int i = 0; i < fMerr.size1(); ++i) {
      if(i != 0)
        out << ", ";
      double err = fMerr(i,i);
      err = (err >= 0. ? std::sqrt(err) : -std::sqrt(-err));
      out << err;
    }
    out << "]\n";

    // Print measurement correlations.

    if(fMerr.size1() > 1) {
      out << "  Measurement correlation matrix:";
      for(unsigned int i = 0; i < fMerr.size1(); ++i) {
        if(i == 0)
          out << "\n  [";
        else
          out << "\n   ";
        for(unsigned int j = 0; j <= i; ++j) {
          if(j != 0)
            out << ", ";
          if(i == j)
            out << 1.;
          else {
            double eiijj = fMerr(i,i) * fMerr(j,j);
            double eij = fMerr(i,j);
            if(eiijj != 0.)
              eij /= std::sqrt(std::abs(eiijj));
            else
              eij = 0.;
            out << eij;
          }
        }
      }
      out << "]\n";
    }

    // Print prediction vector.

    out << "  Prediction vector:\n"
        << "  [";
    for(unsigned int i = 0; i < fPvec.size(); ++i) {
      if(i != 0)
        out << ", ";
      out << fPvec(i);
    }
    out << "]\n";

    // Print diagonal prediction  errors.

    out << "  Diagonal prediction errors:\n"
        << "  [";
    for(unsigned int i = 0; i < fPerr.size1(); ++i) {
      if(i != 0)
        out << ", ";
      double err = fPerr(i,i);
      err = (err >= 0. ? std::sqrt(err) : -std::sqrt(-err));
      out << err;
    }
    out << "]\n";

    // Print prediction correlations.

    if(fPerr.size1() > 1) {
      out << "  Prediction correlation matrix:";
      for(unsigned int i = 0; i < fPerr.size1(); ++i) {
        if(i == 0)
          out << "\n  [";
        else
          out << "\n   ";
        for(unsigned int j = 0; j <= i; ++j) {
          if(j != 0)
            out << ", ";
          if(i == j)
            out << 1.;
          else {
            double eiijj = fPerr(i,i) * fPerr(j,j);
            double eij = fPerr(i,j);
            if(eiijj != 0.)
              eij /= std::sqrt(std::abs(eiijj));
            else
              eij = 0.;
            out << eij;
          }
        }
      }
      out << "]\n";
    }

    // Print residual vector.

    out << "  Residual vector:\n"
        << "  [";
    for(unsigned int i = 0; i < fRvec.size(); ++i) {
      if(i != 0)
        out << ", ";
      out << fRvec(i);
    }
    out << "]\n";

    // Print diagonal residual  errors.

    out << "  Diagonal residual errors:\n"
        << "  [";
    for(unsigned int i = 0; i < fRerr.size1(); ++i) {
      if(i != 0)
        out << ", ";
      double err = fRerr(i,i);
      err = (err >= 0. ? std::sqrt(err) : -std::sqrt(-err));
      out << err;
    }
    out << "]\n";

    // Print residual correlations.

    if(fRerr.size1() > 1) {
      out << "  Residual correlation matrix:";
      for(unsigned int i = 0; i < fRerr.size1(); ++i) {
        if(i == 0)
          out << "\n  [";
        else
          out << "\n   ";
        for(unsigned int j = 0; j <= i; ++j) {
          if(j != 0)
            out << ", ";
          if(i == j)
            out << 1.;
          else {
            double eiijj = fRerr(i,i) * fRerr(j,j);
            double eij = fRerr(i,j);
            if(eiijj != 0.)
              eij /= std::sqrt(std::abs(eiijj));
            else
              eij = 0.;
            out << eij;
          }
        }
      }
      out << "]\n";
    }

    // Print incremental chisquare.

    out << "  Incremental chisquare = " << fChisq << "\n";

    // Done.

    return out;
  }
}

#endif