ClusterMergeAlg_tool.cc

Go to the documentation of this file.

// Framework Includes
#include "art/Framework/Services/Registry/ServiceHandle.h"
#include "art/Utilities/ToolMacros.h"
#include "art_root_io/TFileDirectory.h"
#include "art_root_io/TFileService.h"
#include "cetlib/cpu_timer.h"
#include "fhiclcpp/ParameterSet.h"
#include "messagefacility/MessageLogger/MessageLogger.h"

// LArSoft includes
#include "larreco/RecoAlg/Cluster3DAlgs/Cluster3D.h"
#include "larreco/RecoAlg/Cluster3DAlgs/IClusterModAlg.h"
#include "larreco/RecoAlg/Cluster3DAlgs/PrincipalComponentsAlg.h"

// Root includes
#include "TH1F.h"
#include "TString.h"

// std includes
#include <iostream>

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

namespace lar_cluster3d {

  class ClusterMergeAlg : virtual public IClusterModAlg {
  public:
    explicit ClusterMergeAlg(const fhicl::ParameterSet&);

    ~ClusterMergeAlg();

    void configure(fhicl::ParameterSet const& pset) override;

    void initializeHistograms(art::TFileDirectory&) override;

    void ModifyClusters(reco::ClusterParametersList&) const override;

    float getTimeToExecute() const override { return fTimeToProcess; }

  private:
    bool linearClusters(reco::ClusterParameters&, reco::ClusterParameters&) const;

    bool mergeClusters(reco::ClusterParameters&, reco::ClusterParameters&) const;

    float closestApproach(const Eigen::Vector3f&,
                          const Eigen::Vector3f&,
                          const Eigen::Vector3f&,
                          const Eigen::Vector3f&,
                          Eigen::Vector3f&,
                          Eigen::Vector3f&,
                          Eigen::Vector3f&) const;

    const reco::ClusterHit3D* findClosestHit3D(const Eigen::Vector3f&,
                                               const Eigen::Vector3f&,
                                               const reco::HitPairListPtr&) const;

    const reco::ClusterHit3D* findFurthestHit3D(const Eigen::Vector3f&,
                                                const Eigen::Vector3f&,
                                                const reco::HitPairListPtr&) const;

    bool fEnableMonitoring;   
    float fMinTransEigenVal;  
    float fMinEigenToProcess; 
    bool fOutputHistograms;   

    mutable float fTimeToProcess; 

    std::vector<TH1F*> fFirstEigenValueHists; 
    std::vector<TH1F*> fNextEigenValueHists;  
    TH1F* fNumMergedClusters;                 

    TH1F* f1stTo2ndPosLenHist; 

    TH1F* fRMaxFirstHist;    
    TH1F* fCosMaxFirstHist;  
    TH1F* fCosFirstAxisHist; 
    TH1F*
      f1stTo2ndProjEigenHist; 

    TH1F* fRMaxNextHist;    
    TH1F* fCosMaxNextHist;  
    TH1F* fCosNextAxisHist; 
    TH1F*
      f2ndTo1stProjEigenHist; 

    TH1F* fGapBetweenClusHist;  
    TH1F* fGapRatToLenHist;     
    TH1F* fProjEndPointLenHist; 
    TH1F* fProjEndPointRatHist; 

    TH1F* fAxesDocaHist;       
    TH1F* f1stDocaArcLRatHist; 
    TH1F* f2ndDocaArcLRatHist; 

    TH1F* f1stTo2ndPosLenRatHist; 
    TH1F* fGapRatHist;            

    PrincipalComponentsAlg fPCAAlg; // For running Principal Components Analysis
  };

  ClusterMergeAlg::ClusterMergeAlg(fhicl::ParameterSet const& pset)
    : fPCAAlg(pset.get<fhicl::ParameterSet>("PrincipalComponentsAlg"))
  {
    this->configure(pset);
  }

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

  ClusterMergeAlg::~ClusterMergeAlg() {}

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

  void ClusterMergeAlg::configure(fhicl::ParameterSet const& pset)
  {
    fEnableMonitoring = pset.get<bool>("EnableMonitoring", true);
    fMinTransEigenVal = pset.get<float>("MinTransEigenVal", 0.09);
    fMinEigenToProcess = pset.get<float>("MinEigenToProcess", 2.0);
    fOutputHistograms = pset.get<bool>("OutputHistograms", false);

    fTimeToProcess = 0.;

    // If asked, define some histograms
    if (fOutputHistograms) {
      // Access ART's TFileService, which will handle creating and writing
      // histograms and n-tuples for us.
      art::ServiceHandle<art::TFileService> tfs;

      // Make a directory for these histograms
      art::TFileDirectory dir = tfs->mkdir("MergeClusters");

      fFirstEigenValueHists.resize(3, nullptr);
      fNextEigenValueHists.resize(3, nullptr);

      std::vector<float> maxValsVec = {20., 50., 250.};

      for (size_t idx : {0, 1, 2}) {
        fFirstEigenValueHists[idx] =
          dir.make<TH1F>(Form("FEigen1st%1zu", idx), "Eigen Val", 200, 0., maxValsVec[idx]);
        fNextEigenValueHists[idx] =
          dir.make<TH1F>(Form("FEigen2nd%1zu", idx), "Eigen Val", 200, 0., maxValsVec[idx]);
      }

      fNumMergedClusters = dir.make<TH1F>("NumMergedClus", "Number Merged", 200, 0., 1000.);

      f1stTo2ndPosLenHist =
        dir.make<TH1F>("1stTo2ndPosLen", "Distance between Clusters", 250, 0., 1000.);

      fRMaxFirstHist = dir.make<TH1F>("rMaxFirst", "Radius of First Cluster", 200, 0., 100.);
      fCosMaxFirstHist = dir.make<TH1F>("CosMaxFirst", "Cos Angle First Cyl/Axis", 200, 0., 1.);
      fCosFirstAxisHist = dir.make<TH1F>("CosFirstAxis", "Cos Angle First Next/Axis", 200, 0., 1.);
      f1stTo2ndProjEigenHist =
        dir.make<TH1F>("1stTo2ndProjEigen", "Projected Distance First", 200, 0., 200.);

      fRMaxNextHist = dir.make<TH1F>("rMaxNext", "Radius of Next Cluster", 200, 0., 100.);
      fCosMaxNextHist = dir.make<TH1F>("CosMaxNext", "Cos Angle Next Cyl/Axis", 200, 0., 1.);
      fCosNextAxisHist = dir.make<TH1F>("CosNextAxis", "Cos Angle Next Next/Axis", 200, 0., 1.);
      f2ndTo1stProjEigenHist =
        dir.make<TH1F>("2ndTo1stProjEigen", "Projected Distance Next", 200, 0., 200.);

      fGapBetweenClusHist = dir.make<TH1F>("ClusterGap", "Gap Between Clusters", 400, -200., 200.);
      fGapRatToLenHist = dir.make<TH1F>("GapRatToLen", "Ratio Gap to Distance", 100, -8., 2.);
      fProjEndPointLenHist =
        dir.make<TH1F>("ProjEndPointLen", "Projected End Point Len", 200, -100., 100.);
      fProjEndPointRatHist =
        dir.make<TH1F>("ProjEndPointRat", "Projected End Point Ratio", 100, 0., 1.);

      fAxesDocaHist = dir.make<TH1F>("AxesDocaHist", "DOCA", 200, 0., 25.);
      f1stDocaArcLRatHist = dir.make<TH1F>("ALenPOCA1Hist", "Arc Len to POCA 1", 400, -50., 50.);
      f2ndDocaArcLRatHist = dir.make<TH1F>("ALenPOCA2Hist", "Arc Len to POCA 2", 400, -50., 50.);

      f1stTo2ndPosLenRatHist =
        dir.make<TH1F>("1stTo2ndPosLenRat", "Ratio clus dist to eigen", 200., 0., 20.);
      fGapRatHist = dir.make<TH1F>("GapRat", "Ratio Gap to Next Eigen", 400, -20., 20.);
    }

    return;
  }

  void ClusterMergeAlg::initializeHistograms(art::TFileDirectory&)
  {
    return;
  }

  void ClusterMergeAlg::ModifyClusters(reco::ClusterParametersList& clusterParametersList) const
  {
    // Initial clustering is done, now trim the list and get output parameters
    cet::cpu_timer theClockBuildClusters;

    // Start clocks if requested
    if (fEnableMonitoring) theClockBuildClusters.start();

    // Resort by the largest first eigen value (so the "longest" cluster)
    clusterParametersList.sort([](auto& left, auto& right) {
      return left.getFullPCA().getEigenValues()[2] > right.getFullPCA().getEigenValues()[2];
    });

    // The idea is to continually loop through all clusters until we get to the point where we are no longer doing any merging.
    // If two clusters are merged then we need to recycle through again because it may be that the new cluster can match when
    // it might not have in the past
    size_t lastClusterListCount = clusterParametersList.size() + 1;
    reco::ClusterParametersList::iterator lastFirstClusterItr = clusterParametersList.begin();

    int numMergedClusters(0);
    int nOutsideLoops(0);

    while (clusterParametersList.size() != lastClusterListCount) {
      // Update the last count
      lastClusterListCount = clusterParametersList.size();

      // Keep track of the first cluster iterator each pass through
      reco::ClusterParametersList::iterator firstClusterItr = lastFirstClusterItr++;

      // Loop through the clusters
      while (firstClusterItr != clusterParametersList.end()) {
        reco::ClusterParameters& firstClusterParams = *firstClusterItr;
        reco::ClusterParametersList::iterator nextClusterItr = firstClusterItr;

        // Take a brief interlude to do some histogramming
        if (fOutputHistograms) {
          const reco::PrincipalComponents& firstPCA = firstClusterParams.getFullPCA();

          Eigen::Vector3f firstEigenVals(
            std::min(1.5 * std::sqrt(std::max(firstPCA.getEigenValues()[0], fMinTransEigenVal)),
                     50.),
            std::min(1.5 * std::sqrt(std::max(firstPCA.getEigenValues()[1], fMinTransEigenVal)),
                     100.),
            1.5 * std::sqrt(firstPCA.getEigenValues()[2]));

          for (size_t idx = 0; idx < 3; idx++)
            fFirstEigenValueHists[idx]->Fill(firstEigenVals[idx], 1.);
        }

        // Once you get down to the smallest clusters if they haven't already been absorbed there is no need to check them
        if (firstClusterParams.getFullPCA().getEigenValues()[2] < fMinEigenToProcess) break;

        while (++nextClusterItr != clusterParametersList.end()) {
          reco::ClusterParameters& nextClusterParams = *nextClusterItr;

          // On any given loop through here it **might** be that the first cluster has been modified. So can't cache
          // the parameters, need the curret ones
          if (linearClusters(firstClusterParams, nextClusterParams)) {
            if (mergeClusters(firstClusterParams, nextClusterParams)) {
              // Now remove the "next" cluster
              nextClusterItr = clusterParametersList.erase(nextClusterItr);

              // Our new cluster may be larger than those before it so we should try to move backwards to make sure to
              // give now smaller clusters the chance to get merged as well
              reco::ClusterParametersList::iterator biggestItr = firstClusterItr;

              // Step backwards until we hit the beginning of the list
              while (biggestItr-- != clusterParametersList.begin()) {
                // The list has already been sorted largest to smallest so if we find a cluster that is "larger" then
                // we have hit the limit
                if ((*biggestItr).getFullPCA().getEigenValues()[2] >
                    (*firstClusterItr).getFullPCA().getEigenValues()[2]) {
                  // Want to insert after the cluster with the larger primary axes.. but there is
                  // no point in doing anything if the new cluster is already in the right spot
                  if (++biggestItr != firstClusterItr)
                    clusterParametersList.splice(
                      biggestItr, clusterParametersList, firstClusterItr);

                  break;
                }
              }

              // Restart loop?
              nextClusterItr = firstClusterItr;

              numMergedClusters++;
            }
          }
        }

        firstClusterItr++;
      }

      nOutsideLoops++;
    }

    std::cout << "==> # merged: " << numMergedClusters << ", in " << nOutsideLoops
              << " outside loops " << std::endl;

    if (fOutputHistograms) fNumMergedClusters->Fill(numMergedClusters, 1.);

    if (fEnableMonitoring) {
      theClockBuildClusters.stop();

      fTimeToProcess = theClockBuildClusters.accumulated_real_time();
    }

    mf::LogDebug("Cluster3D") << ">>>>> Merge clusters done, found " << clusterParametersList.size()
                              << " clusters" << std::endl;

    return;
  }

  bool ClusterMergeAlg::linearClusters(reco::ClusterParameters& firstCluster,
                                       reco::ClusterParameters& nextCluster) const
  {
    // Assume failure
    bool consistent(false);

    // The goal here is to compare the two input PCA's and determine if they are effectively colinear and
    // within reasonable range to consider merging them. Note that a key assumption is that the first input
    // PCA is from the "bigger" cluster, the second is "smaller" and may have a less reliable PCA.

    // Dereference the PCA's
    const reco::PrincipalComponents& firstPCA = firstCluster.getFullPCA();
    const reco::PrincipalComponents& nextPCA = nextCluster.getFullPCA();

    // Recover the positions of the centers of the two clusters
    const Eigen::Vector3f& firstCenter = firstPCA.getAvePosition();
    const Eigen::Vector3f& nextCenter = nextPCA.getAvePosition();

    // And form a vector between the two centers
    Eigen::Vector3f firstPosToNextPosVec = nextCenter - firstCenter;
    Eigen::Vector3f firstPosToNextPosUnit = firstPosToNextPosVec.normalized();
    float firstPosToNextPosLen = firstPosToNextPosVec.norm();

    // Now get the first PCA's primary axis and since we'll use them get all of them at once...
    Eigen::Vector3f firstAxis0(firstPCA.getEigenVectors().row(0));
    Eigen::Vector3f firstAxis1(firstPCA.getEigenVectors().row(1));
    Eigen::Vector3f firstAxis2(firstPCA.getEigenVectors().row(2));

    // Will want the distance of closest approach of the next cluser's center to the primary, start by finding arc length
    float arcLenToNextDoca = firstPosToNextPosVec.dot(firstAxis2);

    // Adopt the convention that the cluster axis is in same direction as vector from first to next centers
    // And preserve the overall orientation of the PCA by flipping all if we flip the first one
    if (arcLenToNextDoca < 0.) {
      firstAxis0 = -firstAxis0;
      firstAxis1 = -firstAxis1;
      firstAxis2 = -firstAxis2;
      arcLenToNextDoca = -arcLenToNextDoca;
    }

    // Recover the eigen values of the first and second PCAs for selection cuts
    Eigen::Vector3f firstEigenVals(
      std::min(1.5 * sqrt(std::max(firstPCA.getEigenValues()[0], fMinTransEigenVal)), 20.),
      std::min(1.5 * sqrt(std::max(firstPCA.getEigenValues()[1], fMinTransEigenVal)), 50.),
      std::min(1.5 * sqrt(firstPCA.getEigenValues()[2]), 250.));

    // We treat the PCA as defining an elliptical tube and we use the projection of the vector between centers to
    // get the radius to the edge of the tube, from which we can determine the projected length inside the tube
    Eigen::Vector2f firstProj01Unit =
      Eigen::Vector2f(firstPosToNextPosUnit.dot(firstAxis0), firstPosToNextPosUnit.dot(firstAxis1))
        .normalized();
    float firstEigen0Proj = firstEigenVals[0] * firstProj01Unit[0];
    float firstEigen1Proj = firstEigenVals[1] * firstProj01Unit[1];
    float rMaxFirst =
      std::sqrt(firstEigen0Proj * firstEigen0Proj + firstEigen1Proj * firstEigen1Proj);
    float cosMaxFirst =
      firstEigenVals[2] / std::sqrt(firstEigenVals[2] * firstEigenVals[2] + rMaxFirst * rMaxFirst);
    float cosFirstAxis = firstAxis2.dot(firstPosToNextPosUnit);

    // Now calculate a measure of the length inside the tube along the vector between the cluster centers
    float firstEigenVal2 = firstEigenVals[2];
    float firstToNextProjEigen = firstEigenVal2;

    // There are two cases to consider, the first is that the vector exits out the side of the tube
    if (cosFirstAxis < cosMaxFirst) {
      // In this case we need the sign of the angle between the vector and the cluster primary axis
      float sinFirstAxis = std::sqrt(1. - cosFirstAxis * cosFirstAxis);

      // Here the length will be given by the radius, not the primary eigen value
      firstToNextProjEigen = rMaxFirst / sinFirstAxis;
    }
    else
      firstToNextProjEigen /= cosFirstAxis;

    // Get scale factor for selecting this pair
    float firstPosToNextPosScaleFactor = 8.;

    // A brief interlude to fill a few histograms
    if (fOutputHistograms) {
      f1stTo2ndPosLenHist->Fill(firstPosToNextPosLen, 1.);
      fRMaxFirstHist->Fill(rMaxFirst, 1.);
      fCosMaxFirstHist->Fill(cosMaxFirst, 1.);
      fCosFirstAxisHist->Fill(cosFirstAxis, 1.);
      f1stTo2ndProjEigenHist->Fill(firstToNextProjEigen, 1.);
    }

    // This makes first selection, it should eliminate most of the junk cases:
    if (firstPosToNextPosLen < firstPosToNextPosScaleFactor * firstToNextProjEigen) {
      // Recover the axes for the next PCA and make sure pointing convention is observed
      Eigen::Vector3f nextAxis0(nextPCA.getEigenVectors().row(0));
      Eigen::Vector3f nextAxis1(nextPCA.getEigenVectors().row(1));
      Eigen::Vector3f nextAxis2(nextPCA.getEigenVectors().row(2));

      // Recover the cos of the angle between the primary axes...
      float cosFirstNextAxis = firstAxis2.dot(nextAxis2);

      // And in this case we want to choose the sign of the next cluster's axes so that the
      // primary axes "point" in the same direction
      if (cosFirstNextAxis < 0.) {
        nextAxis0 = -nextAxis0;
        nextAxis1 = -nextAxis1;
        nextAxis2 = -nextAxis2;
        cosFirstNextAxis = -cosFirstNextAxis;
      }

      // Get the eigen values again
      Eigen::Vector3f nextEigenVals(
        std::min(1.5 * sqrt(std::max(nextPCA.getEigenValues()[0], fMinTransEigenVal)), 20.),
        std::min(1.5 * sqrt(std::max(nextPCA.getEigenValues()[1], fMinTransEigenVal)), 50.),
        std::min(1.5 * sqrt(nextPCA.getEigenValues()[2]), 250.));

      // Repeat the calculation of the length of the vector through the cluster "tube"...
      Eigen::Vector2f nextProj01Unit =
        Eigen::Vector2f(firstPosToNextPosUnit.dot(nextAxis0), firstPosToNextPosUnit.dot(nextAxis1))
          .normalized();
      float nextEigen0Proj = nextEigenVals[0] * nextProj01Unit[0];
      float nextEigen1Proj = nextEigenVals[1] * nextProj01Unit[1];
      float rMaxNext = std::sqrt(nextEigen0Proj * nextEigen0Proj + nextEigen1Proj * nextEigen1Proj);
      float cosMaxNext =
        nextEigenVals[2] / std::sqrt(nextEigenVals[2] * nextEigenVals[2] + rMaxNext * rMaxNext);
      float cosNextAxis = std::abs(nextAxis2.dot(firstPosToNextPosUnit));

      // Now calculate a measure of the length inside the cylider along the vector between the cluster centers
      float nextToFirstProjEigen = nextEigenVals[2];

      // There are two cases to consider, the first is that the vector exits out the side of the cylinder
      if (cosNextAxis < cosMaxNext) {
        // In this case we need the sign of the angle between the vector and the cluster primary axis
        float sinNextAxis = std::sqrt(1. - cosNextAxis * cosNextAxis);

        // Here the length will be given by the radius, not the primary eigen value
        nextToFirstProjEigen = rMaxNext / sinNextAxis;
      }
      else
        nextToFirstProjEigen /= cosNextAxis;

      // Allow a generous gap but significantly derate as angle to next cluster increases
      //float nextToFirstScaleFactor = 6. * cosFirstNextAxis;
      float nextToFirstScaleFactor = 8. * cosFirstNextAxis;

      // Form the "gap" along the axis between centers from the projections of the eigen values
      // Note the gap can be negative
      float gapFirstToNext = firstPosToNextPosLen - firstToNextProjEigen - nextToFirstProjEigen;

      // Look at the presumed nearest endpoints of the two clusters
      Eigen::Vector3f firstEndPoint = firstCenter + firstEigenVals[2] * firstAxis2;
      Eigen::Vector3f nextTailPoint = nextCenter - nextEigenVals[2] * nextAxis2;
      Eigen::Vector3f endToTailVec = nextTailPoint - firstEndPoint;

      // Get projection along vector between centers
      float endPointProjection = endToTailVec.dot(firstPosToNextPosUnit);
      float endToTailLen = endToTailVec.norm();

      // Another brief interlude to fill some histograms
      if (fOutputHistograms) {
        for (size_t idx = 0; idx < 3; idx++)
          fNextEigenValueHists[idx]->Fill(nextEigenVals[idx], 1.);

        fRMaxNextHist->Fill(rMaxNext, 1.);
        fCosMaxNextHist->Fill(cosMaxNext, 1.);
        fCosNextAxisHist->Fill(cosNextAxis, 1.);
        f2ndTo1stProjEigenHist->Fill(nextToFirstProjEigen, 1.);
        fGapBetweenClusHist->Fill(gapFirstToNext, 1.);
        fProjEndPointLenHist->Fill(endPointProjection, 1.);
        fProjEndPointRatHist->Fill(std::abs(endPointProjection) / endToTailLen, 1.);
      }

      // We mirror here the first selection above but now operate on the distance between centers less
      // the first cluster's projected eigen value
      if (gapFirstToNext < nextToFirstScaleFactor * nextToFirstProjEigen ||
          (std::abs(endPointProjection) < nextToFirstProjEigen &&
           endToTailLen < nextEigenVals[2])) {
        // Now check the distance of closest approach betweent the two vectors
        // Closest approach calculaiton results vectors
        Eigen::Vector3f firstPoca;
        Eigen::Vector3f nextPoca;
        Eigen::Vector3f firstToNextUnit;

        // Recover the doca of the two axes and their points of closest approach along each axis
        float lineDoca = closestApproach(
          firstCenter, firstAxis2, nextCenter, nextAxis2, firstPoca, nextPoca, firstToNextUnit);

        // Get the range through the first clusters "tube" for this doca vec
        Eigen::Vector3f firstPOCAProjUnit = Eigen::Vector3f(firstToNextUnit.dot(firstAxis0),
                                                            firstToNextUnit.dot(firstAxis1),
                                                            firstToNextUnit.dot(firstAxis1))
                                              .normalized();
        float firstPOCAVecProjEigen =
          std::sqrt(std::pow(firstEigenVals[0] * firstPOCAProjUnit[0], 2) +
                    std::pow(firstEigenVals[1] * firstPOCAProjUnit[1], 2) +
                    std::pow(firstEigenVals[2] * firstPOCAProjUnit[2], 2));

        // Similarly for the next vector
        Eigen::Vector3f nextPOCAProjUnit = Eigen::Vector3f(firstToNextUnit.dot(firstAxis0),
                                                           firstToNextUnit.dot(firstAxis1),
                                                           firstToNextUnit.dot(firstAxis1))
                                             .normalized();
        float nextPOCAVecProjEigen = std::sqrt(std::pow(nextEigenVals[0] * nextPOCAProjUnit[0], 2) +
                                               std::pow(nextEigenVals[1] * nextPOCAProjUnit[1], 2) +
                                               std::pow(nextEigenVals[2] * nextPOCAProjUnit[2], 2));

        if (fOutputHistograms) {
          // The below returned the signed arc lengths to their respective pocas
          float arcLenToFirstPoca = (firstPoca - firstCenter).dot(firstAxis2);
          float arcLenToNextPoca = (nextPoca - nextCenter).dot(nextAxis2);

          fAxesDocaHist->Fill(lineDoca, 1.);
          f1stDocaArcLRatHist->Fill(arcLenToFirstPoca / firstEigenVals[2], 1.);
          f2ndDocaArcLRatHist->Fill(arcLenToNextPoca / nextEigenVals[2], 1.);
        }

        // Scaling factor to increase doca distance as distance grows
        float rMaxScaleFactor = 1.2;

        if (lineDoca < rMaxScaleFactor * (firstPOCAVecProjEigen + nextPOCAVecProjEigen)) {
          consistent = true;

          if (fOutputHistograms) {
            f1stTo2ndPosLenRatHist->Fill(firstPosToNextPosLen / firstToNextProjEigen, 1.);
          }
        }
      }
    }

    return consistent;
  }

  bool ClusterMergeAlg::mergeClusters(reco::ClusterParameters& firstClusterParams,
                                      reco::ClusterParameters& nextClusterParams) const
  {
    bool merged(false);

    // Merge the next cluster into the first one
    // Get the hits
    reco::HitPairListPtr& hitPairListPtr = firstClusterParams.getHitPairListPtr();

    // We copy the hits from the old to new but note that we need to update the parameters for each 2D hit we add
    for (const auto* hit : nextClusterParams.getHitPairListPtr()) {
      hitPairListPtr.push_back(hit);

      for (const auto* hit2D : hit->getHits())
        if (hit2D) firstClusterParams.UpdateParameters(hit2D);
    }

    // Recalculate the PCA
    fPCAAlg.PCAAnalysis_3D(firstClusterParams.getHitPairListPtr(), firstClusterParams.getFullPCA());

    // Must have a valid pca
    if (firstClusterParams.getFullPCA().getSvdOK()) {
      // Finish out the merging here
      reco::Hit3DToEdgeMap& firstEdgeMap = firstClusterParams.getHit3DToEdgeMap();
      reco::Hit3DToEdgeMap& nextEdgeMap = nextClusterParams.getHit3DToEdgeMap();

      for (const auto& pair : nextEdgeMap)
        firstEdgeMap[pair.first] = pair.second;

      // Set the skeleton PCA to make sure it has some value
      firstClusterParams.getSkeletonPCA() = firstClusterParams.getFullPCA();

      // Zap the cluster we merged into the new one...
      nextClusterParams = reco::ClusterParameters();

      merged = true;
    }

    return merged;
  }

  float ClusterMergeAlg::closestApproach(const Eigen::Vector3f& P0,
                                         const Eigen::Vector3f& u0,
                                         const Eigen::Vector3f& P1,
                                         const Eigen::Vector3f& u1,
                                         Eigen::Vector3f& poca0,
                                         Eigen::Vector3f& poca1,
                                         Eigen::Vector3f& firstNextUnit) const
  {
    // Technique is to compute the arclength to each point of closest approach
    Eigen::Vector3f w0 = P0 - P1;
    float a(1.);
    float b(u0.dot(u1));
    float c(1.);
    float d(u0.dot(w0));
    float e(u1.dot(w0));
    float den(a * c - b * b);

    // Make sure lines are not colinear
    if (std::abs(den) > 10. * std::numeric_limits<float>::epsilon()) {
      float arcLen0 = (b * e - c * d) / den;
      float arcLen1 = (a * e - b * d) / den;

      poca0 = P0 + arcLen0 * u0;
      poca1 = P1 + arcLen1 * u1;
    }
    // Otherwise, take the poca to be the distance to the line at the second point
    else {
      float arcLenToNextPoint = w0.dot(u0);

      poca0 = P0 + arcLenToNextPoint * u0;
      poca1 = P1;
    }

    firstNextUnit = poca1 - poca0;

    float docaDist = firstNextUnit.norm();

    firstNextUnit = firstNextUnit.normalized();

    return docaDist;
  }

  const reco::ClusterHit3D* ClusterMergeAlg::findClosestHit3D(
    const Eigen::Vector3f& refPoint,
    const Eigen::Vector3f& refVector,
    const reco::HitPairListPtr& hitList) const
  {
    const reco::ClusterHit3D* nearestHit3D(hitList.front());
    float closest(std::numeric_limits<float>::max());

    for (const auto& hit3D : hitList) {
      Eigen::Vector3f refToHitVec = hit3D->getPosition() - refPoint;
      float arcLenToHit = refToHitVec.dot(refVector);

      if (arcLenToHit < closest) {
        nearestHit3D = hit3D;
        closest = arcLenToHit;
      }
    }

    return nearestHit3D;
  }

  const reco::ClusterHit3D* ClusterMergeAlg::findFurthestHit3D(
    const Eigen::Vector3f& refPoint,
    const Eigen::Vector3f& refVector,
    const reco::HitPairListPtr& hitList) const
  {
    const reco::ClusterHit3D* nearestHit3D(hitList.front());
    float furthest(-std::numeric_limits<float>::max());

    for (const auto& hit3D : hitList) {
      Eigen::Vector3f refToHitVec = hit3D->getPosition() - refPoint;
      float arcLenToHit = refToHitVec.dot(refVector);

      if (arcLenToHit > furthest) {
        nearestHit3D = hit3D;
        furthest = arcLenToHit;
      }
    }

    return nearestHit3D;
  }

  DEFINE_ART_CLASS_TOOL(ClusterMergeAlg)
} // namespace lar_cluster3d