MultiHough2P.cxx

Go to the documentation of this file.

////////////////////////////////////////////////////////////////////////
/// \brief   Perform a "2 point" Hough transform on a collection of hits
/// \authors messier@indiana.edu, mibaird@indiana.edu
/// \date
////////////////////////////////////////////////////////////////////////
#include "HoughTransform/MultiHough2P.h"

#include <algorithm>
#include <cmath>
#include <cstdlib>
#include <cassert>
#include <iostream>

#include "TH2F.h"
#include "TMath.h"

#include "GeometryObjects/Geo.h"
#include "RecoBase/HitList.h"
#include "Utilities/func/MathUtil.h"

namespace hough
{
  // TH2F is a bad temporary storage for a fast algorithm. There's a lot of
  // overhead. Using this simple version is at least 2x faster.
  class LiteTH2
  {
  public:
    LiteTH2(int nx, float x0, float x1,
            int ny, float y0, float y1) 
      : fNx(nx), fX0(x0), fX1(x1),
        fNy(ny), fY0(y0), fY1(y1)
    {
      fBins = new float[(nx+2)*(ny+2)];
      memset(fBins, 0, (nx+2)*(ny+2)*sizeof(float));
    }
    LiteTH2(const LiteTH2& h)
      : fNx(h.fNx), fX0(h.fX0), fX1(h.fX1),
        fNy(h.fNy), fY0(h.fY0), fY1(h.fY1)
    {
      fBins = new float[(fNx+2)*(fNy+2)];
      memcpy(fBins, h.fBins, (fNx+2)*(fNy+2)*sizeof(float));
    }
    ~LiteTH2()
    {
      delete[] fBins;
    }
    int GetNbinsX() const {return fNx;}
    int GetNbinsY() const {return fNy;}
    float GetBinCenterX(int ix) const
    {
      assert(ix >= 0 && ix < fNx+2);
      return fX0+(ix-.5)/fNx*(fX1-fX0);
    }
    float GetBinCenterY(int iy) const
    {
      assert(iy >= 0 && iy < fNy+2);
      return fY0+(iy-.5)/fNy*(fY1-fY0);
    }
    float GetBinContent(int ix, int iy) const
    {
      const int bin = iy*(fNx+2)+ix;
      if(bin < 0 || bin > (fNx+2)*(fNy+2)) return 0;
      return fBins[bin];
    }
    int FindBinX(float x) const
    {
      int ret = fNx*(x-fX0)/(fX1-fX0)+1;
      if(ret < 0) return 0;
      if(ret > fNx+1) return fNx+1;
      return ret;
    }
    int FindBinY(float y) const
    {
      int ret = fNy*(y-fY0)/(fY1-fY0)+1;
      if(ret < 0) return 0;
      if(ret > fNy+1) return fNy+1;
      return ret;
    }
    void SetBinContent(int ix, int iy, float val)
    {
      const int bin = iy*(fNx+2)+ix;
      assert(bin >= 0 && bin < (fNx+2)*(fNy+2));
      fBins[iy*(fNx+2)+ix] = val;
    }
    void AddBinContent(int ix, int iy, float val)
    {
      const int bin = iy*(fNx+2)+ix;
      assert(bin >= 0 && bin < (fNx+2)*(fNy+2));
      fBins[iy*(fNx+2)+ix] += val;
    }
    TH2F* ToTH2F() const
    {
      TH2F* ret = new TH2F("", "", fNx, fX0, fX1, fNy, fY0, fY1);
      for(int ix = 0; ix < fNx+2; ++ix)
        for(int iy = 0; iy < fNy+2; ++iy)
          ret->SetBinContent(ix, iy, GetBinContent(ix, iy));
      return ret;
    }
  protected:
    float* fBins;
    int fNx;
    float fX0, fX1;
    int fNy;
    float fY0, fY1;
  };

  //......................................................................
  
  MultiHough2P::MultiHough2P(geo::View_t v,
                             double rhosz,
                             double thetasz,
                             int rhosm,
                             int thetasm,
                             double rsqr,
                             double pco,
                             int loops,
                             double rmdst) :
    fView(v),
    fRhoSz(rhosz),
    fThetaSz(thetasz),
    fXwinSz(rhosm),
    fYwinSz(thetasm),
    fSigma(3.0/sqrt(12.)),
    fRsqr(rsqr),
    fPco(pco),
    fLoops(loops),
    fRmDst(rmdst),
    fHspaceW(0),
    fH(v, 0, 0, 0, 0)
  { }
  
  //......................................................................
  
  void MultiHough2P::RhoTheta(double x1, double y1,
                              double x2, double y2,
                              double* rho, double* theta,
                              double* sigmarho, double* sigmatheta)
  {
    *theta = atan2(x1-x2,y2-y1);
    *rho   = 0.5*(cos(*theta)*(x1+x2)+sin(*theta)*(y1+y2));
    //
    // Keep rho and theta within range [0,pi) (-R,+R)
    //
    while (*theta<0) {
      *theta +=  M_PI;
      *rho    = -(*rho);
    }
    while (*theta>=M_PI) {
      *theta -=  M_PI;
      *rho    = -(*rho);
    }
    
    // Compute the error estimates
    double d = util::pythag((x2-x1),(y2-y1));
    *sigmarho   = fSigma;
    *sigmatheta = M_SQRT2*fSigma/d;

    // these were failed attempts at making sigmatheta level off at a fixed value
    //*sigmatheta = M_SQRT2*fSigma/d + 0.1;
    /*
    if(d < 20.0) *sigmatheta = M_SQRT2*fSigma/d;
    else *sigmatheta = M_SQRT2*fSigma/20.0;
    */
  }
  
  //......................................................................
  
  MultiHough2P::~MultiHough2P() 
  {
    if (fHspaceW) { delete fHspaceW; fHspaceW = 0;}
  }
  
  //......................................................................
  
  void MultiHough2P::BuildMap(double xlo, double xhi,
                              double ylo, double yhi)
  {
    double r;   // Size of hough map in rho
    int    nr;  // Number of bins to use in rho
    int    nth; // Number of bins to use in theta
    
    // 0.55 gives a little buffer beyond the minimum required 0.5
    r  = 0.55*util::pythag((xhi-xlo),(yhi-ylo));
    nr = (int)rint(r/fRhoSz+0.5);
    
    nth = (int)rint(180.0/fThetaSz+0.5);
    
    // Allocate the Hough space histogram
    if (fHspaceW!=0) { delete fHspaceW; fHspaceW=0; }
    fHspaceW = new LiteTH2(nr, -r, r,
                           nth,-0.5*M_PI/180.0,179.5*M_PI/180.0);
  }

  //......................................................................

  TH2F* MultiHough2P::MapToTH2F() const
  {
    if(!fHspaceW) return 0;
    TH2F* ret = fHspaceW->ToTH2F();
    ret->SetTitle(";rho [cm];theta [radians]");

    const char* tiw = 0;
    if(fView == geo::kX) tiw = "fHough2P_Hx";
    else if (fView == geo::kY) tiw = "fHough2P_Hy";
    else abort(); 
    ret->SetTitle(tiw);

    return ret;
  }
  
  //......................................................................

  // This is special-case version of TMath::Gaus that saves time by assuming
  // the mean is zero and not including a switch for whether to normalize.
  static Double_t Gaus(const Double_t x, const Double_t sigma)
  {
     const Double_t arg = x/sigma;
     // for |arg| > 39  result is zero in double precision
     if (arg < -39.0 || arg > 39.0) return 0.0;
     const Double_t res = exp(-0.5*arg*arg);
     return res/(2.50662827463100024*sigma); //sqrt(2*Pi)=2.50662827463100024
  }

  
  void MultiHough2P::Map(const rb::HitList& h)
  {
    unsigned int i;
    unsigned int j;
    
    //
    // Find a box that completely contains all the hits. A note on
    // coordinates: Since the literature on Hough transforms uses an
    // "x-y" coodinate system I map NOvA z to Hough x and NOvA X/Y to
    // Hough y so that the formalizm matches what one would find in the
    // literature.
    //
    double tlo=9E9, thi=-9e9;
    double xlo=9e9, xhi=-9e9;
    double ylo=9e9, yhi=-9e9;
    for (i=0; i<h.size(); ++i) {
      if (h[i].fView == fView) {
        if (h[i].fZ<xlo)  xlo = h[i].fZ;
        if (h[i].fZ>xhi)  xhi = h[i].fZ;
        if (h[i].fXY<ylo) ylo = h[i].fXY;
        if (h[i].fXY>yhi) yhi = h[i].fXY;
        if (h[i].fT<tlo)  tlo = h[i].fT;
        if (h[i].fT>tlo)  thi = h[i].fT;
      }
    }
    fH.fTNSlo = tlo;
    fH.fTNShi = thi;
    fH.fZ0  = 0.5*(xlo+xhi);
    fH.fXY0 = 0.5*(ylo+yhi);



    //
    // Fill the hough map
    //
    this->BuildMap(xlo,xhi,ylo,yhi);

    for (i=0; i<h.size(); ++i) {
      if (h[i].fView != fView) continue;
      for (j=i+1; j<h.size(); ++j) {
        if (h[j].fView != fView) continue;
        const bool hasweight = h[i].fW != 0.0 && h[j].fW != 0.0;
        if(!hasweight) continue;
        
        // Check distance between hit pair
        double rsqr = util::sqr(h[i].fZ-h[j].fZ) + util::sqr(h[i].fXY-h[j].fXY);

        if (rsqr>fRsqr) continue;
        
        // don't let points in the same column vote unless they are close together
        // if ( abs(h[i].fZ - h[j].fZ) < 5.9 && rsqr > fRsqr/4.0 ) continue;
        
        // don't let points in the same row vote unless they are far apart
        if ( abs(h[i].fXY - h[j].fXY) < 3.9 && rsqr < fRsqr/4.0 ) continue;
        
        double rho,      theta;
        double sigmarho, sigmatheta;
        this->RhoTheta(h[i].fZ-fH.fZ0, h[i].fXY-fH.fXY0,
                       h[j].fZ-fH.fZ0, h[j].fXY-fH.fXY0,
                       &rho,           &theta,
                       &sigmarho,      &sigmatheta);
        //
        // Fill region weighted by errors
        //
        int ixlo = fHspaceW->FindBinX(rho-3.0*sigmarho);
        int ixhi = fHspaceW->FindBinX(rho+3.0*sigmarho);
        int iylo = fHspaceW->FindBinY(theta-3.0*sigmatheta);
        int iyhi = fHspaceW->FindBinY(theta+3.0*sigmatheta);
        
        if (ixlo<1)                 ixlo = 1;
        if (iylo<1)                 iylo = 1;
        if (ixhi>fHspaceW->GetNbinsX()) ixhi = fHspaceW->GetNbinsX();
        if (iyhi>fHspaceW->GetNbinsY()) iyhi = fHspaceW->GetNbinsY();
        
        int ix, iy;

        // Precalcuate gaussian weights since they will be the same every loop
        double yprob[iyhi]; // won't use first elements: use memory to save time
        for (iy=iylo; iy<=iyhi; ++iy) {
          const double y = fHspaceW->GetBinCenterY(iy);
          yprob[iy] = Gaus(y-theta, sigmatheta);
        }

        for (ix=ixlo; ix<=ixhi; ++ix) {
          double x = fHspaceW->GetBinCenterX(ix);
          const double xprob = Gaus(x-rho, sigmarho);
          for (iy=iylo; iy<=iyhi; ++iy) {
            double w = 0.0;

            // if(h[i].fW > 0.1 && h[j].fW > 0.1) w = 2.0*h[i].fW*h[j].fW/(h[i].fW+h[j].fW);
            w = 2.0*h[i].fW*h[j].fW/(h[i].fW+h[j].fW) *
              xprob * yprob[iy];
            fHspaceW->AddBinContent(ix,iy,w);

          }
        }
      }
    }
    if (fXwinSz > 0 || fYwinSz > 0) this->SmoothMap();
    
  }
  
  //......................................................................
  
  void MultiHough2P::MultiMap(rb::HitList h)
  {
    
    int flag = 0;  // flag to jump out of multi hough loop
    int mh   = 1;  // count of number of multihough loops
    
    // make the initial Hough Map
    this->Map(h);
    
    // calculate the threshold (needs to be done once at the beginning so that it is
    // the same for all subsequent maps)
    double Pave = 0.0;     // average peak height
    double Ps = 0.0;       // standard deviation of average peak height
    double th = 0.0;       // threshold to use for peak cutoff
    int nb = 0;            // number of bins counted for avergae peak height
    
    // calculate average peak height to determine threshold
    // empty bins are currently counted
    for (int a = 1; a <= fHspaceW->GetNbinsX(); ++a) {
      for (int b = 1; b <= fHspaceW->GetNbinsY(); ++b) {
        double hab = fHspaceW->GetBinContent(a, b);
        if (hab != 0) {
          Pave += hab;
          Ps += hab*hab;
        } // end if
        nb++;
      } // end for b
    }   // end for a
    
    Pave = Pave/nb;
    Ps = sqrt(Ps/nb - Pave*Pave);
    th = Pave + fPco*Ps;

    // If the threshold is zero, then the initial Hough map is empty so there
    // is no reason to proceed.
    if(th == 0.0) return;
    
    // find the tallest peak
    int ix, iy;
    int xpeak = 0, ypeak = 0;
    double peak  = 0.0;
    double r     = 0.0;
    double theta = 0.0;
    double p;
    for(ix = 1; ix <= fHspaceW->GetNbinsX(); ++ix) {
      for(iy = 1; iy <= fHspaceW->GetNbinsY(); ++iy) {
        p = fHspaceW->GetBinContent(ix,iy);
        if (p>peak) {
          peak   = p;
          xpeak = ix;
          ypeak = iy;
        } // end if p>peak
      } // loop on y bins
    } // loop on x bins      
    
    // Average bins around the peak to get a better rho and theta value
    this->RefinePeak(r, theta, xpeak, ypeak);
    
    // AS OF RIGHT NOW, SIGMA-RHO AND SIGMA-THETA ARE NOT CALCULATED!!! 
    double sr = 0.0;
    double st = 0.0;

    // put tallest peak into Hough Results
    if(peak > 0.0) fH.fPeak.push_back(rb::HoughPeak(r,theta,peak,th,sr,st));

    // remove hits on the line by reweighting them
    this->ReweightHits(r, theta, h);



    
    //
    // start the MultiHough loop
    //
    
    // if either the number of desired loops is only one or the tallest peak was already below threshold,
    // then skip the multi-hough loop because we are already done
    if(peak < th)   flag = 1;
    if(mh >= fLoops) flag = 1;
    
    while(flag == 0) {
      
      mh++;
      if(mh >= fLoops) flag = 1;
      
      // repeat everything done above EXCEPT: calculating the threshold

      // build the new map
      this->Map(h);

      
      
      // find the tallest peak
      xpeak  = 0;
      ypeak  = 0;
      peak   = 0.0;
      r      = 0.0;
      theta  = 0.0;
      p      = 0.0;

      for (ix=1; ix<=fHspaceW->GetNbinsX(); ++ix) {
        for (iy=1; iy<=fHspaceW->GetNbinsY(); ++iy) {
          p = fHspaceW->GetBinContent(ix,iy);
          if (p>peak) {
            peak   = p;
            xpeak = ix;
            ypeak = iy;
          } // end if p>peak
        } // loop on y bins
      } // loop on x bins      

      this->RefinePeak(r, theta, xpeak, ypeak);

      sr = 0.0;
      st = 0.0;


      
      // Run parallel line test: (If two lines have almost the same rho and theta values,
      // they are assumed to be part of the same line and that line is NOT put into the
      // Hough results.)
      //
      // NOTE:  This is a little sensitive to the numbers 15.0 and 0.02.
      int parflag = 0;
      for(unsigned int i = 0; i < fH.fPeak.size(); ++i) {
        if(fabs(r - fH.fPeak[i].fRho) < 15.0 && fabs(theta - fH.fPeak[i].fTheta) < 0.02) parflag = 1;
      }
          
      // put only the tallest peak into Houghresults
      if(parflag != 1 && peak >= th) fH.fPeak.push_back(rb::HoughPeak(r,theta,peak,th,sr,st));
      if(peak < th) flag = 1;
      
      this->ReweightHits(r, theta, h);

    } // end MultiHough loop
    
    fH.SortPeaks();
    
  }
  
  //......................................................................
  
  void MultiHough2P::SmoothMap() 
  {
    
    // NOTE:  Since most of the Hough space map is empty, we only apply smoothing to the non-zero
    //        bins in order to save lots of calculation time.

    int i, j, m, n;
    int nx, ny;
    int ix, iy;
    double sum;
    int wx, wy;
    
    wx = 2*fXwinSz + 1;
    wy = 2*fYwinSz + 1;
    
    double w[wx][wy];
    
    // fill the smoothing weight map
    for(int a = 0; a < wx; a++) {
      for(int b = 0; b < wy; b++) {
        
        w[a][b] = Gaus(a - fXwinSz, fXwinSz/3.0)*Gaus(b - fYwinSz, fYwinSz/3.0);
        
      } // end for b
    } // end for a
    
    LiteTH2* htmp = new LiteTH2(*fHspaceW);
    
    nx = htmp->GetNbinsX();
    ny = htmp->GetNbinsY();
    for (i=1; i<=nx; ++i) {
      for(j=1; j<=ny; ++j) {
        sum = 0.0;
        // skip smoothing if bin is empty
        if(htmp->GetBinContent(i,j) == 0.0) continue;
        for (m=-fXwinSz; m<=fXwinSz; m++) {
          ix = i+m;
          // Don't wrap around in rho
          if (ix>nx) continue;
          if (ix<1)  continue;
          for (n=-fYwinSz; n<=fYwinSz; ++n) {
            iy = j+n;
            // Wrap in theta
            if (iy>ny) iy = iy%ny;
            if (iy<1)  iy = ny+iy;
            
            sum += w[m + fXwinSz][n + fYwinSz]*htmp->GetBinContent(ix,iy);
          }
        }
        fHspaceW->SetBinContent(i,j,sum);
      }
    }
    delete htmp;
    htmp = 0;
    
  }



  //......................................................................
  
  void MultiHough2P::RefinePeak(double &rho, double &theta, int xp, int yp) 
  {
    // make these fcl parameters???
    int nx = 3, ny = 3;      // +/- number of bins around peak to use for averaging
    double peaktotal = 0.0;
    double BinValue  = 0.0;
    int nXbins = fHspaceW->GetNbinsX();
    int nYbins = fHspaceW->GetNbinsY();

  
    // determine if bins to be averaged over will be off the map and constrain nx and ny accordingly
    if((xp+nx) >= nXbins) nx = (nXbins-1) - xp;
    if((xp-nx) < 0)       nx = xp;
    
    if((yp+ny) >= nYbins) ny = (nYbins-1) - yp;
    if((yp-ny) < 0)       ny = yp;



    double xd = 0.0, yd = 0.0;
    double dist = 0.0;

    for(int xi = -nx; xi <= nx; ++xi) {
      xd = fabs(xi);
      for(int yi = -ny; yi <= ny; ++yi) {

        yd = fabs(yi);
        dist = util::pythag(xd,yd);
        if(dist == 0.0) dist = 0.707107;

        BinValue   = (fHspaceW->GetBinContent(xp+xi,yp+yi))/dist;
        peaktotal += BinValue;
        rho       += BinValue*fHspaceW->GetBinCenterX(xp+xi);
        theta     += BinValue*fHspaceW->GetBinCenterY(yp+yi);

      }
    }

    rho   = rho/peaktotal;
    theta = theta/peaktotal;

  }



  //......................................................................
  
  void MultiHough2P::ReweightHits(double r, double theta, rb::HitList &h) 
  {
    //
    // This function does several things:
    //   1. It sets the weights of all hits within 6 cm of the line to zero.
    //   2. Then it resets the weights of the end points of the line to one.
    //   3. Then it applies more scrubbing to remove stray hits that have no neighbors.
    //

    // These variable were used in an old scheme in which I used a Fermi distribution function
    // to determine the new hit weights based on their distance to the line.  I left it in for
    // potential future use.
    // double dc   = 4.0;   // distance cutoff
    // double ds   = 1.0;   // distance spread


    // loop over all points, determine which are on that line and set their weights to zero
    double m = 0.0;
    double b = 0.0;
    double s = sin(theta);
    double c = cos(theta);
    
    // calculate slope/intercept in detector coordinates
    if (s == 0.0) s = 1.0E-9;
    m = -c/s;
    b = r/s - m*fH.fZ0 + fH.fXY0;
    
    double d = 0.0; // perpendicular distance of point to line

    // numbers needed for determining which points are end points
    double zmin = 1.0e9, zmax = -1.0e9;
    unsigned int amax  = 999999;
    unsigned int amin  = 999999;
    // amax2 & amin2 are used for the case that there are 2 end points (when there are two points
    // in the same plane within fRmDist (cm) of the line.)
    // NOTE:  There could be three points that fit this, but I only allow for keeping two.
    unsigned int amax2 = 999999;
    unsigned int amin2 = 999999;
    
    for(unsigned int a = 0; a < h.size(); ++a) {
      
      // if a point is on the line, set its weight to zero
      if(h[a].fView == fView) {
        
        d = geo::DsqrToLine(h[a].fZ, h[a].fXY, 0.0, b, 100.0, 100.0*m + b);
        d = sqrt(d);
        // h[a].fW *= 1.0 - 1.0/(exp((d - dc)/ds) + 1.0);   // The Fermi weight function
        if(d < fRmDst) {
          h[a].fW = 0.0;
          
          // determine end points
          if(h[a].fZ == zmax) { amax2 = a; }
          if(h[a].fZ == zmin) { amin2 = a; }
          if(h[a].fZ > zmax)  { zmax = h[a].fZ; amax = a; }
          if(h[a].fZ < zmin)  { zmin = h[a].fZ; amin = a; }
        }

      } // end if fView
    } // end for a
    
    // Reset the weights of the line end points to one.
    if(amax  != 999999) h[amax].fW  = 1.0;
    if(amin  != 999999) h[amin].fW  = 1.0;
    if(amax2 != 999999) h[amax2].fW = 1.0;
    if(amin2 != 999999) h[amin2].fW = 1.0;


    
    // Rescrub the hit list to remove points with no neighbors within sqrt(ScrubDistSq) (cm).
    double dminsq = 0.0, dsq = 0.0;
    double ScrubDistSq = 900.0;

    // We will keep track of which points are within sqrt(ScrubDistSq) (cm) of
    // another point (i.e. - which points have a buddy) to make this more efficient.
    std::vector<int> BuddyList;
    for(unsigned int a = 0; a < h.size(); ++a) BuddyList.push_back(0);

    // For each point, loop over all others to determine if it has a "buddy" within sqrt(ScrubDistSq).
    // If it has no "buddies," scrub it! (Set the weight to zero.)
    for(unsigned int i = 0; i < h.size(); ++i) {
      if(BuddyList[i] > 0)    continue;
      if(h[i].fView != fView) continue;
      if(h[i].fW == 0.0)      continue;
      dminsq = 1.0e9;
      for(unsigned int j = 0; j < h.size(); ++j) {
        if(i == j)              continue;
        if(h[j].fView != fView) continue;
        if(h[j].fW == 0.0)      continue;
        dsq = (h[i].fZ - h[j].fZ)*(h[i].fZ - h[j].fZ) + (h[i].fXY - h[j].fXY)*(h[i].fXY - h[j].fXY);
        if(dsq < ScrubDistSq) {
          BuddyList[i] += 1;
          BuddyList[j] += 1;
        }
        if(dsq < dminsq) dminsq = dsq;
      }
      if(dminsq >= ScrubDistSq) h[i].fW = 0.0;
    }
    
  }



} // end namespace hough
////////////////////////////////////////////////////////////////////////