10 using namespace FitNewton;
11 using namespace Eigen;
71 float cosphi = cos(phi);
72 float sinphi = sin(phi);
79 Matrix<float,3,5> dpdx = Matrix<float,3,5>::Zero(3,5);
81 dpdx(0,0) = -d*sinphi;dpdx(1,0) = d*cosphi;
83 dpdx(0,1) = cosphi;dpdx(1,1) = sinphi;
91 float duxdphi = -cosphi;
92 float duydphi = sinphi;
103 float dsdl = sqrt(1. - dzdl*dzdl);
105 float dsddzdl = -
x(0) * (1./sqrt(1. - dzdl*dzdl)) * dzdl;
109 Matrix<float,1,5> dDdx = Matrix<float,1,5>::Zero(1,5);
113 float sinpsi = sin(psi);
114 float cospsi = cos(psi);
117 dDdx(0,2) = -2.*sinpsi/(k*
k) + (cospsi/k)*
s;
119 dDdx(0,4) = (2./
k) * ( cospsi * 0.5 * k * dsddzdl );
128 float khalf2 = 0.5*
k;khalf2*=khalf2;
135 dDdx(0,4) -= ( (krun/6.) * 3.*s*s*dsddzdl );
137 dDdx(0,2) -= ( (srun/6.) * (2./4.) *
k );
141 dDdx(0,4) += ( (krun/120.) * 5.*s2*s2*dsddzdl );
142 dDdx(0,2) -= ( (srun/120.) * (4./16.)*k*k*
k );
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 );
149 float dpsidk = 0.5*
s;
float dpsiddzdl = 0.5*k*dsddzdl;
150 if( ((
int)((floor(fabs(s*k/
M_PI)))))%2 == 1 )
154 dpsiddzdl = -dpsiddzdl;
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;
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);}
189 if(psi>0. || psi <
M_PI){psi = 2.*psi;}
203 ux2 = cospsi*ux - sinpsi*
uy;
204 uy2 = sinpsi*ux + cospsi*
uy;
253 float dy = yp - fixedpars[6];
254 float dz = zp - fixedpars[7];
256 val = dx*dx + dy*dy + dz*
dz;
258 grad(0) = 2.*dx*dsdl*ux2 + 2.*dy*dsdl*uy2 + 2.*dz*
dzdl;
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;
266 float xylen = sqrt(1-dzdl*dzdl);
298 for(
int i=0;i<3;i++){grad(i)=0.;}
307 for(
unsigned int i=0;i<
tracks->size();i++)
319 VectorXd start_point = VectorXd::Zero(1);
320 VectorXd min_point = VectorXd::Zero(1);
322 minimizer.
minimize(start_point, min_point, 0
x1.0p-30, 16, 0
x1.0p-40);
326 VectorXd tgrad = VectorXd::Zero(1);
327 MatrixXd thessian = MatrixXd::Zero(1,1);
331 float point[3];
float tangent[3];
344 d[0] =
x(0) - point[0];
345 d[1] =
x(1) - point[1];
346 d[2] =
x(2) - point[2];
348 float F = d[0]*d[0] + d[1]*d[1] + d[2]*d[2];
356 float F_cov = point_covariance(0,0) + point_covariance(1,1) + point_covariance(2,2);
357 float F_err_inv = 1./(F_cov);
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]);
371 float g = -exp(-f*invs2);
373 if( !(g == g) ){
continue;}
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;
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;