7 #include <Geant4/G4SystemOfUnits.hh>
12 #include <TMatrixFfwd.h>
14 #include <TMatrixTUtils.h>
18 #define LogDebug(exp) std::cout << "DEBUG: " << __FILE__ << ": " << __LINE__ << ": " << exp
20 #define LogDebug(exp) (void)0
23 #define LogError(exp) if(Verbosity()>0) std::cout << "ERROR: " << __FILE__ << ": " << __LINE__ << ": " << exp
24 #define LogWarning(exp) if(Verbosity()>0) std::cout << "WARNING: " << __FILE__ << ": " << __LINE__ << ": " << exp
26 using keylist = std::vector<TrkrDefs::cluskey>;
32 template<
class T>
inline constexpr
T square(
const T&
x ) {
return x*
x; }
39 if(
Verbosity()>0) std::cout <<
"WARNING: " << name <<
" is NaN for seed " << num <<
". Aborting this seed.\n";
41 return std::isnan(val);
47 double p[4] = {x*
cm,y*
cm,z*
cm,0.*cm};
50 return bfield[2]/
tesla;
62 TMatrixF localErr(3,3);
72 float clusphi = atan2(global(1), global(0));
74 ROT[0][0] = cos(clusphi);
75 ROT[0][1] = -sin(clusphi);
77 ROT[1][0] = sin(clusphi);
78 ROT[1][1] = cos(clusphi);
87 err = ROT * localErr * ROT_T;
97 std::vector<SvtxTrack_v2> seeds_vector;
101 for(
auto trackKeyChain:trackSeedKeyLists )
103 if(trackKeyChain.size()<2)
continue;
108 const auto& globalpos = globalPositions.at(trackKeyChain.at(0));
109 double x0 = globalpos(0);
110 double y0 = globalpos(1);
111 double z0 = globalpos(2);;
112 LogDebug(
"Initial (x,y,z): (" << x0 <<
"," << y0 <<
"," << z0 <<
")" << std::endl);
114 double alice_x0 = sqrt(x0*x0+y0*y0);
116 double alice_z0 =
z0;
120 trackSeed.
SetX(alice_x0);
121 trackSeed.
SetY(alice_y0);
122 trackSeed.
SetZ(alice_z0);
127 double alice_x = sqrt(x0*x0+y0*y0);
129 double trackCartesian_x = 0.;
130 double trackCartesian_y = 0.;
131 double trackCartesian_z = 0.;
133 const auto& secondpos = globalPositions.at(trackKeyChain.at(1));
135 const double second_x = secondpos(0);
136 const double second_y = secondpos(1);
137 const double second_z = secondpos(2);
138 const double first_phi = atan2(y0,x0);
139 const double second_alice_x = second_x*std::cos(first_phi)+second_y*std::sin(first_phi);
140 const double delta_alice_x = second_alice_x - alice_x0;
142 const double second_alice_y = -second_x*std::sin(first_phi)+second_y*std::cos(first_phi);
143 const double init_SinPhi = second_alice_y / std::sqrt(
square(delta_alice_x) +
square(second_alice_y));
144 const double delta_z = second_z -
z0;
145 const double init_DzDs = -delta_z / std::sqrt(
square(delta_alice_x) +
square(second_alice_y));
147 LogDebug(
"Set initial SinPhi to " << init_SinPhi << std::endl);
149 LogDebug(
"Set initial DzDs to " << init_DzDs << std::endl);
152 std::vector<std::pair<double,double>> pts;
155 const auto& clpos = globalPositions.at(key);
156 return std::make_pair(clpos(0),clpos(1));
163 if(
Verbosity()>1) std::cout <<
"circle fit parameters: R=" << R <<
", X0=" << x_center <<
", Y0=" << y_center << std::endl;
167 if( std::isnan(R) )
continue;
169 double init_QPt = 1./(0.3*R/100.*
get_Bz(x0,y0,z0));
171 double phi_first = atan2(y0,x0);
172 if(
Verbosity()>1) std::cout <<
"phi_first: " << phi_first << std::endl;
173 double phi_second = atan2(second_y,second_x);
174 if(
Verbosity()>1) std::cout <<
"phi_second: " << phi_second << std::endl;
175 double dphi = phi_second - phi_first;
176 if(
Verbosity()>1) std::cout <<
"dphi: " << dphi << std::endl;
178 if(dphi<-
M_PI) dphi = 2*
M_PI + dphi;
179 if(
Verbosity()>1) std::cout <<
"corrected dphi: " << dphi << std::endl;
180 if(dphi<0) init_QPt = -1*init_QPt;
181 LogDebug(
"initial QPt: " << init_QPt << std::endl);
182 trackSeed.
SetQPt(init_QPt);
187 LogDebug(std::endl << std::endl <<
"------------------------" << std::endl <<
"seed size: " << trackKeyChain.size() << std::endl << std::endl << std::endl);
191 std::vector<double> cx;
192 std::vector<double> cy;
193 std::vector<double> cz;
194 std::vector<double> tx;
195 std::vector<double> ty;
196 std::vector<double> tz;
197 std::vector<double> xerr;
198 std::vector<double> yerr;
199 std::vector<double> zerr;
200 std::vector<double>
layer;
201 std::vector<double> xsize;
202 std::vector<double> ysize;
203 std::vector<double> phisize;
204 std::vector<double> phierr;
205 std::vector<double> zsize;
206 for(
auto clusterkey = std::next(trackKeyChain.begin()); clusterkey != trackKeyChain.end(); ++clusterkey)
208 LogDebug(
"-------------------------------------------------------------" << std::endl);
209 LogDebug(
"cluster " << cluster_ctr <<
" -> " << cluster_ctr + 1 << std::endl);
210 LogDebug(
"this cluster (x,y,z) = (" << x <<
"," << y <<
"," << z <<
")" << std::endl);
214 const auto& nextpos = globalPositions.at(*clusterkey);
217 double nextCluster_x = nextpos(0);
218 double nextCluster_xerr = sqrt(
getClusterError(nextCluster,nextpos,0,0));
219 double nextCluster_y = nextpos(1);
220 double nextCluster_yerr = sqrt(
getClusterError(nextCluster,nextpos,1,1));
221 double nextCluster_z = nextpos(2);
222 double nextCluster_zerr = sqrt(
getClusterError(nextCluster,nextpos,2,2));
224 double newPhi = atan2(nextCluster_y,nextCluster_x);
225 LogDebug(
"new phi = " << newPhi << std::endl);
226 double oldPhi = atan2(y,x);
227 LogDebug(
"old phi = " << oldPhi << std::endl);
228 double alpha = newPhi - oldPhi;
229 LogDebug(
"alpha = " << alpha << std::endl);
232 LogWarning(
"Rotate failed! Aborting for this seed...\n");
236 double nextAlice_x = nextCluster_x*cos(newPhi)+nextCluster_y*sin(newPhi);
237 LogDebug(
"track coordinates (ALICE) after rotation: (" << trackSeed.
GetX() <<
"," << trackSeed.
GetY() <<
"," << trackSeed.
GetZ() <<
")" << std::endl);
238 LogDebug(
"Transporting from " << alice_x <<
" to " << nextAlice_x <<
"...");
243 double track_x = trackSeed.
GetX()*cos(newPhi)-trackSeed.
GetY()*sin(newPhi);
244 double track_y = trackSeed.
GetX()*sin(newPhi)+trackSeed.
GetY()*cos(newPhi);
245 double track_z = trackSeed.
GetZ();
248 LogWarning(
"Transport failed! Aborting for this seed...\n");
255 double predicted_alice_x = trackSeed.
GetX();
256 LogDebug(
"new track ALICE x = " << trackSeed.
GetX() << std::endl);
257 double predicted_alice_y = trackSeed.
GetY();
258 LogDebug(
"new track ALICE y = " << trackSeed.
GetY() << std::endl);
259 double predicted_z = trackSeed.
GetZ();
260 LogDebug(
"new track z = " << trackSeed.
GetZ() << std::endl);
261 double cos_phi = x/sqrt(x*x+y*y);
262 LogDebug(
"cos_phi = " << cos_phi << std::endl);
263 double sin_phi = y/sqrt(x*x+y*y);
264 LogDebug(
"sin phi = " << sin_phi << std::endl);
265 trackCartesian_x = predicted_alice_x*cos_phi-predicted_alice_y*sin_phi;
266 trackCartesian_y = predicted_alice_x*sin_phi+predicted_alice_y*cos_phi;
267 trackCartesian_z = predicted_z;
268 LogDebug(
"Track transported to (x,y,z) = (" << trackCartesian_x <<
"," << trackCartesian_y <<
"," << trackCartesian_z <<
")" << std::endl);
269 LogDebug(
"Track position ALICE Y error: " << sqrt(trackSeed.
GetCov(0)) << std::endl);
270 LogDebug(
"Track position x error: " << sqrt(trackSeed.
GetCov(0))*sin_phi << std::endl);
271 LogDebug(
"Track position y error: " << sqrt(trackSeed.
GetCov(0))*cos_phi << std::endl);
272 LogDebug(
"Track position z error: " << sqrt(trackSeed.
GetCov(5)) << std::endl);
273 LogDebug(
"Next cluster is at (x,y,z) = (" << nextCluster_x <<
"," << nextCluster_y <<
"," << nextCluster_z <<
")" << std::endl);
274 LogDebug(
"Cluster errors: (" << nextCluster_xerr <<
", " << nextCluster_yerr <<
", " << nextCluster_zerr <<
")" << std::endl);
275 LogDebug(
"track coordinates (ALICE) after rotation: (" << trackSeed.
GetX() <<
"," << trackSeed.
GetY() <<
"," << trackSeed.
GetZ() <<
")" << std::endl);
278 double nextCluster_alice_y = -nextCluster_x*sin(newPhi)+nextCluster_y*cos(newPhi);
279 LogDebug(
"next cluster ALICE y = " << nextCluster_alice_y << std::endl);
289 if(!trackSeed.
Filter(nextCluster_alice_y,nextCluster_z,y2_error,z2_error,
_max_sin_phi))
291 LogError(
"Kalman filter failed for seed " << nseeds <<
"! Aborting for this seed..." << std::endl);
296 double track_pt = 1./trackSeed.
GetQPt();
297 double track_pY = track_pt*trackSeed.
GetSinPhi();
298 double track_pX = sqrt(track_pt*track_pt-track_pY*track_pY);
299 double track_px = track_pX*cos(newPhi)-track_pY*sin(newPhi);
300 double track_py = track_pX*sin(newPhi)+track_pY*cos(newPhi);
301 double track_pz = -track_pt*trackSeed.
GetDzDs();
304 LogDebug(
"track pt = " << track_pt <<
" +- " << track_pterr << std::endl);
305 LogDebug(
"track ALICE p = (" << track_pX <<
", " << track_pY <<
", " << track_pz <<
")" << std::endl);
306 LogDebug(
"track p = (" << track_px <<
", " << track_py <<
", " << track_pz <<
")" << std::endl);
311 alice_x = nextAlice_x;
318 float nextclusrad = std::sqrt(nextCluster_x*nextCluster_x +
319 nextCluster_y*nextCluster_y);
320 float nextclusphierr = nextCluster->
getRPhiError() / nextclusrad;
322 cx.push_back(nextCluster_x);
323 cy.push_back(nextCluster_y);
324 cz.push_back(nextCluster_z);
325 tx.push_back(trackCartesian_x);
326 ty.push_back(trackCartesian_y);
327 tz.push_back(trackCartesian_z);
328 xerr.push_back(nextCluster_xerr);
329 yerr.push_back(nextCluster_yerr);
330 zerr.push_back(nextCluster_zerr);
332 phierr.push_back(nextclusphierr);
336 if(
Verbosity()>0) std::cout <<
"finished track\n";
364 double track_phi = atan2(y,x);
446 double track_pt = fabs(1./trackSeed.
GetQPt());
448 double track_pY = track_pt*trackSeed.
GetSinPhi();
449 double track_pX = sqrt(track_pt*track_pt-track_pY*track_pY);
450 double track_px = track_pX*cos(track_phi)-track_pY*sin(track_phi);
451 double track_py = track_pX*sin(track_phi)+track_pY*cos(track_phi);
452 double track_pz = track_pt*trackSeed.
GetDzDs();
456 if(trackKeyChain.size()<10) track_pt = fabs(1./init_QPt);
457 LogDebug(
"track pt = " << track_pt <<
" +- " << track_pterr << std::endl);
458 LogDebug(
"track ALICE p = (" << track_pX <<
", " << track_pY <<
", " << track_pz <<
")" << std::endl);
459 LogDebug(
"track p = (" << track_px <<
", " << track_py <<
", " << track_pz <<
")" << std::endl);
474 if(
checknan(track_pt,
"pT",nseeds))
continue;
476 if(
checknan(track_pterr,
"pT err",nseeds))
continue;
477 LogDebug(
"Track pterr = " << track_pterr << std::endl);
478 double track_x = trackSeed.
GetX()*cos(track_phi)-trackSeed.
GetY()*sin(track_phi);
479 double track_y = trackSeed.
GetX()*sin(track_phi)+trackSeed.
GetY()*cos(track_phi);
480 double track_z = trackSeed.
GetZ();
481 if(
checknan(track_z,
"z",nseeds))
continue;
482 double track_zerr = sqrt(trackSeed.
GetErr2Z());
483 if(
checknan(track_zerr,
"zerr",nseeds))
continue;
485 const auto& lclusterglob = globalPositions.at(trackKeyChain.back());
486 const float lclusterrad = sqrt(lclusterglob(0)*lclusterglob(0) + lclusterglob(1)*lclusterglob(1));
487 double last_cluster_phierr = lcluster->getRPhiError() / lclusterrad;
489 double track_phierr = sqrt(pow(last_cluster_phierr,2)+(pow(trackSeed.
GetX(),2)*trackSeed.
GetErr2Y()) /
490 pow(pow(trackSeed.
GetX(),2)+pow(trackSeed.
GetY(),2),2));
491 if(
checknan(track_phierr,
"phierr",nseeds))
continue;
492 LogDebug(
"Track phi = " << atan2(track_py,track_px) << std::endl);
493 LogDebug(
"Track phierr = " << track_phierr << std::endl);
495 if(
checknan(track_curvature,
"curvature",nseeds))
continue;
497 if(
checknan(track_curverr,
"curvature error",nseeds))
continue;
501 for (
unsigned int j = 0; j < trackKeyChain.size(); ++j)
507 int track_charge = 0;
511 double s = sin(track_phi);
512 double c = cos(track_phi);
521 if(
checknan(p,
"ALICE sinPhi",nseeds))
continue;
523 if(
checknan(d,
"ALICE dz/ds",nseeds))
continue;
524 double pY = track_pt*
p;
525 double pX = sqrt(track_pt*track_pt-pY*pY);
529 const double* cov = trackSeed.
GetCov();
530 bool cov_nan =
false;
531 for(
int i=0;i<15;i++)
535 if(cov_nan)
continue;
537 Eigen::Matrix<double,5,5> ecov;
573 Eigen::Matrix<double,6,5> J;
594 J(3,2) = -track_pt*(p*c/sqrt(1-p*p)+
s);
596 J(3,4) = track_pt*track_pt*track_charge*(p*s-c*sqrt(1-p*p));
600 J(4,2) = track_pt*(c-p*s/sqrt(1-p*p));
602 J(4,4) = -track_pt*track_pt*track_charge*(p*c+s*sqrt(1-p*p));
608 J(5,4) = -track_pt*track_pt*track_charge*
d;
609 bool cov_rot_nan =
false;
621 if(cov_rot_nan)
continue;
624 Eigen::Matrix<double,6,6> scov = J*ecov*J.transpose();
703 seeds_vector.push_back(track);
709 if(
Verbosity()>0) std::cout <<
"number of seeds: " << nseeds <<
"\n";
715 Eigen::Matrix<double,6,6> cov;
729 Eigen::Matrix<double,6,6> cov =
getEigenCov(track);
731 Eigen::LLT<Eigen::Matrix<double,6,6>> chDec(cov);
733 return (chDec.info() != Eigen::NumericalIssue);
739 Eigen::Matrix<double,6,6> cov =
getEigenCov(track);
740 Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double,6,6>> solver(cov);
741 Eigen::Matrix<double,6,1>
D = solver.eigenvalues();
742 Eigen::Matrix<double,6,6>
Q = solver.eigenvectors();
743 Eigen::Matrix<double,6,1> Dp = D.cwiseMax(1
e-15);
744 Eigen::Matrix<double,6,6>
Z = Q*Dp.asDiagonal()*Q.transpose();
768 for(
const auto&
point:points )
770 meanX +=
point.first;
771 meanY +=
point.second;
784 for(
const auto&
point:points )
786 double Xi =
point.first - meanX;
787 double Yi =
point.second - meanY;
788 double Zi = Xi*Xi + Yi*Yi;
805 const double Mz = Mxx + Myy;
806 const double Cov_xy = Mxx*Myy - Mxy*Mxy;
807 const double Var_z = Mzz - Mz*Mz;
808 const double A3 = 4*Mz;
809 const double A2 = -3*Mz*Mz - Mzz;
810 const double A1 = Var_z*Mz + 4*Cov_xy*Mz - Mxz*Mxz - Myz*Myz;
811 const double A0 = Mxz*(Mxz*Myy - Myz*Mxy) + Myz*(Myz*Mxx - Mxz*Mxy) - Var_z*Cov_xy;
812 const double A22 = A2 + A2;
813 const double A33 = A3 + A3 + A3;
820 static constexpr
int IterMAX=99;
821 for (
int iter=0; iter<IterMAX; ++iter)
823 double Dy = A1 + x*(A22 + A33*
x);
824 double xnew = x - y/
Dy;
826 double ynew = A0 + xnew*(A1 + xnew*(A2 + xnew*A3));
827 if (fabs(ynew)>=fabs(y))
break;
832 const double DET = x*x - x*Mz + Cov_xy;
834 const double Xcenter = (Mxz*(Myy -
x) - Myz*Mxy)/DET/2;
835 const double Ycenter = (Myz*(Mxx -
x) - Mxz*Mxy)/DET/2;
839 X0 = Xcenter + meanX;
840 Y0 = Ycenter + meanY;
841 R = sqrt(Xcenter*Xcenter + Ycenter*Ycenter + Mz);
849 double xsum=0,x2sum=0,ysum=0,xysum=0;
850 for(
const auto&
point:points )
860 a=(points.size()*xysum-xsum*ysum)/(points.size()*x2sum-xsum*xsum);
861 b=(x2sum*ysum-xsum*xysum)/(x2sum*points.size()-xsum*xsum);
865 for (
unsigned int i=0;i<points.size(); ++i)
867 double r = points[i].first;
868 double z_fit = a * r +
b;
869 std::cout <<
" r " << r <<
" z " << points[i].second <<
" z_fit " << z_fit << std::endl;
878 std::vector<double> residues;
879 std::transform( points.begin(), points.end(), std::back_inserter( residues ), [
R,X0,Y0](
const std::pair<double,double>&
point )
892 std::vector<double> residues;
894 std::transform( points.begin(), points.end(), std::back_inserter( residues ), [
A,
B](
const std::pair<double,double>&
point )