EIC Software
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
VertexFitFunc.cpp
Go to the documentation of this file. Or view the newest version in sPHENIX GitHub for file VertexFitFunc.cpp
1 #include "VertexFitFunc.h"
2 
3 #include "NewtonMinimizerGradHessian.h"
4 #include "SimpleTrack3D.h"
5 
6 #include <cmath>
7 #include <memory>
8 
9 using namespace std;
10 using namespace FitNewton;
11 using namespace Eigen;
12 
13 
14 HelixDCAFunc::HelixDCAFunc() : FunctionGradHessian(1, 8)
15 {
16  tangent.assign(3,0.);
17  point.assign(3,0.);
18 }
19 
20 
21 HelixDCAFunc::~HelixDCAFunc()
22 {
23 
24 }
25 
26 
27 bool HelixDCAFunc::calcValGradHessian(const VectorXd& x, double& val, VectorXd& grad, MatrixXd& hessian)
28 {
29 // the 5 helix parameters are k,d,phi,z0,dzdl
30 // l will be used as the independent variable
31 // l=0 corresponds to (d*cos(phi), d*sin(phi), z0)
32 // fixedpars[5,6,7] correspond to vx,vy,vz of the point we are calculating the DCA to
33 //
34 // a change in l corresponds to what change in azimuth?
35 //
36 // l^2 = s^2 + z^2
37 //
38 // 1 = s^2/l^2 + dzdl^2
39 //
40 // s^2 = (1 - dzdl^2)*l^2
41 //
42 // change in s gives what change in azimuth?
43 //
44 // s = theta*r = theta/k
45 //
46 // theta = s*k
47 //
48 // s = 2.*asin(0.5*k*D)/k;
49 //
50 // s*k*0.5 = asin(0.5*k*D)
51 //
52 // sin(s*k/2) = k*D/2
53 //
54 // D = (2/k)*sin(s*k/2)
55 //
56 //
57 // in what direction do we move by D in the xy plane?
58 //
59 // theta/2 = asin(0.5*k*D)
60 //
61 // l == x
62 
63 
64  point_covariance = Matrix<float,3,3>::Zero(3,3);
65 
66 
67  float k = fixedpars[2];
68  float d = fixedpars[1];
69  float phi = fixedpars[0];
70  float z0 = fixedpars[3];
71  float cosphi = cos(phi);
72  float sinphi = sin(phi);
73  //determine starting point on helix xp,yp,zp
74  float xp = d*cosphi;
75  float yp = d*sinphi;
76  float zp = z0;
77 
78  // pc = (df/dx)*C*(df/dx)^T
79  Matrix<float,3,5> dpdx = Matrix<float,3,5>::Zero(3,5);
80  // dxp/dphi = -d*sinphi , dyp/dphi = d*cosphi
81  dpdx(0,0) = -d*sinphi;dpdx(1,0) = d*cosphi;
82  // dxp/dd = cosphi;dyp/dd = sinphi
83  dpdx(0,1) = cosphi;dpdx(1,1) = sinphi;
84  dpdx(2,3) = 1.;
85  point_covariance = dpdx * covariance * (dpdx.transpose());
86 
87 
88  //determine tangent direction ux,uy in xy plane
89  float ux = sinphi;
90  float uy = -cosphi;
91  float duxdphi = -cosphi;
92  float duydphi = sinphi;
93  if(d <= 0.)
94  {
95  ux=-ux;
96  uy=-uy;
97  duxdphi=-duxdphi;
98  duydphi=-duydphi;
99  }
100 
101  float dzdl = fixedpars[4];
102  //determine point on the helix with parameter l == x(0)
103  float dsdl = sqrt(1. - dzdl*dzdl);
104  float s = dsdl*x(0);
105  float dsddzdl = -x(0) * (1./sqrt(1. - dzdl*dzdl)) * dzdl;
106 
107  float psi = 0.5*s*k;
108  float D = 0.;
109  Matrix<float,1,5> dDdx = Matrix<float,1,5>::Zero(1,5);
110 
111  if(psi > 0.1)
112  {
113  float sinpsi = sin(psi);
114  float cospsi = cos(psi);
115  D = (2./k)*sinpsi;
116  // dsinpsi/dk = cos(psi)*0.5*s
117  dDdx(0,2) = -2.*sinpsi/(k*k) + (cospsi/k)*s;
118  // dsinpsi/ddzdl = cospsi * 0.5 * k * ds/ddzdl
119  dDdx(0,4) = (2./k) * ( cospsi * 0.5 * k * dsddzdl );
120  }
121  else
122  {
123  //sin(del) = del - del^3/6 + del^5/120 - del^7/5040. + ...
124  //(2/k)*sin(s*k/2) = s - s^3*(k/2)^2/6 + s^5*(k/2)^4/120 - s^7*(k/2)^6/5040
125 
126  float srun=s;
127  float s2 = s*s;
128  float khalf2 = 0.5*k;khalf2*=khalf2;
129  float krun = khalf2;
130 
131  D = s;
132  dDdx(0,4) = dsddzdl;
133  srun *= s2;
134  D -= srun*krun/6.;
135  dDdx(0,4) -= ( (krun/6.) * 3.*s*s*dsddzdl );
136  // krun = (1/4) * k^2
137  dDdx(0,2) -= ( (srun/6.) * (2./4.) * k );
138  srun *= s2;
139  krun *= khalf2;
140  D += srun*krun/120.;
141  dDdx(0,4) += ( (krun/120.) * 5.*s2*s2*dsddzdl );
142  dDdx(0,2) -= ( (srun/120.) * (4./16.)*k*k*k );
143  srun *= s2;
144  krun *= khalf2;
145  D -= srun*krun/5040.;
146  dDdx(0,4) -= ( (krun/5040.) * 7.*s2*s2*s2*dsddzdl );
147  dDdx(0,2) -= ( (srun/5040.) * (6./64.)*k*k*k*k*k );
148  }
149  float dpsidk = 0.5*s;float dpsiddzdl = 0.5*k*dsddzdl;
150  if( ((int)((floor(fabs(s*k/M_PI)))))%2 == 1 )
151  {
152  psi = M_PI - psi;
153  dpsidk = -dpsidk;
154  dpsiddzdl = -dpsiddzdl;
155  }
156  float cospsi = cos(psi);
157  float sinpsi = sin(psi);
158  Matrix<float,1,5> dux2dx = Matrix<float,1,5>::Zero(1,5);
159  Matrix<float,1,5> duy2dx = Matrix<float,1,5>::Zero(1,5);
160  float ux2 = cospsi*ux - sinpsi*uy;
161  dux2dx(0,0) = cospsi*duxdphi - sinpsi*duydphi;
162  dux2dx(0,2) = ux*-sinpsi*dpsidk - uy*cospsi*dpsidk;
163  dux2dx(0,4) = ux*-sinpsi*dpsiddzdl - uy*cospsi*dpsiddzdl;
164  float uy2 = sinpsi*ux + cospsi*uy;
165  duy2dx(0,0) = sinpsi*duxdphi + cospsi*duydphi;
166  duy2dx(0,2) = ux*cospsi*dpsidk - uy*sinpsi*dpsidk;
167  duy2dx(0,4) = ux*cospsi*dpsiddzdl - uy*sinpsi*dpsiddzdl;
168 
169  xp += D*ux2;
170  yp += D*uy2;
171  zp += dzdl*x(0);
172 
173  Matrix<float,1,5> temp_1_5 = Matrix<float,1,5>::Zero(1,5);
174  temp_1_5 = D*dux2dx + ux2*dDdx;
175  dpdx = Matrix<float,3,5>::Zero(3,5);
176  for(int i=0;i<5;++i){dpdx(0,i) = temp_1_5(0,i);}
177  temp_1_5 = D*duy2dx + uy2*dDdx;
178  for(int i=0;i<5;++i){dpdx(1,i) = temp_1_5(0,i);}
179  dpdx(2,4) = x(0);
180 
181  point_covariance += dpdx * covariance * (dpdx.transpose());
182 
183  point[0] = xp;
184  point[1] = yp;
185  point[2] = zp;
186 
187 
188  //calculate the new tangent angle
189  if(psi>0. || psi < M_PI){psi = 2.*psi;}
190  else if(psi < 0.)
191  {
192  psi = -psi;
193  psi = M_PI - psi;
194  psi = 2.*psi;
195  }
196  else
197  {
198  psi -= M_PI;
199  psi = 2.*psi;
200  }
201  cospsi = cos(psi);
202  sinpsi = sin(psi);
203  ux2 = cospsi*ux - sinpsi*uy;
204  uy2 = sinpsi*ux + cospsi*uy;
205 
206 
207 
208 
209 // f = (vx - xp)^2 + (vy - yp)^2 + (vz - zp)^2
210 // df/dl = 2*(vx - xp)*dxp/dl + 2*(vy - yp)*dyp/dl + 2*(vz - zp)*dzp/dl
211 //
212 //
213 // dx/dt = r*ux2 = ux2/k
214 // dy/dt = r*uy2 = uy2/k
215 //
216 // dx/ds = ux2
217 // dy/ds = uy2
218 //
219 // dx/dl = (dx/ds)*(ds/dl)
220 //
221 // s^2 = (1 - dzdl^2)*l^2
222 //
223 // s = sqrt(1 - dzdl^2)*l
224 //
225 // ds/dl = sqrt(1 - dzdl^2)
226 //
227 //
228 // x = cx + r*cos(t)
229 // y = cy + r*sin(t)
230 //
231 // ux2 = -sin(t)
232 // uy2 = cos(t)
233 //
234 // dx/dl = sqrt(1 - dzdl^2)*ux2
235 // d^2x/dl^2 = sqrt(1 - dzdl^2)*d(ux2)/dl
236 //
237 // dux2/dt = -cos(t) = -uy2
238 // dt/ds = k
239 // ds/dl = sqrt(1 - dzdl^2)
240 //
241 // d^2x/dl^2 = -(1 - dzdl^2)*uy2*k
242 //
243 // d^2y/dl^2 = sqrt(1 - dzdl^2)*d(uy2)/dl
244 // d^2y/dl^2 = (1 - dzdl^2)*ux2*k
245 //
246 // f = (vx - xp)^2 + (vy - yp)^2 + (vz - zp)^2
247 // df/dl = -2*(vx - xp)*dxp/dl + -2*(vy - yp)*dyp/dl + -2*(vz - zp)*dzp/dl
248 // d^2f/dl^2 = 2*(dxp/dl)^2 + -2*(vx-xp)*d^2xp/dl^2 + ditto for y and z
249 //
250 // dzp/dl = dzdl
251 
252  float dx = xp - fixedpars[5];
253  float dy = yp - fixedpars[6];
254  float dz = zp - fixedpars[7];
255 
256  val = dx*dx + dy*dy + dz*dz;
257 
258  grad(0) = 2.*dx*dsdl*ux2 + 2.*dy*dsdl*uy2 + 2.*dz*dzdl;
259 
260  hessian(0,0) = 2.*dsdl*ux2*dsdl*ux2;
261  hessian(0,0) += 2.*dsdl*uy2*dsdl*uy2;
262  hessian(0,0) += 2.*dzdl*dzdl;
263  hessian(0,0) += -2.*dx*dsdl*dsdl*uy2*k;
264  hessian(0,0) += 2.*dy*dsdl*dsdl*ux2*k;
265 
266  float xylen = sqrt(1-dzdl*dzdl);
267  tangent[0] = ux2*xylen;
268  tangent[1] = uy2*xylen;
269  tangent[2] = dzdl;
270 
271  return true;
272 }
273 
274 
275 
276 
277 
278 //fixedpars[0] is a gaussian regulator sigma
279 VertexFitFunc::VertexFitFunc() : FunctionGradHessian(3,1)
280 {
281 
282 }
283 
284 
285 VertexFitFunc::~VertexFitFunc()
286 {
287 
288 }
289 
290 
291 bool VertexFitFunc::calcValGradHessian(const VectorXd& x, double& val, VectorXd& grad, MatrixXd& hessian)
292 {
293  NewtonMinimizerGradHessian minimizer;
294  HelixDCAFunc helixfunc;
295  minimizer.setFunction(&helixfunc);
296 
297  val = 0;
298  for(int i=0;i<3;i++){grad(i)=0.;}
299  for(int i=0;i<3;i++)
300  {
301  for(int j=0;j<3;j++)
302  {
303  hessian(j,i)=0.;
304  }
305  }
306 
307  for(unsigned int i=0;i<tracks->size();i++)
308  {
309  if(covariances->size() > 0){helixfunc.setCovariance( covariances->at(i) );}
310 
311  helixfunc.setFixedPar(0, tracks->at(i).phi);
312  helixfunc.setFixedPar(1, tracks->at(i).d);
313  helixfunc.setFixedPar(2, tracks->at(i).kappa);
314  helixfunc.setFixedPar(3, tracks->at(i).z0);
315  helixfunc.setFixedPar(4, tracks->at(i).dzdl);
316  helixfunc.setFixedPar(5, x(0));
317  helixfunc.setFixedPar(6, x(1));
318  helixfunc.setFixedPar(7, x(2));
319  VectorXd start_point = VectorXd::Zero(1);
320  VectorXd min_point = VectorXd::Zero(1);
321  //find the point on the helix closest to the point x
322  minimizer.minimize(start_point, min_point, 0x1.0p-30, 16, 0x1.0p-40);
323 
324  //now calculate the chi-square contribution from this track
325  double tval=0.;
326  VectorXd tgrad = VectorXd::Zero(1);
327  MatrixXd thessian = MatrixXd::Zero(1,1);
328 
329  helixfunc.calcValGradHessian(min_point, tval, tgrad, thessian);
330 
331  float point[3];float tangent[3];
332  point[0] = helixfunc.getPoint(0);
333  point[1] = helixfunc.getPoint(1);
334  point[2] = helixfunc.getPoint(2);
335  tangent[0] = helixfunc.getTangent(0);
336  tangent[1] = helixfunc.getTangent(1);
337  tangent[2] = helixfunc.getTangent(2);
338  Eigen::Matrix<float,3,3> point_covariance = helixfunc.getPointCovariance();
339 
340 // cout<<"point_covariance = "<<endl<<point_covariance<<endl<<endl;
341 
342 
343  float d[3];
344  d[0] = x(0) - point[0];
345  d[1] = x(1) - point[1];
346  d[2] = x(2) - point[2];
347 
348  float F = d[0]*d[0] + d[1]*d[1] + d[2]*d[2];
349 
350 // Eigen::Matrix<float,1,3> dFdp = Eigen::Matrix<float,1,3>::Zero(1,3);
351 // dFdp(0,0) = -2.*d[0];
352 // dFdp(0,1) = -2.*d[1];
353 // dFdp(0,2) = -2.*d[2];
354 
355 // float F_cov = dFdp * point_covariance * (dFdp.transpose());
356  float F_cov = point_covariance(0,0) + point_covariance(1,1) + point_covariance(2,2);
357  float F_err_inv = 1./(F_cov);
358  if(covariances->size() == 0){F_err_inv = 1.;}
359 
360  float f = F;
361 
362  float g0 = -2.*tangent[0]*( d[0]*tangent[0] + d[1]*tangent[1] + d[2]*tangent[2] ) + 2*d[0];
363  float g1 = -2.*tangent[1]*( d[0]*tangent[0] + d[1]*tangent[1] + d[2]*tangent[2] ) + 2*d[1];
364  float g2 = -2.*tangent[2]*( d[0]*tangent[0] + d[1]*tangent[1] + d[2]*tangent[2] ) + 2*d[2];
365  float h0 = 2.*(1. - tangent[0]*tangent[0]);
366  float h1 = 2.*(1. - tangent[1]*tangent[1]);
367  float h2 = 2.*(1. - tangent[2]*tangent[2]);
368 
369  float invs2 = 1./(2.*fixedpars[0]*fixedpars[0]);
370 
371  float g = -exp(-f*invs2);
372 
373  if( !(g == g) ){continue;}
374 
375  val += g*F_err_inv;
376 
377  float grd0 = -invs2*g*g0*F_err_inv;
378  float grd1 = -invs2*g*g1*F_err_inv;
379  float grd2 = -invs2*g*g2*F_err_inv;
380 
381  grad(0) += grd0;
382  grad(1) += grd1;
383  grad(2) += grd2;
384 
385  hessian(0,0) += -invs2*F_err_inv*( grd0*g0 + g*h0 );
386  hessian(1,1) += -invs2*F_err_inv*( grd1*g1 + g*h1 );
387  hessian(2,2) += -invs2*F_err_inv*( grd2*g2 + g*h2 );
388  float htemp = -invs2*F_err_inv*( grd1*g0);
389  hessian(0,1) += htemp;
390  hessian(1,0) += htemp;
391  htemp = -invs2*F_err_inv*( grd2*g0 );
392  hessian(0,2) += htemp;
393  hessian(2,0) += htemp;
394  htemp = -invs2*F_err_inv*( grd2*g1 );
395  hessian(1,2) += htemp;
396  hessian(2,1) += htemp;
397  }
398 
399  return true;
400 }
401 
402