KGTrack.cxx

Go to the documentation of this file.

#include <cmath>
#include <iomanip>
#include "lardata/RecoObjects/KGTrack.h"
#include "lardata/RecoObjects/KHitWireX.h"
#include "lardata/RecoObjects/KHitWireLine.h"
#include "lardata/RecoObjects/SurfXYZPlane.h"
#include "lardata/RecoObjects/PropXYZPlane.h"
#include "larcore/Geometry/Geometry.h"
#include "cetlib_except/exception.h"

namespace trkf {

  KGTrack::KGTrack(int prefplane) :
    fPrefPlane(prefplane)
  {}

  KGTrack::~KGTrack()
  {}

  const KHitTrack& KGTrack::startTrack() const
  {

    if(!isValid())
      throw cet::exception("KGTrack") << "Starting track is not valid.\n";

    // Return track.

    return (*fTrackMap.begin()).second;
  }

  const KHitTrack& KGTrack::endTrack() const
  {

    if(!isValid())
      throw cet::exception("KGTrack") << "Ending track is not valid.\n";

    // Return track.

    return (*fTrackMap.rbegin()).second;
  }

  KHitTrack& KGTrack::startTrack()
  {

    if(!isValid())
      throw cet::exception("KGTrack") << "Starting track is not valid.\n";

    // Return track.

    return (*fTrackMap.begin()).second;
  }

  KHitTrack& KGTrack::endTrack()
  {

    if(!isValid())
      throw cet::exception("KGTrack") << "Ending track is not valid.\n";

    // Return track.

    return (*fTrackMap.rbegin()).second;
  }

  void KGTrack::addTrack(const KHitTrack& trh) {
    if(!trh.isValid())
      throw cet::exception("KGTrack") << "Adding invalid track to KGTrack.\n";
    fTrackMap.insert(std::make_pair(trh.getPath() + trh.getHit()->getPredDistance(), trh));
  }

  void KGTrack::recalibrate()
  {
    std::multimap<double, KHitTrack> newmap;

    // Loop over old track map.

    bool first = true;
    double s0 = 0.;
    for(std::multimap<double, KHitTrack>::iterator i = fTrackMap.begin();
        i != fTrackMap.end(); ++i) {
      KHitTrack& trh = (*i).second;
      if(first) {
        first = false;
        s0 = trh.getPath();
      }
      double s = trh.getPath()  - s0;
      trh.setPath(s);
      newmap.insert(std::make_pair(s, trh));
    }

    // Update data member track map.

    fTrackMap.swap(newmap);
  }

  void KGTrack::fillTrack(recob::Track& track, 
                          int id) const
  {

    // Make propagator for propating to standard track surface.

    PropXYZPlane prop(0., false);

    // Fill collections of trajectory points and direction vectors.

    std::vector<recob::tracking::Point_t> xyz;
    std::vector<recob::tracking::Vector_t> pxpypz;
    std::vector<recob::tracking::SMatrixSym55> cov;
    std::vector<recob::TrajectoryPointFlags> outFlags;

    xyz.reserve(fTrackMap.size());
    pxpypz.reserve(fTrackMap.size());
    outFlags.reserve(fTrackMap.size());

    // Loop over KHitTracks.

    int ndof = 0;
    float totChi2 = 0.;
    unsigned int n = 0;
    for(std::multimap<double, KHitTrack>::const_iterator itr = fTrackMap.begin();
        itr != fTrackMap.end(); ++itr, ++n) {
      const KHitTrack& trh = (*itr).second;

      // Get position.

      double pos[3];
      trh.getPosition(pos);
      xyz.push_back({pos[0], pos[1], pos[2]});

      // Get momentum vector.
      // Fill direction unit vector and momentum.

      double mom[3];
      trh.getMomentum(mom);
      double p = std::sqrt(mom[0]*mom[0] + mom[1]*mom[1] + mom[2]*mom[2]);
      if (p == 0.)
        throw cet::exception("KGTrack") << __func__ << ": null momentum\n";
      pxpypz.push_back({mom[0], mom[1], mom[2]});

      ndof += 1;
      totChi2 += trh.getChisq();
      outFlags.emplace_back(n, recob::TrajectoryPointFlags::makeMask());

      // Fill error matrix.

      recob::tracking::SMatrixSym55 covar;

      // Construct surface perpendicular to track momentun, and
      // propagate track to that surface (zero distance).

      const std::shared_ptr<const Surface> psurf(new SurfXYZPlane(pos[0], pos[1], pos[2],
                                                                  mom[0], mom[1], mom[2]));
      KETrack tre(trh);
      boost::optional<double> dist = prop.err_prop(tre, psurf, Propagator::UNKNOWN, false);
      if (!dist.is_initialized())
        throw cet::exception("KGTrack") << __func__ << ": error propagation failed\n";
      for(int i=0; i<5; ++i) {
        for(int j=0; j<5; ++j)
          covar(i,j) = tre.getError()(i,j);
      }

      // Only save first and last error matrix.

      if(cov.size() < 2)
        cov.push_back(covar);
      else
        cov.back() = covar;

    }

    // Fill track.

    ndof = ndof - 4; //fit measures 4 parameters: position and direction on plane
    if(xyz.size() >= 2) {
      track = recob::Track(std::move(xyz), std::move(pxpypz), std::move(outFlags),
                           true, this->startTrack().PdgCode(), totChi2, ndof,
                           std::move(cov.front()), std::move(cov.back()), id);
    }
  }

  void KGTrack::fillHits(art::PtrVector<recob::Hit>& hits,
                         std::vector<unsigned int>& hittpindex) const
  {
    hits.reserve(hits.size() + fTrackMap.size());

    // Loop over KHitTracks and fill hits belonging to this track.

    unsigned int counter = 0; //Index of corresponding trajectory point
    for(std::multimap<double, KHitTrack>::const_iterator it = fTrackMap.begin();
        it != fTrackMap.end(); ++it) {
      const KHitTrack& track = (*it).second;
      ++counter;
      // Extrack Hit from track.
      const std::shared_ptr<const KHitBase>& hit = track.getHit();
      if(const KHitWireX* phit = dynamic_cast<const KHitWireX*>(&*hit)) {
        const art::Ptr<recob::Hit> prhit = phit->getHit();
        if(!prhit.isNull()){
          hits.push_back(prhit);
          hittpindex.push_back(counter-1);
        }
      }
      else if(const KHitWireLine* phit = dynamic_cast<const KHitWireLine*>(&*hit)) {
        const art::Ptr<recob::Hit> prhit = phit->getHit();
        if(!prhit.isNull()){
          hits.push_back(prhit);
          hittpindex.push_back(counter-1);
        }
      }
    }
  }

  std::ostream& KGTrack::Print(std::ostream& out) const {

    int n = 0;

    double oldxyz[3] = {0., 0., 0.};
    double len = 0.;
    bool first = true;
    for(auto const& ele : fTrackMap) {
      double s = ele.first;
      const KHitTrack& trh = ele.second;
      double xyz[3];
      double mom[3];
      trh.getPosition(xyz);
      trh.getMomentum(mom);
      double tmom = std::sqrt(mom[0]*mom[0] + mom[1]*mom[1] + mom[2]*mom[2]);
      if(tmom != 0.) {
        mom[0] /= tmom;
        mom[1] /= tmom;
        mom[2] /= tmom;
      }
      if(!first) {
        double dx = xyz[0] - oldxyz[0];
        double dy = xyz[1] - oldxyz[1];
        double dz = xyz[2] - oldxyz[2];
        len += std::sqrt(dx*dx + dy*dy + dz*dz);
      }
      const KHitBase& hit = *(trh.getHit());
      int plane = hit.getMeasPlane();
      std::ios_base::fmtflags f = out.flags();
      out << "State " << std::setw(4) << n 
          << ", path=" << std::setw(8) << std::fixed << std::setprecision(2) << s 
          << ", length=" << std::setw(8) << len 
          << ", x=" << std::setw(8) << xyz[0] 
          << ", y=" << std::setw(8) << xyz[1] 
          << ", z=" << std::setw(8) << xyz[2]
          << ", dx=" << std::setw(8) << mom[0] 
          << ", dy=" << std::setw(8) << mom[1] 
          << ", dz=" << std::setw(8) << mom[2] 
          << ", plane=" << std::setw(1) << plane
          << "\n";
      out.flags(f);

      oldxyz[0] = xyz[0];
      oldxyz[1] = xyz[1];
      oldxyz[2] = xyz[2];
      
      ++n;
      first = false;
    }
    return out;
  }

  std::ostream& operator<<(std::ostream& out, const KGTrack& trg)
  {
    return trg.Print(out);
  }

} // end namespace trkf