Hough.cxx

Go to the documentation of this file.

// $Id: Hough.cxx,v 1.5 2008/09/11 20:12:15 ishi Exp $
//
// Hough transformation method.
//
// Author:  M. Messier    2006
//          Modified by M. Ishitsuka and S. Cavanaugh in Socal framework
// History  M. Ishitsuka  2007/11/01  import from Socal/MDMReco
#include <cmath>
#include "TH2F.h"
#include "TCanvas.h"
#include "TStyle.h"
#include "TMath.h"
#include "Hough.h"

using namespace rpr;

//This hough map uses a unique data structure written by Steve Cavanaugh, May 2007
//Instead of using a TH2F which would result in many empty bins
//  which would be search over many times when looking for a peak,
//  a linked list of bins over the required threshold is instead created -
//  the search for a peak then only looks at bins which are above the threshold

const static int gsNtheta = 360;
const static int gsNrho   = 500;

const static double minrho = -500;
const static double maxrho = 500;
const static double mintheta = 0.0;
const static double maxtheta = M_PI;

float_t myThresh=0;  //will hold tree threshold when tree is first made

node * nodearray[gsNtheta][gsNrho];
node * thetaarray[gsNtheta];
int thetacount[gsNtheta];
node * treehead =NULL;

hitnode * peaklist=NULL;

Hough::Hough() 
{
  for(int i=0;i<gsNtheta;i++){
    for(int j=0;j<gsNrho;j++)
    {
      nodearray[i][j]=NULL;
    }                   
    thetaarray[i]=NULL;
    thetacount[i]=0;
  }
}

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

Hough::~Hough() 
{
  //make sure to free up all linked list nodes....
  
  for(int i=0;i<gsNtheta;i++){
    if(thetacount[i]>0)DeleteChain(thetaarray[i]);
  }
  
  delete treehead;treehead=NULL;
}

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

void Hough::SetThresh(float thresh){myThresh=thresh;}

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

void Hough::MakeTree(float thresh){
  
  peaklist=NULL;
  
  myThresh=thresh;
  
  //need a infinite weight node for insertion sort
  if (treehead==NULL)
  {
    treehead=newNode(-1,-1);
    treehead->weight=INFINITY;
  }
  else treehead->c1=NULL;
  
  node * toadd=NULL;
  
  for(int i=0;i<gsNtheta;i++){
    toadd=thetaarray[i];
    while(toadd!=NULL){
      toadd->c1=NULL;
      if(toadd->weight>=thresh)
      {
        if(treehead->c1==NULL)treehead->c1=toadd;
        else InsertSort(treehead,toadd);
        toadd->goodsort=1;
      }
      toadd=toadd->c0;
    }
  }
}

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

//UpdateTree remakes the tree by resorting and only keeping bins above myThresh

int Hough::UpdateTree()
{
  
  //MsgDebug("Rpr","Updating Tree\n");
  
  node * toadd=NULL;
  node * temp = treehead->c1;
  node * pretemp = treehead;
  
  while(temp!=NULL){
    
    if(temp->weight<myThresh){pretemp->c1=temp->c1;temp->c1=NULL;temp=pretemp->c1;continue;}
    
    if (temp->goodsort==0){
      pretemp->c1=temp->c1;

      if(toadd==NULL)
      {
        toadd=temp;
        toadd->c1=NULL;
      }
      else
      {
        temp->c1=toadd;
        toadd=temp;
      }
      
      //leave pretemp in place
      temp=pretemp->c1;
      
    }else{
      pretemp=pretemp->c1;
      temp=pretemp->c1;
    }
  }
  
  while(toadd!=NULL){
    node * toaddtemp = toadd;
    toadd=toadd->c1;
    toaddtemp->c1=NULL; 
    
    toaddtemp->goodsort=1;
    if(toaddtemp->weight>=myThresh) InsertSort(treehead,toaddtemp);
  }

  if(treehead->c1!=NULL)return 1;
  return 0;  
}

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

void Hough::AddPoint(double theta, double rho, double ww, int ih1)
{
  int t = (int)TMath::Floor((theta-mintheta)/(maxtheta-mintheta)*(gsNtheta));
  int r = (int)TMath::Floor((rho-minrho)/(maxrho-minrho)*(gsNrho));
  
  if(r<0||t<0||r>=gsNrho||t>=gsNtheta) return;

  if (nodearray[t][r]==NULL){
    nodearray[t][r]=newNode(t,r);
    thetacount[t]++;
    nodearray[t][r]->c0=thetaarray[t];
    thetaarray[t]=nodearray[t][r];
  }
  
  nodeUpdate(nodearray[t][r],ww,ih1);
}

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

void Hough::RemovePoint(double theta, double rho, double ww, int ih1)
{
  int t = (int)TMath::Floor((theta-mintheta)/(maxtheta-mintheta)*(gsNtheta));
  int r = (int)TMath::Floor((rho-minrho)/(maxrho-minrho)*(gsNrho));
  
  if(r<0||t<0||r>=gsNrho||t>=gsNtheta) return;
  if(nodearray[t][r]==NULL)return;

  //use this code if keeping list of hits which made up bin
  /*
    int foundhit=0;
    
    hitnode * hit = nodearray[t][r]->hitlist;
    while(hit!=NULL&&hit->hit==ih1){nodearray[t][r]->hitlist=hit->c0;delete hit;hit=nodearray[t][r]->hitlist;foundhit=1;}
    while(hit!=NULL){if(hit->c0==NULL)break;if(hit->c0->hit==ih1){hitnode* t=hit->c0;hit->c0=hit->c0->c0;delete t;foundhit=1;}else hit=hit->c0;}
    
    if(foundhit==1){
    nodearray[t][r]->hitcount--;
    }
  */
  
  nodearray[t][r]->goodsort=0;
  nodearray[t][r]->weight+=ww;  //ww is <0 here... 
}

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

int Hough::getHit()
{
  int hit=-1;
  
  if(peaklist!=NULL)
  {
    hit=peaklist->hit;
    peaklist=peaklist->c0;
  }
  return hit;
}

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

void Hough::nodeUpdate(node * head, double ww, int ih1)
{
  
  head->weight+=ww;     
  
  //use code below to see which hits made up this bin
  return;
  hitnode *temp = head->hitlist;
  while(temp!=NULL){if(temp->hit==ih1){temp->weight+=ww;return;}temp=temp->c0;}

  hitnode *n = new hitnode;
  n->hit=ih1;
  n->weight=ww;
  n->c0=head->hitlist;
  head->hitlist=n;
  head->hitcount++;
}

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

node* Hough::newNode(int t, int r)
{
  node *temp=new node;
  temp->rho=r;
  temp->theta=t;
  temp->hitcount=0;
  temp->weight=0.0;
  temp->hitlist=NULL;
  temp->c0=NULL;
  temp->c1=NULL;
  temp->goodsort=0;
  temp->treeready=0;
  return temp;
}

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

void Hough::InsertSort(node * head, node *item)
{
        while(head->c1!=NULL&&item->weight<head->c1->weight){head=head->c1;}

                item->c1=head->c1;
                head->c1=item;
}

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

void Hough::DeleteChain(node *head)
{

        while(head!=NULL)
        {
                node *temp = head->c0;
                DeleteChain(head->hitlist);
                delete head;
                head=temp;
        }

}

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

void Hough::DeleteChain(hitnode * head)
{

        while(head!=NULL)
        {
                hitnode *temp=head;
                head=head->c0;
                delete temp;
        }

}


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

void Hough::AddPoint(double x1, double sigx1, double x2, double sigx2, 
                     double y1, double sigy1, double y2, double sigy2, double w, int ih1) 
{
  int itheta;
  int ir;
  double theta;
  double r;
  double yy1, yy2;
  double ww;

  double theta_segment = (y2!=y1)?
    -1.*atan((x2-x1)/(y2-y1)):
    M_PI/2.;
  if (theta_segment<0.) theta_segment += M_PI;
  double r_segment = cos(theta_segment)*x1 + sin(theta_segment)*y1;

  for (itheta=-3; itheta<=3; ++itheta) {
    theta = theta_segment + M_PI*(float)(itheta)/(float)(gsNtheta);
    if(theta<0.) theta = M_PI - theta;
    if(theta>M_PI) theta = theta - M_PI;
    for (ir=-3; ir<=3; ++ir) {
      r = r_segment + (maxrho-minrho)*(float)(ir)/(float)(gsNrho);  //mod by steve, must multiply by (maxrho-minrho)....
      yy1 = (sin(theta)!=0.)?
        (r - cos(theta)*x1)/sin(theta):
        y1;
      yy2 = (sin(theta)!=0.)?
        (r - cos(theta)*x2)/sin(theta):
        y2;
      ww = TMath::Gaus(yy1,y1,sigy1)*TMath::Gaus(yy2,y2,sigy2)*w;

        if(ww>0)AddPoint(theta,r,ww,ih1);       
        if(ww<0)RemovePoint(theta,r,ww,ih1);
    }
  }
}

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

void Hough::FindLines(double* thetamx, double* rmx, double *score)
{
        if (treehead->c1==NULL){*score=0;peaklist=NULL;return;}

        if(treehead->c1->goodsort==0)UpdateTree();  
        //update the tree if we failed to do so
        //so long as treehead->c1 has not been touched
        //it is still the max, so don't bother resorting rest of tree



        if (treehead->c1==NULL){*score=0;peaklist=NULL;return;}


        *thetamx=(float)(treehead->c1->theta+0.5)/(float)(gsNtheta)
                        *(maxtheta-mintheta)+mintheta;
        *rmx=(float)(treehead->c1->rho+0.5)/(float)(gsNrho)
                        *(maxrho-minrho)+minrho;
        *score=treehead->c1->weight;



        peaklist=treehead->c1->hitlist;


}

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

bool Hough::TestHit(double x,     double y,
                    double thmx,  double rhomx,
                    double sigth, double sigr)
{
  int    itheta;
  double theta;
  double r;
  for (itheta=0; itheta<5*gsNtheta; ++itheta) {
    theta = M_PI*(float)itheta/(float)(5*gsNtheta);
    if (fabs(theta-thmx)>sigth) continue;
    r     = cos(theta)*x + sin(theta)*y;
    if (fabs(r-rhomx)<sigr) return true;
  }
  return false;
}

bool Hough::TestHit(double x,     double y,
                    double thmx,  double rhomx,
                    double sigr)
{
  double r;
  r     = cos(thmx)*x + sin(thmx)*y;
  if (fabs(r-rhomx)<sigr) return true;
  return false;
}

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

float Hough::DistHit(double x,     double y,
                     double thmx,  double rhomx)
{
  double r;
  r     = cos(thmx)*x + sin(thmx)*y;
  return fabs(r-rhomx);
}

---