KHitMulti.cxx

Go to the documentation of this file.

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

namespace trkf {

  KHitMulti::KHitMulti() : fMeasDim(0), fChisq(0.) {}

  KHitMulti::KHitMulti(const std::shared_ptr<const Surface>& psurf)
    : KHitBase(psurf), fMeasDim(0), fChisq(0.)
  {}

  KHitMulti::~KHitMulti() {}

  void KHitMulti::addMeas(const std::shared_ptr<const KHitBase>& pmeas)
  {
    // It is an error to pass in a null pointer.

    if (pmeas.get() == 0)
      throw cet::exception("KHitMulti") << "Attempt to add null measurement pointer.\n";

    // Do the dynamic cast.

    std::shared_ptr<const KHit<1>> pmeas1 =
      std::dynamic_pointer_cast<const KHit<1>, const KHitBase>(pmeas);

    // Throw an exception if dynamic cast failed.

    if (pmeas1.get() == 0)
      throw cet::exception("KHitMulti") << "Dynamic cast for KHitBase pointer failed.\n";
    addMeas(pmeas1);
  }

  void KHitMulti::addMeas(const std::shared_ptr<const KHit<1>>& pmeas)
  {
    // It is an error to pass in a null pointer.

    if (pmeas.get() == 0)
      throw cet::exception("KHitMulti") << "Attempt to add null measurement pointer.\n";

    // Add the measurement.

    ++fMeasDim;
    fMeasVec.push_back(pmeas);
  }

  bool KHitMulti::predict(const KETrack& tre, const Propagator& prop, const KTrack* ref) const
  {
    // Resize and clear all linear algebra objects.

    fMvec.resize(fMeasDim, false);
    fMvec.clear();

    fMerr.resize(fMeasDim, false);
    fMerr.clear();

    fPvec.resize(fMeasDim, false);
    fPvec.clear();

    fPerr.resize(fMeasDim, false);
    fPerr.clear();

    fRvec.resize(fMeasDim, false);
    fRvec.clear();

    fRerr.resize(fMeasDim, false);
    fRerr.clear();

    fRinv.resize(fMeasDim, false);
    fRinv.clear();

    fH.resize(fMeasDim, tre.getVector().size());
    fH.clear();

    // Update the prediction surface to be the track surface.

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

    // Result.

    bool ok = true;

    // Loop over one-dimensional measurements.

    for (unsigned int im = 0; ok && im < fMeasVec.size(); ++im) {
      const KHit<1>& meas = *(fMeasVec[im]);

      // Update prediction for this measurement.

      ok = meas.predict(tre, prop, ref);
      if (!ok) break;

      //

      // Update objects that are concatenations of underlying measurements.

      fMvec(im) = meas.getMeasVector()(0);       // Measurement vector.
      fMerr(im, im) = meas.getMeasError()(0, 0); // Measurement error matrix.
      fPvec(im) = meas.getPredVector()(0);       // Prediction vector.

      // H-matrix.

      for (unsigned int j = 0; j < meas.getH().size2(); ++j)
        fH(im, j) = meas.getH()(0, j);
    }
    if (ok) {

      // Calculate prediction error matrix.
      // T = H C H^T.

      ublas::matrix<double> temp(fH.size2(), fMeasDim);
      ublas::matrix<double> temp2(fMeasDim, fMeasDim);
      temp = prod(tre.getError(), trans(fH));
      temp2 = prod(fH, temp);
      fPerr = ublas::symmetric_adaptor<ublas::matrix<double>>(temp2);

      // Update residual

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

        // Calculate incremental chisquare.

        ublas::vector<double> rtemp = prod(fRinv, fRvec);
        fChisq = inner_prod(fRvec, rtemp);
      }
    }

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

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

    // Done.

    return ok;
  }

  void KHitMulti::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("KHitMulti") << "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.

    ublas::matrix<double> temp(size, fMeasDim);
    ublas::matrix<double> gain(size, fMeasDim);
    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);
    ublas::matrix<double> 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);
  }

  std::ostream& KHitMulti::Print(std::ostream& out, bool doTitle) const
  {
    if (doTitle) out << "KHitMulti:\n";
    return out;
  }

} // end namespace trkf