HoughSeedFinderAlg.cxx

Go to the documentation of this file.

/**
 *  @file   HoughSeedFinderAlg.cxx
 *
 *  @brief  Implementation of the Seed Finder Algorithm based on a Hough Transform
 *
 *          The intent of this algorithm is to take an input list of 3D space points and from those
 *          to find candidate track start points and directions. It does so by first performing a
 *          Principal Components Analysis of the input 3D hits and then projects them to the plane
 *          of largest spread. A standard Hough Transform method is then applied to attempt to identify
 *          straight line segments which can be used as seeds to the kalman filter tracker.
 */

// The main include
#include "larreco/RecoAlg/Cluster3DAlgs/HoughSeedFinderAlg.h"

// Framework Includes
#include "art/Framework/Services/Registry/ServiceHandle.h"

// LArSoft includes
#include "larcore/Geometry/Geometry.h"
#include "lardataobj/RecoBase/Seed.h"

// ROOT includes
#include "TCanvas.h"
#include "TDecompSVD.h"
#include "TFrame.h"
#include "TH2D.h"
#include "TMatrixD.h"
#include "TVectorD.h"

// std includes
#include <memory>

//------------------------------------------------------------------------------------------------------------------------------------------

//------------------------------------------------------------------------------------------------------------------------------------------
// implementation follows

namespace lar_cluster3d {

  HoughSeedFinderAlg::HoughSeedFinderAlg(fhicl::ParameterSet const& pset)
    : m_minimum3DHits(5)
    , m_thetaBins(360)
    , m_rhoBins(75)
    , m_hiThresholdMin(5)
    , m_hiThresholdFrac(.05)
    , m_loThresholdFrac(0.85)
    , m_numSeed2DHits(80)
    , m_numAveDocas(6.)
    , m_numSkippedHits(10)
    , m_maxLoopsPerCluster(3)
    , m_maximumGap(5.)
    , m_pcaAlg(pset.get<fhicl::ParameterSet>("PrincipalComponentsAlg"))
    , m_displayHist(false)
  {
    m_minimum3DHits = pset.get<size_t>("Minimum3DHits", 5);
    m_thetaBins = pset.get<int>("ThetaBins", 360);
    m_rhoBins = pset.get<int>("HalfRhoBins", 75);
    m_hiThresholdMin = pset.get<size_t>("HiThresholdMin", 5);
    m_hiThresholdFrac = pset.get<double>("HiThresholdFrac", 0.05);
    m_loThresholdFrac = pset.get<double>("LoThresholdFrac", 0.85);
    m_numSeed2DHits = pset.get<size_t>("NumSeed2DHits", 80);
    m_numAveDocas = pset.get<double>("NumAveDocas", 6.);
    m_numSkippedHits = pset.get<int>("NumSkippedHits", 10);
    m_maxLoopsPerCluster = pset.get<int>("MaxLoopsPerCluster", 3);
    m_maximumGap = pset.get<double>("MaximumGap", 5.);
    m_displayHist = pset.get<bool>("DisplayHoughHist", false);
    m_geometry = art::ServiceHandle<geo::Geometry const>{}.get();
  }

  //------------------------------------------------------------------------------------------------------------------------------------------

  class AccumulatorValues {
    /**
     *  @brief A utility class to contain the values of a given "bin" in Hough Space
     *
     *         Specifically, this is keeping track of the projected x,y coordinates of a given
     *         3D hit projected to the plane of largest spread in PCA and an interator to
     *         that hit in the input container
     */
  public:
    AccumulatorValues() : m_position(0., 0., 0.) {}
    AccumulatorValues(const Eigen::Vector3f& position,
                      const reco::HitPairListPtr::const_iterator& itr)
      : m_position(position), m_hit3DIterator(itr)
    {}

    const Eigen::Vector3f&
    getPosition() const
    {
      return m_position;
    }
    reco::HitPairListPtr::const_iterator
    getHitIterator() const
    {
      return m_hit3DIterator;
    }

  private:
    Eigen::Vector3f
      m_position; ///< We really only need the x,y coordinates here but keep all three for now
    reco::HitPairListPtr::const_iterator
      m_hit3DIterator; ///< This will be used to take us back to our 3D hit
  };

  typedef std::vector<AccumulatorValues> AccumulatorValuesVec;

  class HoughSeedFinderAlg::AccumulatorBin {
    /**
     *  @brief A utility class used to accumulate the above values
     *
     *         One of these objects will exist for each "bin" in rho-theta space and this will
     *         be used to accumulate the 3D hits which contribute to this bin
     */
  public:
    AccumulatorBin() : m_visited(false), m_noise(false), m_inCluster(false) {}
    AccumulatorBin(AccumulatorValues& values) : m_visited(false), m_noise(false), m_inCluster(false)
    {
      m_accumulatorValuesVec.push_back(values);
    }

    void
    setVisited()
    {
      m_visited = true;
    }
    void
    setNoise()
    {
      m_noise = true;
    }
    void
    setInCluster()
    {
      m_inCluster = true;
    }

    void
    addAccumulatorValue(AccumulatorValues& value)
    {
      m_accumulatorValuesVec.push_back(value);
    }

    bool
    isVisited() const
    {
      return m_visited;
    }
    bool
    isNoise() const
    {
      return m_noise;
    }
    bool
    isInCluster() const
    {
      return m_inCluster;
    }

    const AccumulatorValuesVec&
    getAccumulatorValues() const
    {
      return m_accumulatorValuesVec;
    }

  private:
    bool m_visited;
    bool m_noise;
    bool m_inCluster;
    AccumulatorValuesVec m_accumulatorValuesVec;
  };

  class HoughSeedFinderAlg::SortHoughClusterList {
    /**
     * @brief This is used to sort "Hough Clusters" by the maximum entries in a bin
     */
  public:
    SortHoughClusterList(HoughSeedFinderAlg::RhoThetaAccumulatorBinMap& accMap) : m_accMap(accMap)
    {}

    bool
    operator()(const HoughSeedFinderAlg::HoughCluster& left,
               const HoughSeedFinderAlg::HoughCluster& right)
    {
      size_t peakCountLeft(0);
      size_t peakCountRight(0);

      for (const auto& binIndex : left)
        peakCountLeft = std::max(peakCountLeft, m_accMap[binIndex].getAccumulatorValues().size());
      for (const auto& binIndex : right)
        peakCountRight = std::max(peakCountRight, m_accMap[binIndex].getAccumulatorValues().size());

      return peakCountLeft > peakCountRight;
    }

  private:
    HoughSeedFinderAlg::RhoThetaAccumulatorBinMap& m_accMap;
  };

  struct HoughSeedFinderAlg::SortBinIndexList {
    /**
     *  @brief This is used to sort bins in Hough Space
     */
    bool
    operator()(const HoughSeedFinderAlg::RhoThetaAccumulatorBinMap::iterator& left,
               const HoughSeedFinderAlg::RhoThetaAccumulatorBinMap::iterator& right)
    {
      size_t leftSize = left->second.getAccumulatorValues().size();
      size_t rightSize = right->second.getAccumulatorValues().size();

      return leftSize > rightSize;
    }
  };

  void
  HoughSeedFinderAlg::HoughRegionQuery(BinIndex& curBin,
                                       RhoThetaAccumulatorBinMap& rhoThetaAccumulatorBinMap,
                                       HoughCluster& neighborPts,
                                       size_t threshold) const
  {
    /**
     *   @brief Does a query of nearest neighbors to look for matching bins
     */

    // We simply loop over the nearest indices and see if we have any friends over threshold
    for (int rhoIdx = curBin.first - 1; rhoIdx <= curBin.first + 1; rhoIdx++) {
      for (int jdx = curBin.second - 1; jdx <= curBin.second + 1; jdx++) {
        // Skip the self reference
        if (rhoIdx == curBin.first && jdx == curBin.second) continue;

        // Theta bin needs to handle the wrap.
        int thetaIdx(jdx);

        if (thetaIdx < 0)
          thetaIdx = m_thetaBins - 1;
        else if (thetaIdx > m_thetaBins - 1)
          thetaIdx = 0;

        BinIndex binIndex(rhoIdx, thetaIdx);
        RhoThetaAccumulatorBinMap::iterator mapItr = rhoThetaAccumulatorBinMap.find(binIndex);

        if (mapItr != rhoThetaAccumulatorBinMap.end()) {
          if (mapItr->second.getAccumulatorValues().size() >= threshold)
            neighborPts.push_back(binIndex);
        }
      }
    }

    return;
  }

  void
  HoughSeedFinderAlg::expandHoughCluster(BinIndex& curBin,
                                         HoughCluster& neighborPts,
                                         HoughCluster& houghCluster,
                                         RhoThetaAccumulatorBinMap& rhoThetaAccumulatorBinMap,
                                         size_t threshold) const
  {
    /**
     *  @brief The workhorse routine for a DBScan like clustering routine to identify peak bins in Hough Space
     */

    // Start by adding the input point to our Hough Cluster
    houghCluster.push_back(curBin);

    for (auto& binIndex : neighborPts) {
      AccumulatorBin& accBin = rhoThetaAccumulatorBinMap[binIndex];

      if (!accBin.isVisited()) {
        accBin.setVisited();

        HoughCluster nextNeighborPts;

        HoughRegionQuery(binIndex, rhoThetaAccumulatorBinMap, nextNeighborPts, threshold);

        if (!nextNeighborPts.empty()) {
          for (auto& neighborIdx : nextNeighborPts) {
            neighborPts.push_back(neighborIdx);
          }
        }
      }

      if (!accBin.isInCluster()) {
        houghCluster.push_back(binIndex);
        accBin.setInCluster();
      }
    }

    return;
  }

  bool
  Hit3DCompare(const reco::ClusterHit3D* left, const reco::ClusterHit3D* right)
  {
    return *left < *right;
  }

  struct Hit3DSetCompare {
    bool
    operator()(const reco::ClusterHit3D* left, const reco::ClusterHit3D* right) const
    {
      return Hit3DCompare(left, right);
    }
  };

  class OrderHitsAlongWire {
  public:
    OrderHitsAlongWire(int plane = 0) : m_plane(plane) {}

    bool
    operator()(const reco::ClusterHit3D* left, const reco::ClusterHit3D* right)
    {
      int planeToCheck = (m_plane + 1) % 3;

      return left->getHits()[planeToCheck]->WireID().Wire <
             right->getHits()[planeToCheck]->WireID().Wire;
    }

  private:
    int m_plane;
  };

  struct OrderBestPlanes {
    bool
    operator()(const std::pair<size_t, size_t>& left, const std::pair<size_t, size_t>& right)
    {
      return left.second < right.second;
    }
  };

  void
  HoughSeedFinderAlg::findHitGaps(reco::HitPairListPtr& inputHitList,
                                  reco::HitPairListPtr& outputList) const
  {
    // Intention is to try to find the largest "contiguous" block of hits in the input list
    // In a nutshell, the idea is to order input hits according to the pca, then
    // loop through the hits and store them in a new hit list. If a gap is detected then
    // we terminate the previous list, create a new one and continue. After the loop over
    // hits is complete then simply pick the biggest list.
    // Step I is to run the pca and order the hits
    reco::PrincipalComponents pca;

    m_pcaAlg.PCAAnalysis_3D(inputHitList, pca, true);

    // It would seem that the analysis can fail!
    if (!pca.getSvdOK()) {
      outputList = inputHitList;
      return;
    }

    m_pcaAlg.PCAAnalysis_calc3DDocas(inputHitList, pca);

    inputHitList.sort(SeedFinderAlgBase::Sort3DHitsByArcLen3D());
    outputList.clear();

    // Create containers to hold our hit lists
    reco::HitPairListPtr continuousHitList;

    std::map<size_t, reco::HitPairListPtr> gapHitListMap;

    // Remember the distance arc length of the last hit
    double arcLenLastHit = inputHitList.front()->getArclenToPoca();

    // Loop through the input hits
    for (const auto& hit3D : inputHitList) {
      // Hits in order, delta arclength should always be positive
      double arcLen = hit3D->getArclenToPoca();
      double deltaArcLen = arcLen - arcLenLastHit;

      if (deltaArcLen > m_maximumGap) {
        gapHitListMap[continuousHitList.size()] = continuousHitList;
        continuousHitList.clear();
      }

      continuousHitList.emplace_back(hit3D);

      arcLenLastHit = arcLen;
    }

    if (!continuousHitList.empty()) gapHitListMap[continuousHitList.size()] = continuousHitList;

    // Get the largest list of hits
    std::map<size_t, reco::HitPairListPtr>::reverse_iterator longestListItr =
      gapHitListMap.rbegin();

    if (longestListItr != gapHitListMap.rend()) {
      size_t nContinuousHits = longestListItr->first;
      reco::HitPairListPtr longestList = longestListItr->second;

      outputList.resize(nContinuousHits);
      std::copy(longestList.begin(), longestList.end(), outputList.begin());
    }

    return;
  }

  void
  HoughSeedFinderAlg::findHoughClusters(const reco::HitPairListPtr& hitPairListPtr,
                                        reco::PrincipalComponents& pca,
                                        int& nLoops,
                                        RhoThetaAccumulatorBinMap& rhoThetaAccumulatorBinMap,
                                        HoughClusterList& houghClusters) const
  {
    // The goal of this function is to do a basic Hough Transform on the input list of 3D hits.
    // In order to transform this to a 2D problem, the 3D hits are projected to the plane of the two
    // largest eigen values from the input principal components analysis axis.
    // There are two basic steps to the job here:
    // 1) Build and accumlate a rho-theta map
    // 2) Go through that rho-theta map and find candidate Hough "clusters"
    // Unfortunately, the following may not be suitable viewing for those who may be feint of heart
    //
    // Define some constants
    static int histCount(0);
    const double maxTheta(M_PI);                               // Obviously, 180 degrees
    const double thetaBinSize(maxTheta / double(m_thetaBins)); // around 4 degrees (45 bins)
    const double rhoBinSizeMin(m_geometry->WirePitch()); // Wire spacing gives a natural bin size?

    // Recover the parameters from the Principal Components Analysis that we need to project and accumulate
    Eigen::Vector3f pcaCenter(
      pca.getAvePosition()[0], pca.getAvePosition()[1], pca.getAvePosition()[2]);
    Eigen::Vector3f planeVec0(pca.getEigenVectors().row(2));
    Eigen::Vector3f planeVec1(pca.getEigenVectors().row(1));
    Eigen::Vector3f pcaPlaneNrml(pca.getEigenVectors().row(0));
    double eigenVal0 = 3. * sqrt(pca.getEigenValues()[2]);
    double eigenVal1 = 3. * sqrt(pca.getEigenValues()[1]);
    double maxRho = std::sqrt(eigenVal0 * eigenVal0 + eigenVal1 * eigenVal1) * 2. / 3.;
    double rhoBinSize = maxRho / double(m_rhoBins);

    if (rhoBinSize < rhoBinSizeMin) rhoBinSize = rhoBinSizeMin;

    // **********************************************************************
    // Part I: Accumulate values in the rho-theta map
    // **********************************************************************

    size_t maxBinCount(0);
    int nAccepted3DHits(0);

    // Commence looping over the input 3D hits and fill our accumulator bins
    for (reco::HitPairListPtr::const_iterator hit3DItr = hitPairListPtr.begin();
         hit3DItr != hitPairListPtr.end();
         hit3DItr++) {
      // Recover the hit
      const reco::ClusterHit3D* hit3D(*hit3DItr);

      // Skip hits which are not skeleton points
      if (!(hit3D->getStatusBits() & 0x10000000)) continue;

      nAccepted3DHits++;

      Eigen::Vector3f hit3DPosition(
        hit3D->getPosition()[0], hit3D->getPosition()[1], hit3D->getPosition()[2]);
      Eigen::Vector3f pcaToHitVec = hit3DPosition - pcaCenter;
      Eigen::Vector3f pcaToHitPlaneVec(pcaToHitVec.dot(planeVec0), pcaToHitVec.dot(planeVec1), 0.);
      double xPcaToHit = pcaToHitPlaneVec[0];
      double yPcaToHit = pcaToHitPlaneVec[1];

      // Create an accumulator value
      AccumulatorValues accValue(pcaToHitPlaneVec, hit3DItr);

      // Commence loop over theta to fill accumulator bins
      // Note that with theta in the range 0-pi then we can have negative values for rho
      for (int thetaIdx = 0; thetaIdx < m_thetaBins; thetaIdx++) {
        // We need to convert our theta index to an angle
        double theta = thetaBinSize * double(thetaIdx);

        // calculate rho for this angle
        double rho = xPcaToHit * std::cos(theta) + yPcaToHit * std::sin(theta);

        // Get the rho index
        int rhoIdx = std::round(rho / rhoBinSize);

        // Accumulate
        BinIndex binIndex(rhoIdx, thetaIdx);

        rhoThetaAccumulatorBinMap[binIndex].addAccumulatorValue(accValue);

        if (rhoThetaAccumulatorBinMap[binIndex].getAccumulatorValues().size() > maxBinCount)
          maxBinCount = rhoThetaAccumulatorBinMap[binIndex].getAccumulatorValues().size();
      }
    }

    // Accumulation done, if asked now display the hist
    if (m_displayHist) {
      std::ostringstream ostr;
      ostr << "Hough Histogram " << histCount++;
      m_Canvases.emplace_back(new TCanvas(ostr.str().c_str(), ostr.str().c_str(), 1000, 1000));

      std::ostringstream ostr2;
      ostr2 << "Plane";

      m_Canvases.back()->GetFrame()->SetFillColor(46);
      m_Canvases.back()->SetFillColor(19);
      m_Canvases.back()->SetBorderMode(19);
      m_Canvases.back()->cd(1);

      double zmin = 0.06;
      double zmax = 0.94;
      double xmin = 0.04;
      double xmax = 0.95;
      TPad* p = new TPad(ostr2.str().c_str(), ostr2.str().c_str(), zmin, xmin, zmax, xmax);
      p->SetBit(kCanDelete); // Give away ownership.
      p->Range(zmin, xmin, zmax, xmax);
      p->SetFillStyle(4000); // Transparent.
      p->Draw();
      m_Pads.push_back(p);

      TH2D* houghHist = new TH2D("HoughHist",
                                 "Hough Space",
                                 2 * m_rhoBins,
                                 -m_rhoBins + 0.5,
                                 m_rhoBins + 0.5,
                                 m_thetaBins,
                                 0.,
                                 m_thetaBins);

      for (const auto& rhoThetaMap : rhoThetaAccumulatorBinMap) {
        houghHist->Fill(rhoThetaMap.first.first,
                        rhoThetaMap.first.second + 0.5,
                        rhoThetaMap.second.getAccumulatorValues().size());
      }

      houghHist->SetBit(kCanDelete);
      houghHist->Draw();
      m_Canvases.back()->Update();
    }

    // **********************************************************************
    // Part II: Use DBScan (or a slight variation) to find clusters of bins
    // **********************************************************************

    size_t thresholdLo = std::max(size_t(m_hiThresholdFrac * nAccepted3DHits), m_hiThresholdMin);
    size_t thresholdHi = m_loThresholdFrac * maxBinCount;

    std::list<RhoThetaAccumulatorBinMap::iterator> binIndexList;

    for (RhoThetaAccumulatorBinMap::iterator mapItr = rhoThetaAccumulatorBinMap.begin();
         mapItr != rhoThetaAccumulatorBinMap.end();
         mapItr++)
      binIndexList.push_back(mapItr);

    binIndexList.sort(SortBinIndexList());

    for (auto& mapItr : binIndexList) {
      // If we have been here before we skip
      //if (mapItr.second.isVisited()) continue;
      if (mapItr->second.isInCluster()) continue;

      // Mark this bin as visited
      // Actually, don't mark it since we are double thresholding and don't want it missed
      //mapItr.second.setVisited();

      // Make sure over threshold
      if (mapItr->second.getAccumulatorValues().size() < thresholdLo) {
        mapItr->second.setNoise();
        continue;
      }

      // Set the low threshold to make sure we merge bins that might be either side of a boundary trajectory
      thresholdHi = std::max(
        size_t(m_loThresholdFrac * mapItr->second.getAccumulatorValues().size()), m_hiThresholdMin);

      // Recover our neighborhood
      HoughCluster neighborhood;
      BinIndex curBin(mapItr->first);

      HoughRegionQuery(curBin, rhoThetaAccumulatorBinMap, neighborhood, thresholdHi);

      houghClusters.push_back(HoughCluster());

      HoughCluster& houghCluster = houghClusters.back();

      expandHoughCluster(
        curBin, neighborhood, houghCluster, rhoThetaAccumulatorBinMap, thresholdHi);
    }

    // Sort the clusters using the SortHoughClusterList metric
    if (!houghClusters.empty()) houghClusters.sort(SortHoughClusterList(rhoThetaAccumulatorBinMap));

    return;
  }

  bool
  HoughSeedFinderAlg::buildSeed(reco::HitPairListPtr& seed3DHits,
                                SeedHitPairListPair& seedHitPair) const
  {
    if (seed3DHits.size() < m_minimum3DHits) return false;

    reco::PrincipalComponents seedFullPca;

    m_pcaAlg.PCAAnalysis_3D(seed3DHits, seedFullPca, true);

    if (!seedFullPca.getSvdOK()) return false;

    // Use the following to set the 3D doca and arclength for each hit
    m_pcaAlg.PCAAnalysis_calc3DDocas(seed3DHits, seedFullPca);

    // Use this info to sort the hits along the principle axis
    //seed3DHits.sort(SeedFinderAlgBase::Sort3DHitsByArcLen3D());
    seed3DHits.sort(SeedFinderAlgBase::Sort3DHitsByAbsArcLen3D());

    // The idea here is to search for the first hit that lies "close" to the principle axis
    // At that point we count out n hits to use as the seed
    reco::HitPairListPtr seedHit3DList;
    std::set<const reco::ClusterHit2D*> seedHitSet;
    double aveDocaToAxis = seedFullPca.getAveHitDoca();
    int gapCount(0);

    // Now loop through hits to search for a "continuous" block of at least m_numSeed2DHits
    // We'll arrive at that number by collecting 2D hits in an stl set which will keep track of unique occurances
    for (reco::HitPairListPtr::iterator peakBinItr = seed3DHits.begin();
         peakBinItr != seed3DHits.end();
         peakBinItr++) {
      const reco::ClusterHit3D* hit3D = *peakBinItr;

      if (hit3D->getDocaToAxis() < m_numAveDocas * aveDocaToAxis) {
        // Check if we need to reset because of gap count
        if (gapCount > m_numSkippedHits) {
          seedHit3DList.clear();
          seedHitSet.clear();
        }

        seedHit3DList.push_back(hit3D);

        for (const auto& hit : hit3D->getHits())
          seedHitSet.insert(hit);

        gapCount = 0;
      }
      else
        gapCount++;

      if (seedHitSet.size() > m_numSeed2DHits) break;
    }

    // If not enough hits then we are done
    if (seedHit3DList.size() < m_minimum3DHits) return false;

    reco::PrincipalComponents seedPca;

    // Use only the "seed" 3D hits to get new PCA axes
    m_pcaAlg.PCAAnalysis_3D(seedHit3DList, seedPca, true);

    if (!seedPca.getSvdOK()) return false;

    m_pcaAlg.PCAAnalysis_calc3DDocas(seedHit3DList, seedPca);
    //seedHit3DList.sort(SeedFinderAlgBase::Sort3DHitsByAbsArcLen3D());
    seedHit3DList.sort(SeedFinderAlgBase::Sort3DHitsByArcLen3D());

    // Now translate the seedCenter by the arc len to the first hit
    double seedCenter[3] = {
      seedPca.getAvePosition()[0], seedPca.getAvePosition()[1], seedPca.getAvePosition()[2]};
    double seedDir[3] = {seedPca.getEigenVectors().row(2)[0],
                         seedPca.getEigenVectors().row(2)[1],
                         seedPca.getEigenVectors().row(2)[2]};

    double arcLen = seedHit3DList.front()->getArclenToPoca();
    double seedStart[3] = {seedCenter[0] + arcLen * seedDir[0],
                           seedCenter[1] + arcLen * seedDir[1],
                           seedCenter[2] + arcLen * seedDir[2]};

    //seedStart[0] = seedHit3DList.front()->getX();
    //seedStart[1] = seedHit3DList.front()->getY();
    //seedStart[2] = seedHit3DList.front()->getZ();

    if (seedHitSet.size() >= 10) {
      TVector3 newSeedPos;
      TVector3 newSeedDir;
      double chiDOF;

      LineFit2DHits(seedHitSet, seedStart[0], newSeedPos, newSeedDir, chiDOF);

      if (chiDOF > 0.) {
        // check angles between new/old directions
        double cosAng =
          seedDir[0] * newSeedDir[0] + seedDir[1] * newSeedDir[1] + seedDir[2] * newSeedDir[2];

        if (cosAng < 0.) newSeedDir *= -1.;

        seedStart[0] = newSeedPos[0];
        seedStart[1] = newSeedPos[1];
        seedStart[2] = newSeedPos[2];
        seedDir[0] = newSeedDir[0];
        seedDir[1] = newSeedDir[1];
        seedDir[2] = newSeedDir[2];
      }
    }

    // Keep track of this seed and the 3D hits that make it up
    seedHitPair = SeedHitPairListPair(recob::Seed(seedStart, seedDir), seedHit3DList);

    // We are going to drop a few hits off the ends in the hope this facilitates finding more track like objects, provided there are enough hits
    if (seed3DHits.size() > 100) {
      // Need to reset the doca/arclen first
      m_pcaAlg.PCAAnalysis_calc3DDocas(seed3DHits, seedFullPca);

      // Now resort the hits
      seed3DHits.sort(SeedFinderAlgBase::Sort3DHitsByAbsArcLen3D());

      size_t numToKeep = seed3DHits.size() - 10;

      reco::HitPairListPtr::iterator endItr = seed3DHits.begin();

      std::advance(endItr, numToKeep);

      seed3DHits.erase(endItr, seed3DHits.end());
    }

    return true;
  }

  bool
  HoughSeedFinderAlg::findTrackSeeds(reco::HitPairListPtr& inputHitPairListPtr,
                                     reco::PrincipalComponents& inputPCA,
                                     SeedHitPairListPairVec& seedHitPairVec) const
  {
    // This will be a busy routine... the basic tasks are:
    // 1) loop through hits and project to the plane defined by the two largest eigen values, accumulate in Hough space
    // 2) "Cluster" the Hough space to associate hits which are common to a line
    // 3) Process these clusters (still to be defined exactly)

    // Create an interim data structure which will allow us to sort our seeds by "best"
    // before we return them in the seedHitPairVec
    typedef std::map<size_t, SeedHitPairListPairVec> SizeToSeedToHitMap;

    SizeToSeedToHitMap seedHitPairMap;
    SeedHitPairListPair seedHitPair;

    // Make sure we are using the right pca
    reco::HitPairListPtr hitPairListPtr = inputHitPairListPtr;

    int nLoops(0);

    // Make a local copy of the input PCA
    reco::PrincipalComponents pca = inputPCA;

    // We loop over hits in our list until there are no more
    while (!hitPairListPtr.empty()) {
      // We also require that there be some spread in the data, otherwise not worth running?
      double eigenVal0 = 3. * sqrt(pca.getEigenValues()[2]);
      double eigenVal1 = 3. * sqrt(pca.getEigenValues()[1]);

      if (eigenVal0 > 5. && eigenVal1 > 0.001) {
        // **********************************************************************
        // Part I: Build Hough space and find Hough clusters
        // **********************************************************************
        RhoThetaAccumulatorBinMap rhoThetaAccumulatorBinMap;
        HoughClusterList houghClusters;

        findHoughClusters(hitPairListPtr, pca, nLoops, rhoThetaAccumulatorBinMap, houghClusters);

        // If no clusters then done
        if (houghClusters.empty()) break;

        // **********************************************************************
        // Part II: Go through the clusters to find the peak bins
        // **********************************************************************

        // We need to use a set so we can be sure to have unique hits
        reco::HitPairListPtr clusterHitsList;
        std::set<const reco::ClusterHit3D*> masterHitPtrList;
        std::set<const reco::ClusterHit3D*> peakBinPtrList;

        size_t firstPeakCount(0);

        // Loop through the list of all clusters found above
        for (auto& houghCluster : houghClusters) {
          BinIndex peakBin = houghCluster.front();
          size_t peakCount = 0;
          size_t totalHits = 0;

          // Make a local (to this cluster) set of of hits
          std::set<const reco::ClusterHit3D*> localHitPtrList;

          // Now loop through the bins that were attached to this cluster
          for (auto& binIndex : houghCluster) {
            // An even more local list so we can keep track of peak values
            std::set<const reco::ClusterHit3D*> tempHitPtrList;

            // Recover the hits associated to this cluster
            for (auto& hitItr : rhoThetaAccumulatorBinMap[binIndex].getAccumulatorValues()) {
              reco::HitPairListPtr::const_iterator hit3DItr = hitItr.getHitIterator();

              tempHitPtrList.insert(*hit3DItr);
            }

            // count hits before we remove any
            totalHits += tempHitPtrList.size();

            // Trim out any hits already used by a bigger/better cluster
            std::set<const reco::ClusterHit3D*> tempHit3DSet;

            std::set_difference(tempHitPtrList.begin(),
                                tempHitPtrList.end(),
                                masterHitPtrList.begin(),
                                masterHitPtrList.end(),
                                std::inserter(tempHit3DSet, tempHit3DSet.end()));

            tempHitPtrList = tempHit3DSet;

            size_t binCount = tempHitPtrList.size();

            if (peakCount < binCount) {
              peakCount = binCount;
              peakBin = binIndex;
              peakBinPtrList = tempHitPtrList;
            }

            // Add this to our local list
            localHitPtrList.insert(tempHitPtrList.begin(), tempHitPtrList.end());
          }

          if (localHitPtrList.size() < m_minimum3DHits) continue;

          if (!firstPeakCount) firstPeakCount = peakCount;

          // If the peak counts are significantly less than the first cluster's peak then skip
          if (peakCount < firstPeakCount / 10) continue;

          // **********************************************************************
          // Part III: Make a Seed from the peak bin hits
          // **********************************************************************

          reco::HitPairListPtr allPeakBinHits;

          for (const auto& hit3D : localHitPtrList)
            allPeakBinHits.push_back(hit3D);

          reco::HitPairListPtr peakBinHits;

          // Find longest "continuous" set of hits and use these for the seed
          findHitGaps(allPeakBinHits, peakBinHits);

          if (peakBinHits.size() < m_minimum3DHits) continue;

          // We now build the actual seed.
          if (buildSeed(peakBinHits, seedHitPair)) {
            // Keep track of this in our map (which will do ordering for us)
            seedHitPairMap[peakBinHits.size()].push_back(seedHitPair);

            // For visual testing in event display, mark all the hits in the first seed so we can see them
            if (seedHitPairMap.size() == 1) {
              for (const auto& hit3D : peakBinHits)
                hit3D->setStatusBit(0x40000000);
            }

            //                    for(const auto& hit3D : seedHitPair.second) hit3D->setStatusBit(0x40000000);
          }

          // Our peakBinHits collection will most likely be a subset of the localHitPtrList collection
          // We want to remove only the "pure" hits which are those in the peakBinHits collection
          // So, sort them and then add to our master list
          peakBinHits.sort();

          masterHitPtrList.insert(peakBinHits.begin(), peakBinHits.end());

          if (hitPairListPtr.size() - masterHitPtrList.size() < m_minimum3DHits) break;
        } // end loop over hough clusters

        // If the masterHitPtrList is empty then nothing happened and we're done
        if (masterHitPtrList.empty()) break;

        // **********************************************************************
        // Part IV: Remove remaining peak bin hits from HitPairPtrList
        // **********************************************************************

        hitPairListPtr.sort();

        reco::HitPairListPtr::iterator newListEnd = std::set_difference(hitPairListPtr.begin(),
                                                                        hitPairListPtr.end(),
                                                                        masterHitPtrList.begin(),
                                                                        masterHitPtrList.end(),
                                                                        hitPairListPtr.begin());

        hitPairListPtr.erase(newListEnd, hitPairListPtr.end());

        if (hitPairListPtr.size() < m_minimum3DHits) break;

        if (nLoops++ > m_maxLoopsPerCluster) break;

        // ********************************************************
      }
      else
        break; // eigen values not in range

      // At this point run the PCA on the remaining hits
      m_pcaAlg.PCAAnalysis_3D(hitPairListPtr, pca, true);

      if (!pca.getSvdOK()) break;
    }

    // The final task before returning is to transfer the stored seeds into the output seed vector
    // What we want to do is make sure the first seeds are the "best" seeds which is defined as the
    // seeds which were associated to the most hits by the Hough Transform. Our seed map will have
    // the reverse of this ordering so we simply iterate through it "backwards"
    for (SizeToSeedToHitMap::reverse_iterator seedMapItr = seedHitPairMap.rbegin();
         seedMapItr != seedHitPairMap.rend();
         seedMapItr++) {
      for (const auto& seedHitPair : seedMapItr->second) {
        seedHitPairVec.emplace_back(seedHitPair);
      }
    }

    return true;
  }

  bool
  HoughSeedFinderAlg::findTrackHits(reco::HitPairListPtr& inputHitPairListPtr,
                                    reco::PrincipalComponents& inputPCA,
                                    reco::HitPairListPtrList& hitPairListPtrList) const
  {
    // The goal of this routine is run the Hough Transform on the input set of hits
    // and then to return a list of lists of hits which are associated to a given line

    // Make sure we are using the right pca
    reco::HitPairListPtr hitPairListPtr = inputHitPairListPtr;

    int nLoops(0);

    // Make a local copy of the input PCA
    reco::PrincipalComponents pca = inputPCA;

    // We also require that there be some spread in the data, otherwise not worth running?
    double eigenVal0 = 3. * sqrt(pca.getEigenValues()[2]);
    double eigenVal1 = 3. * sqrt(pca.getEigenValues()[1]);

    if (eigenVal0 > 5. && eigenVal1 > 0.001) {
      // **********************************************************************
      // Part I: Build Hough space and find Hough clusters
      // **********************************************************************
      RhoThetaAccumulatorBinMap rhoThetaAccumulatorBinMap;
      HoughClusterList houghClusters;

      findHoughClusters(hitPairListPtr, pca, nLoops, rhoThetaAccumulatorBinMap, houghClusters);

      // **********************************************************************
      // Part II: Go through the clusters to find the peak bins
      // **********************************************************************

      // We need to use a set so we can be sure to have unique hits
      reco::HitPairListPtr clusterHitsList;
      std::set<const reco::ClusterHit3D*> masterHitPtrList;
      std::set<const reco::ClusterHit3D*> peakBinPtrList;

      size_t firstPeakCount(0);

      // Loop through the list of all clusters found above
      for (auto& houghCluster : houghClusters) {
        BinIndex peakBin = houghCluster.front();
        size_t peakCount = 0;
        size_t totalHits = 0;

        // Make a local (to this cluster) set of of hits
        std::set<const reco::ClusterHit3D*> localHitPtrList;

        // Now loop through the bins that were attached to this cluster
        for (auto& binIndex : houghCluster) {
          // An even more local list so we can keep track of peak values
          std::set<const reco::ClusterHit3D*> tempHitPtrList;

          // Recover the hits associated to this cluster
          for (auto& hitItr : rhoThetaAccumulatorBinMap[binIndex].getAccumulatorValues()) {
            reco::HitPairListPtr::const_iterator hit3DItr = hitItr.getHitIterator();

            tempHitPtrList.insert(*hit3DItr);
          }

          // count hits before we remove any
          totalHits += tempHitPtrList.size();

          // Trim out any hits already used by a bigger/better cluster
          std::set<const reco::ClusterHit3D*> tempHit3DSet;

          std::set_difference(tempHitPtrList.begin(),
                              tempHitPtrList.end(),
                              masterHitPtrList.begin(),
                              masterHitPtrList.end(),
                              std::inserter(tempHit3DSet, tempHit3DSet.end()));

          tempHitPtrList = tempHit3DSet;

          size_t binCount = tempHitPtrList.size();

          if (peakCount < binCount) {
            peakCount = binCount;
            peakBin = binIndex;
            peakBinPtrList = tempHitPtrList;
          }

          // Add this to our local list
          localHitPtrList.insert(tempHitPtrList.begin(), tempHitPtrList.end());
        }

        if (localHitPtrList.size() < m_minimum3DHits) continue;

        if (!firstPeakCount) firstPeakCount = peakCount;

        // If the peak counts are significantly less than the first cluster's peak then skip
        if (peakCount < firstPeakCount / 10) continue;

        // **********************************************************************
        // Part III: Make a list of hits from the total number associated
        // **********************************************************************

        hitPairListPtrList.push_back(reco::HitPairListPtr());

        hitPairListPtrList.back().resize(localHitPtrList.size());
        std::copy(
          localHitPtrList.begin(), localHitPtrList.end(), hitPairListPtrList.back().begin());

        // We want to remove the hits which have been used from further contention
        masterHitPtrList.insert(localHitPtrList.begin(), localHitPtrList.end());

        if (hitPairListPtr.size() - masterHitPtrList.size() < m_minimum3DHits) break;
      } // end loop over hough clusters
    }

    return true;
  }

  //------------------------------------------------------------------------------
  void
  HoughSeedFinderAlg::LineFit2DHits(std::set<const reco::ClusterHit2D*>& hit2DSet,
                                    double XOrigin,
                                    TVector3& Pos,
                                    TVector3& Dir,
                                    double& ChiDOF) const
  {
    // The following is lifted from Bruce Baller to try to get better
    // initial parameters for a candidate Seed. It is slightly reworked
    // which is why it is included here instead of used as is.
    //
    // Linear fit using X as the independent variable. Hits to be fitted
    // are passed in the hits vector in a pair form (X, WireID). The
    // fitted track position at XOrigin is returned in the Pos vector.
    // The direction cosines are returned in the Dir vector.
    //
    // SVD fit adapted from $ROOTSYS/tutorials/matrix/solveLinear.C
    // Fit equation is w = A(X)v, where w is a vector of hit wires, A is
    // a matrix to calculate a track projected to a point at X, and v is
    // a vector (Yo, Zo, dY/dX, dZ/dX).
    //
    // Note: The covariance matrix should also be returned
    // B. Baller August 2014

    // assume failure
    ChiDOF = -1;

    if (hit2DSet.size() < 4) return;

    const unsigned int nvars = 4;
    unsigned int npts = hit2DSet.size();

    TMatrixD A(npts, nvars);
    // vector holding the Wire number
    TVectorD w(npts);
    unsigned short ninpl[3] = {0};
    unsigned short nok = 0;
    unsigned short iht(0), cstat, tpc, ipl;
    double x, cw, sw, off;

    // Loop over unique 2D hits from the input list of 3D hits
    for (const auto& hit : hit2DSet) {
      geo::WireID wireID = hit->WireID();

      cstat = wireID.Cryostat;
      tpc = wireID.TPC;
      ipl = wireID.Plane;

      // get the wire plane offset
      off = m_geometry->WireCoordinate(0, 0, ipl, tpc, cstat);

      // get the "cosine-like" component
      cw = m_geometry->WireCoordinate(1, 0, ipl, tpc, cstat) - off;

      // the "sine-like" component
      sw = m_geometry->WireCoordinate(0, 1, ipl, tpc, cstat) - off;

      x = hit->getXPosition() - XOrigin;

      A[iht][0] = cw;
      A[iht][1] = sw;
      A[iht][2] = cw * x;
      A[iht][3] = sw * x;
      w[iht] = wireID.Wire - off;

      ++ninpl[ipl];

      // need at least two points in a plane
      if (ninpl[ipl] == 2) ++nok;

      iht++;
    }

    // need at least 2 planes with at least two points
    if (nok < 2) return;

    TDecompSVD svd(A);
    bool ok;
    TVectorD tVec = svd.Solve(w, ok);

    ChiDOF = 0;

    // not enough points to calculate Chisq
    if (npts <= 4) return;

    double ypr, zpr, diff;

    for (const auto& hit : hit2DSet) {
      geo::WireID wireID = hit->WireID();

      cstat = wireID.Cryostat;
      tpc = wireID.TPC;
      ipl = wireID.Plane;
      off = m_geometry->WireCoordinate(0, 0, ipl, tpc, cstat);
      cw = m_geometry->WireCoordinate(1, 0, ipl, tpc, cstat) - off;
      sw = m_geometry->WireCoordinate(0, 1, ipl, tpc, cstat) - off;
      x = hit->getXPosition() - XOrigin;
      ypr = tVec[0] + tVec[2] * x;
      zpr = tVec[1] + tVec[3] * x;
      diff = ypr * cw + zpr * sw - (wireID.Wire - off);
      ChiDOF += diff * diff;
    }

    float werr2 = m_geometry->WirePitch() * m_geometry->WirePitch();
    ChiDOF /= werr2;
    ChiDOF /= (float)(npts - 4);

    double norm = sqrt(1 + tVec[2] * tVec[2] + tVec[3] * tVec[3]);
    Dir[0] = 1 / norm;
    Dir[1] = tVec[2] / norm;
    Dir[2] = tVec[3] / norm;

    Pos[0] = XOrigin;
    Pos[1] = tVec[0];
    Pos[2] = tVec[1];

  } // TrkLineFit()

} // namespace lar_cluster3d