HoughCalc_service.cc
Go to the documentation of this file.
1 ////////////////////////////////////////////////////////////////////////
2 /// \brief Hough Calculation service
3 /// \author Alexandre Sousa - asousa@physics.harvard.edu
4 /// \date
5 ////////////////////////////////////////////////////////////////////////
6 #include "Utilities/HoughCalc.h"
7 
8 #include <iostream>
9 #include <cmath>
10 
11 
12 #include "TMath.h"
13 
14 namespace util{
15 
16  //This hough map uses a unique data structure written by Steve Cavanaugh,
17  //May 2007
18  // Instead of using a TH2F which would result in many empty bins
19  // which would be search over many times when looking for a peak,
20  // a linked list of bins over the required threshold is instead created -
21  // the search for a peak then only looks at bins which are above the threshold
22 
23  // Don't pollute the util namespace with these
24  namespace{
25  const static int gsNtheta = 360;
26  const static int gsNrho = 500;
27 
28  const static double minrho = -500;
29  const static double maxrho = 500;
30  const static double mintheta = 0.0;
31  const static double maxtheta = M_PI;
32 
33  double myThresh=0; //will hold tree threshold when tree is first made
34 
35 
36  HoughCalc::node *nodearray[gsNtheta][gsNrho];
37  HoughCalc::node *thetaarray[gsNtheta];
38 
39  int thetacount[gsNtheta];
40 
41  HoughCalc::node *treehead =NULL;
42  HoughCalc::hitnode *peaklist=NULL;
43  }
44 
46  {
47  for(int i=0;i<gsNtheta;i++){
48  for(int j=0;j<gsNrho;j++){
49  nodearray[i][j]=NULL;
50  }
51  thetaarray[i]=NULL;
52  thetacount[i]=0;
53  }
54  }
55 
57  {
58  //Free up all linked list nodes....
59  for(int i=0;i<gsNtheta;i++){
60  if(thetacount[i]>0)DeleteChain(thetaarray[i]);
61  }
62 
63  delete treehead;treehead=NULL;
64  }
65 
66 
67  //......................................................................
69  if(treehead!=NULL){
70  for(int i=0;i<gsNtheta;i++){
71  if(thetacount[i]>0)DeleteChain(thetaarray[i]);
72  thetaarray[i]=NULL;
73  thetacount[i]=0;
74  for(int j=0;j<gsNrho;j++){
75  nodearray[i][j]=NULL;
76  }
77  }
78  treehead=NULL;
79  }
80  }
81 
82 
83  //......................................................................
84 
85  void HoughCalc::SetThresh(double thresh){myThresh=thresh;}
86 
87  //......................................................................
88 
89  void HoughCalc::MakeTree(double thresh){
90 
91  peaklist=NULL;
92 
93  myThresh=thresh;
94 
95  //need a infinite weight node for insertion sort
96  if (treehead==NULL)
97  {
98  treehead=newNode(-1,-1);
99  treehead->weight=INFINITY;
100  }
101  else treehead->c1=NULL;
102 
103  node *toadd=NULL;
104 
105  for(int i=0;i<gsNtheta;i++){
106  toadd=thetaarray[i];
107  while(toadd!=NULL){
108  toadd->c1=NULL;
109  if(toadd->weight>=thresh)
110  {
111  if(treehead->c1==NULL)treehead->c1=toadd;
112  else InsertSort(treehead,toadd);
113  toadd->goodsort=1;
114  }
115  toadd=toadd->c0;
116  }
117  }
118  }
119 
120  //......................................................................
121 
122  //UpdateTree remakes the tree by resorting and only keeping bins above myThresh
123 
125  {
126 
127  //MsgDebug("Util","Updating Tree\n");
128 
129  node * toadd=NULL;
130  node * temp = treehead->c1;
131  node * pretemp = treehead;
132 
133  while(temp!=NULL){
134 
135  if(temp->weight<myThresh){pretemp->c1=temp->c1;temp->c1=NULL;temp=pretemp->c1;continue;}
136 
137  if (temp->goodsort==0){
138  pretemp->c1=temp->c1;
139 
140  if(toadd==NULL)
141  {
142  toadd=temp;
143  toadd->c1=NULL;
144  }
145  else
146  {
147  temp->c1=toadd;
148  toadd=temp;
149  }
150 
151  //leave pretemp in place
152  temp=pretemp->c1;
153 
154  }else{
155  pretemp=pretemp->c1;
156  temp=pretemp->c1;
157  }
158  }
159 
160  while(toadd!=NULL){
161  node * toaddtemp = toadd;
162  toadd=toadd->c1;
163  toaddtemp->c1=NULL;
164 
165  toaddtemp->goodsort=1;
166  if(toaddtemp->weight>=myThresh) InsertSort(treehead,toaddtemp);
167  }
168 
169  if(treehead->c1!=NULL)return 1;
170  return 0;
171  }
172 
173  //......................................................................
174 
175  void HoughCalc::AddPoint(double theta, double rho, double ww, int ih1)
176  {
177  int t = (int)TMath::Floor((theta-mintheta)/(maxtheta-mintheta)*(gsNtheta));
178  int r = (int)TMath::Floor((rho-minrho)/(maxrho-minrho)*(gsNrho));
179 
180  if(r<0||t<0||r>=gsNrho||t>=gsNtheta) return;
181 
182  if (nodearray[t][r]==NULL){
183  nodearray[t][r]=newNode(t,r);
184  thetacount[t]++;
185  nodearray[t][r]->c0=thetaarray[t];
186  thetaarray[t]=nodearray[t][r];
187  }
188 
189  nodeUpdate(nodearray[t][r],ww,ih1);
190  }
191 
192  //......................................................................
193 
194  void HoughCalc::RemovePoint(double theta, double rho, double ww, int /*ih1*/)
195  {
196  int t = (int)TMath::Floor((theta-mintheta)/(maxtheta-mintheta)*(gsNtheta));
197  int r = (int)TMath::Floor((rho-minrho)/(maxrho-minrho)*(gsNrho));
198 
199  if(r<0||t<0||r>=gsNrho||t>=gsNtheta) return;
200  if(nodearray[t][r]==NULL)return;
201 
202  //use this code if keeping list of hits which made up bin
203  /*
204  int foundhit=0;
205 
206  hitnode * hit = nodearray[t][r]->hitlist;
207  while(hit!=NULL&&hit->hit==ih1){nodearray[t][r]->hitlist=hit->c0;delete hit;hit=nodearray[t][r]->hitlist;foundhit=1;}
208  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;}
209 
210  if(foundhit==1){
211  nodearray[t][r]->hitcount--;
212  }
213  */
214 
215  nodearray[t][r]->goodsort=0;
216  nodearray[t][r]->weight+=ww; //ww is <0 here...
217  }
218 
219  //......................................................................
220 
222  {
223  int hit=-1;
224 
225  if(peaklist!=NULL)
226  {
227  hit=peaklist->hit;
228  peaklist=peaklist->c0;
229  }
230  return hit;
231  }
232 
233  //......................................................................
234 
235  void HoughCalc::nodeUpdate(node * head, double ww, int ih1)
236  {
237 
238  head->weight+=ww;
239 
240  //use code below to see which hits made up this bin
241  return;
242  hitnode *temp = head->hitlist;
243  while(temp!=NULL){if(temp->hit==ih1){temp->weight+=ww;return;}temp=temp->c0;}
244 
245  hitnode *n = new hitnode;
246  n->hit=ih1;
247  n->weight=ww;
248  n->c0=head->hitlist;
249  head->hitlist=n;
250  head->hitcount++;
251  }
252 
253  //......................................................................
254 
256  {
257  node *temp=new node;
258  temp->rho=r;
259  temp->theta=t;
260  temp->hitcount=0;
261  temp->weight=0.0;
262  temp->hitlist=NULL;
263  temp->c0=NULL;
264  temp->c1=NULL;
265  temp->goodsort=0;
266  temp->treeready=0;
267  return temp;
268  }
269 
270  //......................................................................
271 
273  {
274  while(head->c1 != NULL &&
275  item->weight < head->c1->weight )
276  {head=head->c1;}
277  item->c1=head->c1;
278  head->c1=item;
279  }
280 
281  //......................................................................
282 
284  {
285  while(head != NULL){
286  node *temp = head->c0;
287  DeleteChain(head->hitlist);
288  delete head;
289  head=temp;
290  }
291 
292  }
293 
294  //......................................................................
295 
297  {
298  while(head!=NULL){
299  hitnode *temp=head;
300  head=head->c0;
301  delete temp;
302  }
303  }
304 
305  //......................................................................
306 
307  void HoughCalc::AddPoint(double x1, double /*sigx1*/,
308  double x2, double /*sigx2*/,
309  double y1, double sigy1, double y2, double sigy2,
310  double w, int ih1)
311  {
312  int itheta;
313  int ir;
314  double theta;
315  double r;
316  double yy1, yy2;
317  double ww;
318 
319  double theta_segment = (y2!=y1)?
320  -1.*atan((x2-x1)/(y2-y1)):
321  M_PI/2.;
322  if (theta_segment<0.) theta_segment += M_PI;
323  double r_segment = cos(theta_segment)*x1 + sin(theta_segment)*y1;
324 
325  for (itheta=-3; itheta<=3; ++itheta) {
326  theta = theta_segment + M_PI*(double)(itheta)/(double)(gsNtheta);
327  if(theta<0.) theta = M_PI - theta;
328  if(theta>M_PI) theta = theta - M_PI;
329  for (ir=-3; ir<=3; ++ir) {
330  r = r_segment + (maxrho-minrho)*(double)(ir)/(double)(gsNrho); //mod by steve, must multiply by (maxrho-minrho)....
331  yy1 = (sin(theta)!=0.)?
332  (r - cos(theta)*x1)/sin(theta):
333  y1;
334  yy2 = (sin(theta)!=0.)?
335  (r - cos(theta)*x2)/sin(theta):
336  y2;
337  ww = TMath::Gaus(yy1,y1,sigy1)*TMath::Gaus(yy2,y2,sigy2)*w;
338 
339  if(ww > 0.01) AddPoint(theta,r,ww,ih1);
340  if(ww < 0) RemovePoint(theta,r,ww,ih1);
341  }
342  }
343  }
344 
345  //......................................................................
346 
347  void HoughCalc::FindLines(double* thetamx, double* rmx, double *score)
348  {
349  if (treehead->c1==NULL){*score=0;peaklist=NULL;return;}
350 
351  if(treehead->c1->goodsort==0)UpdateTree();
352  //update the tree if we failed to do so
353  //so long as treehead->c1 has not been touched
354  //it is still the max, so don't bother resorting rest of tree
355 
356  if (treehead->c1==NULL){*score=0;peaklist=NULL;return;}
357 
358 
359  *thetamx=(double)(treehead->c1->theta+0.5)/(double)(gsNtheta)
360  *(maxtheta-mintheta)+mintheta;
361  *rmx=(double)(treehead->c1->rho+0.5)/(double)(gsNrho)
362  *(maxrho-minrho)+minrho;
363  *score=treehead->c1->weight;
364 
365  peaklist=treehead->c1->hitlist;
366 
367  return;
368  }
369 
370  //......................................................................
371 
372  void HoughCalc::FindLineErrs(double* ethetamx, double* ermx)
373  {
374  //this was a trial function, but it doesn't work very well. I wouldn't use it.--TV
375 
376  if (treehead->c1==NULL){peaklist=NULL;return;}
377 
378  if(treehead->c1->goodsort==0)UpdateTree();
379  //update the tree if we failed to do so
380  //so long as treehead->c1 has not been touched
381  //it is still the max, so don't bother resorting rest of tree
382 
383  if (treehead->c1==NULL){peaklist=NULL;return;}
384 
385  double sigr=0.;
386  double sigt=0.;
387  double totw=0.;
388 
389  double thetamx=(double)(treehead->c1->theta);
390  double rmx=(double)(treehead->c1->rho);
391  // std::cout<<"thetamx "<<thetamx<<" rho "<<rmx<<std::endl;
392  node *temp = treehead->c1;
393  while(temp!=NULL){
394  // std::cout<<"temp weight is "<<temp->weight<<" my thres "<<myThresh<<std::endl;
395  if(temp->weight>=myThresh){
396  sigr+=(temp->rho-rmx)*(temp->rho-rmx)*fabs(temp->weight);
397  sigt+=(temp->theta-thetamx)*(temp->theta-thetamx)*fabs(temp->weight);
398  totw+=fabs(temp->weight);
399  // std::cout<<" rho "<<temp->rho<<" theta "<<temp->theta<<std::endl;
400  // std::cout<<"sigr "<<sigr<<" sigt "<<sigt<<" totw "<<totw<<std::endl;
401  }
402  temp = temp->c1;
403  }
404  // std::cout<<"after loop sigr "<<sigr<<" sigt "<<sigt<<" totw "<<totw<<std::endl;
405  *ethetamx=0.;
406  *ermx=0.;
407  if(fabs(totw)>0.001){
408  *ethetamx = sqrt(sigt/totw)/(double)(gsNtheta)*(maxtheta-mintheta);
409  *ermx = sqrt(sigr/totw)/(double)(gsNrho)*(maxrho-minrho);
410  }
411  // std::cout<<"theta "<<thetamx<<" rmx "<<rmx<<std::endl;
412  // std::cout<<"all done err theta "<<*ethetamx<<" ermx "<<*ermx<<std::endl;
413  return;
414  }
415 
416  //......................................................................
417 
418  bool HoughCalc::TestHit(double x, double y,
419  double thmx, double rhomx,
420  double sigth, double sigr)
421  {
422  int itheta;
423  double theta;
424  double r;
425  for (itheta=0; itheta<5*gsNtheta; ++itheta) {
426  theta = M_PI*(double)itheta/(double)(5*gsNtheta);
427  if (fabs(theta-thmx)>sigth) continue;
428  r = cos(theta)*x + sin(theta)*y;
429  if (fabs(r-rhomx)<sigr) return true;
430  }
431  return false;
432  }
433 
434  //......................................................................
435 
436  bool HoughCalc::TestHit(double x, double y,
437  double thmx, double rhomx,
438  double sigr)
439  {
440  double r;
441  r = cos(thmx)*x + sin(thmx)*y;
442  if (fabs(r-rhomx)<sigr) return true;
443  return false;
444  }
445 
446  //......................................................................
447 
448  double HoughCalc::DistHit(double x, double y,
449  double thmx, double rhomx)
450  {
451  double r;
452  r = cos(thmx)*x + sin(thmx)*y;
453  return fabs(r-rhomx);
454  }
455 }//namespace
456 
457 
458 ////////////////////////////////////////////////////////////////////////
459 namespace util
460 {
462 } // namespace
static void MakeTree(double thresh)
Filter events based on their run/event numbers.
fvar< T > fabs(const fvar< T > &x)
Definition: fabs.hpp:15
Float_t y1[n_points_granero]
Definition: compare.C:5
#define DEFINE_ART_SERVICE(svc)
Definition: ServiceMacros.h:93
Float_t x1[n_points_granero]
Definition: compare.C:5
T sqrt(T number)
Definition: d0nt_math.hpp:156
void AddPoint(double x1, double sigx1, double x2, double sigx2, double y1, double sigy1, double y2, double sigy2, double w, int ih1)
static void RemovePoint(double theta, double rho, double ww, int ih1)
void FindLineErrs(double *etheta, double *erho)
#define M_PI
Definition: SbMath.h:34
Eigen::Matrix< T, Eigen::Dynamic, 1 > head(const Eigen::Matrix< T, Eigen::Dynamic, 1 > &v, size_t n)
Definition: head.hpp:24
static void InsertSort(node *head, node *item)
T atan(T number)
Definition: d0nt_math.hpp:66
static int UpdateTree()
double DistHit(double x, double y, double thmx, double rhomx)
double Gaus(TH1D *h, double &err, bool isTruth)
Definition: absCal.cxx:489
const double j
Definition: BetheBloch.cxx:29
bool TestHit(double x, double y, double thmx, double rhomx, double sigth, double sigr)
T sin(T number)
Definition: d0nt_math.hpp:132
Definition: event.h:1
void FindLines(double *theta, double *rho, double *score)
HoughCalc(const Parameters &params, art::ActivityRegistry &reg)
static void nodeUpdate(node *head, double ww, int ih1)
T cos(T number)
Definition: d0nt_math.hpp:78
TRandom3 r(0)
static void DeleteChain(node *head)
static node * newNode(int t, int r)
Float_t w
Definition: plot.C:20
static void SetThresh(double thresh)