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
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
64  }
65
66
67  //......................................................................
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  }
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
97  {
100  }
102
104
105  for(int i=0;i<gsNtheta;i++){
110  {
114  }
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
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
141  {
144  }
145  else
146  {
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
164
167  }
168
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
239
240  //use code below to see which hits made up this bin
241  return;
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;
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;
267  return temp;
268  }
269
270  //......................................................................
271
273  {
279  }
280
281  //......................................................................
282
284  {
290  }
291
292  }
293
294  //......................................................................
295
297  {
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
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  {
350
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
357
358
360  *(maxtheta-mintheta)+mintheta;
362  *(maxrho-minrho)+minrho;
364
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
377
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
384
385  double sigr=0.;
386  double sigt=0.;
387  double totw=0.;
388
391  // std::cout<<"thetamx "<<thetamx<<" rho "<<rmx<<std::endl;
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)
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)