MarlinTrk  02.08
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
HelixFit.cc
Go to the documentation of this file.
1 //
2 // HelixFit.cc
3 // MarlinTrk
4 //
5 // Created by Steve Aplin on 9/16/11.
6 //
7 
8 #include "MarlinTrk/IMarlinTrack.h"
9 
10 #include "MarlinTrk/HelixFit.h"
11 #include "MarlinTrk/HelixTrack.h"
12 #include "streamlog/streamlog.h"
13 
14 #include "CLHEP/Matrix/SymMatrix.h"
15 
16 namespace MarlinTrk{
17 
18 
19  int HelixFit::fastHelixFit(int npt, double* xf, double* yf, float* rf, float* pf, double* wf, float* zf , float* wzf,int iopt,
20  float* vv0, float* ee0, float& ch2ph, float& ch2z){
21 
22 
23 
24  if (npt < 3) {
25  streamlog_out(ERROR) << "Cannot fit less than 3 points return 1" << std::endl;
26  ch2ph = 1.0e30;
27  ch2z = 1.0e30;
28  return 1;
29  }
30 
31  double eps = 1.0e-16;
32 #define ITMAX 15
33 #define MPT 600
34 #define MAX_CHI2 5000.0
35 
36 
37  float sp2[MPT],
38  del[MPT],deln[MPT],delzn[MPT],sxy[MPT],ss0[MPT],eee[MPT],
39  delz[MPT],grad[5],cov[15],vv1[5],dv[5];
40 
41  // double xf[MPT],yf[MPT],wf[MPT],zf[MPT],wzf[MPT];
42 
43  double alf,a0,a1,a2,a22,bet,cur,
44  dd,den,det,dy,d2,f,fact,fg,f1,g,gam,gam0,g1,
45  h,h2,p2,q2,rm,rn,
46  xa,xb,xd,xi,xm,xx,xy,x1,x2,den2,
47  ya,yb,yd,yi,ym,yy,y1,y2,wn,sa2b2,dd0,phic,aaa;
48 
49 
50  xm = 0.;
51  ym = 0.;
52 
53  for(int i=1; i < 15; ++i){
54  ee0[i]=0.0;
55  }
56 
57  for( int i=1; i < 5; ++i){
58  grad[i]= 0.0;
59  vv0[i] = 0.0;
60  }
61 
62  float chi2=0.0;
63  ch2ph = 0.0;
64  ch2z = 0.0;
65 
66  for (int i = 0; i<npt; ++i) {
67  sp2[i] = wf[i]*(rf[i]*rf[i]);
68  }
69 
70 
71  //std::cout << "npt = " << npt << std::endl;
72 
73  // for(int i = 0; i<n; ++i){
74  //std::cout << "xf = " << xf[i] << " " << i << std::endl;
75  //std::cout << "yf = " << yf[i] << " " << i << std::endl;
76  //std::cout << "zf = " << zf[i] << " " << i << std::endl;
77  //std::cout << "rf = " << rf[i] << " " << i << std::endl;
78  //std::cout << "pf = " << pf[i] << " " << i << std::endl;
79  //std::cout << "wf = " << wf[i] << " " << i << std::endl;
80  //std::cout << "wzf = " << wzf[i] << " " << i << std::endl;
81  //}
82 
83 
84  wn=0.0;
85 
86  for (int i =0; i<npt; ++i) {
87  xm = xm + xf[i]*wf[i];
88  ym = ym + yf[i]*wf[i];
89  wn = wn + wf[i];
90  }
91 
92  rn = 1.0/wn;
93 
94  xm = xm * rn;
95  ym = ym * rn;
96  x2 = 0.;
97  y2 = 0.;
98  xy = 0.;
99  xd = 0.;
100  yd = 0.;
101  d2 = 0.;
102 
103  //std::cout << "xm = " << xm << std::endl;
104  //std::cout << "ym = " << ym << std::endl;
105  //std::cout << "rn = " << rn << std::endl;
106 
107  for (int i =0; i<npt; ++i) {
108  xi = xf[i] - xm;
109  yi = yf[i] - ym;
110  xx = xi*xi;
111  yy = yi*yi;
112  x2 = x2 + xx*wf[i];
113  y2 = y2 + yy*wf[i];
114  xy = xy + xi*yi*wf[i];
115  dd = xx + yy;
116  xd = xd + xi*dd*wf[i];
117  yd = yd + yi*dd*wf[i];
118  d2 = d2 + dd*dd*wf[i];
119  }
120 
121 
122  //std::cout << "xd = " << xd << std::endl;
123  //std::cout << "yd = " << yd << std::endl;
124  //std::cout << "d2 = " << d2 << std::endl;
125 
126 
127  x2 = x2*rn;
128  y2 = y2*rn;
129  xy = xy*rn;
130  d2 = d2*rn;
131  xd = xd*rn;
132  yd = yd*rn;
133  f = 3.0* x2 + y2;
134  g = 3.0* y2 + x2;
135  fg = f*g;
136  h = xy + xy;
137  h2 = h*h;
138  p2 = xd*xd;
139  q2 = yd*yd;
140  gam0 = x2 + y2;
141  fact = gam0*gam0;
142  a2 = (fg-h2-d2)/fact;
143  fact = fact*gam0;
144  a1 = (d2*(f+g) - 2.0*(p2+q2))/fact;
145  fact = fact*gam0;
146  a0 = (d2*(h2-fg) + 2.0*(p2*g + q2*f) - 4.0*xd*yd*h)/fact;
147  a22 = a2 + a2;
148  yb = 1.0e30;
149  xa = 1.0;
150 
151  //std::cout << "a0 = " << a0 << std::endl;
152  //std::cout << "a22 = " << a22 << std::endl;
153 
154 
155  // ** main iteration
156 
157  for (int i = 0 ; i < ITMAX; ++i) {
158 
159  ya = a0 + xa*(a1 + xa*(a2 + xa*(xa-4.0)));
160  dy = a1 + xa*(a22 + xa*(4.0*xa - 12.0));
161  xb = xa - ya/dy;
162 
163  if (fabs(ya) > fabs(yb)) {
164  xb = 0.5 * (xb+xa) ;
165  }
166 
167  if (fabs(xa-xb) < eps) break ;
168  xa = xb;
169  yb = ya;
170 
171 
172  }
173 
174  // **
175 
176  gam = gam0*xb;
177  f1 = f - gam;
178  g1 = g - gam;
179  x1 = xd*g1 - yd*h;
180  y1 = yd*f1 - xd*h;
181  det = f1*g1 - h2;
182  den2= 1.0/(x1*x1 + y1*y1 + gam*det*det);
183 
184  if(den2 <= 0.0) {
185  // streamlog_out(ERROR) << "den2 less than or equal to zero"
186  // << " x1 = " << x1
187  // << " y1 = " << y1
188  // << " gam = " << gam
189  // << " det = " << det
190  // << std::endl;
191  ch2ph = 1.0e30;
192  ch2z = 1.0e30;
193  return 1;
194  }
195 
196  den = sqrt(den2);
197  cur = det*den + 0.0000000001 ;
198  alf = -(xm*det + x1)*den ;
199  bet = -(ym*det + y1)*den ;
200  rm = xm*xm + ym*ym ;
201  gam = ((rm-gam)*det + 2.0*(xm*x1 + ym*y1))*den*0.5;
202 
203 
204  //
205  // --------> calculation of standard circle parameters
206  // nb: cur is always positive
207 
208  double rr0=cur;
209  double asym = bet*xm-alf*ym;
210  double sst = 1.0;
211 
212  if(asym < 0.0) {
213  sst = -1.0 ;
214  }
215 
216  rr0 = sst*cur;
217 
218  if( (alf*alf+bet*bet) <= 0.0 ){
219  // streamlog_out(ERROR) << "(alf*alf+bet*bet) less than or equal to zero" << std::endl;
220  ch2ph = 1.0e30;
221  ch2z = 1.0e30;
222  return 1;
223  }
224 
225  sa2b2 = 1.0/sqrt(alf*alf+bet*bet);
226  dd0 = (1.0-1.0/sa2b2)/cur;
227  aaa = alf*sa2b2;
228 
229  if( aaa > 1.0 ) aaa = 1.0;
230  if( aaa < -1.0 ) aaa =-1.0;
231 
232  // std::cout << std::setprecision(10) << "aaa = " << aaa << std::endl;
233 
234  phic = asin(aaa)+ M_PI_2;
235 
236  if( bet > 0 ) phic = 2*M_PI - phic;
237 
238  double ph0 = phic + M_PI_2;
239 
240  if(rr0 <= 0.0) ph0=ph0-M_PI;
241  if(ph0 > 2*M_PI) ph0=ph0-2*M_PI;
242  if(ph0 < 0.0) ph0=ph0+2*M_PI;
243 
244  vv0[0] = rr0;
245  vv0[2] = ph0;
246  vv0[3] = dd0;
247 
248  // std::cout << std::setprecision(10) << "rr0 = " << rr0 << std::endl;
249  // std::cout << "ph0 = " << ph0 << std::endl;
250  // std::cout << "dd0 = " << dd0 << std::endl;
251 
252  double check=sst*rr0*dd0;
253  // std::cout << "check = " << check << std::endl;
254 
255  if(check > 1.0-eps && check < 1.0+eps) {
256  dd0 = dd0 - 0.007;
257  vv0[3]=dd0;
258  }
259 
260  //
261  // -----> calculate phi distances to measured points
262  //
263 
264  double aa0 = sst;
265  double ome = rr0;
266  double gg0 = ome*dd0-aa0;
267 
268  double hh0 = 1.0/gg0;
269 
270  // std::cout << "dd0 = " << dd0 << std::endl;
271  // std::cout << "ome = " << ome << std::endl;
272  // std::cout << "aa0 = " << aa0 << std::endl;
273  // std::cout << "hh0 = " << hh0 << std::endl;
274  // std::cout << "gg0 = " << gg0 << std::endl;
275 
276  for (int i = 0 ; i < npt ; ++i) {
277 
278  asym = bet*xf[i]-alf*yf[i] ;
279  ss0[i] = 1.0 ;
280 
281  if(asym < 0.0) ss0[i] = -1.0 ;
282 
283  double ff0 = ome*(rf[i]*rf[i]-dd0*dd0)/(2.0*rf[i]*gg0) + dd0/rf[i];
284 
285  if(ff0 < -1.0) ff0 = -1.0;
286  if(ff0 > 1.0) ff0 = 1.0;
287 
288  del[i]= ph0 + (ss0[i]-aa0)* M_PI_2 + ss0[i]*asin(ff0) - pf[i];
289 
290  // std::cout << "asin(ff0) = " << asin(ff0) << " i = " << i << std::endl;
291  // std::cout << "aa0 = " << aa0 << " i = " << i << std::endl;
292  // std::cout << "M_PI_2 = " << M_PI_2 << " i = " << i << std::endl;
293  // std::cout << "pf[i] = " << pf[i] << " i = " << i << std::endl;
294  // std::cout << "ff0 = " << ff0 << " i = " << i << std::endl;
295  // std::cout << "ss0[i] = " << ss0[i] << " i = " << i << std::endl;
296  // std::cout << "ph0 + (ss0[i]-aa0)* M_PI_2 = " << ph0 + (ss0[i]-aa0)* M_PI_2 << " i = " << i << std::endl;
297  // std::cout << "ss0[i]*asin(ff0) = " << ss0[i]*asin(ff0) << " i = " << i << std::endl;
298  // std::cout << "ph0 + (ss0[i]-aa0)* M_PI_2 + ss0[i]*asin(ff0) = " << ph0 + (ss0[i]-aa0)* M_PI_2 + ss0[i]*asin(ff0) << std::endl;
299  // std::cout << "del[i] = " << del[i] << " i = " << i << std::endl;
300 
301  if(del[i] > M_PI) del[i] = del[i] - 2*M_PI;
302  if(del[i] < -M_PI) del[i] = del[i] + 2*M_PI;
303 
304 
305 
306  }
307 
308 
309 
310  //
311  // -----> fit straight line in s-z
312  //
313 
314 
315  for (int i = 0 ; i < npt ; ++i) {
316 
317  eee[i] = 0.5*vv0[0] * sqrt( fabs( (rf[i] * rf[i]-vv0[3]*vv0[3]) / (1.0-aa0*vv0[0]*vv0[3]) ) );
318 
319  if(eee[i] > 0.99990) eee[i]= 0.99990;
320  if(eee[i] < -0.99990) eee[i]= -0.99990;
321 
322  sxy[i]=2.0*asin(eee[i])/ome;
323 
324  }
325 
326 
327  double sums = 0.0;
328  double sumss = 0.0;
329  double sumz = 0.0;
330  double sumzz = 0.0;
331  double sumsz = 0.0;
332  double sumw = 0.0;
333 
334  for (int i = 0; i<npt; ++i) {
335  sumw = sumw + wzf[i];
336  sums = sums + sxy[i] * wzf[i];
337  sumss = sumss + sxy[i]*sxy[i] * wzf[i];
338  sumz = sumz + zf[i] * wzf[i];
339  sumzz = sumzz + zf[i]*zf[i] * wzf[i];
340  sumsz = sumsz + zf[i]*sxy[i] * wzf[i];
341  }
342 
343  double denom = sumw*sumss - sums*sums;
344 
345  if (fabs(denom) < eps){
346  // streamlog_out(ERROR) << "fabs(denom) less than or equal to zero" << std::endl;
347  ch2ph = 1.0e30;
348  ch2z = 1.0e30;
349  return 1;
350  }
351 
352  double dzds = (sumw*sumsz-sums*sumz) /denom;
353  double zz0 = (sumss*sumz-sums*sumsz)/denom;
354 
355  vv0[1]= dzds;
356  vv0[4]= zz0;
357 
358  //
359  // -----> calculation chi**2
360  //
361  for (int i = 0 ; i<npt; ++i) {
362 
363  delz[i]= zz0+dzds*sxy[i]-zf[i];
364  ch2ph = ch2ph + sp2[i]*del[i]*del[i];
365  ch2z = ch2z + wzf[i]*delz[i]*delz[i];
366  chi2 = ch2ph + ch2z;
367 
368  }
369 
370  if(chi2 > MAX_CHI2) {
371  // streamlog_out(ERROR) << "Chi2 greater than " << MAX_CHI2 << "return 1 " << std::endl;
372  ch2ph = 1.0e30;
373  ch2z = 1.0e30;
374  return 1;
375  }
376 
377  for (int i = 0 ; i<npt; ++i) {
378 
379  double ff0 = ome*(rf[i]*rf[i]-dd0*dd0)/(2.0*rf[i]*gg0) + dd0/rf[i];
380 
381  if (ff0 > 0.99990) ff0 = 0.99990;
382 
383  if (ff0 < -0.99990) ff0 = -0.99990;
384 
385  double eta = ss0[i]/sqrt(fabs((1.0+ff0)*(1.0-ff0)));
386  double dfd = (1.0+hh0*hh0*(1.0-ome*ome*rf[i]*rf[i]))/(2.0*rf[i]);
387  double dfo = -aa0*(rf[i]*rf[i]-dd0*dd0)*hh0*hh0/(2.0*rf[i]);
388  double dpd = eta*dfd;
389  double dpo = eta*dfo;
390 
391  // -----> derivatives of z component
392  double ggg = eee[i]/sqrt(fabs( (1.0+eee[i])*(1.0-eee[i])));
393  double dza = sxy[i];
394  check = rf[i]*rf[i]-vv0[3]*vv0[3];
395 
396  if(fabs(check) > 1.0-eps) check=2.*0.007;
397 
398  double dzd = 2.0*( vv0[1]/vv0[0] ) * fabs( ggg ) * ( 0.5*aa0*vv0[0]/( 1.0-aa0*vv0[3]*vv0[0] )-vv0[3]/check );
399 
400  double dzo = -vv0[1]*sxy[i]/vv0[0] + vv0[1] * ggg/( vv0[0]*vv0[0]) * ( 2.0+ aa0*vv0[0]*vv0[3]/(1.0-aa0*vv0[0]*vv0[3]) );
401 
402  // -----> error martix
403 
404  ee0[0] = ee0[0] + sp2[i]* dpo*dpo + wzf[i] * dzo*dzo;
405  ee0[1] = ee0[1] + wzf[i] * dza*dzo;
406  ee0[2] = ee0[2] + wzf[i] * dza*dza;
407  ee0[3] = ee0[3] + sp2[i]* dpo;
408  ee0[4] = ee0[4];
409  ee0[5] = ee0[5] + sp2[i];
410  ee0[6] = ee0[6] + sp2[i]* dpo*dpd + wzf[i] * dzo*dzd;
411  ee0[7] = ee0[7] + wzf[i] * dza*dzd;
412  ee0[8] = ee0[8] + sp2[i]* dpd;
413  ee0[9] = ee0[9] + sp2[i]* dpd*dpd + wzf[i] * dzd*dzd;
414  ee0[10]= ee0[10] + wzf[i] * dzo;
415  ee0[11]= ee0[11] + wzf[i] * dza;
416  ee0[12]= ee0[12];
417  ee0[13]= ee0[13] + wzf[i] * dzd;
418  ee0[14]= ee0[14] + wzf[i];
419 
420  // -----> gradient vector
421  grad[0]=grad[0] - del[i] *sp2[i]*dpo - delz[i]*wzf[i]*dzo;
422  grad[1]=grad[1] - delz[i]*wzf[i]*dza;
423  grad[2]=grad[2] - del[i] *sp2[i];
424  grad[3]=grad[3] - del[i] *sp2[i]*dpd - delz[i]*wzf[i]*dzd;
425  grad[4]=grad[4] - delz[i]*wzf[i];
426 
427 
428  }
429 
430  if (iopt < 3) {
431 
432  streamlog_out(DEBUG1) << "HelixFit: " <<
433  " d0 = " << vv0[3] <<
434  " phi0 = " << vv0[2] <<
435  " omega = " << vv0[0] <<
436  " z0 = " << vv0[4] <<
437  " tanL = " << vv0[1] <<
438  std::endl;
439 
440  return 0 ;
441  }
442 
443  // ------> NEWTONS NEXT GUESS
444 
445  for (int i =0; i<15; ++i) {
446  cov[i]=ee0[i];
447  }
448 
449  // Convert Covariance Matrix
450  CLHEP::HepSymMatrix cov0(5) ;
451 
452  int icov = 0 ;
453 
454  for(int irow=0; irow<5; ++irow ){
455  for(int jcol=0; jcol<irow+1; ++jcol){
456  // std::cout << "row = " << irow << " col = " << jcol << std::endl ;
457  // std::cout << "cov["<< icov << "] = " << _cov[icov] << std::endl ;
458  cov0[irow][jcol] = ee0[icov] ;
459  ++icov ;
460  }
461  }
462 
463  int error = 0 ;
464  cov0.invert(error);
465 
466  if( error != 0 ) {
467  streamlog_out(ERROR) << "CLHEP Matrix inversion failed" << "return 1 " << std::endl;
468  ch2ph = 1.0e30;
469  ch2z = 1.0e30;
470  return 1;
471  }
472 
473  icov = 0 ;
474 
475  for(int irow=0; irow<5; ++irow ){
476  for(int jcol=0; jcol<irow+1; ++jcol){
477  // std::cout << "row = " << irow << " col = " << jcol << std::endl ;
478  // std::cout << "cov["<< icov << "] = " << _cov[icov] << std::endl ;
479  cov[icov] = cov0[irow][jcol];
480  ++icov ;
481  }
482  }
483 
484  // here we need to invert the matrix cov[15]
485 
486 
487 
488  for (int i = 0; i<5; ++i) {
489  dv[i] = 0.0 ;
490  for (int j = 0; j<5; ++j) {
491  int index = 0;
492  if ( i >= j ) {
493  index = (i*i-i)/2+j ;
494  }
495  else{
496  index = (j*j-j)/2+i;
497  }
498  dv[i] = dv[i] + cov[index] * grad[j] ;
499  }
500  }
501 
502 
503  for (int i = 0; i<5; ++i) {
504  vv1[i] = vv0[i]+dv[i];
505  }
506 
507  gg0 = vv1[0]*vv1[3]-aa0;
508 
509  for (int i=0; i<npt; ++i) {
510  double ff0 = vv1[0]*(rf[i]*rf[i]-vv1[3]*vv1[3]) / (2.0*rf[i]*gg0) + vv1[3]/rf[i];
511 
512  if (ff0 > 1) ff0 = 1.0;
513  if (ff0 < -1) ff0 = -1.0;
514 
515  deln[i] = vv1[2] + (ss0[i]-aa0)*M_PI_2+ss0[i]*asin(ff0)-pf[i];
516 
517  if(deln[i] > M_PI) deln[i] = deln[i] - 2*M_PI;
518  if(deln[i] < -M_PI) deln[i] = deln[i] + 2*M_PI;
519 
520  eee[i] = 0.5*vv1[0]*sqrt( fabs( (rf[i]*rf[i]-vv1[3]*vv1[3]) / (1.0-aa0*vv1[0]*vv1[3]) ) );
521 
522  if(eee[i] > 0.99990) eee[i]= 0.99990;
523  if(eee[i] < -0.99990) eee[i]= -0.99990;
524 
525  sxy[i] = 2.0*asin(eee[i])/vv1[0];
526  delzn[i]= vv1[4]+vv1[1]*sxy[i]-zf[i];
527 
528  }
529 
530  double chi1 = 0.0;
531  ch2ph = 0.0;
532  ch2z = 0.0;
533 
534  for (int i =0; i<5; ++i) {
535  chi1 = chi1 + sp2[i]*deln[i]*deln[i] + wzf[i]*delzn[i]*delzn[i];
536  ch2ph = ch2ph + sp2[i]*deln[i]*deln[i];
537  ch2z = ch2z + wzf[i]*delzn[i]*delzn[i];
538  }
539 
540  if (chi1<chi2) {
541  for (int i =0; i<5; ++i) {
542  vv0[i] = vv1[i];
543  }
544  chi2 = chi1;
545  }
546 
547  streamlog_out(DEBUG1) << "HelixFit: " <<
548  " d0 = " << vv0[3] <<
549  " phi0 = " << vv0[2] <<
550  " omega = " << vv0[0] <<
551  " z0 = " << vv0[4] <<
552  " tanL = " << vv0[1] <<
553  std::endl;
554 
555 
556  return 0;
557 
558  }
559 
560 
561 }
562 
563 
564 
565 
566 
567 
568 
569 
570 
571 
572 
573 
574 
575 
576 
577 
578 
MAX_CHI2
#define MAX_CHI2
M_PI
#define M_PI
std::endl
T endl(T...args)
MPT
#define MPT
MarlinTrk::HelixFit::fastHelixFit
int fastHelixFit(int npt, double *xf, double *yf, float *rf, float *pf, double *wf, float *zf, float *wzf, int iopt, float *vv0, float *ee0, float &ch2ph, float &ch2z)
Definition: HelixFit.cc:19
ITMAX
#define ITMAX
det
tuple det
Generated on Sun Sep 26 2021 03:45:50 for MarlinTrk by   doxygen 1.8.5