KHit.h

Go to the documentation of this file.

#ifndef KHIT_H
#define KHIT_H

#include "cetlib_except/exception.h"
#include "lardata/RecoObjects/KHitBase.h"
#include "lardata/RecoObjects/Propagator.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, 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.

      // 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;
      std::optional<double> dist = prop.err_prop(
        treprop, getMeasSurface(), Propagator::UNKNOWN, false, prefprop, &prop_matrix);
      if (dist) {

        // 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