EIC Software
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
ALICEKF.cc
Go to the documentation of this file. Or view the newest version in sPHENIX GitHub for file ALICEKF.cc
1 #include "ALICEKF.h"
2 
3 #include "GPUTPCTrackLinearisation.h"
4 #include "GPUTPCTrackParam.h"
5 
6 #include <trackbase/TrkrCluster.h>
7 #include <Geant4/G4SystemOfUnits.hh>
8 #include <trackbase_historic/ActsTransformations.h>
9 #include "TFile.h"
10 #include "TNtuple.h"
11 
12 #include <TMatrixFfwd.h>
13 #include <TMatrixT.h>
14 #include <TMatrixTUtils.h>
15 //#define _DEBUG_
16 
17 #if defined(_DEBUG_)
18 #define LogDebug(exp) std::cout << "DEBUG: " << __FILE__ << ": " << __LINE__ << ": " << exp
19 #else
20 #define LogDebug(exp) (void)0
21 #endif
22 
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
25 
26 using keylist = std::vector<TrkrDefs::cluskey>;
27 
28 // anonymous namespace for local functions
29 namespace
30 {
31  // square
32  template<class T> inline constexpr T square( const T& x ) { return x*x; }
33 }
34 
35 bool ALICEKF::checknan(double val, const std::string &name, int num) const
36 {
37  if(std::isnan(val))
38  {
39  if(Verbosity()>0) std::cout << "WARNING: " << name << " is NaN for seed " << num << ". Aborting this seed.\n";
40  }
41  return std::isnan(val);
42 }
43 
44 double ALICEKF::get_Bz(double x, double y, double z) const
45 {
46  if(_use_const_field) return 1.4;
47  double p[4] = {x*cm,y*cm,z*cm,0.*cm};
48  double bfield[3];
49  _B->GetFieldValue(p,bfield);
50  return bfield[2]/tesla;
51 }
52 
53 double ALICEKF::getClusterError(TrkrCluster* c, Acts::Vector3F global, int i, int j) const
54 {
55  if(_use_fixed_clus_error)
56  {
57  if(i==j) return _fixed_clus_error.at(i)*_fixed_clus_error.at(i);
58  else return 0.;
59  }
60  else
61  {
62  TMatrixF localErr(3,3);
63  localErr[0][0] = 0.;
64  localErr[0][1] = 0.;
65  localErr[0][2] = 0.;
66  localErr[1][0] = 0.;
67  localErr[1][1] = c->getActsLocalError(0,0);
68  localErr[1][2] = c->getActsLocalError(0,1);
69  localErr[2][0] = 0.;
70  localErr[2][1] = c->getActsLocalError(1,0);
71  localErr[2][2] = c->getActsLocalError(2,0);
72  float clusphi = atan2(global(1), global(0));
73  TMatrixF ROT(3,3);
74  ROT[0][0] = cos(clusphi);
75  ROT[0][1] = -sin(clusphi);
76  ROT[0][2] = 0.0;
77  ROT[1][0] = sin(clusphi);
78  ROT[1][1] = cos(clusphi);
79  ROT[1][2] = 0.0;
80  ROT[2][0] = 0.0;
81  ROT[2][1] = 0.0;
82  ROT[2][2] = 1.0;
83  TMatrixF ROT_T(3,3);
84  ROT_T.Transpose(ROT);
85 
86  TMatrixF err(3,3);
87  err = ROT * localErr * ROT_T;
88 
89  return err[i][j];
90  }
91 }
92 
93 std::vector<SvtxTrack_v2> ALICEKF::ALICEKalmanFilter(const std::vector<keylist>& trackSeedKeyLists,bool use_nhits_limit, const PositionMap& globalPositions) const
94 {
95 // TFile* f = new TFile("/sphenix/u/mjpeters/macros_hybrid/detectors/sPHENIX/pull.root", "RECREATE");
96 // TNtuple* ntp = new TNtuple("pull","pull","cx:cy:cz:xerr:yerr:zerr:tx:ty:tz:layer:xsize:ysize:phisize:phierr:zsize");
97  std::vector<SvtxTrack_v2> seeds_vector;
98  int nseeds = 0;
99 
100  if(Verbosity()>0) std::cout << "min clusters per track: " << _min_clusters_per_track << "\n";
101  for( auto trackKeyChain:trackSeedKeyLists )
102  {
103  if(trackKeyChain.size()<2) continue;
104  if(use_nhits_limit && trackKeyChain.size() < _min_clusters_per_track) continue;
105  if(TrkrDefs::getLayer(trackKeyChain.front())<TrkrDefs::getLayer(trackKeyChain.back())) std::reverse(trackKeyChain.begin(),trackKeyChain.end());
106  // get starting cluster from key
107  // Transform sPHENIX coordinates into ALICE-compatible coordinates
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);
113  // ALICE x coordinate = distance from beampipe
114  double alice_x0 = sqrt(x0*x0+y0*y0);
115  double alice_y0 = 0;
116  double alice_z0 = z0;
117  // Initialize track and linearisation
118  GPUTPCTrackParam trackSeed;
119  trackSeed.InitParam();
120  trackSeed.SetX(alice_x0);
121  trackSeed.SetY(alice_y0);
122  trackSeed.SetZ(alice_z0);
123  double x = x0;
124  double y = y0;
125  #if defined(_DEBUG_)
126  double z = z0;
127  double alice_x = sqrt(x0*x0+y0*y0);
128  #endif
129  double trackCartesian_x = 0.;
130  double trackCartesian_y = 0.;
131  double trackCartesian_z = 0.;
132  // Pre-set momentum-based parameters to improve numerical stability
133  const auto& secondpos = globalPositions.at(trackKeyChain.at(1));
134 
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;
141  //double second_alice_y = (second_x/cos(first_phi)-second_y/sin(first_phi))/(sin(first_phi)/cos(first_phi)+cos(first_phi)/sin(first_phi));
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));
146  trackSeed.SetSinPhi(init_SinPhi);
147  LogDebug("Set initial SinPhi to " << init_SinPhi << std::endl);
148  trackSeed.SetDzDs(init_DzDs);
149  LogDebug("Set initial DzDs to " << init_DzDs << std::endl);
150 
151  // get initial pt estimate
152  std::vector<std::pair<double,double>> pts;
153  std::transform( trackKeyChain.begin(), trackKeyChain.end(), std::back_inserter( pts ), [&globalPositions]( const TrkrDefs::cluskey& key )
154  {
155  const auto& clpos = globalPositions.at(key);
156  return std::make_pair(clpos(0),clpos(1));
157  });
158 
159  double R = 0;
160  double x_center = 0;
161  double y_center = 0;
162  CircleFitByTaubin(pts,R,x_center,y_center);
163  if(Verbosity()>1) std::cout << "circle fit parameters: R=" << R << ", X0=" << x_center << ", Y0=" << y_center << std::endl;
164 
165  // check circle fit success
166  /* failed fit will result in infinite momentum for the track, which in turn will break the kalman filter */
167  if( std::isnan(R) ) continue;
168 
169  double init_QPt = 1./(0.3*R/100.*get_Bz(x0,y0,z0));
170  // determine charge
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;
177  if(dphi>M_PI) dphi = 2*M_PI - dphi;
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);
183 
184 
185  GPUTPCTrackLinearisation trackLine(trackSeed);
186 
187  LogDebug(std::endl << std::endl << "------------------------" << std::endl << "seed size: " << trackKeyChain.size() << std::endl << std::endl << std::endl);
188  int cluster_ctr = 1;
189 // bool aborted = false;
190  // starting at second cluster, perform track propagation
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)
207  {
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);
211  LogDebug("layer " << (int)TrkrDefs::getLayer(*clusterkey) << std::endl);
212  // get cluster from key
213  TrkrCluster* nextCluster = _cluster_map->findCluster(*clusterkey);
214  const auto& nextpos = globalPositions.at(*clusterkey);
215 
216  // find ALICE x-coordinate
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));
223  // rotate track coordinates to match orientation of next cluster
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);
230  if(!trackSeed.Rotate(alpha,trackLine,_max_sin_phi))
231  {
232  LogWarning("Rotate failed! Aborting for this seed...\n");
233 // aborted = true;
234  break;
235  }
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 << "...");
239  GPUTPCTrackParam::GPUTPCTrackFitParam fp;
240  trackSeed.CalculateFitParameters(fp);
241  // for(int i=1;i<=10;i++)
242  // {
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();
246  if(!trackSeed.TransportToX(nextAlice_x,_Bzconst*get_Bz(track_x,track_y,track_z),_max_sin_phi)) // remember: trackLine was here
247  {
248  LogWarning("Transport failed! Aborting for this seed...\n");
249 // aborted = true;
250  break;
251  }
252  // }
253  // convert ALICE coordinates to sPHENIX cartesian coordinates, for debugging
254 
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);
276  //double nextCluster_alice_y = (nextCluster_x/cos(newPhi) - nextCluster_y/sin(newPhi))/(tan(newPhi)+1./tan(newPhi));
277  //double nextCluster_alice_y = 0.;
278  double nextCluster_alice_y = -nextCluster_x*sin(newPhi)+nextCluster_y*cos(newPhi);
279  LogDebug("next cluster ALICE y = " << nextCluster_alice_y << std::endl);
280  double y2_error = getClusterError(nextCluster,nextpos,0,0)*sin(newPhi)*sin(newPhi)+2*getClusterError(nextCluster,nextpos,0,1)*cos(newPhi)*sin(newPhi)+getClusterError(nextCluster,nextpos,1,1)*cos(newPhi)*cos(newPhi);
281  double z2_error = getClusterError(nextCluster,nextpos,2,2);
282  LogDebug("track ALICE SinPhi = " << trackSeed.GetSinPhi() << std::endl);
283  LogDebug("track DzDs = " << trackSeed.GetDzDs() << std::endl);
284  LogDebug("chi2 = " << trackSeed.GetChi2() << std::endl);
285  LogDebug("NDF = " << trackSeed.GetNDF() << std::endl);
286  LogDebug("chi2 / NDF = " << trackSeed.GetChi2()/trackSeed.GetNDF() << std::endl);
287 
288  // Apply Kalman filter
289  if(!trackSeed.Filter(nextCluster_alice_y,nextCluster_z,y2_error,z2_error,_max_sin_phi))
290  {
291  LogError("Kalman filter failed for seed " << nseeds << "! Aborting for this seed..." << std::endl);
292 // aborted = true;
293  break;
294  }
295  #if defined(_DEBUG_)
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();
302  double track_pterr = sqrt(trackSeed.GetErr2QPt())/(trackSeed.GetQPt()*trackSeed.GetQPt());
303  #endif
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);
307  x = nextCluster_x;
308  y = nextCluster_y;
309  #if defined(_DEBUG_)
310  z = nextCluster_z;
311  alice_x = nextAlice_x;
312  #endif
313  ++cluster_ctr;
314 
315  //if(cluster_ctr>10)
316  {
317 
318  float nextclusrad = std::sqrt(nextCluster_x*nextCluster_x +
319  nextCluster_y*nextCluster_y);
320  float nextclusphierr = nextCluster->getRPhiError() / nextclusrad;
321 
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);
331  layer.push_back(TrkrDefs::getLayer(*clusterkey));
332  phierr.push_back(nextclusphierr);
333  }
334  }
335 // if(aborted) continue;
336  if(Verbosity()>0) std::cout << "finished track\n";
337 /*
338  // transport to beamline
339 // float old_phi = atan2(y,x);
340  float trackX = trackSeed.GetX();
341  for(int i=99;i>=0;i--)
342  {
343  if(!trackSeed.TransportToX(i/100.*trackX,trackLine,_Bz,_max_sin_phi))
344  {
345  LogWarning("Transport failed! Aborting for this seed...\n");
346  aborted = true;
347  break;
348  }
349 // float new_phi = atan2(trackSeed.GetX()*sin(old_phi)+trackSeed.GetY()*cos(old_phi),trackSeed.GetX()*cos(old_phi)-trackSeed.GetY()*sin(old_phi));
350 // if(!trackSeed.Rotate(new_phi-old_phi,trackLine,_max_sin_phi))
351 // {
352 // LogWarning("Rotate failed! Aborting for this seed...\n");
353 // aborted = true;
354 // break;
355 // }
356 // old_phi = new_phi;
357  }
358  if(aborted) continue;
359  std::cout << "transported to beamline\n";
360  // find nearest vertex
361  double beamline_X = trackSeed.GetX();
362  double beamline_Y = trackSeed.GetY();
363 */
364  double track_phi = atan2(y,x);
365 /*
366  double beamline_x = beamline_X*cos(track_phi)-beamline_Y*sin(track_phi);
367  double beamline_y = beamline_X*sin(track_phi)+beamline_Y*cos(track_phi);
368  double beamline_z = trackSeed.GetZ();
369  double min_dist = 1e9;
370  int best_vtx = -1;
371  for(int i=0;i<_vertex_x.size();++i)
372  {
373  double delta_x = beamline_x-_vertex_x[i];
374  double delta_y = beamline_y-_vertex_y[i];
375  double delta_z = beamline_z-_vertex_z[i];
376  double dist = sqrt(delta_x*delta_x+delta_y*delta_y+delta_z*delta_z);
377  if(dist<min_dist)
378  {
379  min_dist = dist;
380  best_vtx = i;
381  }
382  }
383  std::cout << "best vtx:\n";
384  std::cout << "("<<_vertex_x[best_vtx]<<","<<_vertex_y[best_vtx]<<","<<_vertex_z[best_vtx]<<")\n";
385  // Fit to vertex point
386  double vertex_phi = atan2(_vertex_y[best_vtx],_vertex_x[best_vtx]);
387  std::cout << "vertex_phi: " << vertex_phi << "\n";
388  std::cout << "track_phi: " << track_phi << "\n";
389 // double alpha = vertex_phi - track_phi;
390  // Here's where we need to be careful about the vertex position.
391  // Most clusters are at roughly the same spatial phi, with only a little rotation required between them.
392  // This is no longer guaranteed for the vertex - its phi could be anywhere,
393  // including on the opposite side of the origin.
394  // If it ends up on the opposite side, then we need to transport to *negative* radius in order to get close to it.
395  // We will simplify this condition to abs(alpha)>pi/2, which assumes that (innermost TPC cluster R) >> (vertex R).
396 
397  bool crosses_origin = false;
398  if(alpha<-M_PI/4)
399  {
400  while(alpha<-M_PI/4) alpha += M_PI/2;
401  crosses_origin = true;
402  }
403  if(alpha>M_PI/4)
404  {
405  while(alpha>M_PI/4) alpha -= M_PI/2;
406  crosses_origin = true;
407  }
408  if(crosses_origin) std::cout << "bad\n";
409  std::cout << "alpha: " << alpha << "\n";
410 
411  if(!trackSeed.Rotate(alpha,trackLine,_max_sin_phi))
412  {
413  LogWarning("Rotate failed! Aborting for this seed...\n");
414  aborted = true;
415  continue;
416  }
417  LogDebug("ALICE coordinates after rotation: (" << trackSeed.GetX() << ", " << trackSeed.GetY() << ", " << trackSeed.GetZ() << ")\n");
418  std::cout << "rotated to vertex\n";
419 
420  double vertex_X = sqrt(_vertex_x[best_vtx]*_vertex_x[best_vtx]+_vertex_y[best_vtx]*_vertex_y[best_vtx]);
421  if(crosses_origin) vertex_X = -vertex_X;
422  if(!trackSeed.TransportToX(vertex_X,trackLine,_Bz,_max_sin_phi))
423  {
424  LogWarning("Transport failed! Aborting for this seed...\n");
425  aborted = true;
426  continue;
427  }
428  LogDebug("Track transported to (x,y,z) = (" << trackSeed.GetX()*cos(vertex_phi)-trackSeed.GetY()*sin(vertex_phi) << "," << trackSeed.GetX()*sin(vertex_phi)+trackSeed.GetY()*cos(vertex_phi) << "," << trackSeed.GetZ() << ")" << std::endl);
429  LogDebug("Next cluster is at (x,y,z) = (" << _vertex_x[best_vtx] << "," << _vertex_y[best_vtx] << "," << _vertex_z[best_vtx] << ")" << std::endl);
430 
431  double vertex_Y = -_vertex_x[best_vtx]*sin(vertex_phi)+_vertex_y[best_vtx]*cos(vertex_phi);
432  std::cout << "vertex Y: " << vertex_Y << "\n";
433  std::cout << "transported to vertex\n";
434  double vertex_Yerr = -_vertex_xerr[best_vtx]*sin(vertex_phi)+_vertex_yerr[best_vtx]*cos(vertex_phi);
435  std::cout << "vertex Y err: " << vertex_Yerr << "\n";
436 
437  if(!trackSeed.Filter(vertex_Y,_vertex_z[best_vtx],vertex_Yerr*vertex_Yerr,_vertex_zerr[best_vtx]*_vertex_zerr[best_vtx],_max_sin_phi))
438  {
439  std::cout << "filter failed\n";
440  if (Verbosity() >= 1)
441  LogError("Kalman filter failed for seed " << nseeds << "! Aborting for this seed..." << std::endl);
442  aborted = true;
443  continue;
444  }
445 */
446  double track_pt = fabs(1./trackSeed.GetQPt());
447  #if defined(_DEBUG_)
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();
453  #endif
454  double track_pterr = sqrt(trackSeed.GetErr2QPt())/(trackSeed.GetQPt()*trackSeed.GetQPt());
455  // If Kalman filter doesn't do its job (happens often with short seeds), use the circle-fit estimate as the central value
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);
460 
461 /*
462  if(cluster_ctr!=1 && !trackSeed.CheckNumericalQuality())
463  {
464  std::cout << "ERROR: Track seed failed numerical quality check before conversion to sPHENIX coordinates! Skipping this one.\n";
465  aborted = true;
466  continue;
467  }
468 */
469  // pt:z:dz:phi:dphi:c:dc
470  // Fill NT with track parameters
471  // double StartEta = -log(tan(atan(z0/sqrt(x0*x0+y0*y0))));
472 // if(aborted) continue;
473 // double track_pt = fabs( 1./(trackSeed.GetQPt()));
474  if(checknan(track_pt,"pT",nseeds)) continue;
475 // double track_pterr = sqrt(trackSeed.GetErr2QPt())/(trackSeed.GetQPt()*trackSeed.GetQPt());
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;
484  auto lcluster = _cluster_map->findCluster(trackKeyChain.back());
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;
488  // phi error assuming error in track radial coordinate is zero
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);
494  double track_curvature = trackSeed.GetKappa(_Bzconst*get_Bz(track_x,track_y,track_z));
495  if(checknan(track_curvature,"curvature",nseeds)) continue;
496  double track_curverr = sqrt(trackSeed.GetErr2QPt())*_Bzconst*get_Bz(track_x,track_y,track_z);
497  if(checknan(track_curverr,"curvature error",nseeds)) continue;
498  SvtxTrack_v2 track;
499  track.set_id(nseeds);
500 // track.set_vertex_id(_vertex_ids[best_vtx]);
501  for (unsigned int j = 0; j < trackKeyChain.size(); ++j)
502  {
503  track.insert_cluster_key(trackKeyChain.at(j));
504  }
505  track.set_chisq(trackSeed.GetChi2());
506  track.set_ndf(trackSeed.GetNDF());
507  int track_charge = 0;
508  if(trackSeed.GetQPt()<0) track_charge = -1 * _fieldDir;
509  else track_charge = 1 * _fieldDir;
510  track.set_charge(track_charge);
511  double s = sin(track_phi);
512  double c = cos(track_phi);
513  double p = trackSeed.GetSinPhi();
514  // TrkrCluster *cl = _cluster_map->findCluster(trackKeyChain.at(0));
515  track.set_x(trackSeed.GetX()*c-trackSeed.GetY()*s);//_vertex_x[best_vtx]); //track.set_x(cl->getX());
516  track.set_y(trackSeed.GetX()*s+trackSeed.GetY()*c);//_vertex_y[best_vtx]); //track.set_y(cl->getY());
517  track.set_z(trackSeed.GetZ());//_vertex_z[best_vtx]); //track.set_z(cl->getZ());
518  if(Verbosity()>0) std::cout << "x " << track.get_x() << "\n";
519  if(Verbosity()>0) std::cout << "y " << track.get_y() << "\n";
520  if(Verbosity()>0) std::cout << "z " << track.get_z() << "\n";
521  if(checknan(p,"ALICE sinPhi",nseeds)) continue;
522  double d = trackSeed.GetDzDs();
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);
526  track.set_px(pX*c-pY*s);
527  track.set_py(pX*s+pY*c);
528  track.set_pz(track_pt * trackSeed.GetDzDs());
529  const double* cov = trackSeed.GetCov();
530  bool cov_nan = false;
531  for(int i=0;i<15;i++)
532  {
533  if(checknan(cov[i],"covariance element "+std::to_string(i),nseeds)) cov_nan = true;
534  }
535  if(cov_nan) continue;
536  // make this into an actual Eigen matrix
537  Eigen::Matrix<double,5,5> ecov;
538  ecov(0,0)=cov[0];
539  ecov(0,1)=cov[1];
540  ecov(0,2)=cov[2];
541  ecov(0,3)=cov[3];
542  ecov(0,4)=cov[4];
543  ecov(1,1)=cov[5];
544  ecov(1,2)=cov[6];
545  ecov(1,3)=cov[7];
546  ecov(1,4)=cov[8];
547  ecov(2,2)=cov[9];
548  ecov(2,3)=cov[10];
549  ecov(2,4)=cov[11];
550  ecov(3,3)=cov[12];
551  ecov(3,4)=cov[13];
552  ecov(4,4)=cov[14];
553  // symmetrize
554  ecov(1,0)=ecov(0,1);
555  ecov(2,0)=ecov(0,2);
556  ecov(3,0)=ecov(0,3);
557  ecov(4,0)=ecov(0,4);
558  ecov(2,1)=ecov(1,2);
559  ecov(3,1)=ecov(1,3);
560  ecov(4,1)=ecov(1,4);
561  ecov(3,2)=ecov(2,3);
562  ecov(4,2)=ecov(2,4);
563  ecov(4,3)=ecov(3,4);
564  // make rotation matrix based on the following:
565  // x = X*cos(track_phi) - Y*sin(track_phi)
566  // y = X*sin(track_phi) + Y*cos(track_phi)
567  // z = Z
568  // pY = pt*sinphi
569  // pX = sqrt(pt**2 - pY**2)
570  // px = pX*cos(track_phi) - pY*sin(track_phi)
571  // py = pX*sin(track_phi) + pY*cos(track_phi)
572  // pz = pt*(dz/ds)
573  Eigen::Matrix<double,6,5> J;
574  J(0,0) = -s; // dx/dY
575  J(0,1) = 0.; // dx/dZ
576  J(0,2) = 0.; // dx/d(sinphi)
577  J(0,3) = 0.; // dx/d(dz/ds)
578  J(0,4) = 0.; // dx/d(Q/pt)
579 
580  J(1,0) = c; // dy/dY
581  J(1,1) = 0.; // dy/dZ
582  J(1,2) = 0.; // dy/d(sinphi)
583  J(1,3) = 0.; // dy/d(dz/ds)
584  J(1,4) = 0.; // dy/d(Q/pt)
585 
586  J(2,0) = 0.; // dz/dY
587  J(2,1) = 1.; // dz/dZ
588  J(2,2) = 0.; // dz/d(sinphi)
589  J(2,3) = 0.; // dz/d(dz/ds)
590  J(2,4) = 0.; // dz/d(Q/pt)
591 
592  J(3,0) = 0.; // dpx/dY
593  J(3,1) = 0.; // dpx/dZ
594  J(3,2) = -track_pt*(p*c/sqrt(1-p*p)+s); // dpx/d(sinphi)
595  J(3,3) = 0.; // dpx/d(dz/ds)
596  J(3,4) = track_pt*track_pt*track_charge*(p*s-c*sqrt(1-p*p)); // dpx/d(Q/pt)
597 
598  J(4,0) = 0.; // dpy/dY
599  J(4,1) = 0.; // dpy/dZ
600  J(4,2) = track_pt*(c-p*s/sqrt(1-p*p)); // dpy/d(sinphi)
601  J(4,3) = 0.; // dpy/d(dz/ds)
602  J(4,4) = -track_pt*track_pt*track_charge*(p*c+s*sqrt(1-p*p)); // dpy/d(Q/pt)
603 
604  J(5,0) = 0.; // dpz/dY
605  J(5,1) = 0.; // dpz/dZ
606  J(5,2) = 0.; // dpz/d(sinphi)
607  J(5,3) = track_pt; // dpz/d(dz/ds)
608  J(5,4) = -track_pt*track_pt*track_charge*d; // dpz/d(Q/pt)
609  bool cov_rot_nan = false;
610  for(int i=0;i<6;i++)
611  {
612  for(int j=0;j<5;j++)
613  {
614  if(checknan(J(i,j),"covariance rotator element ("+std::to_string(i)+","+std::to_string(j)+")",nseeds))
615  {
616  cov_rot_nan = true;
617  continue;
618  }
619  }
620  }
621  if(cov_rot_nan) continue;
622 
623  // the heavy lifting happens here
624  Eigen::Matrix<double,6,6> scov = J*ecov*J.transpose();
625 
626  // fill SvtxTrack covariance matrix with results
627  for(int i=0;i<6;i++)
628  {
629  for(int j=0;j<6;j++)
630  {
631  track.set_error(i, j, scov(i,j));
632  }
633  }
634 /*
635  // Proceed with the absolutely hellish coordinate transformation of the covariance matrix.
636  // Derived from:
637  // 1) Taking the Jacobian of the conversion from (Y,Z,SinPhi,DzDs,Q/Pt) to (x,y,z,px,py,pz)
638  // 2) Computing (Jacobian)*(ALICE covariance matrix)*(transpose of Jacobian)
639  track.set_error(0, 0, cov[0]*s*s);
640  track.set_error(0, 1, -cov[0]*c*s);
641  track.set_error(0, 2, -cov[1]*s);
642  track.set_error(0, 3, cov[2]*s*s/q-cov[4]*s*(-c/(q*q)+p*s/(q*q)));
643  track.set_error(0, 4, -cov[2]*c*s/q-cov[4]*s*(-c*p/(q*q)-s/(q*q)));
644  track.set_error(0, 5, cov[4]*d*s/(q*q)-cov[3]*s/q);
645  track.set_error(1, 1, cov[0]*c*c);
646  track.set_error(1, 2, cov[1]*c);
647  track.set_error(1, 3, -cov[2]*c*s/q+cov[4]*c*(-c/(q*q)+p*s/(q*q)));
648  track.set_error(1, 4, cov[2]*c*c/q+cov[4]*c*(-c*p/(q*q)-s/(q*q)));
649  track.set_error(1, 5, cov[4]*d*c/(q*q)+cov[3]*c/q);
650  track.set_error(2, 2, cov[5]);
651  track.set_error(2, 3, -cov[6]*s/q+cov[8]*(-c/(q*q)+p*s/(q*q)));
652  track.set_error(2, 4, cov[6]*c/q+cov[8]*(-c*p/(q*q)-s/(q*q)));
653  track.set_error(2, 5, -cov[8]*d/(q*q)+cov[7]/q);
654  track.set_error(3, 3, cov[9]*s*s/(q*q)-cov[11]*(-c/(q*q*q)+p*s/(q*q*q)) + (-c/(q*q)+p*s/(q*q))*(-cov[11]*s/q+cov[14]*(-c/(q*q)+p*s/(q*q))));
655  track.set_error(3, 4, -cov[9]*c*s/(q*q)+cov[11]*(-c/(q*q*q)+p*s/(q*q*q)) + (-c*p/(q*q)-s/(q*q))*(-cov[11]*s/q+cov[14]*(-c/(q*q)+p*s/(q*q))));
656  track.set_error(3, 5, -cov[10]*s/(q*q)+cov[13]/q*(-c/(q*q)+p*s/(q*q))-d/(q*q)*(-cov[11]*s/q+cov[14]*(-c/(q*q)+p*s/(q*q))));
657  track.set_error(4, 4, c/q*(c/q*cov[9]+cov[11]*(-c*p/(q*q)-s/(q*q)))+(-c*p/(q*q)-s/(q*q))*(c/q*cov[11]+cov[14]*(-c*p/(q*q)-s/(q*q))));
658  track.set_error(4, 5, cov[10]*c/(q*q)+cov[13]/q*(-c*p/(q*q)-s/(q*q))-d/(q*q)*(c/q*cov[11]+cov[14]*(-c*p/(q*q)-s/(q*q))));
659  track.set_error(5, 5, -d/(q*q)*(-d*cov[14]/(q*q)+cov[13]/q)-d*cov[13]/(q*q*q)+cov[12]/(q*q));
660  // symmetrize covariance
661  track.set_error(1, 0, track.get_error(0, 1));
662  track.set_error(2, 0, track.get_error(0, 2));
663  track.set_error(3, 0, track.get_error(0, 3));
664  track.set_error(4, 0, track.get_error(0, 4));
665  track.set_error(5, 0, track.get_error(0, 5));
666  track.set_error(2, 1, track.get_error(1, 2));
667  track.set_error(3, 1, track.get_error(1, 3));
668  track.set_error(4, 1, track.get_error(1, 4));
669  track.set_error(5, 1, track.get_error(1, 5));
670  track.set_error(3, 2, track.get_error(2, 3));
671  track.set_error(4, 2, track.get_error(2, 4));
672  track.set_error(5, 2, track.get_error(2, 5));
673  track.set_error(4, 3, track.get_error(3, 4));
674  track.set_error(5, 3, track.get_error(3, 5));
675  track.set_error(5, 4, track.get_error(4, 5));
676 */
677 
678  if(!covIsPosDef(track))
679  {
680  repairCovariance(track);
681  }
682 /*
683  for(int w=0;w<cx.size();w++)
684  {
685  ntp->Fill(cx[w],cy[w],cz[w],xerr[w],yerr[w],zerr[w],tx[w],ty[w],tz[w],layer[w],xsize[w],ysize[w],phisize[w],phierr[w],zsize[w]);
686  }
687  cx.clear();
688  cy.clear();
689  cz.clear();
690  tx.clear();
691  ty.clear();
692  tz.clear();
693  xerr.clear();
694  yerr.clear();
695  zerr.clear();
696  layer.clear();
697  xsize.clear();
698  ysize.clear();
699  phisize.clear();
700  phierr.clear();
701  zsize.clear();
702 */
703  seeds_vector.push_back(track);
704  ++nseeds;
705  }
706 // f->cd();
707 // ntp->Write();
708 // f->Close();
709  if(Verbosity()>0) std::cout << "number of seeds: " << nseeds << "\n";
710  return seeds_vector;
711 }
712 
713 Eigen::Matrix<double,6,6> ALICEKF::getEigenCov(const SvtxTrack_v2 &track) const
714 {
715  Eigen::Matrix<double,6,6> cov;
716  for(int i=0;i<6;i++)
717  {
718  for(int j=0;j<6;j++)
719  {
720  cov(i,j) = track.get_error(i,j);
721  }
722  }
723  return cov;
724 }
725 
726 bool ALICEKF::covIsPosDef(const SvtxTrack_v2 &track) const
727 {
728  // put covariance matrix into Eigen container
729  Eigen::Matrix<double,6,6> cov = getEigenCov(track);
730  // attempt Cholesky decomposition
731  Eigen::LLT<Eigen::Matrix<double,6,6>> chDec(cov);
732  // if Cholesky decomposition does not exist, matrix is not positive definite
733  return (chDec.info() != Eigen::NumericalIssue);
734 }
735 
736 void ALICEKF::repairCovariance(SvtxTrack_v2 &track) const
737 {
738  // find closest positive definite matrix
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(1e-15);
744  Eigen::Matrix<double,6,6> Z = Q*Dp.asDiagonal()*Q.transpose();
745  // updates covariance matrix
746  for(int i=0;i<6;i++)
747  {
748  for(int j=0;j<6;j++)
749  {
750  track.set_error(i,j,Z(i,j));
751  }
752  }
753 }
754 void ALICEKF::CircleFitByTaubin (const std::vector<std::pair<double,double>>& points, double &R, double &X0, double &Y0) const
755 /*
756  Circle fit to a given set of data points (in 2D)
757  This is an algebraic fit, due to Taubin, based on the journal article
758  G. Taubin, "Estimation Of Planar Curves, Surfaces And Nonplanar
759  Space Curves Defined By Implicit Equations, With
760  Applications To Edge And Range Image Segmentation",
761  IEEE Trans. PAMI, Vol. 13, pages 1115-1138, (1991)
762 */
763 {
764  // Compute x- and y- sample means
765  double meanX = 0;
766  double meanY = 0;
767  double weight = 0;
768  for( const auto& point:points )
769  {
770  meanX += point.first;
771  meanY += point.second;
772  weight++;
773  }
774  meanX /= weight;
775  meanY /= weight;
776 
777  // computing moments
778  double Mxy = 0;
779  double Mxx = 0;
780  double Myy = 0;
781  double Mxz = 0;
782  double Myz = 0;
783  double Mzz = 0;
784  for( const auto& point:points )
785  {
786  double Xi = point.first - meanX; // centered x-coordinates
787  double Yi = point.second - meanY; // centered y-coordinates
788  double Zi = Xi*Xi + Yi*Yi;
789 
790  Mxy += Xi*Yi;
791  Mxx += Xi*Xi;
792  Myy += Yi*Yi;
793  Mxz += Xi*Zi;
794  Myz += Yi*Zi;
795  Mzz += Zi*Zi;
796  }
797  Mxx /= weight;
798  Myy /= weight;
799  Mxy /= weight;
800  Mxz /= weight;
801  Myz /= weight;
802  Mzz /= weight;
803 
804  // computing coefficients of the characteristic polynomial
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;
814 
815  // finding the root of the characteristic polynomial
816  // using Newton's method starting at x=0
817  // (it is guaranteed to converge to the right root)
818  double x = 0;
819  double y = A0;
820  static constexpr int IterMAX=99;
821  for (int iter=0; iter<IterMAX; ++iter) // usually, 4-6 iterations are enough
822  {
823  double Dy = A1 + x*(A22 + A33*x);
824  double xnew = x - y/Dy;
825  if ((xnew == x)||(!std::isfinite(xnew))) break;
826  double ynew = A0 + xnew*(A1 + xnew*(A2 + xnew*A3));
827  if (fabs(ynew)>=fabs(y)) break;
828  x = xnew; y = ynew;
829  }
830 
831  // computing parameters of the fitting circle
832  const double DET = x*x - x*Mz + Cov_xy;
833 
834  const double Xcenter = (Mxz*(Myy - x) - Myz*Mxy)/DET/2;
835  const double Ycenter = (Myz*(Mxx - x) - Mxz*Mxy)/DET/2;
836 
837  // assembling the output
838 
839  X0 = Xcenter + meanX;
840  Y0 = Ycenter + meanY;
841  R = sqrt(Xcenter*Xcenter + Ycenter*Ycenter + Mz);
842 }
843 
844 void ALICEKF::line_fit(const std::vector<std::pair<double,double>>& points, double &a, double &b) const
845 {
846  // copied from: https://www.bragitoff.com
847  // we want to fit z vs radius
848 
849  double xsum=0,x2sum=0,ysum=0,xysum=0; //variables for sums/sigma of xi,yi,xi^2,xiyi etc
850  for( const auto& point:points )
851  {
852  double r = point.first;
853  double z = point.second;
854 
855  xsum=xsum+r; //calculate sigma(xi)
856  ysum=ysum+z; //calculate sigma(yi)
857  x2sum=x2sum+square(r); //calculate sigma(x^2i)
858  xysum=xysum+r*z; //calculate sigma(xi*yi)
859  }
860  a=(points.size()*xysum-xsum*ysum)/(points.size()*x2sum-xsum*xsum); //calculate slope
861  b=(x2sum*ysum-xsum*xysum)/(x2sum*points.size()-xsum*xsum); //calculate intercept
862 
863  if(Verbosity() > 10)
864  {
865  for (unsigned int i=0;i<points.size(); ++i)
866  {
867  double r = points[i].first;
868  double z_fit = a * r + b; //to calculate z(fitted) at given r points
869  std::cout << " r " << r << " z " << points[i].second << " z_fit " << z_fit << std::endl;
870  }
871  }
872 
873  return;
874 }
875 
876 std::vector<double> ALICEKF::GetCircleClusterResiduals(const std::vector<std::pair<double,double>>& points, double R, double X0, double Y0) const
877 {
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 )
880  {
881  double x = point.first;
882  double y = point.second;
883 
884  // The shortest distance of a point from a circle is along the radial; line from the circle center to the point
885  return std::sqrt( square(x-X0) + square(y-Y0) ) - R;
886  } );
887  return residues;
888 }
889 
890 std::vector<double> ALICEKF::GetLineClusterResiduals(const std::vector<std::pair<double,double>>& points, double A, double B) const
891 {
892  std::vector<double> residues;
893  // calculate cluster residuals from the fitted circle
894  std::transform( points.begin(), points.end(), std::back_inserter( residues ), [A,B]( const std::pair<double,double>& point )
895  {
896  double r = point.first;
897  double z = point.second;
898 
899  // The shortest distance of a point from a circle is along the radial; line from the circle center to the point
900 
901  double a = -A;
902  double b = 1.0;
903  double c = -B;
904  return std::abs(a*r+b*z+c)/sqrt(square(a)+square(b));
905  });
906  return residues;
907 }